From 7979de69537f45600e5e926dcdf5e302b09af61b Mon Sep 17 00:00:00 2001
From: Giovanni Di Sirio <gdisirio@gmail.com>
Date: Mon, 21 Dec 2015 11:20:33 +0000
Subject: Input buffers queue working now.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@8629 35acf78f-673a-0410-8e92-d51de3d6d3f4
---
 os/hal/src/hal_buffers.c | 33 +++++++++++++++++++++++++++++----
 1 file changed, 29 insertions(+), 4 deletions(-)

(limited to 'os/hal/src/hal_buffers.c')

diff --git a/os/hal/src/hal_buffers.c b/os/hal/src/hal_buffers.c
index 8e93d7700..85a26383b 100644
--- a/os/hal/src/hal_buffers.c
+++ b/os/hal/src/hal_buffers.c
@@ -47,7 +47,7 @@
 /*===========================================================================*/
 
 /**
- * @brief   Initializes an input double buffer object.
+ * @brief   Initializes an input buffers queue object.
  *
  * @param[out] ibqp     pointer to the @p io_buffers_queue_t object
  * @param[in] bp        pointer to a memory area allocated for buffers
@@ -65,6 +65,7 @@ void ibqObjectInit(input_buffers_queue_t *ibqp, uint8_t *bp,
 
   osalDbgCheck((ibqp != NULL) && (bp != NULL) && (size >= sizeof(size_t) + 2));
 
+  osalThreadQueueObjectInit(&ibqp->waiting);
   ibqp->bcounter = 0;
   ibqp->brdptr   = bp;
   ibqp->bwrptr   = bp;
@@ -78,6 +79,29 @@ void ibqObjectInit(input_buffers_queue_t *ibqp, uint8_t *bp,
   ibqp->link     = link;
 }
 
+/**
+ * @brief   Resets an input buffers queue.
+ * @details All the data in the input buffers queue is erased and lost, any
+ *          waiting thread is resumed with status @p MSG_RESET.
+ * @note    A reset operation can be used by a low level driver in order to
+ *          obtain immediate attention from the high level layers.
+ *
+ * @param[in] ibqp      pointer to the @p input_buffers_queue_t object
+ *
+ * @iclass
+ */
+void ibqResetI(input_buffers_queue_t *ibqp) {
+
+  osalDbgCheckClassI();
+
+  ibqp->bcounter = 0;
+  ibqp->brdptr   = ibqp->buffers;
+  ibqp->bwrptr   = ibqp->buffers;
+  ibqp->ptr      = NULL;
+  ibqp->top      = NULL;
+  osalThreadDequeueAllI(&ibqp->waiting, MSG_RESET);
+}
+
 /**
  * @brief   Gets the next empty buffer from the queue.
  * @note    The function always returns the same buffer if called repeatedly.
@@ -152,7 +176,7 @@ msg_t ibqGetDataTimeoutI(input_buffers_queue_t *ibqp, systime_t timeout) {
 
   while (ibqIsEmptyI(ibqp)) {
     msg_t msg = osalThreadEnqueueTimeoutS(&ibqp->waiting, timeout);
-    if (msg < Q_OK) {
+    if (msg < MSG_OK) {
       return msg;
     }
   }
@@ -299,8 +323,9 @@ size_t ibqReadTimeout(input_buffers_queue_t *ibqp, uint8_t *bp,
     memcpy(bp, ibqp->ptr, size);
     osalSysLock();
 
-    /* Updating the pointers.*/
-    bp += size;
+    /* Updating the pointers and the counter.*/
+    r         += size;
+    bp        += size;
     ibqp->ptr += size;
 
     /* Has the current data buffer been finished? if so then release it.*/
-- 
cgit v1.2.3