zcy
/
tinySA
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

master
gdisirio 2013-06-16 16:30:00 +00:00
parent 2e4ba09bb5
commit 745d8c1504
12 changed files with 230 additions and 199 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
os/hal/platforms/STM32/OTGv1/usb_lld.c
View File

@ -318,7 +318,7 @@ static void otg_fifo_write_from_queue(volatile uint32_t *fifop,
chSysLock();
oqp->q_counter += n;
while (queue_notempty(&oqp->q_waiting))
chSchReadyI(fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_OK;
chSchReadyI(queue_fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_OK;
chSchRescheduleS();
chSysUnlock();
}
@ -430,7 +430,7 @@ static void otg_fifo_read_to_queue(volatile uint32_t *fifop,
chSysLock();
iqp->q_counter += n;
while (queue_notempty(&iqp->q_waiting))
chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK;
chSchReadyI(queue_fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK;
chSchRescheduleS();
chSysUnlock();
}

6
os/kernel/include/ch.h
View File

@ -100,14 +100,17 @@
#endif
/** @} */
/* Forward declaration of the thread structure, it is used in most
modules.*/
typedef struct Thread Thread;
/* Inclusion of all the kernel sub-headers.*/
#include "chconf.h"
#include "chtypes.h"
#include "chdebug.h"
#include "chcore.h"
#include "chthreads.h"
//#include "chlists.h"
#include "chlists.h"
#include "chsys.h"
#include "chvt.h"
#include "chschd.h"
@ -123,7 +126,6 @@ typedef struct Thread Thread;
#include "chmempools.h"
#include "chdynamic.h"
#include "chregistry.h"
#include "chinline.h"
#include "chqueues.h"
#include "chstreams.h"
#include "chfiles.h"

87
os/kernel/include/chinline.h
View File

@ -1,87 +0,0 @@
/*
ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
2011,2012,2013 Giovanni Di Sirio.
This file is part of ChibiOS/RT.
ChibiOS/RT is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.
ChibiOS/RT is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/**
* @file chinline.h
* @brief Kernel inlined functions.
* @details In this file there are a set of inlined functions if the
* @p CH_OPTIMIZE_SPEED is enabled.
*/
#ifndef _CHINLINE_H_
#define _CHINLINE_H_
/* If the performance code path has been chosen then all the following
functions are inlined into the various kernel modules.*/
#if CH_OPTIMIZE_SPEED
static INLINE void prio_insert(Thread *tp, ThreadsQueue *tqp) {
Thread *cp = (Thread *)tqp;
do {
cp = cp->p_next;
} while ((cp != (Thread *)tqp) && (cp->p_prio >= tp->p_prio));
tp->p_next = cp;
tp->p_prev = cp->p_prev;
tp->p_prev->p_next = cp->p_prev = tp;
}
static INLINE void queue_insert(Thread *tp, ThreadsQueue *tqp) {
tp->p_next = (Thread *)tqp;
tp->p_prev = tqp->p_prev;
tp->p_prev->p_next = tqp->p_prev = tp;
}
static INLINE Thread *fifo_remove(ThreadsQueue *tqp) {
Thread *tp = tqp->p_next;
(tqp->p_next = tp->p_next)->p_prev = (Thread *)tqp;
return tp;
}
static INLINE Thread *lifo_remove(ThreadsQueue *tqp) {
Thread *tp = tqp->p_prev;
(tqp->p_prev = tp->p_prev)->p_next = (Thread *)tqp;
return tp;
}
static INLINE Thread *dequeue(Thread *tp) {
tp->p_prev->p_next = tp->p_next;
tp->p_next->p_prev = tp->p_prev;
return tp;
}
static INLINE void list_insert(Thread *tp, ThreadsList *tlp) {
tp->p_next = tlp->p_next;
tlp->p_next = tp;
}
static INLINE Thread *list_remove(ThreadsList *tlp) {
Thread *tp = tlp->p_next;
tlp->p_next = tp->p_next;
return tp;
}
#endif /* CH_OPTIMIZE_SPEED */
#endif /* _CHINLINE_H_ */

194
os/kernel/include/chlists.h Normal file
View File

@ -0,0 +1,194 @@
/*
ChibiOS/RT - Copyright (C) 2006,2007,2008,2009,2010,
2011,2012,2013 Giovanni Di Sirio.
This file is part of ChibiOS/RT.
ChibiOS/RT is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.
ChibiOS/RT is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/**
* @file chlists.h
* @brief Thread queues/lists inlined code.
*
* @addtogroup internals
* @{
*/
#ifndef _CHLISTS_H_
#define _CHLISTS_H_
/*===========================================================================*/
/* Module constants. */
/*===========================================================================*/
/*===========================================================================*/
/* Module pre-compile time settings. */
/*===========================================================================*/
/*===========================================================================*/
/* Derived constants and error checks. */
/*===========================================================================*/
/*===========================================================================*/
/* Module data structures and types. */
/*===========================================================================*/
/*===========================================================================*/
/* Module macros. */
/*===========================================================================*/
/**
* @brief Data part of a static threads queue initializer.
* @details This macro should be used when statically initializing a threads
* queue that is part of a bigger structure.
*
* @param[in] name the name of the threads queue variable
*/
#define _THREADSQUEUE_DATA(name) {(Thread *)&name, (Thread *)&name}
/**
* @brief Static threads queue initializer.
* @details Statically initialized threads queues require no explicit
* initialization using @p queue_init().
*
* @param[in] name the name of the threads queue variable
*/
#define THREADSQUEUE_DECL(name) ThreadsQueue name = _THREADSQUEUE_DATA(name)
/*===========================================================================*/
/* External declarations. */
/*===========================================================================*/
/*===========================================================================*/
/* Module inline functions. */
/*===========================================================================*/
/**
* @brief Threads list initialization.
*
* @notapi
*/
static inline void list_init(ThreadsList *tlp) {
tlp->p_next = (Thread *)tlp;
}
/**
* @brief Evaluates to @p TRUE if the specified threads list is empty.
*
* @notapi
*/
static inline bool_t list_isempty(ThreadsList *tlp) {
return (bool_t)(tlp->p_next == (Thread *)tlp);
}
/**
* @brief Evaluates to @p TRUE if the specified threads list is not empty.
*
* @notapi
*/
static inline bool_t list_notempty(ThreadsList *tlp) {
return (bool_t)(tlp->p_next != (Thread *)tlp);
}
/**
* @brief Threads queue initialization.
*
* @notapi
*/
static inline void queue_init(ThreadsQueue *tqp) {
tqp->p_next = tqp->p_prev = (Thread *)tqp;
}
/**
* @brief Evaluates to @p TRUE if the specified threads queue is empty.
*
* @notapi
*/
static inline bool_t queue_isempty(ThreadsQueue *tqp) {
return (bool_t)(tqp->p_next == (Thread *)tqp);
}
/**
* @brief Evaluates to @p TRUE if the specified threads queue is not empty.
*
* @notapi
*/
static inline bool_t queue_notempty(ThreadsQueue *tqp) {
return (bool_t)(tqp->p_next != (Thread *)tqp);
}
/* If the performance code path has been chosen then all the following
functions are inlined into the various kernel modules.*/
#if CH_OPTIMIZE_SPEED
static inline void list_insert(Thread *tp, ThreadsList *tlp) {
tp->p_next = tlp->p_next;
tlp->p_next = tp;
}
static inline Thread *list_remove(ThreadsList *tlp) {
Thread *tp = tlp->p_next;
tlp->p_next = tp->p_next;
return tp;
}
static inline void queue_prio_insert(Thread *tp, ThreadsQueue *tqp) {
Thread *cp = (Thread *)tqp;
do {
cp = cp->p_next;
} while ((cp != (Thread *)tqp) && (cp->p_prio >= tp->p_prio));
tp->p_next = cp;
tp->p_prev = cp->p_prev;
tp->p_prev->p_next = cp->p_prev = tp;
}
static inline void queue_insert(Thread *tp, ThreadsQueue *tqp) {
tp->p_next = (Thread *)tqp;
tp->p_prev = tqp->p_prev;
tp->p_prev->p_next = tqp->p_prev = tp;
}
static inline Thread *queue_fifo_remove(ThreadsQueue *tqp) {
Thread *tp = tqp->p_next;
(tqp->p_next = tp->p_next)->p_prev = (Thread *)tqp;
return tp;
}
static inline Thread *queue_lifo_remove(ThreadsQueue *tqp) {
Thread *tp = tqp->p_prev;
(tqp->p_prev = tp->p_prev)->p_next = (Thread *)tqp;
return tp;
}
static inline Thread *queue_dequeue(Thread *tp) {
tp->p_prev->p_next = tp->p_next;
tp->p_next->p_prev = tp->p_prev;
return tp;
}
#endif /* CH_OPTIMIZE_SPEED */
#endif /* _CHLISTS_H_ */

78
os/kernel/include/chthreads.h
View File

@ -265,24 +265,6 @@ typedef msg_t (*tfunc_t)(void *);
/* Module macros. */
/*===========================================================================*/
/**
* @brief Data part of a static threads queue initializer.
* @details This macro should be used when statically initializing a threads
* queue that is part of a bigger structure.
*
* @param[in] name the name of the threads queue variable
*/
#define _THREADSQUEUE_DATA(name) {(Thread *)&name, (Thread *)&name}
/**
* @brief Static threads queue initializer.
* @details Statically initialized threads queues require no explicit
* initialization using @p queue_init().
*
* @param[in] name the name of the threads queue variable
*/
#define THREADSQUEUE_DECL(name) ThreadsQueue name = _THREADSQUEUE_DATA(name)
/*===========================================================================*/
/* External declarations. */
/*===========================================================================*/
@ -437,66 +419,6 @@ extern "C" {
/* Module inline functions. */
/*===========================================================================*/
/**
* @brief Threads list initialization.
*
* @notapi
*/
static inline void list_init(ThreadsList *tlp) {
tlp->p_next = (Thread *)tlp;
}
/**
* @brief Evaluates to @p TRUE if the specified threads list is empty.
*
* @notapi
*/
static inline bool_t list_isempty(ThreadsList *tlp) {
return (bool_t)(tlp->p_next == (Thread *)tlp);
}
/**
* @brief Evaluates to @p TRUE if the specified threads list is not empty.
*
* @notapi
*/
static inline bool_t list_notempty(ThreadsList *tlp) {
return (bool_t)(tlp->p_next != (Thread *)tlp);
}
/**
* @brief Threads queue initialization.
*
* @notapi
*/
static inline void queue_init(ThreadsQueue *tqp) {
tqp->p_next = tqp->p_prev = (Thread *)tqp;
}
/**
* @brief Evaluates to @p TRUE if the specified threads queue is empty.
*
* @notapi
*/
static inline bool_t queue_isempty(ThreadsQueue *tqp) {
return (bool_t)(tqp->p_next == (Thread *)tqp);
}
/**
* @brief Evaluates to @p TRUE if the specified threads queue is not empty.
*
* @notapi
*/
static inline bool_t queue_notempty(ThreadsQueue *tqp) {
return (bool_t)(tqp->p_next != (Thread *)tqp);
}
#endif /* _CHTHREADS_H_ */
/** @} */

10
os/kernel/src/chcond.c
View File

@ -69,7 +69,7 @@ void chCondSignal(CondVar *cp) {
chSysLock();
if (queue_notempty(&cp->c_queue))
chSchWakeupS(fifo_remove(&cp->c_queue), RDY_OK);
chSchWakeupS(queue_fifo_remove(&cp->c_queue), RDY_OK);
chSysUnlock();
}
@ -90,7 +90,7 @@ void chCondSignalI(CondVar *cp) {
chDbgCheck(cp != NULL, "chCondSignalI");
if (queue_notempty(&cp->c_queue))
chSchReadyI(fifo_remove(&cp->c_queue))->p_u.rdymsg = RDY_OK;
chSchReadyI(queue_fifo_remove(&cp->c_queue))->p_u.rdymsg = RDY_OK;
}
/**
@ -128,7 +128,7 @@ void chCondBroadcastI(CondVar *cp) {
ready list in FIFO order. The wakeup message is set to @p RDY_RESET in
order to make a chCondBroadcast() detectable from a chCondSignal().*/
while (cp->c_queue.p_next != (void *)&cp->c_queue)
chSchReadyI(fifo_remove(&cp->c_queue))->p_u.rdymsg = RDY_RESET;
chSchReadyI(queue_fifo_remove(&cp->c_queue))->p_u.rdymsg = RDY_RESET;
}
/**
@ -187,7 +187,7 @@ msg_t chCondWaitS(CondVar *cp) {
mp = chMtxUnlockS();
ctp->p_u.wtobjp = cp;
prio_insert(ctp, &cp->c_queue);
queue_prio_insert(ctp, &cp->c_queue);
chSchGoSleepS(THD_STATE_WTCOND);
msg = ctp->p_u.rdymsg;
chMtxLockS(mp);
@ -272,7 +272,7 @@ msg_t chCondWaitTimeoutS(CondVar *cp, systime_t time) {
mp = chMtxUnlockS();
currp->p_u.wtobjp = cp;
prio_insert(currp, &cp->c_queue);
queue_prio_insert(currp, &cp->c_queue);
msg = chSchGoSleepTimeoutS(THD_STATE_WTCOND, time);
if (msg != RDY_TIMEOUT)
chMtxLockS(mp);

8
os/kernel/src/chlists.c
View File

@ -41,7 +41,7 @@
*
* @notapi
*/
void prio_insert(Thread *tp, ThreadsQueue *tqp) {
void queue_prio_insert(Thread *tp, ThreadsQueue *tqp) {
/* cp iterates over the queue.*/
Thread *cp = (Thread *)tqp;
@ -81,7 +81,7 @@ void queue_insert(Thread *tp, ThreadsQueue *tqp) {
*
* @notapi
*/
Thread *fifo_remove(ThreadsQueue *tqp) {
Thread *queue_fifo_remove(ThreadsQueue *tqp) {
Thread *tp = tqp->p_next;
(tqp->p_next = tp->p_next)->p_prev = (Thread *)tqp;
@ -98,7 +98,7 @@ Thread *fifo_remove(ThreadsQueue *tqp) {
*
* @notapi
*/
Thread *lifo_remove(ThreadsQueue *tqp) {
Thread *queue_lifo_remove(ThreadsQueue *tqp) {
Thread *tp = tqp->p_prev;
(tqp->p_prev = tp->p_prev)->p_next = (Thread *)tqp;
@ -115,7 +115,7 @@ Thread *lifo_remove(ThreadsQueue *tqp) {
*
* @notapi
*/
Thread *dequeue(Thread *tp) {
Thread *queue_dequeue(Thread *tp) {
tp->p_prev->p_next = tp->p_next;
tp->p_next->p_prev = tp->p_prev;

2
os/kernel/src/chmsg.c
View File

@ -102,7 +102,7 @@ Thread *chMsgWait(void) {
chSysLock();
if (!chMsgIsPendingI(currp))
chSchGoSleepS(THD_STATE_WTMSG);
tp = fifo_remove(&currp->p_msgqueue);
tp = queue_fifo_remove(&currp->p_msgqueue);
tp->p_state = THD_STATE_SNDMSG;
chSysUnlock();
return tp;

14
os/kernel/src/chmtx.c
View File

@ -132,7 +132,7 @@ void chMtxLockS(Mutex *mp) {
switch (tp->p_state) {
case THD_STATE_WTMTX:
/* Re-enqueues the mutex owner with its new priority.*/
prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
queue_prio_insert(queue_dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
tp = ((Mutex *)tp->p_u.wtobjp)->m_owner;
continue;
#if CH_USE_CONDVARS | \
@ -148,7 +148,7 @@ void chMtxLockS(Mutex *mp) {
case THD_STATE_SNDMSGQ:
#endif
/* Re-enqueues tp with its new priority on the queue.*/
prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
queue_prio_insert(queue_dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
break;
#endif
case THD_STATE_READY:
@ -157,13 +157,13 @@ void chMtxLockS(Mutex *mp) {
tp->p_state = THD_STATE_CURRENT;
#endif
/* Re-enqueues tp with its new priority on the ready list.*/
chSchReadyI(dequeue(tp));
chSchReadyI(queue_dequeue(tp));
break;
}
break;
}
/* Sleep on the mutex.*/
prio_insert(ctp, &mp->m_queue);
queue_prio_insert(ctp, &mp->m_queue);
ctp->p_u.wtobjp = mp;
chSchGoSleepS(THD_STATE_WTMTX);
/* It is assumed that the thread performing the unlock operation assigns
@ -283,7 +283,7 @@ Mutex *chMtxUnlock(void) {
ctp->p_prio = newprio;
/* Awakens the highest priority thread waiting for the unlocked mutex and
assigns the mutex to it.*/
tp = fifo_remove(&ump->m_queue);
tp = queue_fifo_remove(&ump->m_queue);
ump->m_owner = tp;
ump->m_next = tp->p_mtxlist;
tp->p_mtxlist = ump;
@ -342,7 +342,7 @@ Mutex *chMtxUnlockS(void) {
ctp->p_prio = newprio;
/* Awakens the highest priority thread waiting for the unlocked mutex and
assigns the mutex to it.*/
tp = fifo_remove(&ump->m_queue);
tp = queue_fifo_remove(&ump->m_queue);
ump->m_owner = tp;
ump->m_next = tp->p_mtxlist;
tp->p_mtxlist = ump;
@ -373,7 +373,7 @@ void chMtxUnlockAll(void) {
Mutex *ump = ctp->p_mtxlist;
ctp->p_mtxlist = ump->m_next;
if (chMtxQueueNotEmptyS(ump)) {
Thread *tp = fifo_remove(&ump->m_queue);
Thread *tp = queue_fifo_remove(&ump->m_queue);
ump->m_owner = tp;
ump->m_next = tp->p_mtxlist;
tp->p_mtxlist = ump;

8
os/kernel/src/chqueues.c
View File

@ -114,7 +114,7 @@ void chIQResetI(InputQueue *iqp) {
iqp->q_rdptr = iqp->q_wrptr = iqp->q_buffer;
iqp->q_counter = 0;
while (queue_notempty(&iqp->q_waiting))
chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_RESET;
chSchReadyI(queue_fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_RESET;
}
/**
@ -143,7 +143,7 @@ msg_t chIQPutI(InputQueue *iqp, uint8_t b) {
iqp->q_wrptr = iqp->q_buffer;
if (queue_notempty(&iqp->q_waiting))
chSchReadyI(fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK;
chSchReadyI(queue_fifo_remove(&iqp->q_waiting))->p_u.rdymsg = Q_OK;
return Q_OK;
}
@ -294,7 +294,7 @@ void chOQResetI(OutputQueue *oqp) {
oqp->q_rdptr = oqp->q_wrptr = oqp->q_buffer;
oqp->q_counter = chQSizeI(oqp);
while (queue_notempty(&oqp->q_waiting))
chSchReadyI(fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_RESET;
chSchReadyI(queue_fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_RESET;
}
/**
@ -367,7 +367,7 @@ msg_t chOQGetI(OutputQueue *oqp) {
oqp->q_rdptr = oqp->q_buffer;
if (queue_notempty(&oqp->q_waiting))
chSchReadyI(fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_OK;
chSchReadyI(queue_fifo_remove(&oqp->q_waiting))->p_u.rdymsg = Q_OK;
return b;
}

8
os/kernel/src/chschd.c
View File

@ -116,7 +116,7 @@ void chSchGoSleepS(tstate_t newstate) {
time quantum when it will wakeup.*/
otp->p_preempt = CH_TIME_QUANTUM;
#endif
setcurrp(fifo_remove(&rlist.r_queue));
setcurrp(queue_fifo_remove(&rlist.r_queue));
currp->p_state = THD_STATE_CURRENT;
chSysSwitch(currp, otp);
}
@ -150,7 +150,7 @@ static void wakeup(void *p) {
case THD_STATE_WTCOND:
#endif
/* States requiring dequeuing.*/
dequeue(tp);
queue_dequeue(tp);
#endif
}
tp->p_u.rdymsg = RDY_TIMEOUT;
@ -299,7 +299,7 @@ void chSchDoRescheduleBehind(void) {
otp = currp;
/* Picks the first thread from the ready queue and makes it current.*/
setcurrp(fifo_remove(&rlist.r_queue));
setcurrp(queue_fifo_remove(&rlist.r_queue));
currp->p_state = THD_STATE_CURRENT;
#if CH_TIME_QUANTUM > 0
otp->p_preempt = CH_TIME_QUANTUM;
@ -324,7 +324,7 @@ void chSchDoRescheduleAhead(void) {
otp = currp;
/* Picks the first thread from the ready queue and makes it current.*/
setcurrp(fifo_remove(&rlist.r_queue));
setcurrp(queue_fifo_remove(&rlist.r_queue));
currp->p_state = THD_STATE_CURRENT;
otp->p_state = THD_STATE_READY;

10
os/kernel/src/chsem.c
View File

@ -139,7 +139,7 @@ void chSemResetI(Semaphore *sp, cnt_t n) {
cnt = sp->s_cnt;
sp->s_cnt = n;
while (++cnt <= 0)
chSchReadyI(lifo_remove(&sp->s_queue))->p_u.rdymsg = RDY_RESET;
chSchReadyI(queue_lifo_remove(&sp->s_queue))->p_u.rdymsg = RDY_RESET;
}
/**
@ -278,7 +278,7 @@ void chSemSignal(Semaphore *sp) {
chSysLock();
if (++sp->s_cnt <= 0)
chSchWakeupS(fifo_remove(&sp->s_queue), RDY_OK);
chSchWakeupS(queue_fifo_remove(&sp->s_queue), RDY_OK);
chSysUnlock();
}
@ -305,7 +305,7 @@ void chSemSignalI(Semaphore *sp) {
if (++sp->s_cnt <= 0) {
/* Note, it is done this way in order to allow a tail call on
chSchReadyI().*/
Thread *tp = fifo_remove(&sp->s_queue);
Thread *tp = queue_fifo_remove(&sp->s_queue);
tp->p_u.rdymsg = RDY_OK;
chSchReadyI(tp);
}
@ -335,7 +335,7 @@ void chSemAddCounterI(Semaphore *sp, cnt_t n) {
while (n > 0) {
if (++sp->s_cnt <= 0)
chSchReadyI(fifo_remove(&sp->s_queue))->p_u.rdymsg = RDY_OK;
chSchReadyI(queue_fifo_remove(&sp->s_queue))->p_u.rdymsg = RDY_OK;
n--;
}
}
@ -371,7 +371,7 @@ msg_t chSemSignalWait(Semaphore *sps, Semaphore *spw) {
chSysLock();
if (++sps->s_cnt <= 0)
chSchReadyI(fifo_remove(&sps->s_queue))->p_u.rdymsg = RDY_OK;
chSchReadyI(queue_fifo_remove(&sps->s_queue))->p_u.rdymsg = RDY_OK;
if (--spw->s_cnt < 0) {
Thread *ctp = currp;
sem_insert(ctp, &spw->s_queue);