/// 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;
}
/// Initialize the RTOS Kernel.
/// \note API identical to osKernelInitialize
static osStatus_t svcRtxKernelInitialize (void) {
if (osRtxInfo.kernel.state == osRtxKernelReady) {
EvrRtxKernelInitializeCompleted();
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return osOK;
}
if (osRtxInfo.kernel.state != osRtxKernelInactive) {
EvrRtxKernelError((int32_t)osError);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return osError;
}
if (osRtxConfig.thread_stack_size < (64U + 8U)) {
EvrRtxKernelError(osRtxErrorInvalidThreadStack);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return osError;
}
if ((osRtxConfig.isr_queue.data == NULL) || (osRtxConfig.isr_queue.max == 0U)) {
EvrRtxKernelError((int32_t)osError);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return osError;
}
#if (DOMAIN_NS == 1)
// Initialize Secure Process Stack
if (TZ_InitContextSystem_S() == 0U) {
EvrRtxKernelError(osRtxErrorTZ_InitContext_S);
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return osError;
}
#endif
// Initialize osRtxInfo
memset(&osRtxInfo.kernel, 0, sizeof(osRtxInfo) - offsetof(osRtxInfo_t, kernel));
osRtxInfo.isr_queue.data = osRtxConfig.isr_queue.data;
osRtxInfo.isr_queue.max = osRtxConfig.isr_queue.max;
osRtxInfo.thread.robin.timeout = osRtxConfig.robin_timeout;
// Initialize Memory Pools (Variable Block Size)
if (osRtxMemoryInit(osRtxConfig.mem.common_addr, osRtxConfig.mem.common_size) != 0U) {
osRtxInfo.mem.common = osRtxConfig.mem.common_addr;
}
if (osRtxMemoryInit(osRtxConfig.mem.stack_addr, osRtxConfig.mem.stack_size) != 0U) {
osRtxInfo.mem.stack = osRtxConfig.mem.stack_addr;
} else {
osRtxInfo.mem.stack = osRtxInfo.mem.common;
}
if (osRtxMemoryInit(osRtxConfig.mem.mp_data_addr, osRtxConfig.mem.mp_data_size) != 0U) {
osRtxInfo.mem.mp_data = osRtxConfig.mem.mp_data_addr;
} else {
osRtxInfo.mem.mp_data = osRtxInfo.mem.common;
}
if (osRtxMemoryInit(osRtxConfig.mem.mq_data_addr, osRtxConfig.mem.mq_data_size) != 0U) {
osRtxInfo.mem.mq_data = osRtxConfig.mem.mq_data_addr;
} else {
osRtxInfo.mem.mq_data = osRtxInfo.mem.common;
}
// Initialize Memory Pools (Fixed Block Size)
if (osRtxConfig.mpi.stack != NULL) {
if (osRtxMemoryPoolInit(osRtxConfig.mpi.stack,
osRtxConfig.mpi.stack->max_blocks,
osRtxConfig.mpi.stack->block_size,
osRtxConfig.mpi.stack->block_base) != 0U) {
osRtxInfo.mpi.stack = osRtxConfig.mpi.stack;
}
}
if (osRtxConfig.mpi.thread != NULL) {
if (osRtxMemoryPoolInit(osRtxConfig.mpi.thread,
osRtxConfig.mpi.thread->max_blocks,
osRtxConfig.mpi.thread->block_size,
osRtxConfig.mpi.thread->block_base) != 0U) {
osRtxInfo.mpi.thread = osRtxConfig.mpi.thread;
}
}
if (osRtxConfig.mpi.timer != NULL) {
if (osRtxMemoryPoolInit(osRtxConfig.mpi.timer,
osRtxConfig.mpi.timer->max_blocks,
osRtxConfig.mpi.timer->block_size,
osRtxConfig.mpi.timer->block_base) != 0U) {
osRtxInfo.mpi.timer = osRtxConfig.mpi.timer;
}
}
if (osRtxConfig.mpi.event_flags != NULL) {
if (osRtxMemoryPoolInit(osRtxConfig.mpi.event_flags,
osRtxConfig.mpi.event_flags->max_blocks,
osRtxConfig.mpi.event_flags->block_size,
osRtxConfig.mpi.event_flags->block_base) != 0U) {
osRtxInfo.mpi.event_flags = osRtxConfig.mpi.event_flags;
}
}
if (osRtxConfig.mpi.mutex != NULL) {
if (osRtxMemoryPoolInit(osRtxConfig.mpi.mutex,
osRtxConfig.mpi.mutex->max_blocks,
osRtxConfig.mpi.mutex->block_size,
osRtxConfig.mpi.mutex->block_base) != 0U) {
osRtxInfo.mpi.mutex = osRtxConfig.mpi.mutex;
}
}
if (osRtxConfig.mpi.semaphore != NULL) {
if (osRtxMemoryPoolInit(osRtxConfig.mpi.semaphore,
osRtxConfig.mpi.semaphore->max_blocks,
osRtxConfig.mpi.semaphore->block_size,
osRtxConfig.mpi.semaphore->block_base) != 0U) {
osRtxInfo.mpi.semaphore = osRtxConfig.mpi.semaphore;
}
}
if (osRtxConfig.mpi.memory_pool != NULL) {
if (osRtxMemoryPoolInit(osRtxConfig.mpi.memory_pool,
osRtxConfig.mpi.memory_pool->max_blocks,
osRtxConfig.mpi.memory_pool->block_size,
osRtxConfig.mpi.memory_pool->block_base) != 0U) {
osRtxInfo.mpi.memory_pool = osRtxConfig.mpi.memory_pool;
}
}
if (osRtxConfig.mpi.message_queue != NULL) {
if (osRtxMemoryPoolInit(osRtxConfig.mpi.message_queue,
osRtxConfig.mpi.message_queue->max_blocks,
osRtxConfig.mpi.message_queue->block_size,
osRtxConfig.mpi.message_queue->block_base) != 0U) {
osRtxInfo.mpi.message_queue = osRtxConfig.mpi.message_queue;
}
}
osRtxInfo.kernel.state = osRtxKernelReady;
EvrRtxKernelInitializeCompleted();
return osOK;
}
