/// 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;
}
/// Create and Initialize a Mutex object.
/// \note API identical to osMutexNew
osMutexId_t svcRtxMutexNew (const osMutexAttr_t *attr) {
os_mutex_t *mutex;
uint32_t attr_bits;
uint8_t flags;
const char *name;
// Process attributes
if (attr != NULL) {
name = attr->name;
attr_bits = attr->attr_bits;
mutex = attr->cb_mem;
if (mutex != NULL) {
if (((uint32_t)mutex & 3U) || (attr->cb_size < sizeof(os_mutex_t))) {
EvrRtxMutexError(NULL, osRtxErrorInvalidControlBlock);
return NULL;
}
} else {
if (attr->cb_size != 0U) {
EvrRtxMutexError(NULL, osRtxErrorInvalidControlBlock);
return NULL;
}
}
} else {
name = NULL;
attr_bits = 0U;
mutex = NULL;
}
// Allocate object memory if not provided
if (mutex == NULL) {
if (osRtxInfo.mpi.mutex != NULL) {
mutex = osRtxMemoryPoolAlloc(osRtxInfo.mpi.mutex);
} else {
mutex = osRtxMemoryAlloc(osRtxInfo.mem.common, sizeof(os_mutex_t), 1U);
}
if (mutex == NULL) {
EvrRtxMutexError(NULL, osErrorNoMemory);
return NULL;
}
flags = osRtxFlagSystemObject;
} else {
flags = 0U;
}
// Initialize control block
mutex->id = osRtxIdMutex;
mutex->state = osRtxObjectActive;
mutex->flags = flags;
mutex->attr = (uint8_t)attr_bits;
mutex->name = name;
mutex->thread_list = NULL;
mutex->owner_thread = NULL;
mutex->owner_prev = NULL;
mutex->owner_next = NULL;
mutex->lock = 0U;
EvrRtxMutexCreated(mutex);
return mutex;
}
/// Put a Message into a Queue or timeout if Queue is full.
/// \note API identical to osMessageQueuePut
__STATIC_INLINE
osStatus_t isrRtxMessageQueuePut (osMessageQueueId_t mq_id, const 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;
const void **ptr;
// Check parameters
if ((mq == NULL) || (mq->id != osRtxIdMessageQueue) || (msg_ptr == NULL) || (timeout != 0U)) {
EvrRtxMessageQueueError(mq, osErrorParameter);
return osErrorParameter;
}
// Check object state
if (mq->state == osRtxObjectInactive) {
EvrRtxMessageQueueError(mq, osErrorResource);
return osErrorResource;
}
// Try to allocate memory
msg = osRtxMemoryPoolAlloc(&mq->mp_info);
if (msg != NULL) {
// Copy Message
memcpy((uint8_t *)msg + sizeof(os_message_t), msg_ptr, mq->msg_size);
msg->id = osRtxIdMessage;
msg->state = osRtxObjectActive;
msg->flags = 0U;
msg->priority = msg_prio;
// Register post ISR processing
ptr = (void *)&msg->prev;
*ptr = msg_ptr;
ptr = (void *)&msg->next;
*ptr = mq;
osRtxPostProcess((os_object_t *)msg);
} else {
// No memory available
EvrRtxMessageQueueNotInserted(mq, msg_ptr);
return osErrorResource;
}
EvrRtxMessageQueueInsertPending(mq, msg_ptr);
return osOK;
}
/// Create and Initialize a Mutex object.
/// \note API identical to osMutexNew
static osMutexId_t svcRtxMutexNew (const osMutexAttr_t *attr) {
os_mutex_t *mutex;
uint32_t attr_bits;
uint8_t flags;
const char *name;
// Process attributes
if (attr != NULL) {
name = attr->name;
attr_bits = attr->attr_bits;
//lint -e{9079} "conversion from pointer to void to pointer to other type" [MISRA Note 6]
mutex = attr->cb_mem;
if (mutex != NULL) {
//lint -e(923) -e(9078) "cast from pointer to unsigned int" [MISRA Note 7]
if ((((uint32_t)mutex & 3U) != 0U) || (attr->cb_size < sizeof(os_mutex_t))) {
EvrRtxMutexError(NULL, osRtxErrorInvalidControlBlock);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return NULL;
}
} else {
if (attr->cb_size != 0U) {
EvrRtxMutexError(NULL, osRtxErrorInvalidControlBlock);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return NULL;
}
}
} else {
name = NULL;
attr_bits = 0U;
mutex = NULL;
}
// Allocate object memory if not provided
if (mutex == NULL) {
if (osRtxInfo.mpi.mutex != NULL) {
//lint -e{9079} "conversion from pointer to void to pointer to other type" [MISRA Note 5]
mutex = osRtxMemoryPoolAlloc(osRtxInfo.mpi.mutex);
} else {
//lint -e{9079} "conversion from pointer to void to pointer to other type" [MISRA Note 5]
mutex = osRtxMemoryAlloc(osRtxInfo.mem.common, sizeof(os_mutex_t), 1U);
}
#if (defined(OS_OBJ_MEM_USAGE) && (OS_OBJ_MEM_USAGE != 0))
if (mutex != NULL) {
uint32_t used;
osRtxMutexMemUsage.cnt_alloc++;
used = osRtxMutexMemUsage.cnt_alloc - osRtxMutexMemUsage.cnt_free;
if (osRtxMutexMemUsage.max_used < used) {
osRtxMutexMemUsage.max_used = used;
}
}
#endif
flags = osRtxFlagSystemObject;
} else {
flags = 0U;
}
if (mutex != NULL) {
// Initialize control block
mutex->id = osRtxIdMutex;
mutex->flags = flags;
mutex->attr = (uint8_t)attr_bits;
mutex->name = name;
mutex->thread_list = NULL;
mutex->owner_thread = NULL;
mutex->owner_prev = NULL;
mutex->owner_next = NULL;
mutex->lock = 0U;
EvrRtxMutexCreated(mutex, mutex->name);
} else {
EvrRtxMutexError(NULL, (int32_t)osErrorNoMemory);
}
return mutex;
}
/// 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);
}
}
}
/// Create and Initialize a timer.
/// \note API identical to osTimerNew
static osTimerId_t svcRtxTimerNew (osTimerFunc_t func, osTimerType_t type, void *argument, const osTimerAttr_t *attr) {
os_timer_t *timer;
uint8_t flags;
const char *name;
// Check parameters
if ((func == NULL) || ((type != osTimerOnce) && (type != osTimerPeriodic))) {
EvrRtxTimerError(NULL, (int32_t)osErrorParameter);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return NULL;
}
// Process attributes
if (attr != NULL) {
name = attr->name;
//lint -e{9079} "conversion from pointer to void to pointer to other type" [MISRA Note 6]
timer = attr->cb_mem;
if (timer != NULL) {
//lint -e(923) -e(9078) "cast from pointer to unsigned int" [MISRA Note 7]
if ((((uint32_t)timer & 3U) != 0U) || (attr->cb_size < sizeof(os_timer_t))) {
EvrRtxTimerError(NULL, osRtxErrorInvalidControlBlock);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return NULL;
}
} else {
if (attr->cb_size != 0U) {
EvrRtxTimerError(NULL, osRtxErrorInvalidControlBlock);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return NULL;
}
}
} else {
name = NULL;
timer = NULL;
}
// Allocate object memory if not provided
if (timer == NULL) {
if (osRtxInfo.mpi.timer != NULL) {
//lint -e{9079} "conversion from pointer to void to pointer to other type" [MISRA Note 5]
timer = osRtxMemoryPoolAlloc(osRtxInfo.mpi.timer);
} else {
//lint -e{9079} "conversion from pointer to void to pointer to other type" [MISRA Note 5]
timer = osRtxMemoryAlloc(osRtxInfo.mem.common, sizeof(os_timer_t), 1U);
}
#if (defined(OS_OBJ_MEM_USAGE) && (OS_OBJ_MEM_USAGE != 0))
if (timer != NULL) {
uint32_t used;
osRtxTimerMemUsage.cnt_alloc++;
used = osRtxTimerMemUsage.cnt_alloc - osRtxTimerMemUsage.cnt_free;
if (osRtxTimerMemUsage.max_used < used) {
osRtxTimerMemUsage.max_used = used;
}
}
#endif
flags = osRtxFlagSystemObject;
} else {
flags = 0U;
}
if (timer != NULL) {
// Initialize control block
timer->id = osRtxIdTimer;
timer->state = osRtxTimerStopped;
timer->flags = flags;
timer->type = (uint8_t)type;
timer->name = name;
timer->prev = NULL;
timer->next = NULL;
timer->tick = 0U;
timer->load = 0U;
timer->finfo.func = func;
timer->finfo.arg = argument;
EvrRtxTimerCreated(timer, timer->name);
} else {
EvrRtxTimerError(NULL, (int32_t)osErrorNoMemory);
}
return timer;
}
/// Create and Initialize a Semaphore object.
/// \note API identical to osSemaphoreNew
osSemaphoreId_t svcRtxSemaphoreNew (uint32_t max_count, uint32_t initial_count, const osSemaphoreAttr_t *attr) {
os_semaphore_t *semaphore;
uint8_t flags;
const char *name;
// Check parameters
if ((max_count == 0U) || (max_count > osRtxSemaphoreTokenLimit) || (initial_count > max_count)) {
EvrRtxSemaphoreError(NULL, osErrorParameter);
return NULL;
}
// Process attributes
if (attr != NULL) {
name = attr->name;
semaphore = attr->cb_mem;
if (semaphore != NULL) {
if (((uint32_t)semaphore & 3U) || (attr->cb_size < sizeof(os_semaphore_t))) {
EvrRtxSemaphoreError(NULL, osRtxErrorInvalidControlBlock);
return NULL;
}
} else {
if (attr->cb_size != 0U) {
EvrRtxSemaphoreError(NULL, osRtxErrorInvalidControlBlock);
return NULL;
}
}
} else {
name = NULL;
semaphore = NULL;
}
// Allocate object memory if not provided
if (semaphore == NULL) {
if (osRtxInfo.mpi.semaphore != NULL) {
semaphore = osRtxMemoryPoolAlloc(osRtxInfo.mpi.semaphore);
} else {
semaphore = osRtxMemoryAlloc(osRtxInfo.mem.common, sizeof(os_semaphore_t), 1U);
}
if (semaphore == NULL) {
EvrRtxSemaphoreError(NULL, osErrorNoMemory);
return NULL;
}
flags = osRtxFlagSystemObject;
} else {
flags = 0U;
}
// Initialize control block
semaphore->id = osRtxIdSemaphore;
semaphore->state = osRtxObjectActive;
semaphore->flags = flags;
semaphore->name = name;
semaphore->thread_list = NULL;
semaphore->tokens = (uint16_t)initial_count;
semaphore->max_tokens = (uint16_t)max_count;
// Register post ISR processing function
osRtxInfo.post_process.semaphore = osRtxSemaphorePostProcess;
EvrRtxSemaphoreCreated(semaphore);
return semaphore;
}
