New mailboxes implementation.

git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@10150 35acf78f-673a-0410-8e92-d51de3d6d3f4

2 files changed, 177 insertions, 82 deletions

diff --git a/os/common/oslib/include/chmboxes.h b/os/common/oslib/include/chmboxes.h
index cc1d743df..8b4498e3d 100644
--- a/os/common/oslib/include/chmboxes.h
+++ b/os/common/oslib/include/chmboxes.h
@@ -60,10 +60,10 @@ typedef struct {
                                                     after the buffer.       */

   msg_t                 *wrptr;         /**< @brief Write pointer.          */

   msg_t                 *rdptr;         /**< @brief Read pointer.           */

-  semaphore_t           fullsem;        /**< @brief Full counter

-                                                    @p semaphore_t.         */

-  semaphore_t           emptysem;       /**< @brief Empty counter

-                                                    @p semaphore_t.         */

+  cnt_t                 cnt;            /**< @brief Messages in queue.      */

+  bool                  reset;          /**< @brief True in reset state.    */

+  threads_queue_t       qw;             /**< @brief Queued writers.         */

+  threads_queue_t       qr;             /**< @brief Queued readers.         */

 } mailbox_t;

 

 /*===========================================================================*/

@@ -84,8 +84,10 @@ typedef struct {
   (msg_t *)(buffer) + size,                                                 \

   (msg_t *)(buffer),                                                        \

   (msg_t *)(buffer),                                                        \

-  _SEMAPHORE_DATA(name.fullsem, 0),                                         \

-  _SEMAPHORE_DATA(name.emptysem, size),                                     \

+  (cnt_t)0,                                                                 \

+  false,                                                                    \

+  _THREADS_QUEUE_DATA(name.qw),                                             \

+  _THREADS_QUEUE_DATA(name.qr),                                             \

 }

 

 /**

@@ -128,57 +130,54 @@ extern "C" {
 /*===========================================================================*/

 

 /**

- * @brief   Returns the mailbox buffer size.

+ * @brief   Returns the mailbox buffer size as number of messages.

  *

  * @param[in] mbp       the pointer to an initialized mailbox_t object

  * @return              The size of the mailbox.

  *

  * @iclass

  */

-static inline size_t chMBGetSizeI(mailbox_t *mbp) {

+static inline cnt_t chMBGetSizeI(mailbox_t *mbp) {

 

   /*lint -save -e9033 [10.8] Perfectly safe pointers

     arithmetic.*/

-  return (size_t)(mbp->top - mbp->buffer);

+  return (cnt_t)(mbp->top - mbp->buffer);

   /*lint -restore*/

 }

 

 /**

- * @brief   Returns the number of free message slots into a mailbox.

- * @note    Can be invoked in any system state but if invoked out of a locked

- *          state then the returned value may change after reading.

- * @note    The returned value can be less than zero when there are waiting

- *          threads on the internal semaphore.

+ * @brief   Returns the number of used message slots into a mailbox.

  *

  * @param[in] mbp       the pointer to an initialized mailbox_t object

- * @return              The number of empty message slots.

+ * @return              The number of queued messages.

+ * @retval QUEUE_RESET  if the queue is in reset state.

  *

  * @iclass

  */

-static inline cnt_t chMBGetFreeCountI(mailbox_t *mbp) {

+static inline cnt_t chMBGetUsedCountI(mailbox_t *mbp) {

 

   chDbgCheckClassI();

 

-  return chSemGetCounterI(&mbp->emptysem);

+  return mbp->cnt;

 }

 

 /**

- * @brief   Returns the number of used message slots into a mailbox.

+ * @brief   Returns the number of free message slots into a mailbox.

  * @note    Can be invoked in any system state but if invoked out of a locked

  *          state then the returned value may change after reading.

  * @note    The returned value can be less than zero when there are waiting

  *          threads on the internal semaphore.

  *

  * @param[in] mbp       the pointer to an initialized mailbox_t object

- * @return              The number of queued messages.

+ * @return              The number of empty message slots.

  *

  * @iclass

  */

-static inline cnt_t chMBGetUsedCountI(mailbox_t *mbp) {

+static inline cnt_t chMBGetFreeCountI(mailbox_t *mbp) {

 

   chDbgCheckClassI();

 

-  return chSemGetCounterI(&mbp->fullsem);

+  return chMBGetSizeI(mbp) - chMBGetUsedCountI(mbp);

 }

 

 /**

@@ -200,6 +199,18 @@ static inline msg_t chMBPeekI(mailbox_t *mbp) {
   return *mbp->rdptr;

 }

 

+/**

+ * @brief   Terminates the reset state.

+ *

+ * @param[in] mbp       the pointer to an initialized mailbox_t object

+ *

+ * @xclass

+ */

+static inline void chMBResumeX(mailbox_t *mbp) {

+

+  mbp->reset = false;

+}

+

 #endif /* CH_CFG_USE_MAILBOXES == TRUE */

 

 #endif /* CHMBOXES_H */

diff --git a/os/common/oslib/src/chmboxes.c b/os/common/oslib/src/chmboxes.c
index 6a5bc7e65..36e842821 100644
--- a/os/common/oslib/src/chmboxes.c
+++ b/os/common/oslib/src/chmboxes.c
@@ -89,17 +89,22 @@ void chMBObjectInit(mailbox_t *mbp, msg_t *buf, cnt_t n) {
   chDbgCheck((mbp != NULL) && (buf != NULL) && (n > (cnt_t)0));

 

   mbp->buffer = buf;

-  mbp->rdptr = buf;

-  mbp->wrptr = buf;

-  mbp->top = &buf[n];

-  chSemObjectInit(&mbp->emptysem, n);

-  chSemObjectInit(&mbp->fullsem, (cnt_t)0);

+  mbp->rdptr  = buf;

+  mbp->wrptr  = buf;

+  mbp->top    = &buf[n];

+  mbp->cnt    = (cnt_t)0;

+  mbp->reset  = false;

+  chThdQueueObjectInit(&mbp->qw);

+  chThdQueueObjectInit(&mbp->qr);

 }

 

 /**

  * @brief   Resets a @p mailbox_t object.

  * @details All the waiting threads are resumed with status @p MSG_RESET and

  *          the queued messages are lost.

+ * @post    The mailbox is in reset state, all operations will fail and

+ *          return @p MSG reset until the mailbox is enabled again using

+ *          @p chMBResumeX().

  *

  * @param[in] mbp       the pointer to an initialized @p mailbox_t object

  *

@@ -117,6 +122,9 @@ void chMBReset(mailbox_t *mbp) {
  * @brief   Resets a @p mailbox_t object.

  * @details All the waiting threads are resumed with status @p MSG_RESET and

  *          the queued messages are lost.

+ * @post    The mailbox is in reset state, all operations will fail and

+ *          return @p MSG reset until the mailbox is enabled again using

+ *          @p chMBResumeX().

  *

  * @param[in] mbp       the pointer to an initialized @p mailbox_t object

  *

@@ -129,8 +137,10 @@ void chMBResetI(mailbox_t *mbp) {
 

   mbp->wrptr = mbp->buffer;

   mbp->rdptr = mbp->buffer;

-  chSemResetI(&mbp->emptysem, (cnt_t)(mbp->top - mbp->buffer));

-  chSemResetI(&mbp->fullsem, (cnt_t)0);

+  mbp->cnt   = (cnt_t)0;

+  mbp->reset = true;

+  chThdDequeueAllI(&mbp->qw, MSG_RESET);

+  chThdDequeueAllI(&mbp->qr, MSG_RESET);

 }

 

 /**

@@ -147,7 +157,7 @@ void chMBResetI(mailbox_t *mbp) {
  *                      .

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly posted.

- * @retval MSG_RESET    if the mailbox has been reset while waiting.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the operation has timed out.

  *

  * @api

@@ -176,7 +186,7 @@ msg_t chMBPost(mailbox_t *mbp, msg_t msg, systime_t timeout) {
  *                      .

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly posted.

- * @retval MSG_RESET    if the mailbox has been reset while waiting.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the operation has timed out.

  *

  * @sclass

@@ -187,15 +197,29 @@ msg_t chMBPostS(mailbox_t *mbp, msg_t msg, systime_t timeout) {
   chDbgCheckClassS();

   chDbgCheck(mbp != NULL);

 

-  rdymsg = chSemWaitTimeoutS(&mbp->emptysem, timeout);

-  if (rdymsg == MSG_OK) {

-    *mbp->wrptr++ = msg;

-    if (mbp->wrptr >= mbp->top) {

-      mbp->wrptr = mbp->buffer;

+  do {

+    /* If the mailbox is in reset state then returns immediately.*/

+    if (mbp->reset) {

+      return MSG_RESET;

     }

-    chSemSignalI(&mbp->fullsem);

-    chSchRescheduleS();

-  }

+

+    /* Is there a free message slot in queue? if so then post.*/

+    if (chMBGetFreeCountI(mbp) > (cnt_t)0) {

+      *mbp->wrptr++ = msg;

+      if (mbp->wrptr >= mbp->top) {

+        mbp->wrptr = mbp->buffer;

+      }

+      mbp->cnt++;

+

+      /* If there is a reader waiting then makes it ready.*/

+      chThdDequeueNextI(&mbp->qr, MSG_OK);

+

+      return MSG_OK;

+    }

+

+    /* No space in the queue, waiting for a slot to become available.*/

+    rdymsg = chThdEnqueueTimeoutS(&mbp->qw, timeout);

+  } while (rdymsg == MSG_OK);

 

   return rdymsg;

 }

@@ -209,6 +233,7 @@ msg_t chMBPostS(mailbox_t *mbp, msg_t msg, systime_t timeout) {
  * @param[in] msg       the message to be posted on the mailbox

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly posted.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the mailbox is full and the message cannot be

  *                      posted.

  *

@@ -219,18 +244,27 @@ msg_t chMBPostI(mailbox_t *mbp, msg_t msg) {
   chDbgCheckClassI();

   chDbgCheck(mbp != NULL);

 

-  if (chSemGetCounterI(&mbp->emptysem) <= (cnt_t)0) {

-    return MSG_TIMEOUT;

+  /* If the mailbox is in reset state then returns immediately.*/

+  if (mbp->reset) {

+    return MSG_RESET;

   }

 

-  chSemFastWaitI(&mbp->emptysem);

-  *mbp->wrptr++ = msg;

-  if (mbp->wrptr >= mbp->top) {

-     mbp->wrptr = mbp->buffer;

+  /* Is there a free message slot in queue? if so then post.*/

+  if (chMBGetFreeCountI(mbp) > (cnt_t)0) {

+    *mbp->wrptr++ = msg;

+    if (mbp->wrptr >= mbp->top) {

+      mbp->wrptr = mbp->buffer;

+    }

+    mbp->cnt++;

+

+    /* If there is a reader waiting then makes it ready.*/

+    chThdDequeueNextI(&mbp->qr, MSG_OK);

+

+    return MSG_OK;

   }

-  chSemSignalI(&mbp->fullsem);

 

-  return MSG_OK;

+  /* No space, immediate timeout.*/

+  return MSG_TIMEOUT;

 }

 

 /**

@@ -247,7 +281,7 @@ msg_t chMBPostI(mailbox_t *mbp, msg_t msg) {
  *                      .

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly posted.

- * @retval MSG_RESET    if the mailbox has been reset while waiting.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the operation has timed out.

  *

  * @api

@@ -276,7 +310,7 @@ msg_t chMBPostAhead(mailbox_t *mbp, msg_t msg, systime_t timeout) {
  *                      .

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly posted.

- * @retval MSG_RESET    if the mailbox has been reset while waiting.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the operation has timed out.

  *

  * @sclass

@@ -287,15 +321,29 @@ msg_t chMBPostAheadS(mailbox_t *mbp, msg_t msg, systime_t timeout) {
   chDbgCheckClassS();

   chDbgCheck(mbp != NULL);

 

-  rdymsg = chSemWaitTimeoutS(&mbp->emptysem, timeout);

-  if (rdymsg == MSG_OK) {

-    if (--mbp->rdptr < mbp->buffer) {

-      mbp->rdptr = mbp->top - 1;

+  do {

+    /* If the mailbox is in reset state then returns immediately.*/

+    if (mbp->reset) {

+      return MSG_RESET;

     }

-    *mbp->rdptr = msg;

-    chSemSignalI(&mbp->fullsem);

-    chSchRescheduleS();

-  }

+

+    /* Is there a free message slot in queue? if so then post.*/

+    if (chMBGetFreeCountI(mbp) > (cnt_t)0) {

+      if (--mbp->rdptr < mbp->buffer) {

+        mbp->rdptr = mbp->top - 1;

+      }

+      *mbp->rdptr = msg;

+      mbp->cnt++;

+

+      /* If there is a reader waiting then makes it ready.*/

+      chThdDequeueNextI(&mbp->qr, MSG_OK);

+

+      return MSG_OK;

+    }

+

+    /* No space in the queue, waiting for a slot to become available.*/

+    rdymsg = chThdEnqueueTimeoutS(&mbp->qw, timeout);

+  } while (rdymsg == MSG_OK);

 

   return rdymsg;

 }

@@ -309,6 +357,7 @@ msg_t chMBPostAheadS(mailbox_t *mbp, msg_t msg, systime_t timeout) {
  * @param[in] msg       the message to be posted on the mailbox

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly posted.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the mailbox is full and the message cannot be

  *                      posted.

  *

@@ -319,17 +368,27 @@ msg_t chMBPostAheadI(mailbox_t *mbp, msg_t msg) {
   chDbgCheckClassI();

   chDbgCheck(mbp != NULL);

 

-  if (chSemGetCounterI(&mbp->emptysem) <= (cnt_t)0) {

-    return MSG_TIMEOUT;

+  /* If the mailbox is in reset state then returns immediately.*/

+  if (mbp->reset) {

+    return MSG_RESET;

   }

-  chSemFastWaitI(&mbp->emptysem);

-  if (--mbp->rdptr < mbp->buffer) {

-    mbp->rdptr = mbp->top - 1;

+

+  /* Is there a free message slot in queue? if so then post.*/

+  if (chMBGetFreeCountI(mbp) > (cnt_t)0) {

+    if (--mbp->rdptr < mbp->buffer) {

+      mbp->rdptr = mbp->top - 1;

+    }

+    *mbp->rdptr = msg;

+    mbp->cnt++;

+

+    /* If there is a reader waiting then makes it ready.*/

+    chThdDequeueNextI(&mbp->qr, MSG_OK);

+

+    return MSG_OK;

   }

-  *mbp->rdptr = msg;

-  chSemSignalI(&mbp->fullsem);

 

-  return MSG_OK;

+  /* No space, immediate timeout.*/

+  return MSG_TIMEOUT;

 }

 

 /**

@@ -346,7 +405,7 @@ msg_t chMBPostAheadI(mailbox_t *mbp, msg_t msg) {
  *                      .

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly fetched.

- * @retval MSG_RESET    if the mailbox has been reset while waiting.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the operation has timed out.

  *

  * @api

@@ -375,7 +434,7 @@ msg_t chMBFetch(mailbox_t *mbp, msg_t *msgp, systime_t timeout) {
  *                      .

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly fetched.

- * @retval MSG_RESET    if the mailbox has been reset while waiting.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the operation has timed out.

  *

  * @sclass

@@ -386,15 +445,29 @@ msg_t chMBFetchS(mailbox_t *mbp, msg_t *msgp, systime_t timeout) {
   chDbgCheckClassS();

   chDbgCheck((mbp != NULL) && (msgp != NULL));

 

-  rdymsg = chSemWaitTimeoutS(&mbp->fullsem, timeout);

-  if (rdymsg == MSG_OK) {

-    *msgp = *mbp->rdptr++;

-    if (mbp->rdptr >= mbp->top) {

-      mbp->rdptr = mbp->buffer;

+  do {

+    /* If the mailbox is in reset state then returns immediately.*/

+    if (mbp->reset) {

+      return MSG_RESET;

     }

-    chSemSignalI(&mbp->emptysem);

-    chSchRescheduleS();

-  }

+

+    /* Is there a message in queue? if so then fetch.*/

+    if (chMBGetUsedCountI(mbp) > (cnt_t)0) {

+      *msgp = *mbp->rdptr++;

+      if (mbp->rdptr >= mbp->top) {

+        mbp->rdptr = mbp->buffer;

+      }

+      mbp->cnt--;

+

+      /* If there is a writer waiting then makes it ready.*/

+      chThdDequeueNextI(&mbp->qw, MSG_OK);

+

+      return MSG_OK;

+    }

+

+    /* No message in the queue, waiting for a message to become available.*/

+    rdymsg = chThdEnqueueTimeoutS(&mbp->qr, timeout);

+  } while (rdymsg == MSG_OK);

 

   return rdymsg;

 }

@@ -408,6 +481,7 @@ msg_t chMBFetchS(mailbox_t *mbp, msg_t *msgp, systime_t timeout) {
  * @param[out] msgp     pointer to a message variable for the received message

  * @return              The operation status.

  * @retval MSG_OK       if a message has been correctly fetched.

+ * @retval MSG_RESET    if the mailbox has been reset.

  * @retval MSG_TIMEOUT  if the mailbox is empty and a message cannot be

  *                      fetched.

  *

@@ -418,17 +492,27 @@ msg_t chMBFetchI(mailbox_t *mbp, msg_t *msgp) {
   chDbgCheckClassI();

   chDbgCheck((mbp != NULL) && (msgp != NULL));

 

-  if (chSemGetCounterI(&mbp->fullsem) <= (cnt_t)0) {

-    return MSG_TIMEOUT;

+  /* If the mailbox is in reset state then returns immediately.*/

+  if (mbp->reset) {

+    return MSG_RESET;

   }

-  chSemFastWaitI(&mbp->fullsem);

-  *msgp = *mbp->rdptr++;

-  if (mbp->rdptr >= mbp->top) {

-    mbp->rdptr = mbp->buffer;

+

+  /* Is there a message in queue? if so then fetch.*/

+  if (chMBGetUsedCountI(mbp) > (cnt_t)0) {

+    *msgp = *mbp->rdptr++;

+    if (mbp->rdptr >= mbp->top) {

+      mbp->rdptr = mbp->buffer;

+    }

+    mbp->cnt--;

+

+    /* If there is a writer waiting then makes it ready.*/

+    chThdDequeueNextI(&mbp->qw, MSG_OK);

+

+    return MSG_OK;

   }

-  chSemSignalI(&mbp->emptysem);

 

-  return MSG_OK;

+  /* No message, immediate timeout.*/

+  return MSG_TIMEOUT;

 }

 #endif /* CH_CFG_USE_MAILBOXES == TRUE */