/// Release a Semaphore token that was acquired by osSemaphoreAcquire. /// \note API identical to osSemaphoreRelease osStatus_t svcRtxSemaphoreRelease (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; } // Check if Thread is waiting for a token if (semaphore->thread_list != NULL) { EvrRtxSemaphoreReleased(semaphore); // Wakeup waiting Thread with highest Priority thread = osRtxThreadListGet((os_object_t*)semaphore); osRtxThreadWaitExit(thread, (uint32_t)osOK, true); EvrRtxSemaphoreAcquired(semaphore); } else { // Try to release token if (SemaphoreTokenIncrement(semaphore) == 0U) { EvrRtxSemaphoreError(semaphore, osRtxErrorSemaphoreCountLimit); return osErrorResource; } EvrRtxSemaphoreReleased(semaphore); } return osOK; }
/// Release Mutex list when owner Thread terminates. /// \param[in] mutex mutex object. /// \return 1 - success, 0 - failure. void osRtxMutexOwnerRelease (os_mutex_t *mutex_list) { os_mutex_t *mutex; os_thread_t *thread; mutex = mutex_list; while (mutex) { mutex_list = mutex->owner_next; // Check if Mutex is Robust if (mutex->attr & osMutexRobust) { // Clear Lock counter mutex->lock = 0U; EvrRtxMutexReleased(mutex, 0U); // Check if Thread is waiting for a Mutex if (mutex->thread_list != NULL) { // Wakeup waiting Thread with highest Priority thread = osRtxThreadListGet((os_object_t*)mutex); osRtxThreadWaitExit(thread, (uint32_t)osOK, false); // Thread is the new Mutex owner mutex->owner_thread = thread; mutex->owner_next = thread->mutex_list; mutex->owner_prev = NULL; thread->mutex_list = mutex; mutex->lock = 1U; EvrRtxMutexAcquired(mutex, 1U); } } mutex = mutex_list; } }
/// 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; }
/// Start the RTOS Kernel scheduler. /// \note API identical to osKernelStart static osStatus_t svcRtxKernelStart (void) { os_thread_t *thread; if (osRtxInfo.kernel.state != osRtxKernelReady) { EvrRtxKernelError(osRtxErrorKernelNotReady); //lint -e{904} "Return statement before end of function" [MISRA Note 1] return osError; } // Thread startup (Idle and Timer Thread) if (!osRtxThreadStartup()) { EvrRtxKernelError((int32_t)osError); //lint -e{904} "Return statement before end of function" [MISRA Note 1] return osError; } // Setup SVC and PendSV System Service Calls SVC_Setup(); // Setup RTOS Tick if (OS_Tick_Setup(osRtxConfig.tick_freq, OS_TICK_HANDLER) != 0) { EvrRtxKernelError((int32_t)osError); //lint -e{904} "Return statement before end of function" [MISRA Note 1] return osError; } osRtxInfo.tick_irqn = OS_Tick_GetIRQn(); // Enable RTOS Tick OS_Tick_Enable(); // Switch to Ready Thread with highest Priority thread = osRtxThreadListGet(&osRtxInfo.thread.ready); if (thread == NULL) { EvrRtxKernelError((int32_t)osError); //lint -e{904} "Return statement before end of function" [MISRA Note 1] return osError; } osRtxThreadSwitch(thread); if ((osRtxConfig.flags & osRtxConfigPrivilegedMode) != 0U) { // Privileged Thread mode & PSP __set_CONTROL(0x02U); } else { // Unprivileged Thread mode & PSP __set_CONTROL(0x03U); } osRtxInfo.kernel.state = osRtxKernelRunning; EvrRtxKernelStarted(); 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; }
/// Semaphore post ISR processing. /// \param[in] semaphore semaphore object. void osRtxSemaphorePostProcess (os_semaphore_t *semaphore) { os_thread_t *thread; if (semaphore->state == osRtxObjectInactive) { return; } // Check if Thread is waiting for a token if (semaphore->thread_list != NULL) { // Try to acquire token if (SemaphoreTokenDecrement(semaphore) != 0U) { // Wakeup waiting Thread with highest Priority thread = osRtxThreadListGet((os_object_t*)semaphore); osRtxThreadWaitExit(thread, (uint32_t)osOK, false); EvrRtxSemaphoreAcquired(semaphore); } } }
/// 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 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; }
/// Release a Mutex that was acquired by osMutexAcquire. /// \note API identical to osMutexRelease osStatus_t svcRtxMutexRelease (osMutexId_t mutex_id) { os_mutex_t *mutex = (os_mutex_t *)mutex_id; os_mutex_t *mutex0; os_thread_t *thread; os_thread_t *runnig_thread; int8_t priority; runnig_thread = osRtxThreadGetRunning(); if (runnig_thread == NULL) { EvrRtxMutexError(mutex, osRtxErrorKernelNotRunning); return osError; } // 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; } // Check if running Thread is not the owner if (mutex->owner_thread != runnig_thread) { EvrRtxMutexError(mutex, osRtxErrorMutexNotOwned); return osErrorResource; } // Check if Mutex is not locked if (mutex->lock == 0U) { EvrRtxMutexError(mutex, osRtxErrorMutexNotLocked); return osErrorResource; } // Decrement Lock counter mutex->lock--; EvrRtxMutexReleased(mutex, mutex->lock); // Check Lock counter if (mutex->lock != 0U) { return osOK; } // 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 { runnig_thread->mutex_list = mutex->owner_next; } // Restore running Thread priority if (mutex->attr & osMutexPrioInherit) { priority = runnig_thread->priority_base; mutex0 = runnig_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; } runnig_thread->priority = priority; } // Check if Thread is waiting for a Mutex if (mutex->thread_list != NULL) { // Wakeup waiting Thread with highest Priority thread = osRtxThreadListGet((os_object_t*)mutex); osRtxThreadWaitExit(thread, (uint32_t)osOK, false); // Thread is the new Mutex owner mutex->owner_thread = thread; mutex->owner_next = thread->mutex_list; mutex->owner_prev = NULL; thread->mutex_list = mutex; mutex->lock = 1U; EvrRtxMutexAcquired(mutex, 1U); } osRtxThreadDispatch(NULL); 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; }
/// 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); } } }