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