/**
*
* @brief Appends an item to a queue from ISR context.
*
* @param[in] queuep pointer to instance of @p struct pios_queue
* @param[in] itemp pointer to item which will be appended to the queue
* @param[in] timeout_ms timeout for appending item to queue in milliseconds
*
* @returns true on success or false on timeout or failure
*
*/
bool PIOS_Queue_Send_FromISR(struct pios_queue *queuep, const void *itemp, bool *wokenp)
{
chSysLockFromIsr();
void *buf = chPoolAllocI(&queuep->mp);
if (buf == NULL)
{
chSysUnlockFromIsr();
return false;
}
memcpy(buf, itemp, queuep->mp.mp_object_size);
msg_t result = chMBPostI(&queuep->mb, (msg_t)buf);
if (result != RDY_OK)
{
chPoolFreeI(&queuep->mp, buf);
chSysUnlockFromIsr();
return false;
}
chSysUnlockFromIsr();
return true;
}
/*
* GPT2 callback.
*/
static void gpt2cb(GPTDriver *gptp) {
msg_t msg;
(void)gptp;
chSysLockFromIsr();
msg = chMBPostI(&mb[NUM_THREADS - 1], MSG_SEND_LEFT);
if (msg != RDY_OK)
saturated = TRUE;
chSysUnlockFromIsr();
}
/*
* GPT1 callback.
*/
static void gpt1cb(GPTDriver *gptp) {
msg_t msg;
(void)gptp;
chSysLockFromIsr();
msg = chMBPostI(&mb[0], MSG_SEND_RIGHT);
if (msg != RDY_OK)
saturated = TRUE;
chSysUnlockFromIsr();
}
/**
* @brief Queues a sensor read to the reading thread.
*
* @param[in] srdp Pointer to the SensorReadDriver object.
* @param[in] senp Pointer the generic_sensor_t object to be queued.
*
* @return The operation status.
* @retval MSG_OK If a read request has been correctly queued.
* @retval MSG_TIMEOUT If the queue is full and the request cannot be queued.
*
* @iclass
*/
static inline msg_t queueReadI(SensorReadDriver *srdp,
const generic_sensor_t *senp)
{
if (senp->priority_sensor == true)
/* Priority sensor, put it at the front of the queue */
return chMBPostAheadI(&srdp->srd_mailbox, (msg_t)senp);
else
/* Not a priority sensor, put it at the end of the queue */
return chMBPostI(&srdp->srd_mailbox, (msg_t)senp);
}
/**
* @brief GPT2 callback.
*/
void irq_storm_gpt2_cb(GPTDriver *gptp) {
msg_t msg;
(void)gptp;
chSysLockFromISR();
msg = chMBPostI(&mb[IRQ_STORM_CFG_NUM_THREADS - 1], MSG_SEND_LEFT);
if (msg != MSG_OK)
saturated = true;
chSysUnlockFromISR();
}
/**
* @brief GPT1 callback.
*/
void irq_storm_gpt1_cb(GPTDriver *gptp) {
msg_t msg;
(void)gptp;
chSysLockFromISR();
msg = chMBPostI(&mb[0], MSG_SEND_RIGHT);
if (msg != MSG_OK)
saturated = true;
chSysUnlockFromISR();
}
static void test_009_003_execute(void) {
msg_t msg1, msg2;
unsigned i;
/* [9.3.1] Filling the mailbox.*/
test_set_step(1);
{
for (i = 0; i < MB_SIZE; i++) {
msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
test_assert(msg1 == MSG_OK, "wrong wake-up message");
}
}
/* [9.3.2] Testing chMBPost(), chMBPostI(), chMBPostAhead() and
chMBPostAheadI() timeout.*/
test_set_step(2);
{
msg1 = chMBPost(&mb1, 'X', 1);
test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
chSysLock();
msg1 = chMBPostI(&mb1, 'X');
chSysUnlock();
test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
msg1 = chMBPostAhead(&mb1, 'X', 1);
test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
chSysLock();
msg1 = chMBPostAheadI(&mb1, 'X');
chSysUnlock();
test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
}
/* [9.3.3] Resetting the mailbox. The mailbox is then returned in
active state.*/
test_set_step(3);
{
chMBReset(&mb1);
chMBResumeX(&mb1);
}
/* [9.3.4] Testing chMBFetch() and chMBFetchI() timeout.*/
test_set_step(4);
{
msg1 = chMBFetch(&mb1, &msg2, 1);
test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
chSysLock();
msg1 = chMBFetchI(&mb1, &msg2);
chSysUnlock();
test_assert(msg1 == MSG_TIMEOUT, "wrong wake-up message");
}
}
void Infrared_t::IRxEdgeIrq() {
if(IWaitingEdge == Falling) { // Falling edge received, start measure
RxTimer.SetCounter(0);
RxIrqWaitRising();
IWaitingEdge = Rising;
}
else { // Rising edge received, stop timer
int32_t t = RxTimer.GetCounter();
RxIrqWaitFalling();
IWaitingEdge = Falling;
// Put time to queue if it is long enough
if(t > IR_BOTTOM_BOUND_US) chMBPostI(&imailbox, t);
}
}
/*
* Post a mailbox message to main thread. Use helper functions to
* create the message.
*
*/
void sc_event_msg_post(msg_t msg, SC_EVENT_MSG_POST_FROM from)
{
msg_t ret;
if (from == SC_EVENT_MSG_POST_FROM_ISR) {
chSysLockFromIsr();
ret = chMBPostI(&event_mb, msg);
chDbgAssert(ret == RDY_OK, "chMBPostI failed", "#1");
chSysUnlockFromIsr();
} else {
ret = chMBPost(&event_mb, msg, TIME_IMMEDIATE);
chDbgAssert(ret == RDY_OK, "chMBPost failed", "#1");
}
}
/**
* @brief Put a message in the queue.
*/
osStatus osMessagePut(osMessageQId queue_id,
uint32_t info,
uint32_t millisec) {
msg_t msg;
if (port_is_isr_context()) {
/* Waiting makes no sense in ISRs so any value except "immediate"
makes no sense.*/
if (millisec != 0)
return osErrorValue;
chSysLockFromISR();
msg = chMBPostI((mailbox_t *)queue_id, (msg_t)info);
chSysUnlockFromISR();
}
else
msg = chMBPost((mailbox_t *)queue_id, (msg_t)info, (systime_t)millisec);
return msg == MSG_OK ? osOK : osEventTimeout;
}
msg_t Mailbox::postI(msg_t msg) {
return chMBPostI(&mb, msg);
}
static void mbox1_execute(void) {
msg_t msg1, msg2;
unsigned i;
/*
* Testing initial space.
*/
test_assert_lock(1, chMBGetFreeCountI(&mb1) == MB_SIZE, "wrong size");
/*
* Testing enqueuing and backward circularity.
*/
for (i = 0; i < MB_SIZE - 1; i++) {
msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
test_assert(2, msg1 == MSG_OK, "wrong wake-up message");
}
msg1 = chMBPostAhead(&mb1, 'A', TIME_INFINITE);
test_assert(3, msg1 == MSG_OK, "wrong wake-up message");
/*
* Testing post timeout.
*/
msg1 = chMBPost(&mb1, 'X', 1);
test_assert(4, msg1 == MSG_TIMEOUT, "wrong wake-up message");
chSysLock();
msg1 = chMBPostI(&mb1, 'X');
chSysUnlock();
test_assert(5, msg1 == MSG_TIMEOUT, "wrong wake-up message");
msg1 = chMBPostAhead(&mb1, 'X', 1);
test_assert(6, msg1 == MSG_TIMEOUT, "wrong wake-up message");
chSysLock();
msg1 = chMBPostAheadI(&mb1, 'X');
chSysUnlock();
test_assert(7, msg1 == MSG_TIMEOUT, "wrong wake-up message");
/*
* Testing final conditions.
*/
test_assert_lock(8, chMBGetFreeCountI(&mb1) == 0, "still empty");
test_assert_lock(9, chMBGetUsedCountI(&mb1) == MB_SIZE, "not full");
test_assert_lock(10, mb1.rdptr == mb1.wrptr, "pointers not aligned");
/*
* Testing dequeuing.
*/
for (i = 0; i < MB_SIZE; i++) {
msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
test_assert(11, msg1 == MSG_OK, "wrong wake-up message");
test_emit_token(msg2);
}
test_assert_sequence(12, "ABCDE");
/*
* Testing buffer circularity.
*/
msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
test_assert(13, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
test_assert(14, msg1 == MSG_OK, "wrong wake-up message");
test_assert(15, mb1.buffer == mb1.wrptr, "write pointer not aligned to base");
test_assert(16, mb1.buffer == mb1.rdptr, "read pointer not aligned to base");
/*
* Testing fetch timeout.
*/
msg1 = chMBFetch(&mb1, &msg2, 1);
test_assert(17, msg1 == MSG_TIMEOUT, "wrong wake-up message");
chSysLock();
msg1 = chMBFetchI(&mb1, &msg2);
chSysUnlock();
test_assert(18, msg1 == MSG_TIMEOUT, "wrong wake-up message");
/*
* Testing final conditions.
*/
test_assert_lock(19, chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
test_assert_lock(20, chMBGetUsedCountI(&mb1) == 0, "still full");
test_assert_lock(21, mb1.rdptr == mb1.wrptr, "pointers not aligned");
/*
* Testing I-Class.
*/
chSysLock();
msg1 = chMBPostI(&mb1, 'A');
test_assert(22, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBPostI(&mb1, 'B');
test_assert(23, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBPostI(&mb1, 'C');
test_assert(24, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBPostI(&mb1, 'D');
test_assert(25, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBPostI(&mb1, 'E');
chSysUnlock();
test_assert(26, msg1 == MSG_OK, "wrong wake-up message");
test_assert(27, mb1.rdptr == mb1.wrptr, "pointers not aligned");
for (i = 0; i < MB_SIZE; i++) {
chSysLock();
msg1 = chMBFetchI(&mb1, &msg2);
chSysUnlock();
test_assert(28, msg1 == MSG_OK, "wrong wake-up message");
test_emit_token(msg2);
}
test_assert_sequence(29, "ABCDE");
test_assert_lock(30, chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
test_assert_lock(31, chMBGetUsedCountI(&mb1) == 0, "still full");
test_assert(32, mb1.rdptr == mb1.wrptr, "pointers not aligned");
chSysLock();
msg1 = chMBPostAheadI(&mb1, 'E');
test_assert(33, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBPostAheadI(&mb1, 'D');
test_assert(34, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBPostAheadI(&mb1, 'C');
test_assert(35, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBPostAheadI(&mb1, 'B');
test_assert(36, msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBPostAheadI(&mb1, 'A');
chSysUnlock();
test_assert(37, msg1 == MSG_OK, "wrong wake-up message");
test_assert(38, mb1.rdptr == mb1.wrptr, "pointers not aligned");
for (i = 0; i < MB_SIZE; i++) {
chSysLock();
msg1 = chMBFetchI(&mb1, &msg2);
chSysUnlock();
test_assert(39, msg1 == MSG_OK, "wrong wake-up message");
test_emit_token(msg2);
}
test_assert_sequence(40, "ABCDE");
test_assert_lock(41, chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
test_assert_lock(42, chMBGetUsedCountI(&mb1) == 0, "still full");
test_assert(43, mb1.rdptr == mb1.wrptr, "pointers not aligned");
/*
* Testing reset.
*/
chMBReset(&mb1);
/*
* Re-testing final conditions.
*/
test_assert_lock(44, chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
test_assert_lock(45, chMBGetUsedCountI(&mb1) == 0, "still full");
test_assert_lock(46, mb1.buffer == mb1.wrptr, "write pointer not aligned to base");
test_assert_lock(47, mb1.buffer == mb1.rdptr, "read pointer not aligned to base");
}
static void test_008_002_execute(void) {
msg_t msg1, msg2;
unsigned i;
/* [8.2.1] Testing the mailbox size.*/
test_set_step(1);
{
test_assert_lock(chMBGetFreeCountI(&mb1) == MB_SIZE, "wrong size");
}
/* [8.2.2] Resetting the mailbox, conditions are checked, no errors
expected.*/
test_set_step(2);
{
chSysLock();
chMBResetI(&mb1);
chSysUnlock();
test_assert_lock(chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
test_assert_lock(chMBGetUsedCountI(&mb1) == 0, "still full");
test_assert_lock(mb1.buffer == mb1.wrptr, "write pointer not aligned to base");
test_assert_lock(mb1.buffer == mb1.rdptr, "read pointer not aligned to base");
}
/* [8.2.3] Filling the mailbox using chMBPostI() and chMBPostAheadI()
once, no errors expected.*/
test_set_step(3);
{
for (i = 0; i < MB_SIZE - 1; i++) {
chSysLock();
msg1 = chMBPostI(&mb1, 'B' + i);
chSysUnlock();
test_assert(msg1 == MSG_OK, "wrong wake-up message");
}
chSysLock();
msg1 = chMBPostAheadI(&mb1, 'A');
chSysUnlock();
test_assert(msg1 == MSG_OK, "wrong wake-up message");
}
/* [8.2.4] Testing intermediate conditions. Data pointers must be
aligned, semaphore counters are checked.*/
test_set_step(4);
{
test_assert_lock(chMBGetFreeCountI(&mb1) == 0, "still empty");
test_assert_lock(chMBGetUsedCountI(&mb1) == MB_SIZE, "not full");
test_assert_lock(mb1.rdptr == mb1.wrptr, "pointers not aligned");
}
/* [8.2.5] Emptying the mailbox using chMBFetchI(), no errors
expected.*/
test_set_step(5);
{
for (i = 0; i < MB_SIZE; i++) {
chSysLock();
msg1 = chMBFetchI(&mb1, &msg2);
chSysUnlock();
test_assert(msg1 == MSG_OK, "wrong wake-up message");
test_emit_token(msg2);
}
test_assert_sequence("ABCD", "wrong get sequence");
}
/* [8.2.6] Posting and then fetching one more message, no errors
expected.*/
test_set_step(6);
{
msg1 = chMBPost(&mb1, 'B' + i, TIME_INFINITE);
test_assert(msg1 == MSG_OK, "wrong wake-up message");
msg1 = chMBFetch(&mb1, &msg2, TIME_INFINITE);
test_assert(msg1 == MSG_OK, "wrong wake-up message");
}
/* [8.2.7] Testing final conditions. Data pointers must be aligned to
buffer start, semaphore counters are checked.*/
test_set_step(7);
{
test_assert_lock(chMBGetFreeCountI(&mb1) == MB_SIZE, "not empty");
test_assert_lock(chMBGetUsedCountI(&mb1) == 0, "still full");
test_assert(mb1.buffer == mb1.wrptr, "write pointer not aligned to base");
test_assert(mb1.buffer == mb1.rdptr, "read pointer not aligned to base");
}
}
