diff --git a/demos/STM32/RT-STM32L476-DISCOVERY/debug/RT-STM32L476-DISCOVERY (OpenOCD, Flash and Run).launch b/demos/STM32/RT-STM32L476-DISCOVERY/debug/RT-STM32L476-DISCOVERY (OpenOCD, Flash and Run).launch
index ee922c4a8..7e863ea92 100644
--- a/demos/STM32/RT-STM32L476-DISCOVERY/debug/RT-STM32L476-DISCOVERY (OpenOCD, Flash and Run).launch
+++ b/demos/STM32/RT-STM32L476-DISCOVERY/debug/RT-STM32L476-DISCOVERY (OpenOCD, Flash and Run).launch
@@ -1,52 +1,52 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
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,21 +130,37 @@ 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 used message slots into a mailbox.
+ *
+ * @param[in] mbp the pointer to an initialized mailbox_t object
+ * @return The number of queued messages.
+ * @retval QUEUE_RESET if the queue is in reset state.
+ *
+ * @iclass
+ */
+static inline cnt_t chMBGetUsedCountI(mailbox_t *mbp) {
+
+ chDbgCheckClassI();
+
+ return mbp->cnt;
+}
+
/**
* @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
@@ -159,26 +177,7 @@ static inline cnt_t chMBGetFreeCountI(mailbox_t *mbp) {
chDbgCheckClassI();
- return chSemGetCounterI(&mbp->emptysem);
-}
-
-/**
- * @brief Returns the number of used 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.
- *
- * @iclass
- */
-static inline cnt_t chMBGetUsedCountI(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;
- }
- chSemSignalI(&mbp->fullsem);
+ /* 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++;
- return MSG_OK;
+ /* If there is a reader waiting then makes it ready.*/
+ chThdDequeueNextI(&mbp->qr, MSG_OK);
+
+ 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;
- }
- *mbp->rdptr = msg;
- chSemSignalI(&mbp->fullsem);
- return MSG_OK;
+ /* 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, 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;
- }
- chSemSignalI(&mbp->emptysem);
- return MSG_OK;
+ /* 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, immediate timeout.*/
+ return MSG_TIMEOUT;
}
#endif /* CH_CFG_USE_MAILBOXES == TRUE */