From 78167cbb9fcc6c3e215c6827d0df5035d1c6d079 Mon Sep 17 00:00:00 2001
From: gdisirio <gdisirio@35acf78f-673a-0410-8e92-d51de3d6d3f4>
Date: Wed, 6 Jan 2010 09:40:39 +0000
Subject: git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@1504
 35acf78f-673a-0410-8e92-d51de3d6d3f4

---
 os/kernel/src/chqueues.c | 64 +++++++++++++++++++++++++++++++++---------------
 1 file changed, 44 insertions(+), 20 deletions(-)

(limited to 'os/kernel/src/chqueues.c')

diff --git a/os/kernel/src/chqueues.c b/os/kernel/src/chqueues.c
index f4111a814..6a951f223 100644
--- a/os/kernel/src/chqueues.c
+++ b/os/kernel/src/chqueues.c
@@ -131,8 +131,8 @@ msg_t chIQGetTimeout(InputQueue *iqp, systime_t time) {
  *          been reset.
  * @note The function is not atomic, if you need atomicity it is suggested
  *       to use a semaphore or a mutex for mutual exclusion.
- * @note The queue callback is invoked one time <b>for each</b> byte removed
- *       from the queue.
+ * @note The queue callback is invoked before entering a sleep state and at
+ *       the end of the transfer.
  *
  * @param[in] iqp pointer to an @p InputQueue structure
  * @param[out] bp pointer to the data buffer
@@ -149,12 +149,18 @@ size_t chIQReadTimeout(InputQueue *iqp, uint8_t *bp,
   qnotify_t nfy = iqp->q_notify;
   size_t r = 0;
 
-  do {
-    chSysLock();
-    if (chSemWaitTimeoutS(&iqp->q_sem, time) != RDY_OK) {
-      chSysUnlock();
-      return r;
+  chSysLock();
+  while (TRUE) {
+    if (chIQIsEmpty(iqp)) {
+      if (nfy)
+        nfy();
+      if ((chSemWaitTimeoutS(&iqp->q_sem, time) != RDY_OK)) {
+        chSysUnlock();
+        return r;
+      }
     }
+    else
+      chSemFastWaitI(&iqp->q_sem);
     *bp++ = *iqp->q_rdptr++;
     if (iqp->q_rdptr >= iqp->q_top)
       iqp->q_rdptr = iqp->q_buffer;
@@ -162,8 +168,15 @@ size_t chIQReadTimeout(InputQueue *iqp, uint8_t *bp,
       nfy();
     chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
     r++;
-  } while (--n > 0);
-  return r;
+    if (--n == 0) {
+      chSysLock();
+      if (nfy)
+        nfy();
+      chSysUnlock();
+      return r;
+    }
+    chSysLock();
+  }
 }
 
 /**
@@ -270,8 +283,8 @@ msg_t chOQGetI(OutputQueue *oqp) {
  *          been reset.
  * @note The function is not atomic, if you need atomicity it is suggested
  *       to use a semaphore or a mutex for mutual exclusion.
- * @note The queue callback is invoked one time <b>for each</b> byte inserted
- *       into the queue.
+ * @note The queue callback is invoked before entering a sleep state and at
+ *       the end of the transfer.
  *
  * @param[in] oqp pointer to an @p OutputQueue structure
  * @param[out] bp pointer to the data buffer
@@ -288,21 +301,32 @@ size_t chOQWriteTimeout(OutputQueue *oqp, const uint8_t *bp,
   qnotify_t nfy = oqp->q_notify;
   size_t w = 0;
 
-  do {
-    chSysLock();
-    if (chSemWaitTimeoutS(&oqp->q_sem, time) != RDY_OK) {
-      chSysUnlock();
-      return w;
+  chSysLock();
+  while (TRUE) {
+    if (chOQIsFull(oqp)) {
+      if (nfy)
+        nfy();
+      if ((chSemWaitTimeoutS(&oqp->q_sem, time) != RDY_OK)) {
+        chSysUnlock();
+        return w;
+      }
     }
+    else
+      chSemFastWaitI(&oqp->q_sem);
     *oqp->q_wrptr++ = *bp++;
     if (oqp->q_wrptr >= oqp->q_top)
       oqp->q_wrptr = oqp->q_buffer;
-    if (nfy)
-      nfy();
     chSysUnlock(); /* Gives a preemption chance in a controlled point.*/
     w++;
-  } while (--n > 0);
-  return w;
+    if (--n == 0) {
+      chSysLock();
+      if (nfy)
+        nfy();
+      chSysUnlock();
+      return w;
+    }
+    chSysLock();
+  }
 }
 #endif  /* CH_USE_QUEUES */
 
-- 
cgit v1.2.3