static void test_002_002_execute(void) {
/* The function chSemWait() is invoked, after return the counter and
the returned message are tested. The semaphore is signaled by another
thread.*/
test_set_step(1);
{
msg_t msg;
msg = chSemWait(&gsem1);
test_assert_lock(chSemGetCounterI(&gsem1) == 0,
"wrong counter value");
test_assert(MSG_OK == msg,
"wrong returned message");
}
/* The function chSemWait() is invoked, after return the counter and
the returned message are tested. The semaphore is reset by another
thread.*/
test_set_step(2);
{
msg_t msg;
msg = chSemWait(&gsem2);
test_assert_lock(chSemGetCounterI(&gsem2) == 0,
"wrong counter value");
test_assert(MSG_RESET == msg,
"wrong returned message");
}
}
static void test_002_003_execute(void) {
systime_t time;
msg_t msg;
/* The function chSemWaitTimeout() is invoked, after return the system
time, the counter and the returned message are tested.*/
test_set_step(1);
{
time = chVTGetSystemTimeX();
msg = chSemWaitTimeout(&sem1, MS2ST(1000));
test_assert_time_window(time + MS2ST(1000),
time + MS2ST(1000) + 1,
"out of time window");
test_assert_lock(chSemGetCounterI(&sem1) == 0,
"wrong counter value");
test_assert(MSG_TIMEOUT == msg,
"wrong timeout message");
}
/* The function chSemWaitTimeout() is invoked, after return the system
time, the counter and the returned message are tested.*/
test_set_step(2);
{
time = chVTGetSystemTimeX();
msg = chSemWaitTimeout(&sem1, MS2ST(1000));
test_assert_time_window(time + MS2ST(1000),
time + MS2ST(1000) + 1,
"out of time window");
test_assert_lock(chSemGetCounterI(&sem1) == 0,
"wrong counter value");
test_assert(MSG_TIMEOUT == msg,
"wrong timeout message");
}
}
static void rt_test_005_001_execute(void) {
/* [5.1.1] The function chSemWait() is invoked, after return the
counter and the returned message are tested.*/
test_set_step(1);
{
msg_t msg;
msg = chSemWait(&sem1);
test_assert_lock(chSemGetCounterI(&sem1) == 0, "wrong counter value");
test_assert(MSG_OK == msg, "wrong returned message");
}
/* [5.1.2] The function chSemSignal() is invoked, after return the
counter is tested.*/
test_set_step(2);
{
chSemSignal(&sem1);
test_assert_lock(chSemGetCounterI(&sem1) == 1, "wrong counter value");
}
/* [5.1.3] The function chSemReset() is invoked, after return the
counter is tested.*/
test_set_step(3);
{
chSemReset(&sem1, 2);
test_assert_lock(chSemGetCounterI(&sem1) == 2, "wrong counter value");
}
}
static void rt_test_005_006_execute(void) {
binary_semaphore_t bsem;
msg_t msg;
/* [5.6.1] Creating a binary semaphore in "taken" state, the state is
checked.*/
test_set_step(1);
{
chBSemObjectInit(&bsem, true);
test_assert_lock(chBSemGetStateI(&bsem) == true, "not taken");
}
/* [5.6.2] Resetting the binary semaphore in "taken" state, the state
must not change.*/
test_set_step(2);
{
chBSemReset(&bsem, true);
test_assert_lock(chBSemGetStateI(&bsem) == true, "not taken");
}
/* [5.6.3] Starting a signaler thread at a lower priority.*/
test_set_step(3);
{
threads[0] = chThdCreateStatic(wa[0], WA_SIZE,
chThdGetPriorityX()-1, thread4, &bsem);
}
/* [5.6.4] Waiting for the binary semaphore to be signaled, the
semaphore is expected to be taken.*/
test_set_step(4);
{
msg = chBSemWait(&bsem);
test_assert_lock(chBSemGetStateI(&bsem) == true, "not taken");
test_assert(msg == MSG_OK, "unexpected message");
}
/* [5.6.5] Signaling the binary semaphore, checking the binary
semaphore state to be "not taken" and the underlying counter
semaphore counter to be one.*/
test_set_step(5);
{
chBSemSignal(&bsem);
test_assert_lock(chBSemGetStateI(&bsem) ==false, "still taken");
test_assert_lock(chSemGetCounterI(&bsem.sem) == 1, "unexpected counter");
}
/* [5.6.6] Signaling the binary semaphore again, the internal state
must not change from "not taken".*/
test_set_step(6);
{
chBSemSignal(&bsem);
test_assert_lock(chBSemGetStateI(&bsem) == false, "taken");
test_assert_lock(chSemGetCounterI(&bsem.sem) == 1, "unexpected counter");
}
}
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);
}
}
bool_t gfxSemWaitI(gfxSem *psem)
{
if (chSemGetCounterI(&psem->sem) <= 0)
return FALSE;
chSemFastWaitI(&psem->sem);
return TRUE;
}
/**
* @brief Common TX ISR handler.
*
* @param[in] canp pointer to the @p CANDriver object
*
* @notapi
*/
static void can_lld_tx_handler(CANDriver *canp) {
/* All transmit buffers are available.*/
if (canp->can->GSR & CANGSR_TBS) {
chSysLockFromIsr();
while (chSemGetCounterI(&canp->txsem) < 0)
chSemSignalI(&canp->txsem);
chEvtBroadcastFlagsI(&canp->txempty_event, CAN_MAILBOX_TO_MASK(1));
chSysUnlockFromIsr();
}
}
/**
* @brief Gets the semaphore value.
* @details Reads the current semaphore value.
* @warning Thread-safe, but the value may not be consistent after a call to
* this function.
*
* @param[in] semp
* Pointer to an initialized @p UrosSem object.
* @return
* Current semaphore count.
*/
uros_cnt_t uros_lld_sem_value(UrosSem *semp) {
uros_cnt_t value;
urosAssert(semp != NULL);
chSysLock();
value = chSemGetCounterI(semp);
chSysUnlock();
return value;
}
/**
* @brief Retrieves a message from a mailbox.
* @details This variant is non-blocking, the function returns a timeout
* condition if the queue is full.
*
* @param[in] mbp the pointer to an initialized Mailbox object
* @param[out] msgp pointer to a message variable for the received message
* @return The operation status.
* @retval RDY_OK if a message has been correctly fetched.
* @retval RDY_TIMEOUT if the mailbox is empty and a message cannot be
* fetched.
*
* @iclass
*/
msg_t chMBFetchI(Mailbox *mbp, msg_t *msgp) {
chDbgCheck((mbp != NULL) && (msgp != NULL), "chMBFetchI");
if (chSemGetCounterI(&mbp->mb_fullsem) <= 0)
return RDY_TIMEOUT;
*msgp = *mbp->mb_rdptr++;
if (mbp->mb_rdptr >= mbp->mb_top)
mbp->mb_rdptr = mbp->mb_buffer;
chSemSignalI(&mbp->mb_emptysem);
return RDY_OK;
}
/**
* @brief Posts a message into a mailbox.
* @details This variant is non-blocking, the function returns a timeout
* condition if the queue is full.
*
* @param[in] mbp the pointer to an initialized Mailbox object
* @param[in] msg the message to be posted on the mailbox
* @return The operation status.
* @retval RDY_OK if a message has been correctly posted.
* @retval RDY_TIMEOUT if the mailbox is full and the message cannot be
* posted.
*
* @iclass
*/
msg_t chMBPostI(Mailbox *mbp, msg_t msg) {
chDbgCheck(mbp != NULL, "chMBPostI");
if (chSemGetCounterI(&mbp->mb_emptysem) <= 0)
return RDY_TIMEOUT;
chSemFastWaitI(&mbp->mb_emptysem);
*mbp->mb_wrptr++ = msg;
if (mbp->mb_wrptr >= mbp->mb_top)
mbp->mb_wrptr = mbp->mb_buffer;
chSemSignalI(&mbp->mb_fullsem);
return RDY_OK;
}
void geventDetachSource(GListener *pl, GSourceHandle gsh) {
if (pl) {
chMtxLock(&geventMutex);
deleteAssignments(pl, gsh);
if (!gsh && chSemGetCounterI(&pl->waitqueue) < 0) {
chBSemWait(&pl->eventlock); // Obtain the buffer lock
pl->event.type = GEVENT_EXIT; // Set up the EXIT event
chSemSignal(&pl->waitqueue); // Wake up the listener
chBSemSignal(&pl->eventlock); // Release the buffer lock
}
chMtxUnlock();
}
}
bool DataListener< datatype >::pushI(datatype & src){
if(chSemGetCounterI(&sem) + 1 < len + 1){
chSemSignalI(&sem);
*wr_head = src;
if(wr_head == buf_head + len - 1){
wr_head = buf_head;
} else {
wr_head += 1;
}
return true;
}
return false;
}
/**
* @brief Posts an high priority message into a mailbox.
* @details This variant is non-blocking, the function returns a timeout
* condition if the queue is full.
*
* @param[in] mbp the pointer to an initialized Mailbox object
* @param[in] msg the message to be posted on the mailbox
* @return The operation status.
* @retval RDY_OK if a message has been correctly posted.
* @retval RDY_TIMEOUT if the mailbox is full and the message cannot be
* posted.
*
* @iclass
*/
msg_t chMBPostAheadI(Mailbox *mbp, msg_t msg) {
chDbgCheck(mbp != NULL, "chMBPostAheadI");
if (chSemGetCounterI(&mbp->mb_emptysem) <= 0)
return RDY_TIMEOUT;
chSemFastWaitI(&mbp->mb_emptysem);
if (--mbp->mb_rdptr < mbp->mb_buffer)
mbp->mb_rdptr = mbp->mb_top - 1;
*mbp->mb_rdptr = msg;
chSemSignalI(&mbp->mb_fullsem);
return RDY_OK;
}
void geventRegisterCallback(GListener *pl, GEventCallbackFn fn, void *param) {
if (pl) {
chMtxLock(&geventMutex);
chBSemWait(&pl->eventlock); // Obtain the buffer lock
pl->param = param; // Set the param
pl->callback = fn; // Set the callback function
if (chSemGetCounterI(&pl->waitqueue) < 0) {
pl->event.type = GEVENT_EXIT; // Set up the EXIT event
chSemSignal(&pl->waitqueue); // Wake up the listener
}
chBSemSignal(&pl->eventlock); // Release the buffer lock
chMtxUnlock();
}
}
/**
* @brief Retrieves a message from a mailbox.
* @details This variant is non-blocking, the function returns a timeout
* condition if the queue is empty.
*
* @param[in] mbp the pointer to an initialized @p mailbox_t object
* @param[out] msgp pointer to a message variable for the received message
* @return The operation status.
* @retval MSG_OK if a message has been correctly fetched.
* @retval MSG_TIMEOUT if the mailbox is empty and a message cannot be
* fetched.
*
* @iclass
*/
msg_t chMBFetchI(mailbox_t *mbp, msg_t *msgp) {
chDbgCheckClassI();
chDbgCheck((mbp != NULL) && (msgp != NULL));
if (chSemGetCounterI(&mbp->mb_fullsem) <= 0)
return MSG_TIMEOUT;
chSemFastWaitI(&mbp->mb_fullsem);
*msgp = *mbp->mb_rdptr++;
if (mbp->mb_rdptr >= mbp->mb_top)
mbp->mb_rdptr = mbp->mb_buffer;
chSemSignalI(&mbp->mb_emptysem);
return MSG_OK;
}
/**
* @brief Posts an high priority message into a mailbox.
* @details This variant is non-blocking, the function returns a timeout
* condition if the queue is full.
*
* @param[in] mbp the pointer to an initialized @p mailbox_t object
* @param[in] msg the message to be posted on the mailbox
* @return The operation status.
* @retval MSG_OK if a message has been correctly posted.
* @retval MSG_TIMEOUT if the mailbox is full and the message cannot be
* posted.
*
* @iclass
*/
msg_t chMBPostAheadI(mailbox_t *mbp, msg_t msg) {
chDbgCheckClassI();
chDbgCheck(mbp != NULL);
if (chSemGetCounterI(&mbp->mb_emptysem) <= 0)
return MSG_TIMEOUT;
chSemFastWaitI(&mbp->mb_emptysem);
if (--mbp->mb_rdptr < mbp->mb_buffer)
mbp->mb_rdptr = mbp->mb_top - 1;
*mbp->mb_rdptr = msg;
chSemSignalI(&mbp->mb_fullsem);
return MSG_OK;
}
/**
* @brief Posts a message into a mailbox.
* @details This variant is non-blocking, the function returns a timeout
* condition if the queue is full.
*
* @param[in] mbp the pointer to an initialized @p mailbox_t object
* @param[in] msg the message to be posted on the mailbox
* @return The operation status.
* @retval MSG_OK if a message has been correctly posted.
* @retval MSG_TIMEOUT if the mailbox is full and the message cannot be
* posted.
*
* @iclass
*/
msg_t chMBPostI(mailbox_t *mbp, msg_t msg) {
chDbgCheckClassI();
chDbgCheck(mbp != NULL);
if (chSemGetCounterI(&mbp->mb_emptysem) <= 0)
return MSG_TIMEOUT;
chSemFastWaitI(&mbp->mb_emptysem);
*mbp->mb_wrptr++ = msg;
if (mbp->mb_wrptr >= mbp->mb_top)
mbp->mb_wrptr = mbp->mb_buffer;
chSemSignalI(&mbp->mb_fullsem);
return MSG_OK;
}
void geventSendEvent(GSourceListener *psl) {
chMtxLock(&geventMutex);
if (psl->pListener->callback) { // This test needs to be taken inside the mutex
chMtxUnlock();
// We already know we have the event lock
psl->pListener->callback(psl->pListener->param, &psl->pListener->event);
} else {
// Wake up the listener
if (chSemGetCounterI(&psl->pListener->waitqueue) < 0)
chSemSignal(&psl->pListener->waitqueue);
chMtxUnlock();
}
}
/* Null is treated as a wildcard. */
static void deleteAssignments(GListener *pl, GSourceHandle gsh) {
GSourceListener *psl;
for(psl = Assignments; psl < Assignments+GEVENT_MAX_SOURCE_LISTENERS; psl++) {
if ((!pl || psl->pListener == pl) && (!gsh || psl->pSource == gsh)) {
if (chSemGetCounterI(&psl->pListener->waitqueue) < 0) {
chBSemWait(&psl->pListener->eventlock); // Obtain the buffer lock
psl->pListener->event.type = GEVENT_EXIT; // Set up the EXIT event
chSemSignal(&psl->pListener->waitqueue); // Wake up the listener
chBSemSignal(&psl->pListener->eventlock); // Release the buffer lock
}
psl->pListener = 0;
}
}
}
/**
* @brief Common RX ISR handler.
*
* @param[in] canp pointer to the @p CANDriver object
* @param[in] status
* @notapi
*/
static void can_lld_rx_handler(CANDriver *canp, uint32_t status) {
/* No more receive events until the queue 0 has been emptied.*/
canp->can->IER &= ~CANIER_RIE;
chSysLockFromIsr();
while (chSemGetCounterI(&canp->rxsem) < 0)
chSemSignalI(&canp->rxsem);
chEvtBroadcastFlagsI(&canp->rxfull_event, CAN_MAILBOX_TO_MASK(1));
chSysUnlockFromIsr();
if (( status & CANICR_DOI) > 0) {
/* Overflow events handling.*/
canp->can->CMR = CANCMR_CDO;
chSysLockFromIsr();
chEvtBroadcastFlagsI(&canp->error_event, CAN_OVERFLOW_ERROR);
chSysUnlockFromIsr();
}
}
static void rt_test_005_004_execute(void) {
/* [5.4.1] A thread is created, it goes to wait on the semaphore.*/
test_set_step(1);
{
threads[0] = chThdCreateStatic(wa[0], WA_SIZE, chThdGetPriorityX()+1, thread1, "A");
}
/* [5.4.2] The semaphore counter is increased by two, it is then
tested to be one, the thread must have completed.*/
test_set_step(2);
{
chSysLock();
chSemAddCounterI(&sem1, 2);
chSchRescheduleS();
chSysUnlock();
test_wait_threads();
test_assert_lock(chSemGetCounterI(&sem1) == 1, "invalid counter");
test_assert_sequence("A", "invalid sequence");
}
}
/**
* @brief Dequeues and wakes up one thread from the queue, if any.
*
* @param[in] tqp pointer to the threads queue object
* @param[in] msg the message code
*
* @iclass
*/
void osalThreadDequeueNextI(threads_queue_t *tqp, msg_t msg) {
semaphore_t *sp = &tqp->sem;
if (chSemGetCounterI(&tqp->sem) < (cnt_t)0) {
thread_reference_t tr = nil.threads;
while (true) {
/* Is this thread waiting on this semaphore?*/
if (tr->u1.semp == sp) {
sp->cnt++;
chDbgAssert(NIL_THD_IS_WTSEM(tr), "not waiting");
(void) chSchReadyI(tr, msg);
return;
}
tr++;
chDbgAssert(tr < &nil.threads[NIL_CFG_NUM_THREADS],
"pointer out of range");
}
}
}
cnt_t CounterSemaphore::getCounterI(void) {
return chSemGetCounterI(&sem);
}
GEvent *geventEventWait(GListener *pl, systime_t timeout) {
if (pl->callback || chSemGetCounterI(&pl->waitqueue) < 0)
return 0;
return chSemWaitTimeout(&pl->waitqueue, timeout) == RDY_OK ? &pl->event : 0;
}
GEvent *geventGetEventBuffer(GSourceListener *psl) {
// We already know we have the event lock
return &psl->pListener->callback || chSemGetCounterI(&psl->pListener->waitqueue) < 0 ? &psl->pListener->event : 0;
}
