OS_RESULT rt_tsk_prio (OS_TID task_id, U8 new_prio) { /* Change execution priority of a task to "new_prio". */ P_TCB p_task; if (task_id == 0) { /* Change execution priority of calling task. */ os_tsk.run->prio = new_prio; run:if (rt_rdy_prio() > new_prio) { rt_put_prio (&os_rdy, os_tsk.run); os_tsk.run->state = READY; rt_dispatch (NULL); } return (OS_R_OK); } /* Find the task in the "os_active_TCB" array. */ if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) { /* Task with "task_id" not found or not started. */ return (OS_R_NOK); } p_task = os_active_TCB[task_id-1]; p_task->prio = new_prio; if (p_task == os_tsk.run) { goto run; } rt_resort_prio (p_task); if (p_task->state == READY) { /* Task enqueued in a ready list. */ p_task = rt_get_first (&os_rdy); rt_dispatch (p_task); } return (OS_R_OK); }
OS_RESULT rt_mut_release (OS_ID mutex) { /* Release a mutex object */ P_MUCB p_MCB = mutex; P_TCB p_TCB; if (p_MCB->level == 0 || p_MCB->owner != os_tsk.run) { /* Unbalanced mutex release or task is not the owner */ return (OS_R_NOK); } __DMB(); if (--p_MCB->level != 0) { return (OS_R_OK); } /* Restore owner task's priority. */ os_tsk.run->prio = p_MCB->prio; if (p_MCB->p_lnk != NULL) { /* A task is waiting for mutex. */ p_TCB = rt_get_first ((P_XCB)p_MCB); #ifdef __CMSIS_RTOS rt_ret_val(p_TCB, 0/*osOK*/); #else rt_ret_val(p_TCB, OS_R_MUT); #endif rt_rmv_dly (p_TCB); /* A waiting task becomes the owner of this mutex. */ p_MCB->level = 1; p_MCB->owner = p_TCB; p_MCB->prio = p_TCB->prio; /* Priority inversion, check which task continues. */ if (os_tsk.run->prio >= rt_rdy_prio()) { rt_dispatch (p_TCB); } else { /* Ready task has higher priority than running task. */ rt_put_prio (&os_rdy, os_tsk.run); rt_put_prio (&os_rdy, p_TCB); os_tsk.run->state = READY; p_TCB->state = READY; rt_dispatch (NULL); } } else { /* Check if own priority raised by priority inversion. */ if (rt_rdy_prio() > os_tsk.run->prio) { rt_put_prio (&os_rdy, os_tsk.run); os_tsk.run->state = READY; rt_dispatch (NULL); } } return (OS_R_OK); }
OS_RESULT rt_tsk_delete (OS_TID task_id) { /* Terminate the task identified with "task_id". */ P_TCB task_context; if (task_id == 0 || task_id == os_tsk.run->task_id) { /* Terminate itself. */ os_tsk.run->state = INACTIVE; os_tsk.run->tsk_stack = rt_get_PSP (); rt_stk_check (); os_active_TCB[os_tsk.run->task_id-1] = NULL; os_tsk.run->stack = NULL; DBG_TASK_NOTIFY(os_tsk.run, __FALSE); os_tsk.run = NULL; rt_dispatch (NULL); /* The program should never come to this point. */ } else { /* Find the task in the "os_active_TCB" array. */ if (task_id > os_maxtaskrun || os_active_TCB[task_id-1] == NULL) { /* Task with "task_id" not found or not started. */ return (OS_R_NOK); } task_context = os_active_TCB[task_id-1]; rt_rmv_list (task_context); rt_rmv_dly (task_context); os_active_TCB[task_id-1] = NULL; task_context->stack = NULL; DBG_TASK_NOTIFY(task_context, __FALSE); } return (OS_R_OK); }
OS_RESULT rt_sem_delete (OS_ID semaphore) { /* Delete semaphore */ P_SCB p_SCB = semaphore; P_TCB p_TCB; while (p_SCB->p_lnk != NULL) { /* A task is waiting for token */ p_TCB = rt_get_first ((P_XCB)p_SCB); rt_ret_val(p_TCB, 0); rt_rmv_dly(p_TCB); p_TCB->state = READY; rt_put_prio (&os_rdy, p_TCB); } if (os_rdy.p_lnk && (os_rdy.p_lnk->prio > os_tsk.run->prio)) { /* preempt running task */ rt_put_prio (&os_rdy, os_tsk.run); os_tsk.run->state = READY; rt_dispatch (NULL); } p_SCB->cb_type = 0; return (OS_R_OK); }
void rt_evt_set (U16 event_flags, OS_TID task_id) { /* Set one or more event flags of a selectable task. */ P_TCB p_tcb; p_tcb = os_active_TCB[task_id-1]; if (p_tcb == NULL) { return; } p_tcb->events |= event_flags; event_flags = p_tcb->waits; /* If the task is not waiting for an event, it should not be put */ /* to ready state. */ if (p_tcb->state == WAIT_AND) { /* Check for AND-connected events */ if ((p_tcb->events & event_flags) == event_flags) { goto wkup; } } if (p_tcb->state == WAIT_OR) { /* Check for OR-connected events */ if (p_tcb->events & event_flags) { p_tcb->waits &= p_tcb->events; wkup: p_tcb->events &= ~event_flags; rt_rmv_dly (p_tcb); p_tcb->state = READY; #ifdef __CMSIS_RTOS rt_ret_val2(p_tcb, 0x08/*osEventSignal*/, p_tcb->waits); #else rt_ret_val (p_tcb, OS_R_EVT); #endif rt_dispatch (p_tcb); } } }
OS_RESULT rt_mut_delete (OS_ID mutex) { /* Delete a mutex object */ P_MUCB p_MCB = mutex; P_TCB p_TCB; __DMB(); /* Restore owner task's priority. */ if (p_MCB->level != 0) { p_MCB->owner->prio = p_MCB->prio; if (p_MCB->owner != os_tsk.run) { rt_resort_prio (p_MCB->owner); } } while (p_MCB->p_lnk != NULL) { /* A task is waiting for mutex. */ p_TCB = rt_get_first ((P_XCB)p_MCB); rt_ret_val(p_TCB, 0/*osOK*/); rt_rmv_dly(p_TCB); p_TCB->state = READY; rt_put_prio (&os_rdy, p_TCB); } if (os_rdy.p_lnk && (os_rdy.p_lnk->prio > os_tsk.run->prio)) { /* preempt running task */ rt_put_prio (&os_rdy, os_tsk.run); os_tsk.run->state = READY; rt_dispatch (NULL); } p_MCB->cb_type = 0; return (OS_R_OK); }
OS_TID rt_tsk_create (FUNCP task, U32 prio_stksz, void *stk, void *argv) { /* Start a new task declared with "task". */ P_TCB task_context; U32 i; /* Priority 0 is reserved for idle task! */ if ((prio_stksz & 0xFFU) == 0U) { prio_stksz += 1U; } task_context = rt_alloc_box (mp_tcb); if (task_context == NULL) { return (0U); } /* If "size != 0" use a private user provided stack. */ task_context->stack = stk; task_context->priv_stack = (U16)(prio_stksz >> 8); /* Find a free entry in 'os_active_TCB' table. */ i = rt_get_TID (); if (i == 0U) { return (0U); } task_context->task_id = (U8)i; /* Pass parameter 'argv' to 'rt_init_context' */ task_context->msg = argv; task_context->argv = argv; /* For 'size == 0' system allocates the user stack from the memory pool. */ rt_init_context (task_context, (U8)(prio_stksz & 0xFFU), task); os_active_TCB[i-1U] = task_context; DBG_TASK_NOTIFY(task_context, __TRUE); rt_dispatch (task_context); return ((OS_TID)i); }
OS_RESULT rt_sem_send (OS_ID semaphore) { /* Return a token to semaphore */ P_SCB p_SCB = semaphore; P_TCB p_TCB; if (p_SCB->tokens == p_SCB->max_tokens) { return OS_R_NOK; } if (p_SCB->p_lnk != NULL) { /* A task is waiting for token */ p_TCB = rt_get_first ((P_XCB)p_SCB); #ifdef __CMSIS_RTOS rt_ret_val(p_TCB, 1U); #else rt_ret_val(p_TCB, OS_R_SEM); #endif rt_rmv_dly (p_TCB); rt_dispatch (p_TCB); } else { /* Store token. */ p_SCB->tokens++; } return (OS_R_OK); }
OS_RESULT rt_mbx_wait (OS_ID mailbox, void **message, U16 timeout) { /* Receive a message; possibly wait for it */ P_MCB p_MCB = mailbox; P_TCB p_TCB; /* If a message is available in the fifo buffer */ /* remove it from the fifo buffer and return. */ if (p_MCB->count) { *message = p_MCB->msg[p_MCB->last]; if (++p_MCB->last == p_MCB->size) { p_MCB->last = 0; } if (p_MCB->state == 2) { /* A task is waiting to send message */ p_TCB = rt_get_first ((P_XCB)p_MCB); if (p_MCB->p_lnk == NULL) { p_MCB->state = 0; } #ifdef __CMSIS_RTOS rt_ret_val(p_TCB, 0/*osOK*/); #else rt_ret_val(p_TCB, OS_R_OK); #endif p_MCB->msg[p_MCB->first] = p_TCB->msg; if (++p_MCB->first == p_MCB->size) { p_MCB->first = 0; } rt_rmv_dly (p_TCB); rt_dispatch (p_TCB); } else { rt_dec (&p_MCB->count); } return (OS_R_OK); } /* No message available: wait for one */ if (timeout == 0) { return (OS_R_TMO); } if (p_MCB->p_lnk != NULL) { rt_put_prio ((P_XCB)p_MCB, os_tsk.run); } else { p_MCB->p_lnk = os_tsk.run; os_tsk.run->p_lnk = NULL; os_tsk.run->p_rlnk = (P_TCB)p_MCB; /* Task is waiting to receive a message */ p_MCB->state = 1; } rt_block(timeout, WAIT_MBX); #ifndef __CMSIS_RTOS os_tsk.run->msg = message; #endif return (OS_R_TMO); }
OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) { /* Send message to a mailbox */ P_MCB p_MCB = mailbox; P_TCB p_TCB; if (p_MCB->state == 1) { /* A task is waiting for message */ p_TCB = rt_get_first ((P_XCB)p_MCB); if (p_MCB->p_lnk == NULL) { p_MCB->state = 0; } #ifdef __CMSIS_RTOS rt_ret_val2(p_TCB, 0x10/*osEventMessage*/, (U32)p_msg); #else *p_TCB->msg = p_msg; rt_ret_val (p_TCB, OS_R_MBX); #endif rt_rmv_dly (p_TCB); rt_dispatch (p_TCB); } else { /* Store message in mailbox queue */ if (p_MCB->count == p_MCB->size) { /* No free message entry, wait for one. If message queue is full, */ /* then no task is waiting for message. The 'p_MCB->p_lnk' list */ /* pointer can now be reused for send message waits task list. */ if (timeout == 0) { return (OS_R_TMO); } if (p_MCB->p_lnk != NULL) { rt_put_prio ((P_XCB)p_MCB, os_tsk.run); } else { p_MCB->p_lnk = os_tsk.run; os_tsk.run->p_lnk = NULL; os_tsk.run->p_rlnk = (P_TCB)p_MCB; /* Task is waiting to send a message */ p_MCB->state = 2; } os_tsk.run->msg = p_msg; rt_block (timeout, WAIT_MBX); return (OS_R_TMO); } /* Yes, there is a free entry in a mailbox. */ p_MCB->msg[p_MCB->first] = p_msg; rt_inc (&p_MCB->count); if (++p_MCB->first == p_MCB->size) { p_MCB->first = 0; } } return (OS_R_OK); }
/*--------------------------- os_mem_free -----------------------------------*/ OS_RESULT rt_mem_free (void *box_mem, void *box) { first = rt_get_first(&wait_list); if( first != NULL ) { first->ret_val = (U32)(box); rt_dispatch(first); return OS_R_OK; } else { return (OS_RESULT)rt_free_box(box_mem, box); } }
OS_RESULT rt_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) { /* Send message to a mailbox */ P_MCB p_MCB = mailbox; P_TCB p_TCB; if (p_MCB->p_lnk != NULL && p_MCB->count == 0) { /* A task is waiting for message */ p_TCB = rt_get_first ((P_XCB)p_MCB); *p_TCB->msg = p_msg; p_TCB->ret_val = OS_R_MBX; rt_rmv_dly (p_TCB); rt_dispatch (p_TCB); os_tsk.run->ret_val = OS_R_OK; } else { /* Store message in mailbox queue */ if (p_MCB->count == p_MCB->size) { /* No free message entry, wait for one. If message queue is full, */ /* then no task is waiting for message. The 'p_MCB->p_lnk' list */ /* pointer can now be reused for send message waits task list. */ if (timeout == 0) { return (OS_R_TMO); } if (p_MCB->p_lnk != NULL) { rt_put_prio ((P_XCB)p_MCB, os_tsk.run); } else { p_MCB->p_lnk = os_tsk.run; os_tsk.run->p_lnk = NULL; os_tsk.run->p_rlnk = (P_TCB)p_MCB; /* Signal the 'isr_mbx_receive ()' that the task is waiting */ /* to send a message */ p_MCB->isr_st = 1; } os_tsk.run->msg = p_msg; rt_block (timeout, WAIT_MBX); return (OS_R_TMO); } /* Yes, there is a free entry in a mailbox. */ p_MCB->msg[p_MCB->first] = p_msg; rt_inc (&p_MCB->count); if (++p_MCB->first == p_MCB->size) { p_MCB->first = 0; } } return (OS_R_OK); }
OS_RESULT rt_mut_delete (OS_ID mutex) { /* Delete a mutex object */ P_MUCB p_MCB = mutex; P_TCB p_TCB; P_MUCB p_mlnk; U8 prio; if (p_MCB->level != 0U) { p_TCB = p_MCB->owner; /* Remove mutex from task mutex owner list. */ p_mlnk = p_TCB->p_mlnk; if (p_mlnk == p_MCB) { p_TCB->p_mlnk = p_MCB->p_mlnk; } else { while (p_mlnk) { if (p_mlnk->p_mlnk == p_MCB) { p_mlnk->p_mlnk = p_MCB->p_mlnk; break; } p_mlnk = p_mlnk->p_mlnk; } } /* Restore owner task's priority. */ prio = p_TCB->prio_base; p_mlnk = p_TCB->p_mlnk; while (p_mlnk) { if ((p_mlnk->p_lnk != NULL) && (p_mlnk->p_lnk->prio > prio)) { /* A task with higher priority is waiting for mutex. */ prio = p_mlnk->p_lnk->prio; } p_mlnk = p_mlnk->p_mlnk; } if (p_TCB->prio != prio) { p_TCB->prio = prio; if (p_TCB != os_tsk.run) { rt_resort_prio (p_TCB); } } } while (p_MCB->p_lnk != NULL) { /* A task is waiting for mutex. */ p_TCB = rt_get_first ((P_XCB)p_MCB); rt_ret_val(p_TCB, 0U/*osOK*/); rt_rmv_dly(p_TCB); p_TCB->state = READY; rt_put_prio (&os_rdy, p_TCB); } if ((os_rdy.p_lnk != NULL) && (os_rdy.p_lnk->prio > os_tsk.run->prio)) { /* preempt running task */ rt_put_prio (&os_rdy, os_tsk.run); os_tsk.run->state = READY; rt_dispatch (NULL); } p_MCB->cb_type = 0U; return (OS_R_OK); }
OS_RESULT rt_mut_release (OS_ID mutex) { /* Release a mutex object */ P_MUCB p_MCB = mutex; P_TCB p_TCB; P_MUCB p_mlnk; U8 prio; if ((p_MCB->level == 0U) || (p_MCB->owner != os_tsk.run)) { /* Unbalanced mutex release or task is not the owner */ return (OS_R_NOK); } if (--p_MCB->level != 0U) { return (OS_R_OK); } /* Remove mutex from task mutex owner list. */ p_mlnk = os_tsk.run->p_mlnk; if (p_mlnk == p_MCB) { os_tsk.run->p_mlnk = p_MCB->p_mlnk; } else { while (p_mlnk) { if (p_mlnk->p_mlnk == p_MCB) { p_mlnk->p_mlnk = p_MCB->p_mlnk; break; } p_mlnk = p_mlnk->p_mlnk; } } /* Restore owner task's priority. */ prio = os_tsk.run->prio_base; p_mlnk = os_tsk.run->p_mlnk; while (p_mlnk) { if ((p_mlnk->p_lnk != NULL) && (p_mlnk->p_lnk->prio > prio)) { /* A task with higher priority is waiting for mutex. */ prio = p_mlnk->p_lnk->prio; } p_mlnk = p_mlnk->p_mlnk; } os_tsk.run->prio = prio; if (p_MCB->p_lnk != NULL) { /* A task is waiting for mutex. */ p_TCB = rt_get_first ((P_XCB)p_MCB); #ifdef __CMSIS_RTOS rt_ret_val(p_TCB, 0U/*osOK*/); #else rt_ret_val(p_TCB, OS_R_MUT); #endif rt_rmv_dly (p_TCB); /* A waiting task becomes the owner of this mutex. */ p_MCB->level = 1U; p_MCB->owner = p_TCB; p_MCB->p_mlnk = p_TCB->p_mlnk; p_TCB->p_mlnk = p_MCB; /* Priority inversion, check which task continues. */ if (os_tsk.run->prio >= rt_rdy_prio()) { rt_dispatch (p_TCB); } else { /* Ready task has higher priority than running task. */ rt_put_prio (&os_rdy, os_tsk.run); rt_put_prio (&os_rdy, p_TCB); os_tsk.run->state = READY; p_TCB->state = READY; rt_dispatch (NULL); } } else { /* Check if own priority lowered by priority inversion. */ if (rt_rdy_prio() > os_tsk.run->prio) { rt_put_prio (&os_rdy, os_tsk.run); os_tsk.run->state = READY; rt_dispatch (NULL); } } return (OS_R_OK); }
OS_RESULT rt_tsk_delete (OS_TID task_id) { /* Terminate the task identified with "task_id". */ P_TCB task_context; P_TCB p_TCB; P_MUCB p_MCB, p_MCB0; if ((task_id == 0U) || (task_id == os_tsk.run->task_id)) { /* Terminate itself. */ os_tsk.run->state = INACTIVE; os_tsk.run->tsk_stack = rt_get_PSP (); rt_stk_check (); p_MCB = os_tsk.run->p_mlnk; while (p_MCB) { /* Release mutexes owned by this task */ if (p_MCB->p_lnk) { /* A task is waiting for mutex. */ p_TCB = rt_get_first ((P_XCB)p_MCB); #ifdef __CMSIS_RTOS rt_ret_val (p_TCB, 0U/*osOK*/); #else rt_ret_val (p_TCB, OS_R_MUT); #endif rt_rmv_dly (p_TCB); p_TCB->state = READY; rt_put_prio (&os_rdy, p_TCB); /* A waiting task becomes the owner of this mutex. */ p_MCB0 = p_MCB->p_mlnk; p_MCB->level = 1U; p_MCB->owner = p_TCB; p_MCB->p_mlnk = p_TCB->p_mlnk; p_TCB->p_mlnk = p_MCB; p_MCB = p_MCB0; } else { p_MCB0 = p_MCB->p_mlnk; p_MCB->level = 0U; p_MCB->owner = NULL; p_MCB->p_mlnk = NULL; p_MCB = p_MCB0; } } os_active_TCB[os_tsk.run->task_id-1U] = NULL; rt_free_box (mp_stk, os_tsk.run->stack); os_tsk.run->stack = NULL; DBG_TASK_NOTIFY(os_tsk.run, __FALSE); rt_free_box (mp_tcb, os_tsk.run); os_tsk.run = NULL; rt_dispatch (NULL); /* The program should never come to this point. */ } else { /* Find the task in the "os_active_TCB" array. */ if ((task_id > os_maxtaskrun) || (os_active_TCB[task_id-1U] == NULL)) { /* Task with "task_id" not found or not started. */ return (OS_R_NOK); } task_context = os_active_TCB[task_id-1U]; rt_rmv_list (task_context); rt_rmv_dly (task_context); p_MCB = task_context->p_mlnk; while (p_MCB) { /* Release mutexes owned by this task */ if (p_MCB->p_lnk) { /* A task is waiting for mutex. */ p_TCB = rt_get_first ((P_XCB)p_MCB); #ifdef __CMSIS_RTOS rt_ret_val (p_TCB, 0U/*osOK*/); #else rt_ret_val (p_TCB, OS_R_MUT); #endif rt_rmv_dly (p_TCB); p_TCB->state = READY; rt_put_prio (&os_rdy, p_TCB); /* A waiting task becomes the owner of this mutex. */ p_MCB0 = p_MCB->p_mlnk; p_MCB->level = 1U; p_MCB->owner = p_TCB; p_MCB->p_mlnk = p_TCB->p_mlnk; p_TCB->p_mlnk = p_MCB; p_MCB = p_MCB0; } else { p_MCB0 = p_MCB->p_mlnk; p_MCB->level = 0U; p_MCB->owner = NULL; p_MCB->p_mlnk = NULL; p_MCB = p_MCB0; } } os_active_TCB[task_id-1U] = NULL; rt_free_box (mp_stk, task_context->stack); task_context->stack = NULL; DBG_TASK_NOTIFY(task_context, __FALSE); rt_free_box (mp_tcb, task_context); if (rt_rdy_prio() > os_tsk.run->prio) { /* Ready task has higher priority than running task. */ os_tsk.run->state = READY; rt_put_prio (&os_rdy, os_tsk.run); rt_dispatch (NULL); } } return (OS_R_OK); }