/**
* @brief Signals all the Event Listeners registered on the specified Event
* Source.
* @details This function variants ORs the specified event flags to all the
* threads registered on the @p event_source_t in addition to the
* event flags specified by the threads themselves in the
* @p event_listener_t objects.
*
* @param[in] esp pointer to the @p event_source_t structure
* @param[in] flags the flags set to be added to the listener flags mask
*
* @api
*/
void chEvtBroadcastFlags(event_source_t *esp, eventflags_t flags) {
chSysLock();
chEvtBroadcastFlagsI(esp, flags);
chSchRescheduleS();
chSysUnlock();
}
bool Middleware::advertise(RemotePublisher * pub) {
Node * node;
LocalSubscriber * waiting_subscriber;
if (_remote_publishers == NULL) {
_remote_publishers = pub;
} else {
pub->next(_remote_publishers);
_remote_publishers = pub;
}
node = _nodes;
while (node) {
waiting_subscriber = node->queue();
while (waiting_subscriber != NULL) {
if (compareTopics(pub->topic(),
waiting_subscriber->topic())) {
node->unqueueSubscriber(waiting_subscriber);
node->remoteSubscribe(pub, waiting_subscriber);
}
waiting_subscriber = (LocalSubscriber *) waiting_subscriber->next();
}
node = node->next();
}
chSysLock();
chSchRescheduleS();
chSysUnlock();
return true;
}
/**
* @brief Posts an high priority message into a mailbox.
* @details The invoking thread waits until a empty slot in the mailbox becomes
* available or the specified time runs out.
*
* @param[in] mbp the pointer to an initialized @p mailbox_t object
* @param[in] msg the message to be posted on the mailbox
* @param[in] timeout 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 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 operation has timed out.
*
* @sclass
*/
msg_t chMBPostAheadTimeoutS(mailbox_t *mbp, msg_t msg, sysinterval_t timeout) {
msg_t rdymsg;
chDbgCheckClassS();
chDbgCheck(mbp != NULL);
do {
/* If the mailbox is in reset state then returns immediately.*/
if (mbp->reset) {
return MSG_RESET;
}
/* Is there a free message slot in queue? if so then post.*/
if (chMBGetFreeCountI(mbp) > (size_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);
chSchRescheduleS();
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;
}
/**
* @brief Releases an object into a guarded memory pool.
* @pre The guarded memory pool must already be initialized.
* @pre The freed object must be of the right size for the specified
* guarded memory pool.
* @pre The added object must be properly aligned.
*
* @param[in] gmp pointer to a @p guarded_memory_pool_t structure
* @param[in] objp the pointer to the object to be released
*
* @api
*/
void chGuardedPoolFree(guarded_memory_pool_t *gmp, void *objp) {
chSysLock();
chGuardedPoolFreeI(gmp, objp);
chSchRescheduleS();
chSysUnlock();
}
/**
* @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
*
* @api
*/
void chMBReset(mailbox_t *mbp) {
chSysLock();
chMBResetI(mbp);
chSchRescheduleS();
chSysUnlock();
}
void gfxSemSignal(gfxSem *psem) {
chSysLock();
if (gfxSemCounterI(psem) < psem->limit)
chSemSignalI(&psem->sem);
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Unlocks all mutexes owned by the invoking thread.
* @post The stack of owned mutexes is emptied and all the found
* mutexes are unlocked.
* @note This function is <b>MUCH MORE</b> efficient than releasing the
* mutexes one by one and not just because the call overhead,
* this function does not have any overhead related to the priority
* inheritance mechanism.
*
* @api
*/
void chMtxUnlockAll(void) {
thread_t *ctp = currp;
chSysLock();
if (ctp->p_mtxlist != NULL) {
do {
mutex_t *mp = ctp->p_mtxlist;
ctp->p_mtxlist = mp->m_next;
if (chMtxQueueNotEmptyS(mp)) {
#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE
mp->m_cnt = (cnt_t)1;
#endif
thread_t *tp = queue_fifo_remove(&mp->m_queue);
mp->m_owner = tp;
mp->m_next = tp->p_mtxlist;
tp->p_mtxlist = mp;
(void) chSchReadyI(tp);
}
else {
#if CH_CFG_USE_MUTEXES_RECURSIVE == TRUE
mp->m_cnt = (cnt_t)0;
#endif
mp->m_owner = NULL;
}
} while (ctp->p_mtxlist != NULL);
ctp->p_prio = ctp->p_realprio;
chSchRescheduleS();
}
chSysUnlock();
}
/**
* @brief Signals all threads that are waiting on the condition variable.
*
* @param[in] cp pointer to the @p CondVar structure
*
* @api
*/
void chCondBroadcast(CondVar *cp) {
chSysLock();
chCondBroadcastI(cp);
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Retrieves a message from a mailbox.
* @details The invoking thread waits until a message is posted in the mailbox
* or the specified time runs out.
*
* @param[in] mbp the pointer to an initialized @p mailbox_t object
* @param[out] msgp pointer to a message variable for the received message
* @param[in] timeout 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 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 operation has timed out.
*
* @sclass
*/
msg_t chMBFetchTimeoutS(mailbox_t *mbp, msg_t *msgp, sysinterval_t timeout) {
msg_t rdymsg;
chDbgCheckClassS();
chDbgCheck((mbp != NULL) && (msgp != NULL));
do {
/* If the mailbox is in reset state then returns immediately.*/
if (mbp->reset) {
return MSG_RESET;
}
/* Is there a message in queue? if so then fetch.*/
if (chMBGetUsedCountI(mbp) > (size_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);
chSchRescheduleS();
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;
}
/**
* @brief Signals all the Event Listeners registered on the specified Event
* Source.
* @details This function variants ORs the specified event flags to all the
* threads registered on the @p EventSource in addition to the event
* flags specified by the threads themselves in the
* @p EventListener objects.
*
* @param[in] esp pointer to the @p EventSource structure
* @param[in] flags the flags set to be added to the listener flags mask
*
* @api
*/
void chEvtBroadcastFlags(EventSource *esp, flagsmask_t flags) {
chSysLock();
chEvtBroadcastFlagsI(esp, flags);
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Performs a signal operation on a semaphore.
* @post This function does not reschedule so a call to a rescheduling
* function must be performed before unlocking the kernel. Note that
* interrupt handlers always reschedule on exit so an explicit
* reschedule must not be performed in ISRs.
*
* @param[in] sp pointer to a @p semaphore_t structure
*
* @api
*/
void chSemSignal(semaphore_t *sp) {
chSysLock();
chSemSignalI(sp);
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Performs atomic signal and wait operations on two semaphores.
*
* @param[in] sps pointer to a @p semaphore_t structure to be signaled
* @param[in] spw pointer to a @p semaphore_t structure to wait on
* @return A message specifying how the invoking thread has been
* released from the semaphore.
* @retval MSG_OK if the thread has not stopped on the semaphore or the
* semaphore has been signaled.
* @retval MSG_RESET if the semaphore has been reset using @p chSemReset().
*
* @api
*/
msg_t chSemSignalWait(semaphore_t *sps, semaphore_t *spw) {
msg_t msg;
chDbgCheck((sps != NULL) && (spw != NULL));
chDbgAssert(((sps->s_cnt >= (cnt_t)0) && queue_isempty(&sps->s_queue)) ||
((sps->s_cnt < (cnt_t)0) && queue_notempty(&sps->s_queue)),
"inconsistent semaphore");
chDbgAssert(((spw->s_cnt >= (cnt_t)0) && queue_isempty(&spw->s_queue)) ||
((spw->s_cnt < (cnt_t)0) && queue_notempty(&spw->s_queue)),
"inconsistent semaphore");
chSysLock();
if (++sps->s_cnt <= (cnt_t)0) {
chSchReadyI(queue_fifo_remove(&sps->s_queue))->p_u.rdymsg = MSG_OK;
}
if (--spw->s_cnt < (cnt_t)0) {
thread_t *ctp = currp;
sem_insert(ctp, &spw->s_queue);
ctp->p_u.wtsemp = spw;
chSchGoSleepS(CH_STATE_WTSEM);
msg = ctp->p_u.rdymsg;
}
else {
chSchRescheduleS();
msg = MSG_OK;
}
chSysUnlock();
return msg;
}
/**
* @brief Adds a set of event flags directly to the specified @p thread_t.
*
* @param[in] tp the thread to be signaled
* @param[in] mask the event flags set to be ORed
*
* @api
*/
void chEvtSignal(thread_t *tp, eventmask_t mask) {
chSysLock();
chEvtSignalI(tp, mask);
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Signals all the Event Listeners registered on the specified Event
* Source.
* @details This function variants ORs the specified event flags to all the
* threads registered on the @p EventSource in addition to the event
* flags specified by the threads themselves in the
* @p EventListener objects.
*
* @param[in] esp pointer to the @p EventSource structure
* @param[in] mask the event flags set to be ORed
*
* @api
*/
void chEvtBroadcastFlags(EventSource *esp, eventmask_t mask) {
chSysLock();
chEvtBroadcastFlagsI(esp, mask);
chSchRescheduleS();
chSysUnlock();
}
THD_FUNCTION(test_support, arg) {
#if CH_CFG_USE_EVENTS == TRUE
thread_t *tp = (thread_t *)arg;
#else
(void)arg;
#endif
/* Initializing global resources.*/
chSemObjectInit(&gsem1, 0);
chSemObjectInit(&gsem2, 0);
while (true) {
chSysLock();
if (chSemGetCounterI(&gsem1) < 0)
chSemSignalI(&gsem1);
chSemResetI(&gsem2, 0);
chThdResumeI(>r1, MSG_OK);
#if CH_CFG_USE_EVENTS == TRUE
chEvtSignalI(tp, 0x55);
#endif
chSchRescheduleS();
chSysUnlock();
chThdSleepMilliseconds(250);
}
}
/**
* @brief Signals all threads that are waiting on the condition variable.
*
* @param[in] cp pointer to the @p condition_variable_t structure
*
* @api
*/
void chCondBroadcast(condition_variable_t *cp) {
chSysLock();
chCondBroadcastI(cp);
chSchRescheduleS();
chSysUnlock();
}
static void test_005_007_execute(void) {
/* [5.7.1] Starting the five threads with increasing priority, the
threads will queue on the condition variable.*/
test_set_step(1);
{
tprio_t prio = chThdGetPriorityX();
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, prio+1, thread6, "E");
threads[1] = chThdCreateStatic(wa[1], WA_SIZE, prio+2, thread6, "D");
threads[2] = chThdCreateStatic(wa[2], WA_SIZE, prio+3, thread6, "C");
threads[3] = chThdCreateStatic(wa[3], WA_SIZE, prio+4, thread6, "B");
threads[4] = chThdCreateStatic(wa[4], WA_SIZE, prio+5, thread6, "A");
}
/* [5.7.2] Atomically signaling the condition variable five times
then waiting for the threads to terminate in priority order, the
order is tested.*/
test_set_step(2);
{
chSysLock();
chCondSignalI(&c1);
chCondSignalI(&c1);
chCondSignalI(&c1);
chCondSignalI(&c1);
chCondSignalI(&c1);
chSchRescheduleS();
chSysUnlock();
test_wait_threads();
test_assert_sequence("ABCDE", "invalid sequence");
}
}
/**
* @brief Performs a reset operation on the semaphore.
* @post After invoking this function all the threads waiting on the
* semaphore, if any, are released and the semaphore counter is set
* to the specified, non negative, value.
* @post This function does not reschedule so a call to a rescheduling
* function must be performed before unlocking the kernel. Note that
* interrupt handlers always reschedule on exit so an explicit
* reschedule must not be performed in ISRs.
*
* @param[in] sp pointer to a @p semaphore_t structure
* @param[in] n the new value of the semaphore counter. The value must
* be non-negative.
*
* @api
*/
void chSemReset(semaphore_t *sp, cnt_t n) {
chSysLock();
chSemResetI(sp, n);
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Performs atomic signal and wait operations on two semaphores.
* @pre The configuration option @p CH_USE_SEMSW must be enabled in order
* to use this function.
*
* @param[in] sps pointer to a @p Semaphore structure to be signaled
* @param[in] spw pointer to a @p Semaphore structure to be wait on
* @return A message specifying how the invoking thread has been
* released from the semaphore.
* @retval RDY_OK if the thread has not stopped on the semaphore or the
* semaphore has been signaled.
* @retval RDY_RESET if the semaphore has been reset using @p chSemReset().
*
* @api
*/
msg_t chSemSignalWait(Semaphore *sps, Semaphore *spw) {
msg_t msg;
chDbgCheck((sps != NULL) && (spw != NULL), "chSemSignalWait");
chDbgAssert(((sps->s_cnt >= 0) && isempty(&sps->s_queue)) ||
((sps->s_cnt < 0) && notempty(&sps->s_queue)),
"chSemSignalWait(), #1",
"inconsistent semaphore");
chDbgAssert(((spw->s_cnt >= 0) && isempty(&spw->s_queue)) ||
((spw->s_cnt < 0) && notempty(&spw->s_queue)),
"chSemSignalWait(), #2",
"inconsistent semaphore");
chSysLock();
if (++sps->s_cnt <= 0)
chSchReadyI(fifo_remove(&sps->s_queue))->p_u.rdymsg = RDY_OK;
if (--spw->s_cnt < 0) {
Thread *ctp = currp;
sem_insert(ctp, &spw->s_queue);
ctp->p_u.wtobjp = spw;
chSchGoSleepS(THD_STATE_WTSEM);
msg = ctp->p_u.rdymsg;
}
else {
chSchRescheduleS();
msg = RDY_OK;
}
chSysUnlock();
return msg;
}
static THD_FUNCTION(thread1, p) {
chSysLock();
chThdResumeI(&tr1, MSG_OK);
chSchRescheduleS();
chSysUnlock();
test_emit_token(*(char *)p);
}
static THD_FUNCTION(thread2, p) {
(void)p;
chThdSleepMilliseconds(50);
chSysLock();
chSemSignalI(&sem1); /* For coverage reasons */
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Change thread priority.
* @note This can interfere with the priority inheritance mechanism.
*/
osStatus osThreadSetPriority(osThreadId thread_id, osPriority newprio) {
osPriority oldprio;
thread_t * tp = (thread_t *)thread_id;
chSysLock();
/* Changing priority.*/
#if CH_CFG_USE_MUTEXES
oldprio = (osPriority)tp->p_realprio;
if ((tp->p_prio == tp->p_realprio) || ((tprio_t)newprio > tp->p_prio))
tp->p_prio = (tprio_t)newprio;
tp->p_realprio = (tprio_t)newprio;
#else
oldprio = tp->p_prio;
tp->p_prio = (tprio_t)newprio;
#endif
/* The following states need priority queues reordering.*/
switch (tp->p_state) {
#if CH_CFG_USE_MUTEXES | \
CH_CFG_USE_CONDVARS | \
(CH_CFG_USE_SEMAPHORES && CH_CFG_USE_SEMAPHORES_PRIORITY) | \
(CH_CFG_USE_MESSAGES && CH_CFG_USE_MESSAGES_PRIORITY)
#if CH_CFG_USE_MUTEXES
case CH_STATE_WTMTX:
#endif
#if CH_CFG_USE_CONDVARS
case CH_STATE_WTCOND:
#endif
#if CH_CFG_USE_SEMAPHORES && CH_CFG_USE_SEMAPHORES_PRIORITY
case CH_STATE_WTSEM:
#endif
#if CH_CFG_USE_MESSAGES && CH_CFG_USE_MESSAGES_PRIORITY
case CH_STATE_SNDMSGQ:
#endif
/* Re-enqueues tp with its new priority on the queue.*/
queue_prio_insert(queue_dequeue(tp),
(threads_queue_t *)tp->p_u.wtobjp);
break;
#endif
case CH_STATE_READY:
#if CH_DBG_ENABLE_ASSERTS
/* Prevents an assertion in chSchReadyI().*/
tp->p_state = CH_STATE_CURRENT;
#endif
/* Re-enqueues tp with its new priority on the ready list.*/
chSchReadyI(queue_dequeue(tp));
break;
}
/* Rescheduling.*/
chSchRescheduleS();
chSysUnlock();
return oldprio;
}
/**
* @brief Adds (OR) a set of event flags on the specified @p Thread.
*
* @param[in] tp the thread to be signaled
* @param[in] mask the event flags set to be ORed
*
* @api
*/
void chEvtSignalFlags(Thread *tp, eventmask_t mask) {
chDbgCheck(tp != NULL, "chEvtSignal");
chSysLock();
chEvtSignalFlagsI(tp, mask);
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Adds a set of event flags directly to the specified @p thread_t.
*
* @param[in] tp the thread to be signaled
* @param[in] events the events set to be ORed
*
* @api
*/
void chEvtSignal(thread_t *tp, eventmask_t events) {
chDbgCheck(tp != NULL);
chSysLock();
chEvtSignalI(tp, events);
chSchRescheduleS();
chSysUnlock();
}
static msg_t thread2(void *p) {
(void)p;
chThdSleepMilliseconds(50);
chSysLock();
chSemSignalI(&sem1); /* For coverage reasons */
chSchRescheduleS();
chSysUnlock();
return 0;
}
static THD_FUNCTION(thread4B, p) {
(void)p;
chThdSleepMilliseconds(150);
chSysLock();
chMtxLockS(&m2); /* For coverage of the chMtxLockS() function variant.*/
chMtxUnlockS(&m2); /* For coverage of the chMtxUnlockS() function variant.*/
chSchRescheduleS();
chSysUnlock();
}
/**
* @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.
*
* @param[in] mbp the pointer to an initialized @p mailbox_t object
*
* @api
*/
void chMBReset(mailbox_t *mbp) {
chDbgCheck(mbp != NULL);
chSysLock();
mbp->mb_wrptr = mbp->mb_rdptr = mbp->mb_buffer;
chSemResetI(&mbp->mb_emptysem, mbp->mb_top - mbp->mb_buffer);
chSemResetI(&mbp->mb_fullsem, 0);
chSchRescheduleS();
chSysUnlock();
}
/*
set the priority of the main APM task
*/
void hal_chibios_set_priority(uint8_t priority)
{
chSysLock();
#if CH_CFG_USE_MUTEXES == TRUE
if ((daemon_task->prio == daemon_task->realprio) || (priority > daemon_task->prio)) {
daemon_task->prio = priority;
}
daemon_task->realprio = priority;
#endif
chSchRescheduleS();
chSysUnlock();
}
/**
* @brief Restores the specified execution status and leaves a critical zone.
* @note A call to @p chSchRescheduleS() is automatically performed
* if exiting the critical zone and if not in ISR context.
*
* @param[in] sts the system status to be restored.
*
* @xclass
*/
void chSysRestoreStatusX(syssts_t sts) {
if (port_irq_enabled(sts)) {
if (port_is_isr_context()) {
chSysUnlockFromISR();
}
else {
chSchRescheduleS();
chSysUnlock();
}
}
}
void DataSource< datatype >::put(datatype &src, bool reschedule){
DataListener<datatype> * iter;
chSysLock();
for(iter = listener_head; iter; iter = iter->next){
iter->pushI(src);
}
if(reschedule && listener_head){
// Reschedule
chSchRescheduleS();
}
chSysUnlock();
}
