From 807c5f1882224c2afd471a44889b83c2adf80589 Mon Sep 17 00:00:00 2001 From: gdisirio Date: Wed, 18 May 2011 17:54:55 +0000 Subject: Fixed bug 3303908. git-svn-id: svn://svn.code.sf.net/p/chibios/svn/trunk@2972 35acf78f-673a-0410-8e92-d51de3d6d3f4 --- os/kernel/src/chmtx.c | 8 +-- os/kernel/src/chqueues.c | 124 +++++++++++++++++++++++++---------------------- os/kernel/src/chschd.c | 6 ++- 3 files changed, 75 insertions(+), 63 deletions(-) (limited to 'os/kernel/src') diff --git a/os/kernel/src/chmtx.c b/os/kernel/src/chmtx.c index af2b7f347..df71d1cc6 100644 --- a/os/kernel/src/chmtx.c +++ b/os/kernel/src/chmtx.c @@ -134,14 +134,16 @@ void chMtxLockS(Mutex *mp) { prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp); tp = ((Mutex *)tp->p_u.wtobjp)->m_owner; continue; -#if CH_USE_CONDVARS | CH_USE_SEMAPHORES_PRIORITY | CH_USE_MESSAGES_PRIORITY +#if CH_USE_CONDVARS | \ + (CH_USE_SEMAPHORES && CH_USE_SEMAPHORES_PRIORITY) | \ + (CH_USE_MESSAGES && CH_USE_MESSAGES_PRIORITY) #if CH_USE_CONDVARS case THD_STATE_WTCOND: #endif -#if CH_USE_SEMAPHORES_PRIORITY +#if CH_USE_SEMAPHORES && CH_USE_SEMAPHORES_PRIORITY case THD_STATE_WTSEM: #endif -#if CH_USE_MESSAGES_PRIORITY +#if CH_USE_MESSAGES && CH_USE_MESSAGES_PRIORITY case THD_STATE_SNDMSGQ: #endif /* Re-enqueues tp with its new priority on the queue.*/ 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 @@ -47,6 +47,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 @@ -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; diff --git a/os/kernel/src/chschd.c b/os/kernel/src/chschd.c index 54e7918b1..d41649b4c 100644 --- a/os/kernel/src/chschd.c +++ b/os/kernel/src/chschd.c @@ -130,12 +130,16 @@ static void wakeup(void *p) { /* Handling the special case where the thread has been made ready by another thread with higher priority.*/ return; -#if CH_USE_SEMAPHORES || (CH_USE_CONDVARS && CH_USE_CONDVARS_TIMEOUT) +#if CH_USE_SEMAPHORES || CH_USE_QUEUES || \ + (CH_USE_CONDVARS && CH_USE_CONDVARS_TIMEOUT) #if CH_USE_SEMAPHORES case THD_STATE_WTSEM: chSemFastSignalI((Semaphore *)tp->p_u.wtobjp); /* Falls into, intentional. */ #endif +#if CH_USE_QUEUES + case THD_STATE_WTQUEUE: +#endif #if CH_USE_CONDVARS && CH_USE_CONDVARS_TIMEOUT case THD_STATE_WTCOND: #endif -- cgit v1.2.3