1 files changed, 8 insertions, 8 deletions

diff --git a/os/hal/src/hal_queues.c b/os/hal/src/hal_queues.c
index 1e2386c2a..6e1814343 100644
--- a/os/hal/src/hal_queues.c
+++ b/os/hal/src/hal_queues.c
@@ -389,13 +389,13 @@ size_t iqReadI(input_queue_t *iqp, uint8_t *bp, size_t n) {
 size_t iqReadTimeout(input_queue_t *iqp, uint8_t *bp,

                      size_t n, sysinterval_t timeout) {

   qnotify_t nfy = iqp->q_notify;

-  size_t rd = 0;

+  size_t max = n;

 

   osalDbgCheck(n > 0U);

 

   osalSysLock();

 

-  while (rd < n) {

+  while (n > 0U) {

     size_t done;

 

     done = iq_read(iqp, bp, n);

@@ -417,7 +417,7 @@ size_t iqReadTimeout(input_queue_t *iqp, uint8_t *bp,
       /* Giving a preemption chance in a controlled point.*/

       osalSysUnlock();

 

-      rd += done;

+      n  -= done;

       bp += done;

 

       osalSysLock();

@@ -425,7 +425,7 @@ size_t iqReadTimeout(input_queue_t *iqp, uint8_t *bp,
   }

 

   osalSysUnlock();

-  return rd;

+  return max - n;

 }

 

 /**

@@ -658,13 +658,13 @@ size_t oqWriteI(output_queue_t *oqp, const uint8_t *bp, size_t n) {
 size_t oqWriteTimeout(output_queue_t *oqp, const uint8_t *bp,

                       size_t n, sysinterval_t timeout) {

   qnotify_t nfy = oqp->q_notify;

-  size_t wr = 0;

+  size_t max = n;

 

   osalDbgCheck(n > 0U);

 

   osalSysLock();

 

-  while (wr < n) {

+  while (n > 0U) {

     size_t done;

 

     done = oq_write(oqp, bp, n);

@@ -686,7 +686,7 @@ size_t oqWriteTimeout(output_queue_t *oqp, const uint8_t *bp,
       /* Giving a preemption chance in a controlled point.*/

       osalSysUnlock();

 

-      wr += done;

+      n  -= done;

       bp += done;

 

       osalSysLock();

@@ -694,7 +694,7 @@ size_t oqWriteTimeout(output_queue_t *oqp, const uint8_t *bp,
   }

 

   osalSysUnlock();

-  return wr;

+  return max - n;

 }

 

 /** @} */