/// Reset a Message Queue to initial empty state.
/// \note API identical to osMessageQueueReset
osStatus_t svcRtxMessageQueueReset (osMessageQueueId_t mq_id) {
os_message_queue_t *mq = (os_message_queue_t *)mq_id;
os_message_t *msg;
os_thread_t *thread;
uint32_t *reg;
// Check parameters
if ((mq == NULL) || (mq->id != osRtxIdMessageQueue)) {
EvrRtxMessageQueueError(mq, osErrorParameter);
return osErrorParameter;
}
// Check object state
if (mq->state == osRtxObjectInactive) {
EvrRtxMessageQueueError(mq, osErrorResource);
return osErrorResource;
}
// Remove Messages from Queue
for (;;) {
// Get Message from Queue
msg = MessageQueueGet(mq);
if (msg == NULL) {
break;
}
MessageQueueRemove(mq, msg);
EvrRtxMessageQueueRetrieved(mq, NULL);
// Free memory
msg->state = osRtxObjectInactive;
osRtxMemoryPoolFree(&mq->mp_info, msg);
}
// Check if Threads are waiting to send Messages
if ((mq->thread_list != NULL) && (mq->thread_list->state == osRtxThreadWaitingMessagePut)) {
do {
// Try to allocate memory
msg = osRtxMemoryPoolAlloc(&mq->mp_info);
if (msg != NULL) {
// Wakeup waiting Thread with highest Priority
thread = osRtxThreadListGet((os_object_t*)mq);
osRtxThreadWaitExit(thread, (uint32_t)osOK, false);
// Copy Message (R2: const void *msg_ptr, R3: uint8_t msg_prio)
reg = osRtxThreadRegPtr(thread);
memcpy((uint8_t *)msg + sizeof(os_message_t), (void *)reg[2], mq->msg_size);
// Store Message into Queue
msg->id = osRtxIdMessage;
msg->state = osRtxObjectActive;
msg->flags = 0U;
msg->priority = (uint8_t)reg[3];
MessageQueuePut(mq, msg);
EvrRtxMessageQueueInserted(mq, (void *)reg[2]);
}
} while ((msg != NULL) && (mq->thread_list != NULL));
osRtxThreadDispatch(NULL);
}
EvrRtxMessageQueueResetDone(mq);
return osOK;
}
/// Delete a Message Queue object.
/// \note API identical to osMessageQueueDelete
osStatus_t svcRtxMessageQueueDelete (osMessageQueueId_t mq_id) {
os_message_queue_t *mq = (os_message_queue_t *)mq_id;
os_thread_t *thread;
// Check parameters
if ((mq == NULL) || (mq->id != osRtxIdMessageQueue)) {
EvrRtxMessageQueueError(mq, osErrorParameter);
return osErrorParameter;
}
// Check object state
if (mq->state == osRtxObjectInactive) {
EvrRtxMessageQueueError(mq, osErrorResource);
return osErrorResource;
}
// Mark object as inactive
mq->state = osRtxObjectInactive;
// Unblock waiting threads
if (mq->thread_list != NULL) {
do {
thread = osRtxThreadListGet((os_object_t*)mq);
osRtxThreadWaitExit(thread, (uint32_t)osErrorResource, false);
} while (mq->thread_list != NULL);
osRtxThreadDispatch(NULL);
}
// Free data memory
if (mq->flags & osRtxFlagSystemMemory) {
osRtxMemoryFree(osRtxInfo.mem.mq_data, mq->mp_info.block_base);
}
// Free object memory
if (mq->flags & osRtxFlagSystemObject) {
if (osRtxInfo.mpi.message_queue != NULL) {
osRtxMemoryPoolFree(osRtxInfo.mpi.message_queue, mq);
} else {
osRtxMemoryFree(osRtxInfo.mem.common, mq);
}
}
EvrRtxMessageQueueDestroyed(mq);
return osOK;
}
/// Delete a Semaphore object.
/// \note API identical to osSemaphoreDelete
osStatus_t svcRtxSemaphoreDelete (osSemaphoreId_t semaphore_id) {
os_semaphore_t *semaphore = (os_semaphore_t *)semaphore_id;
os_thread_t *thread;
// Check parameters
if ((semaphore == NULL) || (semaphore->id != osRtxIdSemaphore)) {
EvrRtxSemaphoreError(semaphore, osErrorParameter);
return osErrorParameter;
}
// Check object state
if (semaphore->state == osRtxObjectInactive) {
EvrRtxSemaphoreError(semaphore, osErrorResource);
return osErrorResource;
}
// Mark object as inactive
semaphore->state = osRtxObjectInactive;
// Unblock waiting threads
if (semaphore->thread_list != NULL) {
do {
thread = osRtxThreadListGet((os_object_t*)semaphore);
osRtxThreadWaitExit(thread, (uint32_t)osErrorResource, false);
} while (semaphore->thread_list != NULL);
osRtxThreadDispatch(NULL);
}
// Free object memory
if (semaphore->flags & osRtxFlagSystemObject) {
if (osRtxInfo.mpi.semaphore != NULL) {
osRtxMemoryPoolFree(osRtxInfo.mpi.semaphore, semaphore);
} else {
osRtxMemoryFree(osRtxInfo.mem.common, semaphore);
}
}
EvrRtxSemaphoreDestroyed(semaphore);
return osOK;
}
/// Delete a timer.
/// \note API identical to osTimerDelete
static osStatus_t svcRtxTimerDelete (osTimerId_t timer_id) {
os_timer_t *timer = osRtxTimerId(timer_id);
// Check parameters
if ((timer == NULL) || (timer->id != osRtxIdTimer)) {
EvrRtxTimerError(timer, (int32_t)osErrorParameter);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return osErrorParameter;
}
// Check object state
if (timer->state == osRtxTimerInactive) {
EvrRtxTimerError(timer, (int32_t)osErrorResource);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return osErrorResource;
}
if (timer->state == osRtxTimerRunning) {
TimerRemove(timer);
}
// Mark object as inactive
timer->state = osRtxTimerInactive;
// Free object memory
if ((timer->flags & osRtxFlagSystemObject) != 0U) {
if (osRtxInfo.mpi.timer != NULL) {
(void)osRtxMemoryPoolFree(osRtxInfo.mpi.timer, timer);
} else {
(void)osRtxMemoryFree(osRtxInfo.mem.common, timer);
}
#if (defined(OS_OBJ_MEM_USAGE) && (OS_OBJ_MEM_USAGE != 0))
osRtxTimerMemUsage.cnt_free++;
#endif
}
EvrRtxTimerDestroyed(timer);
return osOK;
}
/// Delete a Mutex object.
/// \note API identical to osMutexDelete
osStatus_t svcRtxMutexDelete (osMutexId_t mutex_id) {
os_mutex_t *mutex = (os_mutex_t *)mutex_id;
os_mutex_t *mutex0;
os_thread_t *thread;
int8_t priority;
// Check parameters
if ((mutex == NULL) || (mutex->id != osRtxIdMutex)) {
EvrRtxMutexError(mutex, osErrorParameter);
return osErrorParameter;
}
// Check object state
if (mutex->state == osRtxObjectInactive) {
EvrRtxMutexError(mutex, osErrorResource);
return osErrorResource;
}
// Mark object as inactive
mutex->state = osRtxObjectInactive;
// Check if Mutex is locked
if (mutex->lock != 0U) {
thread = mutex->owner_thread;
// Remove Mutex from Thread owner list
if (mutex->owner_next != NULL) {
mutex->owner_next->owner_prev = mutex->owner_prev;
}
if (mutex->owner_prev != NULL) {
mutex->owner_prev->owner_next = mutex->owner_next;
} else {
thread->mutex_list = mutex->owner_next;
}
// Restore owner Thread priority
if (mutex->attr & osMutexPrioInherit) {
priority = thread->priority_base;
mutex0 = thread->mutex_list;
while (mutex0) {
// Mutexes owned by running Thread
if ((mutex0->thread_list != NULL) && (mutex0->thread_list->priority > priority)) {
// Higher priority Thread is waiting for Mutex
priority = mutex0->thread_list->priority;
}
mutex0 = mutex0->owner_next;
}
if (thread->priority != priority) {
thread->priority = priority;
osRtxThreadListSort(thread);
}
}
// Unblock waiting threads
if (mutex->thread_list != NULL) {
do {
thread = osRtxThreadListGet((os_object_t*)mutex);
osRtxThreadWaitExit(thread, (uint32_t)osErrorResource, false);
} while (mutex->thread_list != NULL);
}
osRtxThreadDispatch(NULL);
}
// Free object memory
if (mutex->flags & osRtxFlagSystemObject) {
if (osRtxInfo.mpi.mutex != NULL) {
osRtxMemoryPoolFree(osRtxInfo.mpi.mutex, mutex);
} else {
osRtxMemoryFree(osRtxInfo.mem.common, mutex);
}
}
EvrRtxMutexDestroyed(mutex);
return osOK;
}
/// Delete a Mutex object.
/// \note API identical to osMutexDelete
static osStatus_t svcRtxMutexDelete (osMutexId_t mutex_id) {
os_mutex_t *mutex = osRtxMutexId(mutex_id);
const os_mutex_t *mutex0;
os_thread_t *thread;
int8_t priority;
// Check parameters
if ((mutex == NULL) || (mutex->id != osRtxIdMutex)) {
EvrRtxMutexError(mutex, (int32_t)osErrorParameter);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return osErrorParameter;
}
// Check if Mutex is locked
if (mutex->lock != 0U) {
thread = mutex->owner_thread;
// Remove Mutex from Thread owner list
if (mutex->owner_next != NULL) {
mutex->owner_next->owner_prev = mutex->owner_prev;
}
if (mutex->owner_prev != NULL) {
mutex->owner_prev->owner_next = mutex->owner_next;
} else {
thread->mutex_list = mutex->owner_next;
}
// Restore owner Thread priority
if ((mutex->attr & osMutexPrioInherit) != 0U) {
priority = thread->priority_base;
mutex0 = thread->mutex_list;
while (mutex0 != NULL) {
// Mutexes owned by running Thread
if ((mutex0->thread_list != NULL) && (mutex0->thread_list->priority > priority)) {
// Higher priority Thread is waiting for Mutex
priority = mutex0->thread_list->priority;
}
mutex0 = mutex0->owner_next;
}
if (thread->priority != priority) {
thread->priority = priority;
osRtxThreadListSort(thread);
}
}
// Unblock waiting threads
if (mutex->thread_list != NULL) {
do {
thread = osRtxThreadListGet(osRtxObject(mutex));
osRtxThreadWaitExit(thread, (uint32_t)osErrorResource, FALSE);
} while (mutex->thread_list != NULL);
}
osRtxThreadDispatch(NULL);
}
// Mark object as invalid
mutex->id = osRtxIdInvalid;
// Free object memory
if ((mutex->flags & osRtxFlagSystemObject) != 0U) {
if (osRtxInfo.mpi.mutex != NULL) {
(void)osRtxMemoryPoolFree(osRtxInfo.mpi.mutex, mutex);
} else {
(void)osRtxMemoryFree(osRtxInfo.mem.common, mutex);
}
#if (defined(OS_OBJ_MEM_USAGE) && (OS_OBJ_MEM_USAGE != 0))
osRtxMutexMemUsage.cnt_free++;
#endif
}
EvrRtxMutexDestroyed(mutex);
return osOK;
}
/// Get a Message from a Queue or timeout if Queue is empty.
/// \note API identical to osMessageQueueGet
osStatus_t svcRtxMessageQueueGet (osMessageQueueId_t mq_id, void *msg_ptr, uint8_t *msg_prio, uint32_t timeout) {
os_message_queue_t *mq = (os_message_queue_t *)mq_id;
os_message_t *msg;
os_thread_t *thread;
uint32_t *reg;
// Check parameters
if ((mq == NULL) || (mq->id != osRtxIdMessageQueue) || (msg_ptr == NULL)) {
EvrRtxMessageQueueError(mq, osErrorParameter);
return osErrorParameter;
}
// Check object state
if (mq->state == osRtxObjectInactive) {
EvrRtxMessageQueueError(mq, osErrorResource);
return osErrorResource;
}
// Get Message from Queue
msg = MessageQueueGet(mq);
if (msg != NULL) {
MessageQueueRemove(mq, msg);
// Copy Message
memcpy(msg_ptr, (uint8_t *)msg + sizeof(os_message_t), mq->msg_size);
if (msg_prio != NULL) {
*msg_prio = msg->priority;
}
EvrRtxMessageQueueRetrieved(mq, msg_ptr);
// Free memory
msg->state = osRtxObjectInactive;
osRtxMemoryPoolFree(&mq->mp_info, msg);
} else {
// No Message available
if (timeout != 0U) {
EvrRtxMessageQueueGetPending(mq, msg_ptr, timeout);
// Suspend current Thread
osRtxThreadListPut((os_object_t*)mq, osRtxThreadGetRunning());
osRtxThreadWaitEnter(osRtxThreadWaitingMessageGet, timeout);
// Save arguments (R2: void *msg_ptr, R3: uint8_t *msg_prio)
reg = (uint32_t *)(__get_PSP());
reg[2] = (uint32_t)msg_ptr;
reg[3] = (uint32_t)msg_prio;
return osErrorTimeout;
} else {
EvrRtxMessageQueueNotRetrieved(mq, msg_ptr);
return osErrorResource;
}
}
// Check if Thread is waiting to send a Message
if ((mq->thread_list != NULL) && (mq->thread_list->state == osRtxThreadWaitingMessagePut)) {
// Try to allocate memory
msg = osRtxMemoryPoolAlloc(&mq->mp_info);
if (msg != NULL) {
// Wakeup waiting Thread with highest Priority
thread = osRtxThreadListGet((os_object_t*)mq);
osRtxThreadWaitExit(thread, (uint32_t)osOK, true);
// Copy Message (R2: const void *msg_ptr, R3: uint8_t msg_prio)
reg = osRtxThreadRegPtr(thread);
memcpy((uint8_t *)msg + sizeof(os_message_t), (void *)reg[2], mq->msg_size);
// Store Message into Queue
msg->id = osRtxIdMessage;
msg->state = osRtxObjectActive;
msg->flags = 0U;
msg->priority = (uint8_t)reg[3];
MessageQueuePut(mq, msg);
EvrRtxMessageQueueInserted(mq, (void *)reg[2]);
}
}
return osOK;
}
/// Create and Initialize a Message Queue object.
/// \note API identical to osMessageQueueNew
osMessageQueueId_t svcRtxMessageQueueNew (uint32_t msg_count, uint32_t msg_size, const osMessageQueueAttr_t *attr) {
os_message_queue_t *mq;
void *mq_mem;
uint32_t mq_size;
uint32_t block_size;
uint32_t size;
uint8_t flags;
const char *name;
// Check parameters
if ((msg_count == 0U) || (msg_size == 0U)) {
EvrRtxMessageQueueError(NULL, osErrorParameter);
return NULL;
}
msg_size = (msg_size + 3U) & ~3UL;
block_size = msg_size + sizeof(os_message_t);
if ((__CLZ(msg_count) + __CLZ(block_size)) < 32) {
EvrRtxMessageQueueError(NULL, osErrorParameter);
return NULL;
}
size = msg_count * block_size;
// Process attributes
if (attr != NULL) {
name = attr->name;
mq = attr->cb_mem;
mq_mem = attr->mq_mem;
mq_size = attr->mq_size;
if (mq != NULL) {
if (((uint32_t)mq & 3U) || (attr->cb_size < sizeof(os_message_queue_t))) {
EvrRtxMessageQueueError(NULL, osRtxErrorInvalidControlBlock);
return NULL;
}
} else {
if (attr->cb_size != 0U) {
EvrRtxMessageQueueError(NULL, osRtxErrorInvalidControlBlock);
return NULL;
}
}
if (mq_mem != NULL) {
if (((uint32_t)mq_mem & 3U) || (mq_size < size)) {
EvrRtxMessageQueueError(NULL, osRtxErrorInvalidDataMemory);
return NULL;
}
} else {
if (mq_size != 0U) {
EvrRtxMessageQueueError(NULL, osRtxErrorInvalidDataMemory);
return NULL;
}
}
} else {
name = NULL;
mq = NULL;
mq_mem = NULL;
}
// Allocate object memory if not provided
if (mq == NULL) {
if (osRtxInfo.mpi.message_queue != NULL) {
mq = osRtxMemoryPoolAlloc(osRtxInfo.mpi.message_queue);
} else {
mq = osRtxMemoryAlloc(osRtxInfo.mem.common, sizeof(os_message_queue_t), 1U);
}
if (mq == NULL) {
EvrRtxMessageQueueError(NULL, osErrorNoMemory);
return NULL;
}
flags = osRtxFlagSystemObject;
} else {
flags = 0U;
}
// Allocate data memory if not provided
if (mq_mem == NULL) {
mq_mem = osRtxMemoryAlloc(osRtxInfo.mem.mq_data, size, 0U);
if (mq_mem == NULL) {
EvrRtxMessageQueueError(NULL, osErrorNoMemory);
if (flags & osRtxFlagSystemObject) {
if (osRtxInfo.mpi.message_queue != NULL) {
osRtxMemoryPoolFree(osRtxInfo.mpi.message_queue, mq);
} else {
osRtxMemoryFree(osRtxInfo.mem.common, mq);
}
}
return NULL;
}
memset(mq_mem, 0, size);
flags |= osRtxFlagSystemMemory;
}
// Initialize control block
mq->id = osRtxIdMessageQueue;
mq->state = osRtxObjectActive;
mq->flags = flags;
mq->name = name;
mq->thread_list = NULL;
mq->msg_size = msg_size;
mq->msg_count = 0U;
mq->msg_first = NULL;
mq->msg_last = NULL;
osRtxMemoryPoolInit(&mq->mp_info, msg_count, block_size, mq_mem);
// Register post ISR processing function
osRtxInfo.post_process.message_queue = osRtxMessageQueuePostProcess;
EvrRtxMessageQueueCreated(mq);
return mq;
}
/// Message Queue post ISR processing.
/// \param[in] msg message object.
void osRtxMessageQueuePostProcess (os_message_t *msg) {
os_message_queue_t *mq;
os_thread_t *thread;
uint32_t *reg;
void **ptr;
if (msg->state == osRtxObjectInactive) {
return;
}
if (msg->flags != 0U) {
// Remove Message
ptr = (void *)((uint8_t *)msg + sizeof(os_message_t));
mq = *ptr;
if (mq->state == osRtxObjectInactive) {
return;
}
MessageQueueRemove(mq, msg);
// Free memory
msg->state = osRtxObjectInactive;
osRtxMemoryPoolFree(&mq->mp_info, msg);
// Check if Thread is waiting to send a Message
if ((mq->thread_list != NULL) && (mq->thread_list->state == osRtxThreadWaitingMessagePut)) {
// Try to allocate memory
msg = osRtxMemoryPoolAlloc(&mq->mp_info);
if (msg != NULL) {
// Wakeup waiting Thread with highest Priority
thread = osRtxThreadListGet((os_object_t*)mq);
osRtxThreadWaitExit(thread, (uint32_t)osOK, false);
// Copy Message (R2: const void *msg_ptr, R3: uint8_t msg_prio)
reg = osRtxThreadRegPtr(thread);
memcpy((uint8_t *)msg + sizeof(os_message_t), (void *)reg[2], mq->msg_size);
// Store Message into Queue
msg->id = osRtxIdMessage;
msg->state = osRtxObjectActive;
msg->flags = 0U;
msg->priority = (uint8_t)reg[3];
MessageQueuePut(mq, msg);
EvrRtxMessageQueueInserted(mq, (void *)reg[2]);
}
}
} else {
// New Message
mq = (void *)msg->next;
if (mq->state == osRtxObjectInactive) {
return;
}
// Check if Thread is waiting to receive a Message
if ((mq->thread_list != NULL) && (mq->thread_list->state == osRtxThreadWaitingMessageGet)) {
EvrRtxMessageQueueInserted(mq, (void *)msg->prev);
// Wakeup waiting Thread with highest Priority
thread = osRtxThreadListGet((os_object_t*)mq);
osRtxThreadWaitExit(thread, (uint32_t)osOK, false);
// Copy Message (R2: void *msg_ptr, R3: uint8_t *msg_prio)
reg = osRtxThreadRegPtr(thread);
memcpy((void *)reg[2], (uint8_t *)msg + sizeof(os_message_t), mq->msg_size);
if (reg[3] != 0U) {
*((uint8_t *)reg[3]) = msg->priority;
}
EvrRtxMessageQueueRetrieved(mq, (void *)reg[2]);
// Free memory
msg->state = osRtxObjectInactive;
osRtxMemoryPoolFree(&mq->mp_info, msg);
} else {
EvrRtxMessageQueueInserted(mq, (void *)msg->prev);
MessageQueuePut(mq, msg);
}
}
}