noknl3s.c

NORTi3 is a realtime multitasking operating system conforming to the micro-ITRON 3.0 specification.

#ifdef TSLP_TSK
/************************************************/
/* Sleep Task with Timeout                      */
/************************************************/

ER v3_tslp_tsk(TMO tmout)
{
    T_TCB NEAR *tcb;
    ID tskid;
    INT wup;

  #if ERC
    if ((INEST)||(DDISP)||(RDQ[0] == (UB)(CFG.tskid_max+1)))
        return SYSER = E_CTX;
  #endif
    tskid = RDQ[0];
    tcb = &TCB[tskid];
    dis_ims();
    wup = tcb->wup;
    if (wup == 0)
    {   if (tmout != TMO_POL)
        {   if (tmout != TMO_FEVR)
            {   add_tmq(tmout, tcb, tskid);
                tcb->sts = (UB)(S_SLP|S_TMO);
            }
            else
                tcb->sts = S_SLP;
            return del_que_dispatch1(tcb, tskid);
        }
        ret_ims2();
        return E_TMOUT;
    }
    tcb->wup = (B)(wup - 1);
    return_ret_ims();
}

#endif
#ifdef WUP_TSK
/************************************************/
/* Wakeup Task                                  */
/************************************************/

ER wup_tsk(ID tskid)
{
    T_TCB NEAR *tcb;
    INT sts;

  #if ERC
    if (tskid == TSK_SELF)
        return SYSER = E_OBJ;
    else if (tskid > CFG.tskid_max)
        return SYSER = E_ID;
    if (!INEST && RDQ[0] == (UB)tskid)
        return SYSER = E_OBJ;
  #endif
    dis_ims();
    tcb = &TCB[tskid];
    if (tcb->ctsk == NULL)
    {   ret_ims2();
        return E_NOEXS;
    }
    sts = tcb->sts;
    if (sts == (S_SLP|S_TMO))
    {   tcb->sts = S_RDY;
        del_tmq(tcb, tskid);
        if (tcb->sus == 0)
            return add_rdq_dispatch(tcb, tskid);
        else
            return_ret_ims();
    }
    if (sts == S_SLP)
    {   tcb->sts = S_RDY;
        if (tcb->sus == 0)
            return add_rdq_dispatch(tcb, tskid);
        else
            return_ret_ims();
    }
    if (sts != S_DMT)
    {   if (++tcb->wup != (B)(WUPCNT_MAX+1))
            return_ret_ims();
        tcb->wup = (B)WUPCNT_MAX;
        ret_ims2();
        return E_QOVR;
    }
    ret_ims2();
    return E_OBJ;
}

#endif
#ifdef CAN_WUP
/************************************************/
/* Cancel Wakeup Task                           */
/************************************************/

ER v3_can_wup(INT *p_wupcnt, ID tskid)
{
    T_TCB NEAR *tcb;

    if (tskid == TSK_SELF)
    {   tskid = RDQ[0];
        tcb = &TCB[tskid];
        dis_ims();
        *p_wupcnt = (UB)tcb->wup;
        tcb->wup = 0;
        return_ret_ims();
    }
  #if ERC
    if (tskid > CFG.tskid_max)
        return SYSER = E_ID;
  #endif
    dis_ims();
    tcb = &TCB[tskid];
    if (tcb->ctsk == NULL)
    {   ret_ims2();
        return E_NOEXS;
    }
    *p_wupcnt = (UB)tcb->wup;
    tcb->wup = 0;
    return_ret_ims();
}

#endif
#ifdef VCAN_WUP
/************************************************/
/* Cancel Wakeup Task Itself                    */
/************************************************/

void v3_vcan_wup(void)
{

    TCB[RDQ[0]].wup = 0;
}

#endif
#ifdef SIG_SEM
/************************************************/
/* Signal Semaphore                             */
/************************************************/

ER v3_sig_sem(ID semid)
{
    T_SEM NEAR *sem;
    T_TCB NEAR *tcb;
    ID tskid;

  #if ERC
    if (semid == 0)
        return SYSER = E_ID;
    else if (semid > CFG.semid_max)
        return SYSER = E_ID;
  #endif
    dis_ims();
    sem = &SEM[semid];
    if (sem->cnt == 0)
    {   tskid = sem->que[0];
        if (tskid != 0)
        {   tcb = &TCB[tskid];
            if (tcb->sts & S_TMO)
                del_tmq(tcb, tskid);
            tcb->sts = S_RDY;
            if (tcb->sus == 0)
                return chg_rdq_dispatch(tcb, tskid);
            else
                return del_que_ret_ims(tcb, tskid);
        }
    }
    if (sem->cnt++ == (B)SEMCNT_MAX)
    {   sem->cnt = (B)SEMCNT_MAX;
        ret_ims2();
        return E_QOVR;
    }
    return_ret_ims();
}

#endif
#ifdef TWAI_SEM
/************************************************/
/* Wait on Semaphore with Timeout               */
/************************************************/

ER v3_twai_sem(ID semid, TMO tmout)
{
    T_SEM NEAR *sem;
    T_TCB NEAR *tcb;
    ID tskid;

  #if ERC
    if (semid == 0)
        return SYSER = E_ID;
    else if (semid > CFG.semid_max)
        return SYSER = E_ID;
  #endif
    dis_ims();
    sem = &SEM[semid];
    if (sem->cnt == 0)
    {
        if (tmout != TMO_POL)
        {
          #if ERC
            if ((INEST)||(DDISP)||(RDQ[0] == (UB)(CFG.tskid_max+1)))
            {   ret_ims2();
                return SYSER = E_CTX;
            }
          #endif
            tskid = RDQ[0];
            tcb = &TCB[tskid];
            if (tmout != TMO_FEVR)
            {   add_tmq(tmout, tcb, tskid);
                tcb->sts = (UB)(S_SEM|S_TMO);
            }
            else
                tcb->sts = S_SEM;
            return chg_que_dispatch1(sem->que, tcb, tskid);
        }
        ret_ims2();
        return E_TMOUT;
    }
    sem->cnt--;
    return_ret_ims();
}

#endif
#ifdef REF_SEM
/************************************************/
/* Refer Semaphore Status                       */
/************************************************/

ER v3_ref_sem(T_RSEM *pk_rsem, ID semid)
{
    T_SEM NEAR *sem;
    ID tskid;

  #if ERC
    if (semid == 0)
        return SYSER = E_ID;
    else
  #endif
    if (semid > CFG.semid_max)
        return E_ID;
    dis_ims();
    sem = &SEM[semid];
    pk_rsem->exinf = 0;
    pk_rsem->semcnt = (INT)((UB)sem->cnt);
    if (sem->cnt == 0)
    {   tskid = sem->que[0];
        if (tskid != 0)
        {   pk_rsem->wtsk = tskid;
            return_ret_ims();
        }
    }
    pk_rsem->wtsk = FALSE;
    return_ret_ims();
}

#endif
#ifdef SET_FLG
/************************************************/
/* Set EventFlag                                */
/************************************************/

static ER NEAR set_flg_sub(T_FLG NEAR *flg, UINT setptn)
{
    T_TCB NEAR *tcb;
    ID tskid, nxt, end;
    INT sts, pri, disp;
    UINT waiptn;

    setptn |= flg->ptn;
    nxt = flg->que;
    tcb = &TCB[nxt];
    end = tcb->pid;
    disp = FALSE;

    for (;;)
    {   tskid = nxt;
        tcb = &TCB[tskid];
        nxt = tcb->nid;
        sts = tcb->sts;
        if (!(sts & S_ORW))
        {   waiptn = (UINT)tcb->sp->ptn;
            if ((UINT)(waiptn & setptn) == waiptn)
            {
            L2: *(UINT *)(tcb->sp->ptr) = setptn;
                if (sts & S_TMO)
                    del_tmq(tcb, tskid);
                tcb->sts = S_RDY;
                if (tcb->sus == 0)
                {   pri = tcb->pri;
                    if (pri < NOWPRI)
                        disp = TRUE;
                    chg_que((UB NEAR *)RDQ + pri, tcb, tskid);
                }
                else
                    del_que(tcb, tskid);
                if (sts & S_CLR)
                {   flg->ptn = 0;
                    if (disp)
                        return dispatch();
                    return_ret_ims();
                }
            }
        }
        else
        {   if (tcb->sp->ptn & setptn)
                goto L2;
        }
        if (tskid == end)
        {   flg->ptn = setptn;
            if (disp)
                return dispatch();
            return_ret_ims();
        }
    }
}

ER v3_set_flg(ID flgid, UINT setptn)
{
    T_FLG NEAR *flg;
    T_TCB NEAR *tcb;
    ID tskid;
    int sts;
    UINT waiptn;

  #if ERC
    if (flgid == 0)
        return SYSER = E_ID;
    else if (flgid > CFG.flgid_max)
        return SYSER = E_ID;
  #endif
    dis_ims();
    flg = &FLG[flgid];
    tskid = flg->que;
    if (tskid != 0)
    {   tcb = &TCB[tskid];
        if (tcb->pid == (UB)tskid)
        {   sts = tcb->sts;
            if (!(sts & S_ORW))
            {   waiptn = (UINT)tcb->sp->ptn;
                setptn |= flg->ptn;
                if ((UINT)(waiptn & setptn) == waiptn)
                {
                L1: *(UINT *)(tcb->sp->ptr) = setptn;
                    if (sts & S_CLR)
                        setptn = 0;
                    flg->ptn = setptn;
                    if (sts & S_TMO)
                        del_tmq(tcb, tskid);
                    tcb->sts = S_RDY;
                    if (tcb->sus == 0)
                        return chg_rdq_dispatch(tcb, tskid);
                    else
                        return del_que_ret_ims(tcb, tskid);
                }
            }
            else
            {   setptn |= flg->ptn;
                if (tcb->sp->ptn & setptn)
                    goto L1;
            }
            flg->ptn = setptn;
            return_ret_ims();
        }
        return set_flg_sub(flg, setptn);
    }
    flg->ptn |= setptn;
    return_ret_ims();
}

#endif
#ifdef CLR_FLG
/************************************************/
/* Clear EventFlag                              */
/************************************************/

ER v3_clr_flg(ID flgid, UINT clrptn)
{
    T_FLG NEAR *flg;

  #if ERC
    if (flgid == 0)
        return SYSER = E_ID;
    else if (flgid > CFG.flgid_max)
        return SYSER = E_ID;
  #endif
    dis_ims();
    flg = &FLG[flgid];
    flg->ptn &= clrptn;
    return_ret_ims();
}

#endif
#ifdef TWAI_FLG
/************************************************/
/* Wait EventFlag with Timeout                  */
/************************************************/

ER v3_twai_flg(UINT *p_flgptn, ID flgid, UINT waiptn, UINT wfmode, TMO tmout)
{
    T_FLG NEAR *flg;
    T_TCB NEAR *tcb;
    ID tskid;
    UINT ptn;

  #if ERC
    if (flgid == 0)
        return SYSER = E_ID;
    else if (flgid > CFG.flgid_max)
        return SYSER = E_ID;
    if ((waiptn == 0)
     || (wfmode & ~(S_ORW|S_CLR)))
        return SYSER = E_PAR;
    if (tmout < (-2))
        return SYSER = E_PAR;
  #endif
    dis_ims();
    flg = &FLG[flgid];
    ptn = flg->ptn;
    if (!(wfmode & S_ORW))
    {   if ((UINT)(ptn & waiptn) != waiptn)
        {
    L1:
            if (tmout != TMO_POL)
            {
              #if ERC
                if ((INEST)||(DDISP)||(RDQ[0] == (UB)(CFG.tskid_max+1)))
                {   ret_ims2();
                    return SYSER = E_CTX;
                }
              #endif
                tskid = RDQ[0];
                tcb = &TCB[tskid];
                if (tmout != TMO_FEVR)
                {   add_tmq(tmout, tcb, tskid);
                    tcb->sts = (UB)(wfmode | (S_FLG|S_TMO));
                }
                else
                    tcb->sts = (UB)(wfmode | S_FLG);
                CTXPTN = waiptn;
                CTXPTR = p_flgptn;
                return chg_que_dispatch1(&flg->que, tcb, tskid);
            }
            ret_ims2();
            return E_TMOUT;
        }
    }
    else
    {   if ((ptn & waiptn) == 0)
            goto L1;
    }
    *p_flgptn = ptn;
    if (wfmode & S_CLR)
        flg->ptn = 0;
    return_ret_ims();
}

#endif
#ifdef REF_FLG
/************************************************/
/* Refer EventFlag Status                       */
/************************************************/

ER v3_ref_flg(T_RFLG *pk_rflg, ID flgid)
{
    T_FLG NEAR *flg;

  #if ERC
    if (flgid == 0)
        return SYSER = E_ID;
    else
  #endif
    if (flgid > CFG.flgid_max)
        return E_ID;
    dis_ims();
    flg = &FLG[flgid];
    pk_rflg->exinf = 0;
    pk_rflg->flgptn = flg->ptn;
    pk_rflg->wtsk = flg->que;
    return_ret_ims();
}

#endif
#ifdef SND_MSG
/************************************************/
/* Send Message to Mailbox                      */
/************************************************/

ER v3_snd_msg(ID mbxid, T_MSG *pk_msg)
{
    T_MBX NEAR *mbx;
    T_TCB NEAR *tcb;
    ID tskid;

  #if ERC
    if (mbxid == 0)
        return SYSER = E_ID;
    else if (mbxid > CFG.mbxid_max)
        return SYSER = E_ID;
  #endif
    dis_ims();
    mbx = &MBX[mbxid];
    if (mbx->top == NULL)
    {   tskid = mbx->que[0];
        if (tskid != 0)
        {   tcb = &TCB[tskid];
            *(T_MSG **)tcb->sp->ptr = pk_msg;
            if (tcb->sts & S_TMO)
                del_tmq(tcb, tskid);
            tcb->sts = S_RDY;
            if (tcb->sus == 0)
                return chg_rdq_dispatch(tcb, tskid);
            else
                return del_que_ret_ims(tcb, tskid);
        }
        mbx->top = pk_msg;
    }
    else
        mbx->end->next = pk_msg;
    mbx->end = pk_msg;
    pk_msg->next = NULL;
    return_ret_ims();
}

#endif
#ifdef TRCV_MSG
/************************************************/
/* Receive Message from Mailbox with Timeout    */
/************************************************/

ER v3_trcv_msg(T_MSG **ppk_msg, ID mbxid, TMO tmout)
{
    T_MBX NEAR *mbx;
    T_TCB NEAR *tcb;
    ID tskid;
    T_MSG *top;