/// 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; }