void os_mbx_psh (P_MCB p_CB, void *p_msg) { /* Store the message to the mailbox queue or pass it to task directly. */ P_TCB p_TCB; /* Check if this was an 'isr_mbx_receive ()' post service request. */ if (p_CB->p_lnk != NULL && p_CB->isr_st == 2) { /* A task is waiting to send message, remove it from the waiting list. */ p_CB->isr_st = 0; p_TCB = os_get_first ((P_XCB)p_CB); p_TCB->ret_val = OS_R_OK; goto rdy; } /* A task is waiting for message, pass the message to task directly. */ if (p_CB->p_lnk != NULL && p_CB->count == 0) { p_TCB = os_get_first ((P_XCB)p_CB); p_TCB->p_msg = p_msg; p_TCB->ret_val = OS_R_MBX; rdy:p_TCB->state = READY; os_rmv_dly (p_TCB); os_put_prio (&os_rdy, p_TCB); } else { /* No task is waiting for message, store it to the mailbox queue. */ if (p_CB->count < p_CB->size) { p_CB->msg[p_CB->first] = p_msg; _incw (&p_CB->count); if (++p_CB->first == p_CB->size) { p_CB->first = 0; } } else { os_error (OS_ERR_MBX_OVF); } } }
OS_RESULT os_mbx_send (OS_ID mailbox, void *p_msg, U16 timeout) { /* Send message to a mailbox */ P_MCB p_MCB = mailbox; P_TCB p_TCB; tsk_lock(); m:if (p_MCB->p_lnk != NULL && p_MCB->count == 0) { /* A task is waiting for message */ p_TCB = os_get_first ((P_XCB)p_MCB); p_TCB->p_msg = p_msg; p_TCB->ret_val = OS_R_MBX; os_rmv_dly (p_TCB); os_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) { goto wto; } if (p_MCB->p_lnk != NULL) { os_put_prio ((P_XCB)p_MCB, os_runtask); } else { p_MCB->p_lnk = os_runtask; os_runtask->p_lnk = NULL; os_runtask->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; } if (os_block(timeout, WAIT_MBX) == OS_R_TMO) { wto: tsk_unlock(); return (OS_R_TMO); } /* A message has been fetched, check the mailbox again. */ goto m; } /* Yes, there is a free entry in a mailbox. */ p_MCB->msg[p_MCB->first] = p_msg; _incw (&p_MCB->count); if (++p_MCB->first == p_MCB->size) { p_MCB->first = 0; } } tsk_unlock(); return (OS_R_OK); }
OS_RESULT os_mut_release (OS_ID mutex) { /* Release a mutex object */ P_MUCB p_MCB = mutex; P_TCB p_TCB; tsk_lock(); if (p_MCB->level == 0 || p_MCB->owner != os_runtask) { /* Unbalanced mutex release or task is not the owner */ tsk_unlock (); return (OS_R_NOK); } if (--p_MCB->level != 0) { goto ok; } /* Restore owner task's priority. */ os_runtask->prio = p_MCB->prio; if (p_MCB->p_lnk != NULL) { /* A task is waiting for this mutex. */ p_TCB = os_get_first ((P_XCB)p_MCB); p_TCB->ret_val = OS_R_MUT; os_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_runtask->prio >= os_rdy_prio()) { os_dispatch (p_TCB); } else { /* Ready task has higher priority than running task. */ os_put_prio (&os_rdy, os_runtask); os_put_prio (&os_rdy, p_TCB); os_runtask->state = READY; p_TCB->state = READY; os_dispatch (NULL); } } else { /* Check if own priority raised by priority inversion. */ if (os_rdy_prio() > os_runtask->prio) { os_put_prio (&os_rdy, os_runtask); os_runtask->state = READY; os_dispatch (NULL); } } ok:tsk_unlock(); return (OS_R_OK); }
OS_RESULT os_block (U16 timeout, U8 block_state) { /* Block running task and choose next ready task. */ /* "timeout" sets a time-out value or is 0xffff (=no time-out). */ /* "block_state" defines the appropriate task state */ P_TCB next_TCB; if (timeout) { if (timeout < 0xffff) { os_put_dly (os_runtask, timeout); } os_runtask->state = block_state; os_runtask->ret_val = OS_R_OK; next_TCB = os_get_first (&os_rdy); os_switch_tasks (next_TCB); return (os_runtask->ret_val); } return (OS_R_TMO); }
OS_RESULT os_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; tsk_lock(); /* 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]; _decw (&p_MCB->count); if (++p_MCB->last == p_MCB->size) { p_MCB->last = 0; } if (p_MCB->p_lnk != NULL) { /* A task is waiting to send message */ p_TCB = os_get_first ((P_XCB)p_MCB); os_rmv_dly (p_TCB); os_dispatch (p_TCB); } tsk_unlock(); return (OS_R_OK); } /* No message available: wait for one */ if (timeout == 0) { goto rto; } if (p_MCB->p_lnk != NULL) { os_put_prio ((P_XCB)p_MCB, os_runtask); } else { p_MCB->p_lnk = os_runtask; os_runtask->p_lnk = NULL; os_runtask->p_rlnk = (P_TCB)p_MCB; } if (os_block(timeout, WAIT_MBX) == OS_R_TMO) { rto:tsk_unlock(); *message = NULL; return (OS_R_TMO); } tsk_unlock(); *message = os_runtask->p_msg; return (OS_R_MBX); }
OS_RESULT os_tsk_prio (OS_TID task_id, U8 new_prio) { /* Change execution priority of a task to "new_prio". */ P_TCB p_task; tsk_lock(); if (task_id == 0) { /* Change execution priority of calling task. */ os_runtask->prio = new_prio; os_runtask->prio_base = new_prio; run:if (os_rdy_prio() > new_prio) { os_put_prio (&os_rdy, os_runtask); os_runtask->state = READY; os_dispatch (NULL); } tsk_unlock(); 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. */ tsk_unlock (); return (OS_R_NOK); } p_task = os_active_TCB[task_id-1]; p_task->prio = new_prio; p_task->prio_base = new_prio; if (p_task == os_runtask) { goto run; } os_resort_prio (p_task); if (p_task->state == READY) { /* Task enqueued in a ready list. */ p_task = os_get_first (&os_rdy); os_dispatch (p_task); } tsk_unlock (); return (OS_R_OK); }
void os_dispatch (P_TCB next_TCB) { /* Dispatch next task if any identified or dispatch highest ready task */ /* "next_TCB" identifies a task to run or has value NULL (=no next task) */ if (next_TCB == NULL) { /* Running task was blocked: continue with highest ready task */ next_TCB = os_get_first (&os_rdy); os_switch_tasks (next_TCB); } else { /* Check which task continues */ if (next_TCB->prio > os_runtask->prio) { /* preempt running task */ os_put_rdy_first (os_runtask); os_runtask->state = READY; os_switch_tasks (next_TCB); } else { /* put next task into ready list, no task switch takes place */ next_TCB->state = READY; os_put_prio (&os_rdy, next_TCB); } } }
OS_RESULT os_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; tsk_lock(); if (task_id == 0 || task_id == os_runtask->task_id) { /* Terminate itself. */ os_runtask->state = INACTIVE; os_stk_check (0); p_MCB = os_runtask->p_mlnk; while (p_MCB) { /* Release mutexes owned by this task */ if (p_MCB->p_lnk) { /* A task is waiting for mutex. */ p_TCB = os_get_first ((P_XCB)p_MCB); p_TCB->ret_val = OS_R_MUT; os_rmv_dly (p_TCB); p_TCB->state = READY; os_put_prio (&os_rdy, p_TCB); /* A waiting task becomes the owner of this mutex. */ p_MCB0 = p_MCB; p_MCB->level = 1; p_MCB->owner = p_TCB; p_MCB->p_mlnk = p_TCB->p_mlnk; p_TCB->p_mlnk = p_MCB; p_MCB = p_MCB0->p_mlnk; } else { p_MCB = p_MCB->p_mlnk; } } os_active_TCB[os_runtask->task_id-1] = NULL; _free_box (mp_stk, os_runtask->stack); os_runtask->stack = NULL; rt_notify (os_runtask->ptask, (__FALSE << 8) | os_runtask->task_id); _free_box (mp_tcb, os_runtask); os_runtask = NULL; os_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. */ tsk_unlock (); return (OS_R_NOK); } task_context = os_active_TCB[task_id-1]; os_rmv_list (task_context); os_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 = os_get_first ((P_XCB)p_MCB); p_TCB->ret_val = OS_R_MUT; os_rmv_dly (p_TCB); p_TCB->state = READY; os_put_prio (&os_rdy, p_TCB); /* A waiting task becomes the owner of this mutex. */ p_MCB0 = p_MCB; p_MCB->level = 1; p_MCB->owner = p_TCB; p_MCB->p_mlnk = p_TCB->p_mlnk; p_TCB->p_mlnk = p_MCB; p_MCB = p_MCB0->p_mlnk; } else { p_MCB = p_MCB->p_mlnk; } } os_active_TCB[task_id-1] = NULL; _free_box (mp_stk, task_context->stack); task_context->stack = NULL; rt_notify (task_context->ptask, (__FALSE << 8) | task_context->task_id); _free_box (mp_tcb, task_context); tsk_unlock (); if (os_rdy_prio() > os_runtask->prio) { /* Ready task has higher priority than running task. */ os_runtask->state = READY; os_put_prio (&os_rdy, os_runtask); os_dispatch (NULL); } } return (OS_R_OK); }