/// Pending Service Call Handler.
void osRtxPendSV_Handler (void) {
os_object_t *object;
for (;;) {
object = isr_queue_get();
if (object == NULL) {
break;
}
switch (object->id) {
case osRtxIdThread:
osRtxInfo.post_process.thread((os_thread_t *)object);
break;
case osRtxIdEventFlags:
osRtxInfo.post_process.event_flags((os_event_flags_t *)object);
break;
case osRtxIdSemaphore:
osRtxInfo.post_process.semaphore((os_semaphore_t *)object);
break;
case osRtxIdMemoryPool:
osRtxInfo.post_process.memory_pool((os_memory_pool_t *)object);
break;
case osRtxIdMessage:
osRtxInfo.post_process.message_queue((os_message_t *)object);
break;
default:
break;
}
}
osRtxThreadDispatch(NULL);
}
/// 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;
}
/// 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;
}
/// Tick Handler.
void osRtxTick_Handler (void) {
os_thread_t *thread;
OS_Tick_AcknowledgeIRQ();
osRtxInfo.kernel.tick++;
// Process Timers
if (osRtxInfo.timer.tick != NULL) {
osRtxInfo.timer.tick();
}
// Process Thread Delays
osRtxThreadDelayTick();
osRtxThreadDispatch(NULL);
// Check Round Robin timeout
if (osRtxInfo.thread.robin.timeout != 0U) {
if (osRtxInfo.thread.robin.thread != osRtxInfo.thread.run.next) {
// Reset Round Robin
osRtxInfo.thread.robin.thread = osRtxInfo.thread.run.next;
osRtxInfo.thread.robin.tick = osRtxInfo.thread.robin.timeout;
} else {
if (osRtxInfo.thread.robin.tick != 0U) {
osRtxInfo.thread.robin.tick--;
}
if (osRtxInfo.thread.robin.tick == 0U) {
// Round Robin Timeout
if (osRtxKernelGetState() == osRtxKernelRunning) {
thread = osRtxInfo.thread.ready.thread_list;
if ((thread != NULL) && (thread->priority == osRtxInfo.thread.robin.thread->priority)) {
osRtxThreadListRemove(thread);
osRtxThreadReadyPut(osRtxInfo.thread.robin.thread);
osRtxThreadSwitch(thread);
osRtxInfo.thread.robin.thread = thread;
osRtxInfo.thread.robin.tick = osRtxInfo.thread.robin.timeout;
}
}
}
}
}
}
/// 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;
}
/// Resume the RTOS Kernel scheduler.
/// \note API identical to osKernelResume
static void svcRtxKernelResume (uint32_t sleep_ticks) {
os_thread_t *thread;
os_timer_t *timer;
uint32_t delay;
if (osRtxInfo.kernel.state != osRtxKernelSuspended) {
EvrRtxKernelResumed();
//lint -e{904} "Return statement before end of function" [MISRA Note 1]
return;
}
// Process Thread Delay list
thread = osRtxInfo.thread.delay_list;
if (thread != NULL) {
delay = sleep_ticks;
if (delay >= thread->delay) {
delay -= thread->delay;
osRtxInfo.kernel.tick += thread->delay;
thread->delay = 1U;
do {
osRtxThreadDelayTick();
if (delay == 0U) {
break;
}
delay--;
osRtxInfo.kernel.tick++;
} while (osRtxInfo.thread.delay_list != NULL);
} else {
thread->delay -= delay;
osRtxInfo.kernel.tick += delay;
}
} else {
osRtxInfo.kernel.tick += sleep_ticks;
}
// Process Active Timer list
timer = osRtxInfo.timer.list;
if (timer != NULL) {
if (sleep_ticks >= timer->tick) {
sleep_ticks -= timer->tick;
timer->tick = 1U;
do {
osRtxInfo.timer.tick();
if (sleep_ticks == 0U) {
break;
}
sleep_ticks--;
} while (osRtxInfo.timer.list != NULL);
} else {
timer->tick -= sleep_ticks;
}
}
osRtxInfo.kernel.state = osRtxKernelRunning;
osRtxThreadDispatch(NULL);
KernelUnblock();
EvrRtxKernelResumed();
}