noknl3s.c

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

  #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)
    {   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_MBX|S_TMO);
            }
            else
                tcb->sts = S_MBX;
            CTXPTR = ppk_msg;
            return chg_que_dispatch1(mbx->que, tcb, tskid);
        }
        ret_ims2();
        return E_TMOUT;
    }
    top = mbx->top;
    mbx->top = top->next;
    *ppk_msg = top;
    return_ret_ims();
}

#endif
#ifdef REF_MBX
/************************************************/
/* Refer Mailbox Status                         */
/************************************************/

ER v3_ref_mbx(T_RMBX *pk_rmbx, ID mbxid)
{
    T_MBX NEAR *mbx;
    ID tskid;

  #if ERC
    if (mbxid == 0)
        return SYSER = E_ID;
    else
  #endif
    if (mbxid > CFG.mbxid_max)
        return E_ID;
    dis_ims();
    mbx = &MBX[mbxid];
    pk_rmbx->exinf = 0;
    if (mbx->top == NULL)
    {   pk_rmbx->pk_msg = NADR;
        tskid = mbx->que[0];
        if (tskid != 0)
        {   pk_rmbx->wtsk = tskid;
            return_ret_ims();
        }
    }
    else
        pk_rmbx->pk_msg = mbx->top;
    pk_rmbx->wtsk = FALSE;
    return_ret_ims();
}

#endif
#ifdef CRE_MBF
/************************************************/
/* Create MessageBuffer                         */
/************************************************/

ER v3_cre_mbf(ID mbfid, const T_CMBF *pk_cmbf)
{
    T_MBF NEAR *mbf;
    B *buf;
    unsigned bufsz;

  #if ERC
    if (INEST)
        return SYSER = E_CTX;
    if (mbfid == 0)
        return SYSER = E_ID;
    else if (mbfid > CFG.mbfid_max)
        return SYSER = E_ID;
  #endif
    dis_ims();
    mbf = &MBF[mbfid];
    if (mbf->buf != NULL)
    {   ret_ims2();
        return E_OBJ;
    }
    if ((bufsz = pk_cmbf->bufsz) != 0)
    {   buf = mpl_alloc((size_t)bufsz);
        if (buf == NULL)
        {   ret_ims2();
            return SYSER = E_NOMEM;
        }
    }
    mbf->buf = (bufsz == 0)?NADR:buf;
    mbf->putp = 0;
    mbf->getp = 0;
    mbf->allsz = bufsz;
    mbf->frsz =  bufsz;
    mbf->maxsz = pk_cmbf->maxmsz;
    mbf->wtsk[0] = 0;
    mbf->stsk = 0;

    return_ret_ims();
}

#endif
#ifdef TSND_MBF
/************************************************/
/* Send Message to MessageBuffer with Timeout   */
/************************************************/

ER v3_tsnd_mbf(ID mbfid, VP msg, int msgsz, TMO tmout)
{
    T_MBF NEAR *mbf;
    T_TCB NEAR *tcb;
    ID tskid;

  #if ERC
    if (mbfid == 0)
        return SYSER = E_ID;
    else if (mbfid > CFG.mbfid_max)
        return SYSER = E_ID;
    if (msgsz == 0)
        return SYSER = E_PAR;
  #endif
    dis_ims();
    mbf = &MBF[mbfid];
    if (mbf->buf == NULL)
    {   ret_ims2();
        return E_NOEXS;
    }
    if ((unsigned)msgsz > mbf->maxsz)
    {   ret_ims2();
        return E_PAR;
    }
    tskid = mbf->wtsk[0];
    if (tskid != 0)
    {
        tcb = &TCB[tskid];
        osmem_cpy((B *)((T_MBFCTX *)tcb->sp->ptr)->buf, (B *)msg, msgsz);
        *((T_MBFCTX *)tcb->sp->ptr)->p_size = msgsz;
        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 (mbf->stsk == 0)
    {   if (msg_2_mbf(mbf, msg, msgsz))
            return_ret_ims();
    }

    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_SMB|S_TMO);
        }
        else
            tcb->sts = S_SMB;
        CTXPTR = msg;
        CTXPTN = (UINT)msgsz;
        return chg_que_dispatch1(&mbf->stsk, tcb, tskid);
    }
    ret_ims2();
    return E_TMOUT;
}

#endif
#ifdef TRCV_MBF
/****************************************************/
/* Receive Message from MessageBuffer with Timeout  */
/****************************************************/

ER v3_trcv_mbf(VP msg, int *p_msgsz, ID mbfid, TMO tmout)
{
    T_MBF NEAR *mbf;
    T_TCB NEAR *tcb;
    T_MBFCTX mbfctx;
    ID tskid;
    UB NEAR *que;
    BOOL disp;
    unsigned sndsz;
    INT pri;

  #if ERC
    if (mbfid == 0)
        return SYSER = E_ID;
    else if (mbfid > CFG.mbfid_max)
        return SYSER = E_ID;
  #endif
    dis_ims();
    mbf = &MBF[mbfid];
    if (mbf->buf == NULL)
    {   ret_ims2();
        return E_NOEXS;
    }

    if (mbf->allsz == mbf->frsz)
    {
        if ((tskid = mbf->stsk) != 0)
        {   tcb = &TCB[tskid];
            sndsz = tcb->sp->ptn;
            *p_msgsz = sndsz;
            osmem_cpy((B *)msg, (B *)tcb->sp->ptr, sndsz);
            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 (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_MBF|S_TMO);
            }
            else
                tcb->sts = S_MBF;
            mbfctx.buf = msg;
            mbfctx.p_size = p_msgsz;
            CTXPTR = &mbfctx;
            return chg_que_dispatch1(mbf->wtsk, tcb, tskid);
        }
        ret_ims2();
        return E_TMOUT;
    }

    *p_msgsz = (int)mbf_2_msg(mbf, msg);

    disp = FALSE;
    que = &mbf->stsk;
    tskid = *que;
    for(;;)
    {   tskid = *que;
        if (tskid == 0)
            break;
        tcb = &TCB[tskid];
        sndsz = tcb->sp->ptn;
        if (!msg_2_mbf(mbf, tcb->sp->ptr, sndsz))
            break;
        if (tcb->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 (disp == TRUE)
        return dispatch();
    return_ret_ims();
}

#endif
#ifdef REF_MBF
/************************************************/
/* Refer MessageBuffer Status                   */
/************************************************/

ER v3_ref_mbf(T_RMBF *pk_rmbf, ID mbfid)
{
    T_MBF NEAR *mbf;
    ID tskid;
    unsigned i, getp;
    UH msgsz;

  #if ERC
    if (mbfid == 0)
        return SYSER = E_ID;
    else
  #endif
    if (mbfid > CFG.mbfid_max)
        return E_ID;
    dis_ims();
    mbf = &MBF[mbfid];
    if (mbf->buf == NULL)
    {   ret_ims2();
        return E_NOEXS;
    }
    pk_rmbf->exinf = 0;
    tskid = mbf->wtsk[0];
    if (tskid == 0)
        tskid = FALSE;
    pk_rmbf->wtsk = tskid;
    tskid = mbf->stsk;
    if (tskid == 0)
        tskid = FALSE;
    pk_rmbf->stsk = tskid;
    if (mbf->allsz != mbf->frsz)
    {   getp = mbf->getp;
        i = 0;
        do
        {   *((B *)&msgsz + i) = *((B *)mbf->buf + getp);
            if (++getp == mbf->allsz)
                getp = 0;
        } while (++i != sizeof(msgsz));
        pk_rmbf->msgsz = (int)msgsz;
    }
    else if (tskid != FALSE)
        pk_rmbf->msgsz = (TCB[tskid].sp)->ptn;
    else
        pk_rmbf->msgsz = 0;
    pk_rmbf->frbufsz = mbf->frsz;

    return_ret_ims();
}

#endif
#ifdef TSND_MBF
/************************************************/
/* Copy Message to MessageBuffer                */
/************************************************/

BOOL NEAR msg_2_mbf(T_MBF NEAR *mbf, VP msg, unsigned size)
{
    unsigned i;
    UH putp;
    B *frm, *to;
    union {
        UH sz_uh;
        UB sz_ub[2];
    } sz;

    if (mbf->frsz < sizeof(UH) + size)
        return FALSE;
    frm = (B *)msg;
    putp = mbf->putp;
    mbf->frsz -= sizeof(UH) + (sz.sz_uh = (UH)size);
    mbf->buf[putp++] = sz.sz_ub[0];
    if (putp == mbf->allsz)
        putp = 0;
    mbf->buf[putp++] = sz.sz_ub[1];
    if (putp == mbf->allsz)
        putp = 0;
    if (putp + sz.sz_uh >= mbf->allsz)
    {   to = mbf->buf + putp;
        for(sz.sz_uh -= (i = mbf->allsz - putp); i != 0; i--)
            *to++ = *frm++;
        putp = 0;
    }
    to = mbf->buf + putp;
    mbf->putp = putp + sz.sz_uh;
    for(i = sz.sz_uh; i != 0; i--)
        *to++ = *frm++;
    return TRUE;
}

#endif
#ifdef TRCV_MBF
/************************************************/
/* Copy MessageBuffer to Message                */
/************************************************/

unsigned NEAR mbf_2_msg(T_MBF NEAR *mbf, VP msg)
{
    unsigned i, size;
    UH getp;
    B *frm, *to;
    union {
        UH sz_uh;
        UB sz_ub[2];
    } sz;

    to = (B *)msg;
    getp = mbf->getp;
    sz.sz_ub[0] = mbf->buf[getp++];
    if (getp == mbf->allsz)
        getp = 0;
    sz.sz_ub[1] = mbf->buf[getp++];
    if (getp == mbf->allsz)
        getp = 0;
    mbf->frsz += sizeof(UH) + sz.sz_uh;
    size = (unsigned)sz.sz_uh;
    if (getp + sz.sz_uh >= mbf->allsz)
    {   frm = mbf->buf + getp;
        for(sz.sz_uh -= (i = mbf->allsz - getp); i != 0; i--)
            *to++ = *frm++;
        getp = 0;
    }
    frm = mbf->buf + getp;
    mbf->getp = getp + sz.sz_uh;
    for(i = sz.sz_uh; i != 0; i--)
        *to++ = *frm++;
    return size;
}

#endif
#ifdef LOC_CPU
/************************************************/
/* Lock CPU                                     */
/************************************************/

ER v3_loc_cpu(void)
{
  #if ERC
     if ((INEST)||(RDQ[0] == (UB)(CFG.tskid_max+1)))
        return SYSER = E_CTX;
  #endif
    dis_ims();
    DDISP = 1;
    return E_OK;
}

#endif
#ifdef UNL_CPU
/************************************************/
/* Unlock CPU                                   */
/************************************************/

ER v3_unl_cpu(void)
{
  #if ERC
     if ((INEST)||(RDQ[0] == (UB)(CFG.tskid_max+1)))
        return SYSER = E_CTX;
  #endif
    dis_ims();
    unl_cpu_sub();
    DDISP = 0;
    if (DELAY)
        return dispatch2();
    return_ret_ims();
}

#endif
#ifdef CHG_IMS
/************************************************/
/* Change Interrupt Mask                        */
/************************************************/

ER v3_chg_ims(UINT imask)
{
    dis_ims();
    chg_ims_sub(imask);
    if (!imask && !INEST && !DDISP && DELAY)
        return dispatch2();
    return_ret_ims();
}

#endif
#ifdef REF_IMS
/************************************************/
/* Refer Interrupt Mask Status                  */
/************************************************/

ER v3_ref_ims(UINT *p_imask)
{
    UINT ims;

    dis_ims();
    ims = IMASK;
    ret_ims2();
    *p_imask = ref_ims_sub(ims);
    return E_OK;
}

#endif
#ifdef CRE_MPF
/************************************************/
/* Create Fixed-size MemoryPool                 */
/************************************************/

ER v3_cre_mpf(ID mpfid, const T_CMPF *pk_cmpf)
{
    T_MPF NEAR *mpf;
    T_TCB NEAR *tcb;
    B *blf;
    ID tskid;
    int i;
    BOOL disp;
    INT pri;
    unsigned blfsz;
  #if (ERC && SIZEOF_INT <= 2)
    UW allsz;
  #else
    size_t allsz;
  #endif

  #if ERC
    if (INEST)
        return SYSER = E_CTX;
    if (mpfid == 0)
        return SYSER = E_ID;
    else if (mpfid > CFG.mpfid_max)
        return SYSER = E_ID;
    if (pk_cmpf->blfsz == 0)
        return SYSER = E_PAR;
    if (pk_cmpf->mpfcnt == 0)
        return SYSER = E_PAR;
  #endif
  #if (SIZEOF_INT == 4)
    blfsz = (unsigned)((pk_cmpf->blfsz + 3) & ~3);
  #elif (SIZEOF_INT == 2)
    blfsz = (unsigned)((pk_cmpf->blfsz + 1) & ~1);
  #else
    blfsz = (unsigned)pk_cmpf->blfsz;