コード例 #1
0
ファイル: os_q.c プロジェクト: kylemanna/kinetis-sdk1
OS_OBJ_QTY  OSQPendAbort (OS_Q    *p_q,
                          OS_OPT   opt,
                          OS_ERR  *p_err)
{
    OS_PEND_LIST  *p_pend_list;
    OS_TCB        *p_tcb;
    CPU_TS         ts;
    OS_OBJ_QTY     nbr_tasks;
    CPU_SR_ALLOC();



#ifdef OS_SAFETY_CRITICAL
    if (p_err == (OS_ERR *)0) {
        OS_SAFETY_CRITICAL_EXCEPTION();
        return ((OS_OBJ_QTY)0u);
    }
#endif

#if OS_CFG_CALLED_FROM_ISR_CHK_EN > 0u
    if (OSIntNestingCtr > (OS_NESTING_CTR)0u) {             /* Not allowed to Pend Abort from an ISR                  */
       *p_err =  OS_ERR_PEND_ABORT_ISR;
        return ((OS_OBJ_QTY)0u);
    }
#endif

#if OS_CFG_ARG_CHK_EN > 0u
    if (p_q == (OS_Q *)0) {                                 /* Validate 'p_q'                                         */
       *p_err =  OS_ERR_OBJ_PTR_NULL;
        return ((OS_OBJ_QTY)0u);
    }
    switch (opt) {                                          /* Validate 'opt'                                         */
        case OS_OPT_PEND_ABORT_1:
        case OS_OPT_PEND_ABORT_ALL:
        case OS_OPT_PEND_ABORT_1   | OS_OPT_POST_NO_SCHED:
        case OS_OPT_PEND_ABORT_ALL | OS_OPT_POST_NO_SCHED:
             break;

        default:
            *p_err =  OS_ERR_OPT_INVALID;
             return ((OS_OBJ_QTY)0u);
    }
#endif

#if OS_CFG_OBJ_TYPE_CHK_EN > 0u
    if (p_q->Type != OS_OBJ_TYPE_Q) {                       /* Make sure queue was created                            */
       *p_err =  OS_ERR_OBJ_TYPE;
        return ((OS_OBJ_QTY)0u);
    }
#endif

    CPU_CRITICAL_ENTER();
    p_pend_list = &p_q->PendList;
    if (p_pend_list->NbrEntries == (OS_OBJ_QTY)0u) {        /* Any task waiting on queue?                             */
        CPU_CRITICAL_EXIT();                                /* No                                                     */
       *p_err =  OS_ERR_PEND_ABORT_NONE;
        return ((OS_OBJ_QTY)0u);
    }

    OS_CRITICAL_ENTER_CPU_EXIT();
    nbr_tasks = 0u;
    ts        = OS_TS_GET();                                /* Get local time stamp so all tasks get the same time    */
    while (p_pend_list->NbrEntries > (OS_OBJ_QTY)0u) {
        p_tcb = p_pend_list->HeadPtr->TCBPtr;
        OS_PendAbort((OS_PEND_OBJ *)((void *)p_q),
                     p_tcb,
                     ts);
        nbr_tasks++;
        if (opt != OS_OPT_PEND_ABORT_ALL) {                 /* Pend abort all tasks waiting?                          */
            break;                                          /* No                                                     */
        }
    }
    OS_CRITICAL_EXIT_NO_SCHED();

    if ((opt & OS_OPT_POST_NO_SCHED) == (OS_OPT)0u) {
        OSSched();                                          /* Run the scheduler                                      */
    }

   *p_err = OS_ERR_NONE;
    return (nbr_tasks);
}
コード例 #2
0
ファイル: os_sem.c プロジェクト: diqiuche/vineyard
OS_OBJ_QTY  OSSemDel (OS_SEM  *p_sem,
                      OS_OPT   opt,
                      OS_ERR  *p_err)
{
    OS_OBJ_QTY     cnt;
    OS_OBJ_QTY     nbr_tasks;
    OS_PEND_DATA  *p_pend_data;
    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) {
        OS_SAFETY_CRITICAL_EXCEPTION();
        return ((OS_OBJ_QTY)0);
    }
#endif

#if OS_CFG_CALLED_FROM_ISR_CHK_EN > 0u
    if (OSIntNestingCtr > (OS_NESTING_CTR)0) {              /* Not allowed to delete a semaphore from an ISR          */
       *p_err = OS_ERR_DEL_ISR;
        return ((OS_OBJ_QTY)0);
    }
#endif

#if OS_CFG_ARG_CHK_EN > 0u
    if (p_sem == (OS_SEM *)0) {                             /* Validate 'p_sem'                                       */
       *p_err = OS_ERR_OBJ_PTR_NULL;
        return ((OS_OBJ_QTY)0);
    }
    switch (opt) {                                          /* Validate 'opt'                                         */
        case OS_OPT_DEL_NO_PEND:
        case OS_OPT_DEL_ALWAYS:
             break;

        default:
            *p_err = OS_ERR_OPT_INVALID;
             return ((OS_OBJ_QTY)0);
    }
#endif

#if OS_CFG_OBJ_TYPE_CHK_EN > 0u
    if (p_sem->Type != OS_OBJ_TYPE_SEM) {                   /* Make sure semaphore was created                        */
       *p_err = OS_ERR_OBJ_TYPE;
        return ((OS_OBJ_QTY)0);
    }
#endif

    CPU_CRITICAL_ENTER();
    p_pend_list = &p_sem->PendList;
    cnt         = p_pend_list->NbrEntries;
    nbr_tasks   = cnt;
    switch (opt) {
        case OS_OPT_DEL_NO_PEND:                            /* Delete semaphore only if no task waiting               */
             if (nbr_tasks == (OS_OBJ_QTY)0) {
#if OS_CFG_DBG_EN > 0u
                 OS_SemDbgListRemove(p_sem);
#endif
                 OSSemQty--;
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
                 TRACE_OS_SEM_DEL(p_sem);                   /* Record the event.                                      */
#endif
                 OS_SemClr(p_sem);
                 CPU_CRITICAL_EXIT();
                *p_err = OS_ERR_NONE;
             } else {
                 CPU_CRITICAL_EXIT();
                *p_err = OS_ERR_TASK_WAITING;
             }
             break;

        case OS_OPT_DEL_ALWAYS:                             /* Always delete the semaphore                            */
             OS_CRITICAL_ENTER_CPU_EXIT();
             ts = OS_TS_GET();                              /* Get local time stamp so all tasks get the same time    */
             while (cnt > 0u) {                             /* Remove all tasks on the pend list                      */
                 p_pend_data = p_pend_list->HeadPtr;
                 p_tcb       = p_pend_data->TCBPtr;
                 OS_PendObjDel((OS_PEND_OBJ *)((void *)p_sem),
                               p_tcb,
                               ts);
                 cnt--;
             }
#if OS_CFG_DBG_EN > 0u
             OS_SemDbgListRemove(p_sem);
#endif
             OSSemQty--;
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_SEM_DEL(p_sem);                       /* Record the event.                                      */
#endif
             OS_SemClr(p_sem);
             OS_CRITICAL_EXIT_NO_SCHED();
             OSSched();                                     /* Find highest priority task ready to run                */
            *p_err = OS_ERR_NONE;
             break;

        default:
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_OPT_INVALID;
             break;
    }
    return ((OS_OBJ_QTY)nbr_tasks);
}
コード例 #3
0
ファイル: os_sem.c プロジェクト: VENGEL/Micrium
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);
}
コード例 #4
0
ファイル: os_q.c プロジェクト: kylemanna/kinetis-sdk1
void  *OSQPend (OS_Q         *p_q,
                OS_TICK       timeout,
                OS_OPT        opt,
                OS_MSG_SIZE  *p_msg_size,
                CPU_TS       *p_ts,
                OS_ERR       *p_err)
{
    OS_PEND_DATA  pend_data;
    void         *p_void;
    CPU_SR_ALLOC();



#ifdef OS_SAFETY_CRITICAL
    if (p_err == (OS_ERR *)0) {
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_Q_PEND_FAILED(p_q);                        /* Record the event.                                      */
#endif
        OS_SAFETY_CRITICAL_EXCEPTION();
        return ((void *)0);
    }
#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_Q_PEND_FAILED(p_q);                        /* Record the event.                                      */
#endif
       *p_err = OS_ERR_PEND_ISR;
        return ((void *)0);
    }
#endif

#if OS_CFG_ARG_CHK_EN > 0u
    if (p_q == (OS_Q *)0) {                                 /* Validate arguments                                     */
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_Q_PEND_FAILED(p_q);                        /* Record the event.                                      */
#endif
       *p_err = OS_ERR_OBJ_PTR_NULL;
        return ((void *)0);
    }
    if (p_msg_size == (OS_MSG_SIZE *)0) {
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_Q_PEND_FAILED(p_q);                        /* Record the event.                                      */
#endif
       *p_err = OS_ERR_PTR_INVALID;
        return ((void *)0);
    }
    switch (opt) {
        case OS_OPT_PEND_BLOCKING:
        case OS_OPT_PEND_NON_BLOCKING:
             break;

        default:
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_Q_PEND_FAILED(p_q);                   /* Record the event.                                      */
#endif
            *p_err = OS_ERR_OPT_INVALID;
             return ((void *)0);
    }
#endif

#if OS_CFG_OBJ_TYPE_CHK_EN > 0u
    if (p_q->Type != OS_OBJ_TYPE_Q) {                       /* Make sure message queue was created                    */
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_Q_PEND_FAILED(p_q);                        /* Record the event.                                      */
#endif
       *p_err = OS_ERR_OBJ_TYPE;
        return ((void *)0);
    }
#endif

    if (p_ts != (CPU_TS *)0) {
       *p_ts  = (CPU_TS  )0;                                /* Initialize the returned timestamp                      */
    }

    CPU_CRITICAL_ENTER();
    p_void = OS_MsgQGet(&p_q->MsgQ,                         /* Any message waiting in the message queue?              */
                        p_msg_size,
                        p_ts,
                        p_err);
    if (*p_err == OS_ERR_NONE) {
        CPU_CRITICAL_EXIT();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_Q_PEND(p_q);                               /* Record the event.                                      */
#endif
        return (p_void);                                    /* Yes, Return message received                           */
    }

    if ((opt & OS_OPT_PEND_NON_BLOCKING) != (OS_OPT)0) {    /* Caller wants to block if not available?                */
        CPU_CRITICAL_EXIT();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_Q_PEND_FAILED(p_q);                        /* Record the event.                                      */
#endif
       *p_err = OS_ERR_PEND_WOULD_BLOCK;                    /* No                                                     */
        return ((void *)0);
    } else {
        if (OSSchedLockNestingCtr > (OS_NESTING_CTR)0) {    /* Can't pend when the scheduler is locked                */
            CPU_CRITICAL_EXIT();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
            TRACE_OS_Q_PEND_FAILED(p_q);                    /* Record the event.                                      */
#endif
           *p_err = OS_ERR_SCHED_LOCKED;
            return ((void *)0);
        }
    }
                                                            /* Lock the scheduler/re-enable interrupts                */
    OS_CRITICAL_ENTER_CPU_EXIT();
    OS_Pend(&pend_data,                                     /* Block task pending on Message Queue                    */
            (OS_PEND_OBJ *)((void *)p_q),
            OS_TASK_PEND_ON_Q,
            timeout);
    OS_CRITICAL_EXIT_NO_SCHED();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
    TRACE_OS_Q_PEND_BLOCK(p_q);                             /* Record the event.                                      */
#endif
    OSSched();                                              /* Find the next highest priority task ready to run       */

    CPU_CRITICAL_ENTER();
    switch (OSTCBCurPtr->PendStatus) {
        case OS_STATUS_PEND_OK:                             /* Extract message from TCB (Put there by Post)           */
             p_void     = OSTCBCurPtr->MsgPtr;
            *p_msg_size = OSTCBCurPtr->MsgSize;
             if (p_ts  != (CPU_TS *)0) {
                *p_ts   =  OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_Q_PEND(p_q);                          /* Record the event.                                      */
#endif
            *p_err      = OS_ERR_NONE;
             break;

        case OS_STATUS_PEND_ABORT:                          /* Indicate that we aborted                               */
             p_void     = (void      *)0;
            *p_msg_size = (OS_MSG_SIZE)0;
             if (p_ts  != (CPU_TS *)0) {
                *p_ts   =  OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_Q_PEND_FAILED(p_q);                   /* Record the event.                                      */
#endif
            *p_err      = OS_ERR_PEND_ABORT;
             break;

        case OS_STATUS_PEND_TIMEOUT:                        /* Indicate that we didn't get event within TO            */
             p_void     = (void      *)0;
            *p_msg_size = (OS_MSG_SIZE)0;
             if (p_ts  != (CPU_TS *)0) {
                *p_ts   = (CPU_TS  )0;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_Q_PEND_FAILED(p_q);                   /* Record the event.                                      */
#endif
            *p_err      = OS_ERR_TIMEOUT;
             break;

        case OS_STATUS_PEND_DEL:                            /* Indicate that object pended on has been deleted        */
             p_void     = (void      *)0;
            *p_msg_size = (OS_MSG_SIZE)0;
             if (p_ts  != (CPU_TS *)0) {
                *p_ts   =  OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_Q_PEND_FAILED(p_q);                   /* Record the event.                                      */
#endif
            *p_err      = OS_ERR_OBJ_DEL;
             break;

        default:
             p_void     = (void      *)0;
            *p_msg_size = (OS_MSG_SIZE)0;
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_Q_PEND_FAILED(p_q);                   /* Record the event.                                      */
#endif
            *p_err      = OS_ERR_STATUS_INVALID;
             break;
    }
    CPU_CRITICAL_EXIT();
    return (p_void);
}
コード例 #5
0
ファイル: os_pend_multi.c プロジェクト: Lyanzh/StepCounter
OS_OBJ_QTY  OSPendMulti (OS_PEND_DATA  *p_pend_data_tbl,
                         OS_OBJ_QTY     tbl_size,
                         OS_TICK        timeout,
                         OS_OPT         opt,
                         OS_ERR        *p_err)
{
    CPU_BOOLEAN   valid;
    OS_OBJ_QTY    nbr_obj_rdy;
    CPU_SR_ALLOC();



#ifdef OS_SAFETY_CRITICAL
    if (p_err == (OS_ERR *)0) {
        OS_SAFETY_CRITICAL_EXCEPTION();
        return ((OS_OBJ_QTY)0);
    }
#endif

#if OS_CFG_CALLED_FROM_ISR_CHK_EN > 0u
    if (OSIntNestingCtr > (OS_NESTING_CTR)0) {              /* Can't pend from an ISR                                 */
       *p_err = OS_ERR_PEND_ISR;
        return ((OS_OBJ_QTY)0);
    }
#endif

#if OS_CFG_ARG_CHK_EN > 0u
    if (p_pend_data_tbl == (OS_PEND_DATA *)0) {             /* Validate 'p_pend_data_tbl'                             */
       *p_err = OS_ERR_PTR_INVALID;
        return ((OS_OBJ_QTY)0);
    }
    if (tbl_size == (OS_OBJ_QTY)0) {                        /* Array size must be > 0                                 */
       *p_err = OS_ERR_PTR_INVALID;
        return ((OS_OBJ_QTY)0);
    }
    switch (opt) {
        case OS_OPT_PEND_BLOCKING:
        case OS_OPT_PEND_NON_BLOCKING:
             break;

        default:
            *p_err = OS_ERR_OPT_INVALID;
             return ((OS_OBJ_QTY)0);
    }
#endif

    valid = OS_PendMultiValidate(p_pend_data_tbl,           /* -------- Validate objects to be OS_SEM or OS_Q ------- */
                                 tbl_size);
    if (valid == DEF_FALSE) {
       *p_err = OS_ERR_OBJ_TYPE;                            /* Invalid, not OS_SEM or OS_Q                            */
        return ((OS_OBJ_QTY)0);
    }


    CPU_CRITICAL_ENTER();
    nbr_obj_rdy = OS_PendMultiGetRdy(p_pend_data_tbl,       /* --------- SEE IF OBJECT(s) HAVE BEEN POSTED ---------- */
                                     tbl_size);
    if (nbr_obj_rdy > (OS_OBJ_QTY)0) {
        CPU_CRITICAL_EXIT();
       *p_err = OS_ERR_NONE;
        return ((OS_OBJ_QTY)nbr_obj_rdy);
    }

    if ((opt & OS_OPT_PEND_NON_BLOCKING) != (OS_OPT)0) {    /* Caller wants to block if not available?                */
        CPU_CRITICAL_EXIT();
       *p_err = OS_ERR_PEND_WOULD_BLOCK;                    /* No                                                     */
        return ((OS_OBJ_QTY)0);
    } else {
        if (OSSchedLockNestingCtr > (OS_NESTING_CTR)0) {    /* Can't pend when the scheduler is locked                */
            CPU_CRITICAL_EXIT();
           *p_err = OS_ERR_SCHED_LOCKED;
            return ((OS_OBJ_QTY)0);
        }
    }
                                                            /* Lock the scheduler/re-enable interrupts                */
    OS_CRITICAL_ENTER_CPU_EXIT();
                                                            /* ------ NO OBJECT READY, PEND ON MULTIPLE OBJECTS ----- */
    OS_PendMultiWait(p_pend_data_tbl,                       /* Suspend task until object posted or timeout occurs     */
                     tbl_size,
                     timeout);

    OS_CRITICAL_EXIT_NO_SCHED();

    OSSched();                                              /* Find next highest priority task ready                  */

    CPU_CRITICAL_ENTER();
    switch (OSTCBCurPtr->PendStatus) {
        case OS_STATUS_PEND_OK:                             /* We got one of the objects posted to                    */
            *p_err = OS_ERR_NONE;
             break;

        case OS_STATUS_PEND_ABORT:                          /* Indicate that the multi-pend was aborted               */
            *p_err = OS_ERR_PEND_ABORT;
             break;

        case OS_STATUS_PEND_TIMEOUT:                        /* Indicate that we didn't get semaphore within timeout   */
            *p_err = OS_ERR_TIMEOUT;
             break;

        case OS_STATUS_PEND_DEL:                            /* Indicate that an object pended on has been deleted     */
            *p_err = OS_ERR_OBJ_DEL;
            break;

        default:
            *p_err = OS_ERR_STATUS_INVALID;
             break;
    }

    OSTCBCurPtr->PendStatus = OS_STATUS_PEND_OK;
    CPU_CRITICAL_EXIT();

    return ((OS_OBJ_QTY)1);
}
コード例 #6
0
ファイル: os_sem.c プロジェクト: VENGEL/Micrium
OS_SEM_CTR  OSSemPend (OS_SEM   *p_sem,
                       OS_TICK   timeout,
                       OS_OPT    opt,
                       CPU_TS   *p_ts,
                       OS_ERR   *p_err)
{
    OS_SEM_CTR    ctr;
    OS_PEND_DATA  pend_data;
    CPU_SR_ALLOC();



#ifdef OS_SAFETY_CRITICAL
    if (p_err == (OS_ERR *)0) {
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_SEM_PEND_FAILED(p_sem);                    /* Record the event.                                      */
#endif
        OS_SAFETY_CRITICAL_EXCEPTION();
        return ((OS_SEM_CTR)0);
    }
#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_SEM_PEND_FAILED(p_sem);                    /* Record the event.                                      */
#endif
       *p_err = OS_ERR_PEND_ISR;
        return ((OS_SEM_CTR)0);
    }
#endif

#if OS_CFG_ARG_CHK_EN > 0u
    if (p_sem == (OS_SEM *)0) {                             /* Validate 'p_sem'                                       */
       *p_err = OS_ERR_OBJ_PTR_NULL;
        return ((OS_SEM_CTR)0);
    }
    switch (opt) {                                          /* Validate 'opt'                                         */
        case OS_OPT_PEND_BLOCKING:
        case OS_OPT_PEND_NON_BLOCKING:
             break;

        default:
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_SEM_PEND_FAILED(p_sem);               /* Record the event.                                      */
#endif
            *p_err = OS_ERR_OPT_INVALID;
             return ((OS_SEM_CTR)0);
    }
#endif

#if OS_CFG_OBJ_TYPE_CHK_EN > 0u
    if (p_sem->Type != OS_OBJ_TYPE_SEM) {                   /* Make sure semaphore was created                        */
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_SEM_PEND_FAILED(p_sem);                    /* Record the event.                                      */
#endif
       *p_err = OS_ERR_OBJ_TYPE;
        return ((OS_SEM_CTR)0);
    }
#endif

    if (p_ts != (CPU_TS *)0) {
       *p_ts  = (CPU_TS)0;                                  /* Initialize the returned timestamp                      */
    }
    CPU_CRITICAL_ENTER();
    if (p_sem->Ctr > (OS_SEM_CTR)0) {                       /* Resource available?                                    */
        p_sem->Ctr--;                                       /* Yes, caller may proceed                                */
        if (p_ts != (CPU_TS *)0) {
           *p_ts  = p_sem->TS;                              /*      get timestamp of last post                        */
        }
        ctr   = p_sem->Ctr;
        CPU_CRITICAL_EXIT();
       *p_err = OS_ERR_NONE;
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_SEM_PEND(p_sem);                           /* Record the event.                                      */
#endif
        return (ctr);
    }

    if ((opt & OS_OPT_PEND_NON_BLOCKING) != (OS_OPT)0) {    /* Caller wants to block if not available?                */
        ctr   = p_sem->Ctr;                                 /* No                                                     */
        CPU_CRITICAL_EXIT();
       *p_err = OS_ERR_PEND_WOULD_BLOCK;
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_SEM_PEND_FAILED(p_sem);                    /* Record the event.                                      */
#endif
        return (ctr);
    } else {                                                /* Yes                                                    */
        if (OSSchedLockNestingCtr > (OS_NESTING_CTR)0) {    /* Can't pend when the scheduler is locked                */
            CPU_CRITICAL_EXIT();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
            TRACE_OS_SEM_PEND_FAILED(p_sem);                /* Record the event.                                      */
#endif
           *p_err = OS_ERR_SCHED_LOCKED;
            return ((OS_SEM_CTR)0);
        }
    }
                                                            /* Lock the scheduler/re-enable interrupts                */
    OS_CRITICAL_ENTER_CPU_EXIT();
    OS_Pend(&pend_data,                                     /* Block task pending on Semaphore                        */
            (OS_PEND_OBJ *)((void *)p_sem),
            OS_TASK_PEND_ON_SEM,
            timeout);
    OS_CRITICAL_EXIT_NO_SCHED();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
    TRACE_OS_SEM_PEND_BLOCK(p_sem);                         /* Record the event.                                      */
#endif
    OSSched();                                              /* Find the next highest priority task ready to run       */

    CPU_CRITICAL_ENTER();
    switch (OSTCBCurPtr->PendStatus) {
        case OS_STATUS_PEND_OK:                             /* We got the semaphore                                   */
             if (p_ts != (CPU_TS *)0) {
                *p_ts  =  OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_SEM_PEND(p_sem);                      /* Record the event.                                      */
#endif
            *p_err = OS_ERR_NONE;
             break;

        case OS_STATUS_PEND_ABORT:                          /* Indicate that we aborted                               */
             if (p_ts != (CPU_TS *)0) {
                *p_ts  =  OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_SEM_PEND_FAILED(p_sem);               /* Record the event.                                      */
#endif
            *p_err = OS_ERR_PEND_ABORT;
             break;

        case OS_STATUS_PEND_TIMEOUT:                        /* Indicate that we didn't get semaphore within timeout   */
             if (p_ts != (CPU_TS *)0) {
                *p_ts  = (CPU_TS  )0;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_SEM_PEND_FAILED(p_sem);               /* Record the event.                                      */
#endif
            *p_err = OS_ERR_TIMEOUT;
             break;

        case OS_STATUS_PEND_DEL:                            /* Indicate that object pended on has been deleted        */
             if (p_ts != (CPU_TS *)0) {
                *p_ts  =  OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_SEM_PEND_FAILED(p_sem);               /* Record the event.                                      */
#endif
            *p_err = OS_ERR_OBJ_DEL;
             break;

        default:
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_SEM_PEND_FAILED(p_sem);               /* Record the event.                                      */
#endif
            *p_err = OS_ERR_STATUS_INVALID;
             CPU_CRITICAL_EXIT();
             return ((OS_SEM_CTR)0);
    }
    ctr = p_sem->Ctr;
    CPU_CRITICAL_EXIT();
    return (ctr);
}
コード例 #7
0
OS_OBJ_QTY  OSSemPendAbort (OS_SEM  *p_sem,
                            OS_OPT   opt,
                            OS_ERR  *p_err)
{
    OS_PEND_LIST  *p_pend_list;
    OS_TCB        *p_tcb;
    CPU_TS         ts;
    OS_OBJ_QTY     nbr_tasks;
    CPU_SR_ALLOC();



#ifdef OS_SAFETY_CRITICAL
    if (p_err == DEF_NULL) {
        OS_SAFETY_CRITICAL_EXCEPTION();
        return (0u);
    }
#endif

#if (OS_CFG_CALLED_FROM_ISR_CHK_EN == DEF_ENABLED)
    if (OSIntNestingCtr > 0u) {                                 /* Not allowed to Pend Abort from an ISR                */
       *p_err =  OS_ERR_PEND_ABORT_ISR;
        return (0u);
    }
#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 (0u);
    }
#endif

#if (OS_CFG_ARG_CHK_EN == DEF_ENABLED)
    if (p_sem == DEF_NULL) {                                    /* Validate 'p_sem'                                     */
       *p_err =  OS_ERR_OBJ_PTR_NULL;
        return (0u);
    }
    switch (opt) {                                              /* Validate 'opt'                                       */
        case OS_OPT_PEND_ABORT_1:
        case OS_OPT_PEND_ABORT_ALL:
        case OS_OPT_PEND_ABORT_1   | OS_OPT_POST_NO_SCHED:
        case OS_OPT_PEND_ABORT_ALL | OS_OPT_POST_NO_SCHED:
             break;

        default:
            *p_err =  OS_ERR_OPT_INVALID;
             return (0u);
    }
#endif

#if (OS_CFG_OBJ_TYPE_CHK_EN == DEF_ENABLED)
    if (p_sem->Type != OS_OBJ_TYPE_SEM) {                       /* Make sure semaphore was created                      */
       *p_err =  OS_ERR_OBJ_TYPE;
        return (0u);
    }
#endif

    CPU_CRITICAL_ENTER();
    p_pend_list = &p_sem->PendList;
    if (p_pend_list->HeadPtr == DEF_NULL) {                     /* Any task waiting on semaphore?                       */
        CPU_CRITICAL_EXIT();                                    /* No                                                   */
       *p_err =  OS_ERR_PEND_ABORT_NONE;
        return (0u);
    }

    OS_CRITICAL_ENTER_CPU_EXIT();
    nbr_tasks = 0u;
#if (OS_CFG_TS_EN == DEF_ENABLED)
    ts        = OS_TS_GET();                                    /* Get local time stamp so all tasks get the same time  */
#else
    ts        = 0u;
#endif
    while (p_pend_list->HeadPtr != DEF_NULL) {
        p_tcb = p_pend_list->HeadPtr->TCBPtr;
        OS_PendAbort((OS_PEND_OBJ *)((void *)p_sem),
                     p_tcb,
                     ts);
        nbr_tasks++;
        if (opt != OS_OPT_PEND_ABORT_ALL) {                     /* Pend abort all tasks waiting?                        */
            break;                                              /* No                                                   */
        }
    }
    OS_CRITICAL_EXIT_NO_SCHED();

    if ((opt & OS_OPT_POST_NO_SCHED) == 0u) {
        OSSched();                                              /* Run the scheduler                                    */
    }

   *p_err = OS_ERR_NONE;
    return (nbr_tasks);
}
コード例 #8
0
OS_OBJ_QTY  OSSemDel (OS_SEM  *p_sem,
                      OS_OPT   opt,
                      OS_ERR  *p_err)
{
    OS_OBJ_QTY     nbr_tasks;
    OS_PEND_DATA  *p_pend_data;
    OS_PEND_LIST  *p_pend_list;
    OS_TCB        *p_tcb;
    CPU_TS         ts;
    CPU_SR_ALLOC();



#ifdef OS_SAFETY_CRITICAL
    if (p_err == DEF_NULL) {
        OS_SAFETY_CRITICAL_EXCEPTION();
        return (0u);
    }
#endif

#ifdef OS_SAFETY_CRITICAL_IEC61508
    if (OSSafetyCriticalStartFlag == DEF_TRUE) {
       *p_err = OS_ERR_ILLEGAL_DEL_RUN_TIME;
        return (0u);
    }
#endif

#if (OS_CFG_CALLED_FROM_ISR_CHK_EN == DEF_ENABLED)
    if (OSIntNestingCtr > 0u) {                                 /* Not allowed to delete a semaphore from an ISR        */
       *p_err = OS_ERR_DEL_ISR;
        return (0u);
    }
#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 (0u);
    }
#endif

#if (OS_CFG_ARG_CHK_EN == DEF_ENABLED)
    if (p_sem == DEF_NULL) {                                    /* Validate 'p_sem'                                     */
       *p_err = OS_ERR_OBJ_PTR_NULL;
        return (0u);
    }
#endif

#if (OS_CFG_OBJ_TYPE_CHK_EN == DEF_ENABLED)
    if (p_sem->Type != OS_OBJ_TYPE_SEM) {                       /* Make sure semaphore was created                      */
       *p_err = OS_ERR_OBJ_TYPE;
        return (0u);
    }
#endif

    CPU_CRITICAL_ENTER();
    p_pend_list = &p_sem->PendList;
    nbr_tasks   = 0u;
    switch (opt) {
        case OS_OPT_DEL_NO_PEND:                                /* Delete semaphore only if no task waiting             */
             if (p_pend_list->HeadPtr == DEF_NULL) {
#if (OS_CFG_DBG_EN == DEF_ENABLED)
                 OS_SemDbgListRemove(p_sem);
                 OSSemQty--;
#endif
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN == DEF_ENABLED))
                 TRACE_OS_SEM_DEL(p_sem);                       /* Record the event.                                    */
#endif
                 OS_SemClr(p_sem);
                 CPU_CRITICAL_EXIT();
                *p_err = OS_ERR_NONE;
             } else {
                 CPU_CRITICAL_EXIT();
                *p_err = OS_ERR_TASK_WAITING;
             }
             break;

        case OS_OPT_DEL_ALWAYS:                                 /* Always delete the semaphore                          */
             OS_CRITICAL_ENTER_CPU_EXIT();
#if (OS_CFG_TS_EN == DEF_ENABLED)
             ts = OS_TS_GET();                                  /* Get local time stamp so all tasks get the same time  */
#else
             ts = 0u;
#endif
             while (p_pend_list->HeadPtr != DEF_NULL) {         /* Remove all tasks on the pend list                    */
                 p_pend_data = p_pend_list->HeadPtr;
                 p_tcb       = p_pend_data->TCBPtr;
                 OS_PendObjDel((OS_PEND_OBJ *)((void *)p_sem),
                               p_tcb,
                               ts);
                 nbr_tasks++;
             }
#if (OS_CFG_DBG_EN == DEF_ENABLED)
             OS_SemDbgListRemove(p_sem);
             OSSemQty--;
#endif
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN == DEF_ENABLED))
             TRACE_OS_SEM_DEL(p_sem);                           /* Record the event.                                    */
#endif
             OS_SemClr(p_sem);
             OS_CRITICAL_EXIT_NO_SCHED();
             OSSched();                                         /* Find highest priority task ready to run              */
            *p_err = OS_ERR_NONE;
             break;

        default:
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_OPT_INVALID;
             break;
    }
    return (nbr_tasks);
}
コード例 #9
0
ファイル: os_time.c プロジェクト: diqiuche/vineyard
void  OSTimeDlyResume (OS_TCB  *p_tcb,
                       OS_ERR  *p_err)
{
    CPU_SR_ALLOC();



#ifdef OS_SAFETY_CRITICAL
    if (p_err == (OS_ERR *)0) {
        OS_SAFETY_CRITICAL_EXCEPTION();
        return;
    }
#endif

#if OS_CFG_CALLED_FROM_ISR_CHK_EN > 0u
    if (OSIntNestingCtr > (OS_NESTING_CTR)0u) {             /* Not allowed to call from an ISR                        */
       *p_err = OS_ERR_TIME_DLY_RESUME_ISR;
        return;
    }
#endif

#if OS_CFG_ARG_CHK_EN > 0u
    if (p_tcb == (OS_TCB *)0) {                             /* Not possible for the running task to be delayed!       */
       *p_err = OS_ERR_TASK_NOT_DLY;
        return;
    }
#endif

    CPU_CRITICAL_ENTER();
    if (p_tcb == OSTCBCurPtr) {                             /* Not possible for the running task to be delayed!       */
       *p_err = OS_ERR_TASK_NOT_DLY;
        CPU_CRITICAL_EXIT();
        return;
    }

    switch (p_tcb->TaskState) {
        case OS_TASK_STATE_RDY:                             /* Cannot Abort delay if task is ready                    */
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_TASK_NOT_DLY;
             break;

        case OS_TASK_STATE_DLY:
             OS_CRITICAL_ENTER_CPU_EXIT();
             p_tcb->TaskState = OS_TASK_STATE_RDY;
             OS_TickListRemove(p_tcb);                      /* Remove task from tick list                             */
             OS_RdyListInsert(p_tcb);                       /* Add to ready list                                      */
             OS_CRITICAL_EXIT_NO_SCHED();
            *p_err = OS_ERR_NONE;
             break;

        case OS_TASK_STATE_PEND:
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_TASK_NOT_DLY;
             break;

        case OS_TASK_STATE_PEND_TIMEOUT:
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_TASK_NOT_DLY;
             break;

        case OS_TASK_STATE_SUSPENDED:
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_TASK_NOT_DLY;
             break;

        case OS_TASK_STATE_DLY_SUSPENDED:
             OS_CRITICAL_ENTER_CPU_EXIT();
             p_tcb->TaskState = OS_TASK_STATE_SUSPENDED;
             OS_TickListRemove(p_tcb);                      /* Remove task from tick list                             */
             OS_CRITICAL_EXIT_NO_SCHED();
            *p_err            = OS_ERR_TASK_SUSPENDED;
             break;

        case OS_TASK_STATE_PEND_SUSPENDED:
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_TASK_NOT_DLY;
             break;

        case OS_TASK_STATE_PEND_TIMEOUT_SUSPENDED:
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_TASK_NOT_DLY;
             break;

        default:
             CPU_CRITICAL_EXIT();
            *p_err = OS_ERR_STATE_INVALID;
             break;
    }

    OSSched();
}
コード例 #10
0
ファイル: os_mutex.c プロジェクト: wugsh/wgs
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;
}
コード例 #11
0
ファイル: os_mutex.c プロジェクト: wugsh/wgs
void  OSMutexPend (OS_MUTEX  *p_mutex,
                   OS_TICK    timeout,
                   OS_OPT     opt,
                   CPU_TS    *p_ts,
                   OS_ERR    *p_err)
{
    OS_PEND_DATA  pend_data;
    OS_TCB       *p_tcb;
    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_PEND_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_PEND_FAILED(p_mutex);                /* Record the event.                                      */
#endif
       *p_err = OS_ERR_PEND_ISR;
        return;
    }
#endif

#if OS_CFG_ARG_CHK_EN > 0u
    if (p_mutex == (OS_MUTEX *)0) {                         /* Validate arguments                                     */
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_MUTEX_PEND_FAILED(p_mutex);                /* Record the event.                                      */
#endif
       *p_err = OS_ERR_OBJ_PTR_NULL;
        return;
    }
    switch (opt) {                                          /* Validate 'opt'                                         */
        case OS_OPT_PEND_BLOCKING:
        case OS_OPT_PEND_NON_BLOCKING:
             break;

        default:
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_MUTEX_PEND_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_PEND_FAILED(p_mutex);                /* Record the event.                                      */
#endif
       *p_err = OS_ERR_OBJ_TYPE;
        return;
    }
#endif

    if (p_ts != (CPU_TS *)0) {
       *p_ts  = (CPU_TS  )0;                                /* Initialize the returned timestamp                      */
    }

    CPU_CRITICAL_ENTER();
    if (p_mutex->OwnerNestingCtr == (OS_NESTING_CTR)0) {    /* Resource available?                                    */
        p_mutex->OwnerTCBPtr       =  OSTCBCurPtr;          /* Yes, caller may proceed                                */
        p_mutex->OwnerOriginalPrio =  OSTCBCurPtr->Prio;
        p_mutex->OwnerNestingCtr   = (OS_NESTING_CTR)1;
        if (p_ts != (CPU_TS *)0) {
           *p_ts  = p_mutex->TS;
        }
        CPU_CRITICAL_EXIT();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_MUTEX_PEND(p_mutex);                       /* Record the event.                                      */
#endif
       *p_err = OS_ERR_NONE;
        return;
    }

    if (OSTCBCurPtr == p_mutex->OwnerTCBPtr) {              /* See if current task is already the owner of the mutex  */
        p_mutex->OwnerNestingCtr++;
        if (p_ts != (CPU_TS *)0) {
           *p_ts  = p_mutex->TS;
        }
        CPU_CRITICAL_EXIT();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_MUTEX_PEND_FAILED(p_mutex);                /* Record the event.                                      */
#endif
       *p_err = OS_ERR_MUTEX_OWNER;                         /* Indicate that current task already owns the mutex      */
        return;
    }

    if ((opt & OS_OPT_PEND_NON_BLOCKING) != (OS_OPT)0) {    /* Caller wants to block if not available?                */
        CPU_CRITICAL_EXIT();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
        TRACE_OS_MUTEX_PEND_FAILED(p_mutex);                /* Record the event.                                      */
#endif
       *p_err = OS_ERR_PEND_WOULD_BLOCK;                    /* No                                                     */
        return;
    } else {
        if (OSSchedLockNestingCtr > (OS_NESTING_CTR)0) {    /* Can't pend when the scheduler is locked                */
            CPU_CRITICAL_EXIT();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
            TRACE_OS_MUTEX_PEND_FAILED(p_mutex);            /* Record the event.                                      */
#endif
           *p_err = OS_ERR_SCHED_LOCKED;
            return;
        }
    }
                                                            /* Lock the scheduler/re-enable interrupts                */
    OS_CRITICAL_ENTER_CPU_EXIT();
    p_tcb = p_mutex->OwnerTCBPtr;                           /* Point to the TCB of the Mutex owner                    */
    if (p_tcb->Prio > OSTCBCurPtr->Prio) {                  /* See if mutex owner has a lower priority than current   */
        switch (p_tcb->TaskState) {
            case OS_TASK_STATE_RDY:
                 OS_RdyListRemove(p_tcb);                   /* Remove from ready list at current priority             */
                 p_tcb->Prio = OSTCBCurPtr->Prio;           /* Raise owner's priority                                 */
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
                 TRACE_OS_MUTEX_TASK_PRIO_INHERIT(p_tcb, p_tcb->Prio);
#endif
                 OS_PrioInsert(p_tcb->Prio);
                 OS_RdyListInsertHead(p_tcb);               /* Insert in ready list at new priority                   */
                 break;

            case OS_TASK_STATE_DLY:
            case OS_TASK_STATE_DLY_SUSPENDED:
            case OS_TASK_STATE_SUSPENDED:
                 p_tcb->Prio = OSTCBCurPtr->Prio;           /* Only need to raise the owner's priority                */
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
                 TRACE_OS_MUTEX_TASK_PRIO_INHERIT(p_tcb, p_tcb->Prio);
#endif
                 break;

            case OS_TASK_STATE_PEND:                        /* Change the position of the task in the wait list       */
            case OS_TASK_STATE_PEND_TIMEOUT:
            case OS_TASK_STATE_PEND_SUSPENDED:
            case OS_TASK_STATE_PEND_TIMEOUT_SUSPENDED:
                 OS_PendListChangePrio(p_tcb,
                                       OSTCBCurPtr->Prio);
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
                 TRACE_OS_MUTEX_TASK_PRIO_INHERIT(p_tcb, p_tcb->Prio);
#endif
                 break;

            default:
                 OS_CRITICAL_EXIT();
                *p_err = OS_ERR_STATE_INVALID;
                 return;
        }
    }

    OS_Pend(&pend_data,                                     /* Block task pending on Mutex                            */
            (OS_PEND_OBJ *)((void *)p_mutex),
             OS_TASK_PEND_ON_MUTEX,
             timeout);

    OS_CRITICAL_EXIT_NO_SCHED();
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
    TRACE_OS_MUTEX_PEND_BLOCK(p_mutex);                     /* Record the event.                                      */
#endif
    OSSched();                                              /* Find the next highest priority task ready to run       */

    CPU_CRITICAL_ENTER();
    switch (OSTCBCurPtr->PendStatus) {
        case OS_STATUS_PEND_OK:                             /* We got the mutex                                       */
             if (p_ts != (CPU_TS *)0) {
                *p_ts  = OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_MUTEX_PEND(p_mutex);                  /* Record the event.                                      */
#endif
            *p_err = OS_ERR_NONE;
             break;

        case OS_STATUS_PEND_ABORT:                          /* Indicate that we aborted                               */
             if (p_ts != (CPU_TS *)0) {
                *p_ts  = OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_MUTEX_PEND_FAILED(p_mutex);           /* Record the event.                                      */
#endif
            *p_err = OS_ERR_PEND_ABORT;
             break;

        case OS_STATUS_PEND_TIMEOUT:                        /* Indicate that we didn't get mutex within timeout       */
             if (p_ts != (CPU_TS *)0) {
                *p_ts  = (CPU_TS  )0;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_MUTEX_PEND_FAILED(p_mutex);           /* Record the event.                                      */
#endif
            *p_err = OS_ERR_TIMEOUT;
             break;

        case OS_STATUS_PEND_DEL:                            /* Indicate that object pended on has been deleted        */
             if (p_ts != (CPU_TS *)0) {
                *p_ts  = OSTCBCurPtr->TS;
             }
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_MUTEX_PEND_FAILED(p_mutex);           /* Record the event.                                      */
#endif
            *p_err = OS_ERR_OBJ_DEL;
             break;

        default:
#if (defined(TRACE_CFG_EN) && (TRACE_CFG_EN > 0u))
             TRACE_OS_MUTEX_PEND_FAILED(p_mutex);           /* Record the event.                                      */
#endif
            *p_err = OS_ERR_STATUS_INVALID;
             break;
    }
    CPU_CRITICAL_EXIT();
}