/// Acquire a Semaphore token or timeout if no tokens are available. /// \note API identical to osSemaphoreAcquire osStatus_t svcRtxSemaphoreAcquire (osSemaphoreId_t semaphore_id, uint32_t timeout) { os_semaphore_t *semaphore = (os_semaphore_t *)semaphore_id; // 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; } // Try to acquire token if (SemaphoreTokenDecrement(semaphore) == 0U) { // No token available if (timeout != 0U) { EvrRtxSemaphoreAcquirePending(semaphore, timeout); // Suspend current Thread osRtxThreadListPut((os_object_t*)semaphore, osRtxThreadGetRunning()); osRtxThreadWaitEnter(osRtxThreadWaitingSemaphore, timeout); return osErrorTimeout; } else { EvrRtxSemaphoreNotAcquired(semaphore); return osErrorResource; } } EvrRtxSemaphoreAcquired(semaphore); return osOK; }
/// Acquire a Mutex or timeout if it is locked. /// \note API identical to osMutexAcquire osStatus_t svcRtxMutexAcquire (osMutexId_t mutex_id, uint32_t timeout) { os_mutex_t *mutex = (os_mutex_t *)mutex_id; os_thread_t *runnig_thread; 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 Mutex is not locked if (mutex->lock == 0U) { // Acquire Mutex mutex->owner_thread = runnig_thread; mutex->owner_next = runnig_thread->mutex_list; mutex->owner_prev = NULL; if (runnig_thread->mutex_list != NULL) { runnig_thread->mutex_list->owner_prev = mutex; } runnig_thread->mutex_list = mutex; mutex->lock = 1U; EvrRtxMutexAcquired(mutex, mutex->lock); return osOK; } // Check if Mutex is recursive and running Thread is the owner if ((mutex->attr & osMutexRecursive) && (mutex->owner_thread == runnig_thread)) { // Increment lock counter if (mutex->lock == osRtxMutexLockLimit) { EvrRtxMutexError(mutex, osRtxErrorMutexLockLimit); return osErrorResource; } mutex->lock++; EvrRtxMutexAcquired(mutex, mutex->lock); return osOK; } // Check if timeout is specified if (timeout != 0U) { // Check if Priority inheritance protocol is enabled if (mutex->attr & osMutexPrioInherit) { // Raise priority of owner Thread if lower than priority of running Thread if (mutex->owner_thread->priority < runnig_thread->priority) { mutex->owner_thread->priority = runnig_thread->priority; osRtxThreadListSort(mutex->owner_thread); } } EvrRtxMutexAcquirePending(mutex, timeout); // Suspend current Thread osRtxThreadListPut((os_object_t*)mutex, runnig_thread); osRtxThreadWaitEnter(osRtxThreadWaitingMutex, timeout); return osErrorTimeout; } // Mutex was not acquired EvrRtxMutexNotAcquired(mutex); return osErrorResource; }
/// Acquire a Mutex or timeout if it is locked. /// \note API identical to osMutexAcquire static osStatus_t svcRtxMutexAcquire (osMutexId_t mutex_id, uint32_t timeout) { os_mutex_t *mutex = osRtxMutexId(mutex_id); os_thread_t *runnig_thread; osStatus_t status; // Check running thread runnig_thread = osRtxThreadGetRunning(); if (runnig_thread == NULL) { EvrRtxMutexError(mutex, osRtxErrorKernelNotRunning); //lint -e{904} "Return statement before end of function" [MISRA Note 1] return osError; } // 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 not locked if (mutex->lock == 0U) { // Acquire Mutex mutex->owner_thread = runnig_thread; mutex->owner_next = runnig_thread->mutex_list; mutex->owner_prev = NULL; if (runnig_thread->mutex_list != NULL) { runnig_thread->mutex_list->owner_prev = mutex; } runnig_thread->mutex_list = mutex; mutex->lock = 1U; EvrRtxMutexAcquired(mutex, mutex->lock); status = osOK; } else { // Check if Mutex is recursive and running Thread is the owner if (((mutex->attr & osMutexRecursive) != 0U) && (mutex->owner_thread == runnig_thread)) { // Try to increment lock counter if (mutex->lock == osRtxMutexLockLimit) { EvrRtxMutexError(mutex, osRtxErrorMutexLockLimit); status = osErrorResource; } else { mutex->lock++; EvrRtxMutexAcquired(mutex, mutex->lock); status = osOK; } } else { // Check if timeout is specified if (timeout != 0U) { // Check if Priority inheritance protocol is enabled if ((mutex->attr & osMutexPrioInherit) != 0U) { // Raise priority of owner Thread if lower than priority of running Thread if (mutex->owner_thread->priority < runnig_thread->priority) { mutex->owner_thread->priority = runnig_thread->priority; osRtxThreadListSort(mutex->owner_thread); } } EvrRtxMutexAcquirePending(mutex, timeout); // Suspend current Thread if (osRtxThreadWaitEnter(osRtxThreadWaitingMutex, timeout)) { osRtxThreadListPut(osRtxObject(mutex), runnig_thread); } else { EvrRtxMutexAcquireTimeout(mutex); } status = osErrorTimeout; } else { EvrRtxMutexNotAcquired(mutex); status = osErrorResource; } } } return status; }
/// 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; }