aboutsummaryrefslogtreecommitdiffstats
path: root/os/kernel/src/chqueues.c
diff options
context:
space:
mode:
Diffstat (limited to 'os/kernel/src/chqueues.c')
-rw-r--r--os/kernel/src/chqueues.c124
1 files changed, 65 insertions, 59 deletions
diff --git a/os/kernel/src/chqueues.c b/os/kernel/src/chqueues.c
index 05fbddfb9..f6ae10257 100644
--- a/os/kernel/src/chqueues.c
+++ b/os/kernel/src/chqueues.c
@@ -48,6 +48,30 @@
#if CH_USE_QUEUES || defined(__DOXYGEN__)
/**
+ * @brief Puts the invoking thread into the queue's threads queue.
+ *
+ * @param[out] qp pointer to an @p GenericQueue structure
+ * @param[in] time the number of ticks before the operation timeouts,
+ * the following special values are allowed:
+ * - @a TIME_IMMEDIATE immediate timeout.
+ * - @a TIME_INFINITE no timeout.
+ * .
+ * @return A message specifying how the invoking thread has been
+ * released from threads queue.
+ * @retval RDY_OK is the normal exit, thread signaled.
+ * @retval RDY_RESET if the queue has been reset.
+ * @retval RDY_TIMEOUT if the queue operation timed out.
+ */
+static msg_t qwait(GenericQueue *qp, systime_t time) {
+
+ if (TIME_IMMEDIATE == time)
+ return RDY_TIMEOUT;
+ currp->p_u.wtobjp = qp;
+ queue_insert(currp, &qp->q_waiting);
+ return chSchGoSleepTimeoutS(THD_STATE_WTQUEUE, time);
+}
+
+/**
* @brief Initializes an input queue.
* @details A Semaphore is internally initialized and works as a counter of
* the bytes contained in the queue.
@@ -64,28 +88,11 @@
*/
void chIQInit(InputQueue *iqp, uint8_t *bp, size_t size, qnotify_t infy) {
+ queue_init(&iqp->q_waiting);
+ iqp->q_counter = 0;
iqp->q_buffer = iqp->q_rdptr = iqp->q_wrptr = bp;
iqp->q_top = bp + size;
iqp->q_notify = infy;
- chSemInit(&iqp->q_sem, 0);
-}
-
-/**
- * @brief Returns the filled space into an input queue.
- *
- * @param[in] iqp pointer to an @p InputQueue structure
- * @return The number of bytes in the queue.
- * @retval 0 if the queue is empty.
- *
- * @iclass
- */
-size_t chIQGetFullI(InputQueue *iqp) {
- cnt_t cnt;
-
- cnt = chQSpaceI(iqp);
- if (cnt < 0)
- return 0;
- return (size_t)cnt;
}
/**
@@ -102,7 +109,9 @@ size_t chIQGetFullI(InputQueue *iqp) {
void chIQResetI(InputQueue *iqp) {
iqp->q_rdptr = iqp->q_wrptr = iqp->q_buffer;
- chSemResetI(&iqp->q_sem, 0);
+ iqp->q_counter = 0;
+ while (notempty(&iqp->q_waiting))
+ chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = RDY_RESET;
}
/**
@@ -123,10 +132,12 @@ msg_t chIQPutI(InputQueue *iqp, uint8_t b) {
if (chIQIsFullI(iqp))
return Q_FULL;
+ iqp->q_counter++;
*iqp->q_wrptr++ = b;
if (iqp->q_wrptr >= iqp->q_top)
iqp->q_wrptr = iqp->q_buffer;
- chSemSignalI(&iqp->q_sem);
+ if (notempty(&iqp->q_waiting))
+ chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = RDY_OK;
return Q_OK;
}
@@ -150,17 +161,21 @@ msg_t chIQPutI(InputQueue *iqp, uint8_t b) {
*/
msg_t chIQGetTimeout(InputQueue *iqp, systime_t time) {
uint8_t b;
- msg_t msg;
chSysLock();
-
if (iqp->q_notify)
iqp->q_notify(iqp);
- if ((msg = chSemWaitTimeoutS(&iqp->q_sem, time)) < RDY_OK) {
- chSysUnlock();
- return msg;
+ while (chIQIsEmptyI(iqp)) {
+ msg_t msg;
+
+ if ((msg = qwait((GenericQueue *)iqp, time)) < RDY_OK) {
+ chSysUnlock();
+ return msg;
+ }
}
+
+ iqp->q_counter--;
b = *iqp->q_rdptr++;
if (iqp->q_rdptr >= iqp->q_top)
iqp->q_rdptr = iqp->q_buffer;
@@ -202,16 +217,16 @@ size_t chIQReadTimeout(InputQueue *iqp, uint8_t *bp,
chSysLock();
while (TRUE) {
- if (chIQIsEmptyI(iqp)) {
+ while (chIQIsEmptyI(iqp)) {
if (nfy)
nfy(iqp);
- if ((chSemWaitTimeoutS(&iqp->q_sem, time) != RDY_OK)) {
+ if (qwait((GenericQueue *)iqp, time) != RDY_OK) {
chSysUnlock();
return r;
}
}
- else
- chSemFastWaitI(&iqp->q_sem);
+
+ iqp->q_counter--;
*bp++ = *iqp->q_rdptr++;
if (iqp->q_rdptr >= iqp->q_top)
iqp->q_rdptr = iqp->q_buffer;
@@ -245,28 +260,11 @@ size_t chIQReadTimeout(InputQueue *iqp, uint8_t *bp,
*/
void chOQInit(OutputQueue *oqp, uint8_t *bp, size_t size, qnotify_t onfy) {
+ queue_init(&oqp->q_waiting);
+ oqp->q_counter = size;
oqp->q_buffer = oqp->q_rdptr = oqp->q_wrptr = bp;
oqp->q_top = bp + size;
oqp->q_notify = onfy;
- chSemInit(&oqp->q_sem, (cnt_t)size);
-}
-
-/**
- * @brief Returns the filled space into an output queue.
- *
- * @param[in] oqp pointer to an @p OutputQueue structure
- * @return The number of bytes in the queue.
- * @retval 0 if the queue is empty.
- *
- * @iclass
- */
-size_t chOQGetFullI(OutputQueue *oqp) {
- cnt_t cnt;
-
- cnt = chQSpaceI(oqp);
- if (cnt < 0)
- return chQSizeI(oqp);
- return chQSizeI(oqp) - (size_t)cnt;
}
/**
@@ -283,7 +281,9 @@ size_t chOQGetFullI(OutputQueue *oqp) {
void chOQResetI(OutputQueue *oqp) {
oqp->q_rdptr = oqp->q_wrptr = oqp->q_buffer;
- chSemResetI(&oqp->q_sem, (cnt_t)(oqp->q_top - oqp->q_buffer));
+ oqp->q_counter = chQSizeI(oqp);
+ while (notempty(&oqp->q_waiting))
+ chSchReadyI(fifo_remove(&oqp->q_waiting))->p_u.rdymsg = RDY_RESET;
}
/**
@@ -307,13 +307,18 @@ void chOQResetI(OutputQueue *oqp) {
* @api
*/
msg_t chOQPutTimeout(OutputQueue *oqp, uint8_t b, systime_t time) {
- msg_t msg;
chSysLock();
- if ((msg = chSemWaitTimeoutS(&oqp->q_sem, time)) < RDY_OK) {
- chSysUnlock();
- return msg;
+ while (chOQIsFullI(oqp)) {
+ msg_t msg;
+
+ if ((msg = qwait((GenericQueue *)oqp, time)) < RDY_OK) {
+ chSysUnlock();
+ return msg;
+ }
}
+
+ oqp->q_counter--;
*oqp->q_wrptr++ = b;
if (oqp->q_wrptr >= oqp->q_top)
oqp->q_wrptr = oqp->q_buffer;
@@ -341,10 +346,12 @@ msg_t chOQGetI(OutputQueue *oqp) {
if (chOQIsEmptyI(oqp))
return Q_EMPTY;
+ oqp->q_counter++;
b = *oqp->q_rdptr++;
if (oqp->q_rdptr >= oqp->q_top)
oqp->q_rdptr = oqp->q_buffer;
- chSemSignalI(&oqp->q_sem);
+ if (notempty(&oqp->q_waiting))
+ chSchReadyI(fifo_remove(&oqp->q_waiting))->p_u.rdymsg = RDY_OK;
return b;
}
@@ -381,16 +388,15 @@ size_t chOQWriteTimeout(OutputQueue *oqp, const uint8_t *bp,
chSysLock();
while (TRUE) {
- if (chOQIsFullI(oqp)) {
+ while (chOQIsFullI(oqp)) {
if (nfy)
nfy(oqp);
- if ((chSemWaitTimeoutS(&oqp->q_sem, time) != RDY_OK)) {
+ if (qwait((GenericQueue *)oqp, time) != RDY_OK) {
chSysUnlock();
return w;
}
}
- else
- chSemFastWaitI(&oqp->q_sem);
+ oqp->q_counter--;
*oqp->q_wrptr++ = *bp++;
if (oqp->q_wrptr >= oqp->q_top)
oqp->q_wrptr = oqp->q_buffer;
generated by cgit v1.2.3 (git 2.25.1) at 2024-08-03 12:18:10 +0000