void OS_QPost (OS_Q *p_q, void *p_void, OS_MSG_SIZE msg_size, OS_OPT opt, CPU_TS ts, OS_ERR *p_err) { OS_OBJ_QTY cnt; OS_OPT post_type; OS_PEND_LIST *p_pend_list; OS_PEND_DATA *p_pend_data; OS_PEND_DATA *p_pend_data_next; OS_TCB *p_tcb; CPU_SR_ALLOC(); OS_CRITICAL_ENTER(); p_pend_list = &p_q->PendList; if (p_pend_list->NbrEntries == (OS_OBJ_QTY)0) { /* Any task waiting on message queue? */ if ((opt & OS_OPT_POST_LIFO) == (OS_OPT)0) { /* Determine whether we post FIFO or LIFO */ post_type = OS_OPT_POST_FIFO; } else { post_type = OS_OPT_POST_LIFO; } OS_MsgQPut(&p_q->MsgQ, /* Place message in the message queue */ p_void, msg_size, post_type, ts, p_err); OS_CRITICAL_EXIT(); return; } if ((opt & OS_OPT_POST_ALL) != (OS_OPT)0) { /* Post message to all tasks waiting? */ cnt = p_pend_list->NbrEntries; /* Yes */ } else { cnt = (OS_OBJ_QTY)1; /* No */ } p_pend_data = p_pend_list->HeadPtr; while (cnt > 0u) { p_tcb = p_pend_data->TCBPtr; p_pend_data_next = p_pend_data->NextPtr; OS_Post((OS_PEND_OBJ *)((void *)p_q), p_tcb, p_void, msg_size, ts); p_pend_data = p_pend_data_next; cnt--; } OS_CRITICAL_EXIT_NO_SCHED(); if ((opt & OS_OPT_POST_NO_SCHED) == (OS_OPT)0) { OSSched(); /* Run the scheduler */ } *p_err = OS_ERR_NONE; }
OS_SEM_CTR OS_SemPost (OS_SEM *p_sem, OS_OPT opt, CPU_TS ts, OS_ERR *p_err) { OS_SEM_CTR ctr; OS_PEND_LIST *p_pend_list; OS_PEND_DATA *p_pend_data; OS_PEND_DATA *p_pend_data_next; OS_TCB *p_tcb; CPU_SR_ALLOC(); CPU_CRITICAL_ENTER(); p_pend_list = &p_sem->PendList; if (p_pend_list->HeadPtr == DEF_NULL) { /* Any task waiting on semaphore? */ if (p_sem->Ctr == (OS_SEM_CTR)-1) { CPU_CRITICAL_EXIT(); *p_err = OS_ERR_SEM_OVF; return (0u); } p_sem->Ctr++; /* No */ ctr = p_sem->Ctr; #if (OS_CFG_TS_EN == DEF_ENABLED) p_sem->TS = ts; /* Save timestamp in semaphore control block */ #endif CPU_CRITICAL_EXIT(); *p_err = OS_ERR_NONE; return (ctr); } OS_CRITICAL_ENTER_CPU_EXIT(); p_pend_data = p_pend_list->HeadPtr; while (p_pend_data != DEF_NULL) { p_tcb = p_pend_data->TCBPtr; p_pend_data_next = p_pend_data->NextPtr; OS_Post((OS_PEND_OBJ *)((void *)p_sem), p_tcb, DEF_NULL, 0u, ts); if ((opt & OS_OPT_POST_ALL) == 0) { /* Post to all tasks waiting? */ break; /* No */ } p_pend_data = p_pend_data_next; } OS_CRITICAL_EXIT_NO_SCHED(); if ((opt & OS_OPT_POST_NO_SCHED) == 0u) { OSSched(); /* Run the scheduler */ } *p_err = OS_ERR_NONE; return (0u); }
OS_SEM_CTR OS_SemPost (OS_SEM *p_sem, OS_OPT opt, CPU_TS ts, OS_ERR *p_err) { OS_OBJ_QTY cnt; OS_SEM_CTR ctr; OS_PEND_LIST *p_pend_list; OS_PEND_DATA *p_pend_data; OS_PEND_DATA *p_pend_data_next; OS_TCB *p_tcb; CPU_SR_ALLOC(); CPU_CRITICAL_ENTER(); p_pend_list = &p_sem->PendList; if (p_pend_list->NbrEntries == (OS_OBJ_QTY)0) { /* Any task waiting on semaphore? */ switch (sizeof(OS_SEM_CTR)) { case 1u: if (p_sem->Ctr == DEF_INT_08U_MAX_VAL) { CPU_CRITICAL_EXIT(); *p_err = OS_ERR_SEM_OVF; return ((OS_SEM_CTR)0); } break; case 2u: if (p_sem->Ctr == DEF_INT_16U_MAX_VAL) { CPU_CRITICAL_EXIT(); *p_err = OS_ERR_SEM_OVF; return ((OS_SEM_CTR)0); } break; case 4u: if (p_sem->Ctr == DEF_INT_32U_MAX_VAL) { CPU_CRITICAL_EXIT(); *p_err = OS_ERR_SEM_OVF; return ((OS_SEM_CTR)0); } break; default: break; } p_sem->Ctr++; /* No */ ctr = p_sem->Ctr; p_sem->TS = ts; /* Save timestamp in semaphore control block */ CPU_CRITICAL_EXIT(); *p_err = OS_ERR_NONE; return (ctr); } OS_CRITICAL_ENTER_CPU_EXIT(); if ((opt & OS_OPT_POST_ALL) != (OS_OPT)0) { /* Post message to all tasks waiting? */ cnt = p_pend_list->NbrEntries; /* Yes */ } else { cnt = (OS_OBJ_QTY)1; /* No */ } p_pend_data = p_pend_list->HeadPtr; while (cnt > 0u) { p_tcb = p_pend_data->TCBPtr; p_pend_data_next = p_pend_data->NextPtr; OS_Post((OS_PEND_OBJ *)((void *)p_sem), p_tcb, (void *)0, (OS_MSG_SIZE)0, ts); p_pend_data = p_pend_data_next; cnt--; } ctr = p_sem->Ctr; OS_CRITICAL_EXIT_NO_SCHED(); if ((opt & OS_OPT_POST_NO_SCHED) == (OS_OPT)0) { OSSched(); /* Run the scheduler */ } *p_err = OS_ERR_NONE; return (ctr); }
void OSMonOp (OS_MON *p_mon, OS_TICK timeout, void *p_arg, OS_MON_ON_ENTER_PTR p_on_enter, OS_MON_ON_EVAL_PTR p_on_eval, OS_OPT opt, OS_ERR *p_err) { CPU_INT32U op_res; CPU_INT32U mon_res; OS_PEND_LIST *p_pend_list; OS_TCB *p_tcb; OS_TCB *p_tcb_next; void *p_eval_data; CPU_BOOLEAN sched; CPU_SR_ALLOC(); #ifdef OS_SAFETY_CRITICAL if (p_err == DEF_NULL) { OS_SAFETY_CRITICAL_EXCEPTION(); return; } #endif #if (OS_CFG_INVALID_OS_CALLS_CHK_EN == DEF_ENABLED) /* Is the kernel running? */ if (OSRunning != OS_STATE_OS_RUNNING) { *p_err = OS_ERR_OS_NOT_RUNNING; return; } #endif #if (OS_CFG_ARG_CHK_EN == DEF_ENABLED) if (p_mon == DEF_NULL) { /* Validate 'p_mon' */ *p_err = OS_ERR_OBJ_PTR_NULL; return; } #endif sched = DEF_NO; CPU_CRITICAL_ENTER(); if (p_on_enter != DEF_NULL) { op_res = (*p_on_enter)(p_mon, p_arg); } else { op_res = OS_MON_RES_BLOCK | OS_MON_RES_STOP_EVAL; } if (DEF_BIT_IS_SET(op_res, OS_MON_RES_BLOCK) == DEF_YES) { OS_Pend((OS_PEND_OBJ *)(p_mon), /* Block task pending on Condition Variable */ OS_TASK_PEND_ON_COND_VAR, timeout); sched = DEF_YES; } OSTCBCurPtr->MonData.p_eval_data = p_arg; OSTCBCurPtr->MonData.p_on_eval = p_on_eval; if (DEF_BIT_IS_CLR(op_res, OS_MON_RES_STOP_EVAL) == DEF_YES) { p_pend_list = &p_mon->PendList; if (p_pend_list->HeadPtr != DEF_NULL) { p_tcb = p_pend_list->HeadPtr; while (p_tcb != DEF_NULL) { p_tcb_next = p_tcb->PendNextPtr; p_on_eval = p_tcb->MonData.p_on_eval; p_eval_data = p_tcb->MonData.p_eval_data; if (p_on_eval != DEF_NULL) { mon_res = (*p_on_eval)(p_mon, p_eval_data, p_arg); } else { mon_res = OS_MON_RES_STOP_EVAL; } if (DEF_BIT_IS_CLR(mon_res, OS_MON_RES_BLOCK) == DEF_YES) { OS_Post((OS_PEND_OBJ *)(p_mon), p_tcb, DEF_NULL, 0u, 0u); if (DEF_BIT_IS_CLR(opt, OS_OPT_POST_NO_SCHED) == DEF_YES) { sched = DEF_YES; } } if (DEF_BIT_IS_SET(mon_res, OS_MON_RES_STOP_EVAL) == DEF_YES) { break; } p_tcb = p_tcb_next; } } } CPU_CRITICAL_EXIT(); if (sched == DEF_YES) { OSSched(); /* Find the next highest priority task ready to run */ } if (DEF_BIT_IS_SET(op_res, OS_MON_RES_BLOCK) == DEF_YES) { CPU_CRITICAL_ENTER(); switch (OSTCBCurPtr->PendStatus) { case OS_STATUS_PEND_OK: /* We got the monitor */ *p_err = OS_ERR_NONE; break; case OS_STATUS_PEND_ABORT: /* Indicate that we aborted */ *p_err = OS_ERR_PEND_ABORT; break; case OS_STATUS_PEND_TIMEOUT: /* Indicate that we didn't get monitor within timeout */ *p_err = OS_ERR_TIMEOUT; break; case OS_STATUS_PEND_DEL: /* Indicate that object pended on has been deleted */ *p_err = OS_ERR_OBJ_DEL; break; default: *p_err = OS_ERR_STATUS_INVALID; } CPU_CRITICAL_EXIT(); } else { *p_err = OS_ERR_NONE; } }
void OSMutexPost (OS_MUTEX *p_mutex, OS_OPT opt, OS_ERR *p_err) { OS_PEND_LIST *p_pend_list; OS_TCB *p_tcb; CPU_TS ts; CPU_SR_ALLOC(); #ifdef OS_SAFETY_CRITICAL if (p_err == (OS_ERR *)0) { #if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u)) TRACE_OS_MUTEX_POST_FAILED(p_mutex); /* Record the event. */ #endif OS_SAFETY_CRITICAL_EXCEPTION(); return; } #endif #if OS_CFG_CALLED_FROM_ISR_CHK_EN > 0u if (OSIntNestingCtr > (OS_NESTING_CTR)0) { /* Not allowed to call from an ISR */ #if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u)) TRACE_OS_MUTEX_POST_FAILED(p_mutex); /* Record the event. */ #endif *p_err = OS_ERR_POST_ISR; return; } #endif #if OS_CFG_ARG_CHK_EN > 0u if (p_mutex == (OS_MUTEX *)0) { /* Validate 'p_mutex' */ #if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u)) TRACE_OS_MUTEX_POST_FAILED(p_mutex); /* Record the event. */ #endif *p_err = OS_ERR_OBJ_PTR_NULL; return; } switch (opt) { /* Validate 'opt' */ case OS_OPT_POST_NONE: case OS_OPT_POST_NO_SCHED: break; default: #if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u)) TRACE_OS_MUTEX_POST_FAILED(p_mutex); /* Record the event. */ #endif *p_err = OS_ERR_OPT_INVALID; return; } #endif #if OS_CFG_OBJ_TYPE_CHK_EN > 0u if (p_mutex->Type != OS_OBJ_TYPE_MUTEX) { /* Make sure mutex was created */ #if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u)) TRACE_OS_MUTEX_POST_FAILED(p_mutex); /* Record the event. */ #endif *p_err = OS_ERR_OBJ_TYPE; return; } #endif CPU_CRITICAL_ENTER(); if (OSTCBCurPtr != p_mutex->OwnerTCBPtr) { /* Make sure the mutex owner is releasing the mutex */ CPU_CRITICAL_EXIT(); #if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u)) TRACE_OS_MUTEX_POST_FAILED(p_mutex); /* Record the event. */ #endif *p_err = OS_ERR_MUTEX_NOT_OWNER; return; } #if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u)) TRACE_OS_MUTEX_POST(p_mutex); /* Record the event. */ #endif OS_CRITICAL_ENTER_CPU_EXIT(); ts = OS_TS_GET(); /* Get timestamp */ p_mutex->TS = ts; p_mutex->OwnerNestingCtr--; /* Decrement owner's nesting counter */ if (p_mutex->OwnerNestingCtr > (OS_NESTING_CTR)0) { /* Are we done with all nestings? */ OS_CRITICAL_EXIT(); /* No */ *p_err = OS_ERR_MUTEX_NESTING; return; } p_pend_list = &p_mutex->PendList; if (p_pend_list->NbrEntries == (OS_OBJ_QTY)0) { /* Any task waiting on mutex? */ p_mutex->OwnerTCBPtr = (OS_TCB *)0; /* No */ p_mutex->OwnerNestingCtr = (OS_NESTING_CTR)0; OS_CRITICAL_EXIT(); *p_err = OS_ERR_NONE; return; } /* Yes */ if (OSTCBCurPtr->Prio != p_mutex->OwnerOriginalPrio) { OS_RdyListRemove(OSTCBCurPtr); OSTCBCurPtr->Prio = p_mutex->OwnerOriginalPrio; /* Lower owner's priority back to its original one */ #if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u)) TRACE_OS_MUTEX_TASK_PRIO_DISINHERIT(OSTCBCurPtr, OSTCBCurPtr->Prio); #endif OS_PrioInsert(OSTCBCurPtr->Prio); OS_RdyListInsertTail(OSTCBCurPtr); /* Insert owner in ready list at new priority */ OSPrioCur = OSTCBCurPtr->Prio; } /* Get TCB from head of pend list */ p_tcb = p_pend_list->HeadPtr->TCBPtr; p_mutex->OwnerTCBPtr = p_tcb; /* Give mutex to new owner */ p_mutex->OwnerOriginalPrio = p_tcb->Prio; p_mutex->OwnerNestingCtr = (OS_NESTING_CTR)1; /* Post to mutex */ OS_Post((OS_PEND_OBJ *)((void *)p_mutex), (OS_TCB *)p_tcb, (void *)0, (OS_MSG_SIZE )0, (CPU_TS )ts); OS_CRITICAL_EXIT_NO_SCHED(); if ((opt & OS_OPT_POST_NO_SCHED) == (OS_OPT)0) { OSSched(); /* Run the scheduler */ } *p_err = OS_ERR_NONE; }