static void bmk4_execute(void) {
Thread *tp;
uint32_t n;
tp = threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriority()+1, thread4, NULL);
n = 0;
test_wait_tick();
test_start_timer(1000);
do {
chSysLock();
chSchWakeupS(tp, RDY_OK);
chSchWakeupS(tp, RDY_OK);
chSchWakeupS(tp, RDY_OK);
chSchWakeupS(tp, RDY_OK);
chSysUnlock();
n += 4;
#if defined(SIMULATOR)
ChkIntSources();
#endif
} while (!test_timer_done);
chSysLock();
chSchWakeupS(tp, RDY_TIMEOUT);
chSysUnlock();
test_wait_threads();
test_print("--- Score : ");
test_printn(n * 2);
test_println(" ctxswc/S");
}
/**
* @brief Creates a new thread allocating the memory from the heap.
* @pre The configuration options @p CH_CFG_USE_DYNAMIC and
* @p CH_CFG_USE_HEAP must be enabled in order to use this function.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note The memory allocated for the thread is not released automatically,
* it is responsibility of the creator thread to call @p chThdWait()
* and then release the allocated memory.
*
* @param[in] heapp heap from which allocate the memory or @p NULL for the
* default heap
* @param[in] size size of the working area to be allocated
* @param[in] name thread name
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
* @retval NULL if the memory cannot be allocated.
*
* @api
*/
thread_t *chThdCreateFromHeap(memory_heap_t *heapp, size_t size,
const char *name, tprio_t prio,
tfunc_t pf, void *arg) {
thread_t *tp;
void *wsp;
wsp = chHeapAllocAligned(heapp, size, PORT_WORKING_AREA_ALIGN);
if (wsp == NULL) {
return NULL;
}
thread_descriptor_t td = {
name,
wsp,
(stkalign_t *)((uint8_t *)wsp + size),
prio,
pf,
arg
};
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + size,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateSuspendedI(&td);
tp->flags = CH_FLAG_MODE_HEAP;
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
/**
* @brief Creates a new thread allocating the memory from the heap.
* @pre The configuration options @p CH_USE_DYNAMIC and @p CH_USE_HEAP
* must be enabled in order to use this function.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note The memory allocated for the thread is not released when the thread
* terminates but when a @p chThdWait() is performed.
*
* @param[in] heapp heap from which allocate the memory or @p NULL for the
* default heap
* @param[in] size size of the working area to be allocated
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p Thread structure allocated for
* the thread into the working space area.
* @retval NULL if the memory cannot be allocated.
*
* @api
*/
Thread *chThdCreateFromHeap(MemoryHeap *heapp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
void *wsp;
Thread *tp;
wsp = chHeapAlloc(heapp, size);
if (wsp == NULL)
return NULL;
#if CH_DBG_FILL_THREADS
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + sizeof(Thread),
CH_THREAD_FILL_VALUE);
_thread_memfill((uint8_t *)wsp + sizeof(Thread),
(uint8_t *)wsp + size,
CH_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateI(wsp, size, prio, pf, arg);
tp->p_flags = THD_MEM_MODE_HEAP;
chSchWakeupS(tp, RDY_OK);
chSysUnlock();
return tp;
}
/**
* @brief Creates a new thread allocating the memory from the specified
* memory pool.
* @pre The configuration options @p CH_USE_DYNAMIC and @p CH_USE_MEMPOOLS
* must be enabled in order to use this function.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note The memory allocated for the thread is not released when the thread
* terminates but when a @p chThdWait() is performed.
*
* @param[in] mp pointer to the memory pool object
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p Thread structure allocated for
* the thread into the working space area.
* @retval NULL if the memory pool is empty.
*
* @api
*/
Thread *chThdCreateFromMemoryPool(MemoryPool *mp, tprio_t prio,
tfunc_t pf, void *arg) {
void *wsp;
Thread *tp;
chDbgCheck(mp != NULL, "chThdCreateFromMemoryPool");
wsp = chPoolAlloc(mp);
if (wsp == NULL)
return NULL;
#if CH_DBG_FILL_THREADS
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + sizeof(Thread),
CH_THREAD_FILL_VALUE);
_thread_memfill((uint8_t *)wsp + sizeof(Thread),
(uint8_t *)wsp + mp->mp_object_size,
CH_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateI(wsp, mp->mp_object_size, prio, pf, arg);
tp->p_flags = THD_MEM_MODE_MEMPOOL;
tp->p_mpool = mp;
chSchWakeupS(tp, RDY_OK);
chSysUnlock();
return tp;
}
/**
* @brief Creates a new thread allocating the memory from the specified
* memory pool.
* @pre The configuration options @p CH_CFG_USE_DYNAMIC and
* @p CH_CFG_USE_MEMPOOLS must be enabled in order to use this
* function.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note The memory allocated for the thread is not released when the thread
* terminates but when a @p chThdWait() is performed.
*
* @param[in] mp pointer to the memory pool object
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
* @retval NULL if the memory pool is empty.
*
* @api
*/
thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, tprio_t prio,
tfunc_t pf, void *arg) {
void *wsp;
thread_t *tp;
chDbgCheck(mp != NULL);
wsp = chPoolAlloc(mp);
if (wsp == NULL) {
return NULL;
}
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + sizeof(thread_t),
CH_DBG_THREAD_FILL_VALUE);
_thread_memfill((uint8_t *)wsp + sizeof(thread_t),
(uint8_t *)wsp + mp->mp_object_size,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateI(wsp, mp->mp_object_size, prio, pf, arg);
tp->p_flags = CH_FLAG_MODE_MPOOL;
tp->p_mpool = mp;
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
/**
* @brief Creates a new thread allocating the memory from the heap.
* @pre The configuration options @p CH_CFG_USE_DYNAMIC and
* @p CH_CFG_USE_HEAP must be enabled in order to use this function.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note The memory allocated for the thread is not released when the thread
* terminates but when a @p chThdWait() is performed.
*
* @param[in] heapp heap from which allocate the memory or @p NULL for the
* default heap
* @param[in] size size of the working area to be allocated
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
* @retval NULL if the memory cannot be allocated.
*
* @api
*/
thread_t *chThdCreateFromHeap(memory_heap_t *heapp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
void *wsp;
thread_t *tp;
wsp = chHeapAlloc(heapp, size);
if (wsp == NULL) {
return NULL;
}
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + sizeof(thread_t),
CH_DBG_THREAD_FILL_VALUE);
_thread_memfill((uint8_t *)wsp + sizeof(thread_t),
(uint8_t *)wsp + size,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateI(wsp, size, prio, pf, arg);
tp->p_flags = CH_FLAG_MODE_HEAP;
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
void LedRGB_t::SetColorSmoothly(Color_t AColor) {
INeededColor = AColor;
if(IsSleeping) {
chSysLock();
chSchWakeupS(PThread, 0);
IsSleeping = false;
chSysUnlock();
}
}
/**
* @brief Signals one thread that is waiting on the condition variable.
*
* @param[in] cp pointer to the @p CondVar structure
*
* @api
*/
void chCondSignal(CondVar *cp) {
chDbgCheck(cp != NULL, "chCondSignal");
chSysLock();
if (notempty(&cp->c_queue))
chSchWakeupS(fifo_remove(&cp->c_queue), RDY_OK);
chSysUnlock();
}
/**
* @brief Signals one thread that is waiting on the condition variable.
*
* @param[in] cp pointer to the @p condition_variable_t structure
*
* @api
*/
void chCondSignal(condition_variable_t *cp) {
chDbgCheck(cp != NULL);
chSysLock();
if (queue_notempty(&cp->c_queue))
chSchWakeupS(queue_fifo_remove(&cp->c_queue), MSG_OK);
chSysUnlock();
}
/**
* @brief Resumes a suspended thread.
* @pre The specified thread pointer must refer to an initialized thread
* in the @p THD_STATE_SUSPENDED state.
* @post The specified thread is immediately started or put in the ready
* list depending on the relative priority levels.
* @note Use this function to start threads created with @p chThdInit().
*
* @param[in] tp pointer to the thread
* @return The pointer to the thread.
*
* @api
*/
Thread *chThdResume(Thread *tp) {
chSysLock();
chDbgAssert(tp->p_state == THD_STATE_SUSPENDED,
"chThdResume(), #1",
"thread not in THD_STATE_SUSPENDED state");
chSchWakeupS(tp, RDY_OK);
chSysUnlock();
return tp;
}
static void resumeReader(void)
/*
called whenever debug data is written
to resume the queue reader
*/
{
chSysLock();
if(debugReader->p_state == THD_STATE_WTQUEUE)
chSchWakeupS(debugReader, 0);
chSysUnlock();
}
/**
* @brief Wakes up a thread waiting on a thread reference object.
* @note This function must reschedule, it can only be called from thread
* context.
*
* @param[in] trp a pointer to a thread reference object
* @param[in] msg the message code
*
* @iclass
*/
void chThdResumeS(thread_reference_t *trp, msg_t msg) {
if (*trp != NULL) {
thread_t *tp = *trp;
chDbgAssert(tp->state == CH_STATE_SUSPENDED, "not CH_STATE_SUSPENDED");
*trp = NULL;
chSchWakeupS(tp, msg);
}
}
/**
* @brief Performs a signal operation on a semaphore.
*
* @param[in] sp pointer to a @p semaphore_t structure
*
* @api
*/
void chSemSignal(semaphore_t *sp) {
chDbgCheck(sp != NULL);
chDbgAssert(((sp->s_cnt >= 0) && queue_isempty(&sp->s_queue)) ||
((sp->s_cnt < 0) && queue_notempty(&sp->s_queue)),
"inconsistent semaphore");
chSysLock();
if (++sp->s_cnt <= 0)
chSchWakeupS(queue_fifo_remove(&sp->s_queue), MSG_OK);
chSysUnlock();
}
void wifiWriteDATA(uint8_t *WDATA[] ){
*temda = *WDATA;
// run send can
chSysLock();
thread_t *ntp = tp_W;
if(ntp){
tp_W = NULL;
chSchWakeupS(ntp, MSG_OK);
}
chSysUnlock();
// auto stop
}
/**
* @brief Performs a signal operation on a semaphore.
*
* @param[in] sp pointer to a @p Semaphore structure
*
* @api
*/
void chSemSignal(Semaphore *sp) {
chDbgCheck(sp != NULL, "chSemSignal");
chDbgAssert(((sp->s_cnt >= 0) && isempty(&sp->s_queue)) ||
((sp->s_cnt < 0) && notempty(&sp->s_queue)),
"chSemSignal(), #1",
"inconsistent semaphore");
chSysLock();
if (++sp->s_cnt <= 0)
chSchWakeupS(fifo_remove(&sp->s_queue), RDY_OK);
chSysUnlock();
}
void ThreadReference::resume(msg_t msg) {
chSysLock();
chDbgAssert(thread_ref != NULL,
"not referenced");
if (thread_ref) {
thread_t *tp = thread_ref;
thread_ref = NULL;
chSchWakeupS(tp, msg);
}
chSysUnlock();
}
/**
* @brief Creates a new thread into a static memory area.
* @details The new thread is initialized and make ready to execute.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
*
* @param[out] tdp pointer to the thread descriptor
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
*
* @iclass
*/
thread_t *chThdCreate(const thread_descriptor_t *tdp) {
thread_t *tp;
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)tdp->wbase,
(uint8_t *)tdp->wend,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateSuspendedI(tdp);
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
/**
* @brief Unlocks the next owned mutex in reverse lock order.
* @pre The invoking thread <b>must</b> have at least one owned mutex.
* @post The mutex is unlocked and removed from the per-thread stack of
* owned mutexes.
*
* @return A pointer to the unlocked mutex.
*
* @api
*/
Mutex *chMtxUnlock(void) {
Thread *ctp = currp;
Mutex *ump, *mp;
chSysLock();
chDbgAssert(ctp->p_mtxlist != NULL,
"chMtxUnlock(), #1",
"owned mutexes list empty");
chDbgAssert(ctp->p_mtxlist->m_owner == ctp,
"chMtxUnlock(), #2",
"ownership failure");
/* Removes the top Mutex from the Thread's owned mutexes list and marks it
as not owned.*/
ump = ctp->p_mtxlist;
ctp->p_mtxlist = ump->m_next;
/* If a thread is waiting on the mutex then the fun part begins.*/
if (chMtxQueueNotEmptyS(ump)) {
Thread *tp;
/* Recalculates the optimal thread priority by scanning the owned
mutexes list.*/
tprio_t newprio = ctp->p_realprio;
mp = ctp->p_mtxlist;
while (mp != NULL) {
/* If the highest priority thread waiting in the mutexes list has a
greater priority than the current thread base priority then the final
priority will have at least that priority.*/
if (chMtxQueueNotEmptyS(mp) && (mp->m_queue.p_next->p_prio > newprio))
newprio = mp->m_queue.p_next->p_prio;
mp = mp->m_next;
}
/* Assigns to the current thread the highest priority among all the
waiting threads.*/
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);
ump->m_owner = tp;
ump->m_next = tp->p_mtxlist;
tp->p_mtxlist = ump;
chSchWakeupS(tp, RDY_OK);
}
else
ump->m_owner = NULL;
chSysUnlock();
return ump;
}
/**
* @brief Creates a new thread into a static memory area.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
*
* @param[out] wsp pointer to a working area dedicated to the thread stack
* @param[in] size size of the working area
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p Thread structure allocated for
* the thread into the working space area.
*
* @api
*/
Thread *chThdCreateStatic(void *wsp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
Thread *tp;
#if CH_DBG_FILL_THREADS
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + sizeof(Thread),
CH_THREAD_FILL_VALUE);
_thread_memfill((uint8_t *)wsp + sizeof(Thread),
(uint8_t *)wsp + size,
CH_STACK_FILL_VALUE);
#endif
chSysLock();
chSchWakeupS(tp = chThdCreateI(wsp, size, prio, pf, arg), RDY_OK);
chSysUnlock();
return tp;
}
/** button interupt universal **/
void buttonInterupt(){
static virtual_timer_t vt4;
chSysLock();
thread_t *ntp = tp_button;
if(ntp){
tp_button = NULL;
chSchWakeupS(ntp, MSG_OK);
}
chSysUnlock();
palSetPad(GPIOD, GPIOD_LED6);
chSysLockFromISR();
chVTResetI(&vt4);
/* LED4 set to OFF after 200mS.*/
chVTSetI(&vt4, MS2ST(200), led5off, NULL);
chSysUnlockFromISR();
}
/**
* @brief Creates a new thread into a static memory area.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
*
* @param[out] wsp pointer to a working area dedicated to the thread stack
* @param[in] size size of the working area
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
*
* @api
*/
thread_t *chThdCreateStatic(void *wsp, size_t size,
tprio_t prio, tfunc_t pf, void *arg) {
thread_t *tp;
chDbgCheck((wsp != NULL) &&
MEM_IS_ALIGNED(wsp, PORT_WORKING_AREA_ALIGN) &&
(size >= THD_WORKING_AREA_SIZE(0)) &&
MEM_IS_ALIGNED(size, PORT_STACK_ALIGN) &&
(prio <= HIGHPRIO) && (pf != NULL));
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + size,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
/* The thread structure is laid out in the upper part of the thread
workspace. The thread position structure is aligned to the required
stack alignment because it represents the stack top.*/
tp = (thread_t *)((uint8_t *)wsp + size -
MEM_ALIGN_NEXT(sizeof (thread_t), PORT_STACK_ALIGN));
/* Stack boundary.*/
tp->stklimit = (stkalign_t *)wsp;
/* Setting up the port-dependent part of the working area.*/
PORT_SETUP_CONTEXT(tp, wsp, tp, pf, arg);
tp = _thread_init(tp, "noname", prio);
/* Starting the thread immediately.*/
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
/**
* @brief Creates a new thread allocating the memory from the specified
* memory pool.
* @pre The configuration options @p CH_CFG_USE_DYNAMIC and
* @p CH_CFG_USE_MEMPOOLS must be enabled in order to use this
* function.
* @pre The pool must be initialized to contain only objects with
* alignment @p PORT_WORKING_AREA_ALIGN.
* @note A thread can terminate by calling @p chThdExit() or by simply
* returning from its main function.
* @note The memory allocated for the thread is not released automatically,
* it is responsibility of the creator thread to call @p chThdWait()
* and then release the allocated memory.
*
* @param[in] mp pointer to the memory pool object
* @param[in] name thread name
* @param[in] prio the priority level for the new thread
* @param[in] pf the thread function
* @param[in] arg an argument passed to the thread function. It can be
* @p NULL.
* @return The pointer to the @p thread_t structure allocated for
* the thread into the working space area.
* @retval NULL if the memory pool is empty.
*
* @api
*/
thread_t *chThdCreateFromMemoryPool(memory_pool_t *mp, const char *name,
tprio_t prio, tfunc_t pf, void *arg) {
thread_t *tp;
void *wsp;
chDbgCheck(mp != NULL);
wsp = chPoolAlloc(mp);
if (wsp == NULL) {
return NULL;
}
thread_descriptor_t td = {
name,
wsp,
(stkalign_t *)((uint8_t *)wsp + mp->object_size),
prio,
pf,
arg
};
#if CH_DBG_FILL_THREADS == TRUE
_thread_memfill((uint8_t *)wsp,
(uint8_t *)wsp + mp->object_size,
CH_DBG_STACK_FILL_VALUE);
#endif
chSysLock();
tp = chThdCreateSuspendedI(&td);
tp->flags = CH_FLAG_MODE_MPOOL;
tp->mpool = mp;
chSchWakeupS(tp, MSG_OK);
chSysUnlock();
return tp;
}
OsTask *osCreateTask(const char_t *name, OsTaskCode taskCode,
void *params, size_t stackSize, int_t priority)
{
uint_t i;
void *wa;
OsTask *task = NULL;
//Compute the size of the stack in bytes
stackSize *= sizeof(uint_t);
//Allocate a memory block to hold the working area
wa = osAllocMem(THD_WA_SIZE(stackSize));
//Successful memory allocation?
if(wa != NULL)
{
//Enter critical section
chSysLock();
//Loop through task table
for(i = 0; i < OS_PORT_MAX_TASKS; i++)
{
//Check whether the current entry is free
if(taskTable[i].tp == NULL)
break;
}
//Any entry available in the table?
if(i < OS_PORT_MAX_TASKS)
{
//Create a new task
taskTable[i].tp = chThdCreateI(wa, THD_WA_SIZE(stackSize),
priority, (tfunc_t) taskCode, params);
//Check whether the task was successfully created
if(taskTable[i].tp != NULL)
{
//Insert the newly created task in the ready list
chSchWakeupS(taskTable[i].tp, RDY_OK);
//Save task pointer
task = &taskTable[i];
//Save working area base address
waTable[i] = wa;
//Leave critical section
chSysUnlock();
}
else
{
//Leave critical section
chSysUnlock();
//Clean up side effects
osFreeMem(wa);
}
}
else
{
//Leave critical section
chSysUnlock();
//No entry available in the table
osFreeMem(wa);
}
}
//Return a pointer to the newly created task
return task;
}
/**
* @brief Unlocks the specified mutex.
* @note Mutexes must be unlocked in reverse lock order. Violating this
* rules will result in a panic if assertions are enabled.
* @pre The invoking thread <b>must</b> have at least one owned mutex.
* @post The mutex is unlocked and removed from the per-thread stack of
* owned mutexes.
*
* @param[in] mp pointer to the @p mutex_t structure
*
* @api
*/
void chMtxUnlock(mutex_t *mp) {
thread_t *ctp = currp;
mutex_t *lmp;
chDbgCheck(mp != NULL);
chSysLock();
chDbgAssert(ctp->p_mtxlist != NULL, "owned mutexes list empty");
chDbgAssert(ctp->p_mtxlist->m_owner == ctp, "ownership failure");
#if CH_CFG_USE_MUTEXES_RECURSIVE
chDbgAssert(mp->m_cnt >= 1, "counter is not positive");
if (--mp->m_cnt == 0) {
#endif
chDbgAssert(ctp->p_mtxlist == mp, "not next in list");
/* Removes the top mutex from the thread's owned mutexes list and marks
it as not owned. Note, it is assumed to be the same mutex passed as
parameter of this function.*/
ctp->p_mtxlist = mp->m_next;
/* If a thread is waiting on the mutex then the fun part begins.*/
if (chMtxQueueNotEmptyS(mp)) {
thread_t *tp;
/* Recalculates the optimal thread priority by scanning the owned
mutexes list.*/
tprio_t newprio = ctp->p_realprio;
lmp = ctp->p_mtxlist;
while (lmp != NULL) {
/* If the highest priority thread waiting in the mutexes list has a
greater priority than the current thread base priority then the
final priority will have at least that priority.*/
if (chMtxQueueNotEmptyS(lmp) && (lmp->m_queue.p_next->p_prio > newprio))
newprio = lmp->m_queue.p_next->p_prio;
lmp = lmp->m_next;
}
/* Assigns to the current thread the highest priority among all the
waiting threads.*/
ctp->p_prio = newprio;
/* Awakens the highest priority thread waiting for the unlocked mutex and
assigns the mutex to it.*/
#if CH_CFG_USE_MUTEXES_RECURSIVE
mp->m_cnt = 1;
#endif
tp = queue_fifo_remove(&mp->m_queue);
mp->m_owner = tp;
mp->m_next = tp->p_mtxlist;
tp->p_mtxlist = mp;
chSchWakeupS(tp, MSG_OK);
}
else
mp->m_owner = NULL;
#if CH_CFG_USE_MUTEXES_RECURSIVE
}
#endif
chSysUnlock();
}
