noknl3s.c

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

/******************************************************************************
* NORTi3 Standard / Kernel                                                    *
*                                                                             *
*  Copyright (c)  1995-2001, MiSPO Co., Ltd.                                  *
*  All rights reserved.                                                       *
*                                                                             *
* 15/Jul/1999 Ver.3.05                                                        *
* 27/Mar/2000 Ver.3.06  corrected chg_pri                                     *
* 03/Apr/2000 Ver.3.07  corrected sysini                                      *
* 28/Sep/2000 Ver.3.08  corrected chg_pri                                     *
* 06/Nov/2000 Ver.3.08  corrected cre_mpf (calculation size for H8)           *
* 23/Feb/2001 Ver.3.09  corrected sig_tim                                     *
******************************************************************************/
#define ALL
#define NOKNL3_C

#include "norti3.h"
#include "nosys3s.h"

#ifdef ALL
#define OS_CORE
#define ACT_CYC
#define CAN_WUP
#define CHG_IMS
#define CHG_PRI
#define CLR_FLG
#define CPOL_1FLG
#define CRE_MBF
#define CRE_MPF
#define CRE_TSK
#define CWAI_1FLG
#define DEF_ALM
#define DEF_CYC
#define DIS_DSP
#define DLY_TSK
#define ENA_DSP
#define EXT_TSK
#define FRSM_TSK
#define GET_TID
#define GET_TIM
#define GET_VER
#define LOC_CPU
#define POL_1FLG
#define REF_ALM
#define REF_CFG
#define REF_CYC
#define REF_FLG
#define REF_IMS
#define REF_MBF
#define REF_MBX
#define REF_MPF
#define REF_SEM
#define REF_SYS
#define REF_TSK
#define REL_BLF
#define REL_WAI
#define ROT_RDQ
#define RSM_TSK
#define SET_FLG
#define SET_TIM
#define SIG_SEM
#define SND_MSG
#define STA_TSK
#define SUS_TSK
#define TER_TSK
#define TGET_BLF
#define TRCV_MBF
#define TRCV_MSG
#define TSLP_TSK
#define TSND_MBF
#define TWAI_1FLG
#define TWAI_FLG
#define TWAI_SEM
#define UNL_CPU
#define VCAN_WUP
#define VGET_TID
#define WUP_TSK
#define V2_ALH_STS
#define V2_CRE_TSK
#define V2_CYH_STS
#define V2_FLG_STS
#define V2_GET_TIM
#define V2_MBX_STS
#define V2_MPL_STS
#define V2_SEM_STS
#define V2_SET_TIM
#define V2_TSK_STS
#define V2_XCRE_TSK
#endif

#ifndef ERC
#define ERC         1           /* error check (0:No, 1:Yes) */
#endif

#ifndef CLKBITS
#define CLKBITS     48
#endif

#ifndef NULL
#if SIZEOF_PTR == 2
#define NULL  ((void *)0)
#else
#define NULL  ((void *)0L)
#endif
#endif

#ifndef NNULL
#if SIZEOF_PTR == 2
#define NNULL ((void *)0)
#else
#define NNULL ((void NEAR *)0)
#endif
#endif

/* kernel C function prototype */

BOOL NEAR msg_2_mbf(T_MBF NEAR *, VP, unsigned);
unsigned NEAR mbf_2_msg(T_MBF NEAR *, VP);
void NEAR chk_tmq(void);
void NEAR chk_cyc(void);
void NEAR chk_alm(void);
void NEAR tim_add(const SYSTIME *, const SYSTIME *, T_CYC NEAR *);
void NEAR tim_dif(const SYSTIME *, T_CYC NEAR *, SYSTIME *);
void NEAR del_cycq(T_CYC NEAR *, ID);
void NEAR del_almq(T_ALM NEAR *, ID);
void NEAR add_cycq(T_CYC NEAR *, ID);
void NEAR add_almq(T_ALM NEAR *, ID);
void NEAR del_tmq(T_TCB NEAR *, ID);
void NEAR add_tmq(TMO, T_TCB NEAR *, ID);
void NEAR del_que(T_TCB NEAR *, ID);
void NEAR add_que(UB NEAR *, T_TCB NEAR *, ID);
void NEAR chg_que(UB NEAR *, T_TCB NEAR *, ID);
void NEAR chg_rdq(T_TCB NEAR *, ID);
ER NEAR del_que_ret_ims(T_TCB NEAR *, ID);
ER NEAR add_rdq_dispatch(T_TCB NEAR *, ID);
ER NEAR chg_rdq_dispatch(T_TCB NEAR *, ID);
ER NEAR del_que_dispatch1(T_TCB NEAR *, ID);
ER NEAR chg_que_dispatch1(UB NEAR *, T_TCB NEAR *, ID);
ER NEAR osmem_init(T_MEM *, size_t);
void * NEAR blk_alloc(T_MEM **, size_t);
#if defined(CPU_86)
ER NEAR osnmem_init(T_NMEM NEAR *, unsigned);
void NEAR * NEAR blk_nalloc(T_NMEM NEAR * NEAR *, unsigned);
#else
#define osnmem_init(m,s)    osmem_init(m,s)
#define blk_nalloc(m,s)     blk_alloc(m,s)
#endif
void NEAR osmem_cpy(B *, B *, unsigned);
void * NEAR stk_alloc(size_t);
void * NEAR mpl_alloc(size_t);

/* kernel ASM function prototype */

#ifndef dispatch
ER NEAR dispatch(void);     /* dispatch with INEST/IMASK check */
ER NEAR dispatch1(void);    /* dispatch without INEST/IMASK check */
ER NEAR dispatch2(void);    /* dispatch for syssta, chg_ims */
void NEAR dispatch3(void);  /* dispatch for ext_tsk, exd_tsk */
#endif
#ifndef dis_ims
void NEAR dis_ims(void);    /* set mask at entry of system call */
#endif
#ifndef return_ret_ims
#define return_ret_ims()    return ret_ims()
#ifndef ret_ims
ER NEAR ret_ims(void);      /* recover mask at exit of system call */
#endif
#endif
#ifndef ret_ims2
void NEAR ret_ims2(void);
#endif
#ifndef ope_ims
void NEAR ope_ims(void);
#endif

/* kernel ASM interface variable */

#if defined(CPU_86)
extern int NEAR cdecl IDS;      /* initial DS */
extern int NEAR cdecl IFLAG;    /* initial FLAG */
#endif


#ifdef OS_CORE
/************************************************/
/* System Initialize                            */
/************************************************/

static void NEAR _fastcall memclr(void NEAR *p, int n)
{
    B NEAR *cp = (B NEAR *)p;
    while (n-- != 0)
        *cp++ = 0;
}

ER NEAR c_sysini(B *stktop, B *sp)
{
    ER ercd;
    int i;

    dis_ims();
    c_sysini_sub();

    STKTOP = (T_MEM *)stktop;
    ercd = osmem_init(STKTOP, (size_t)(sp - stktop));
    if (ercd != E_OK)
        return ercd;
    if ((MPLTOP = (T_MEM *)pMPLMEM) != NULL)
    {   ercd = osmem_init(MPLTOP, CFG.mplmsz);
        if (ercd != E_OK)
            return ercd;
    }
  #if (CLKBITS>32)
    SYSCK.utime = 0;
  #endif
    SYSCK.ltime = 0;
    IDLCNT = 0;
    NOWPRI = 0;
    SYSER = E_OK;
    DELAY = FALSE;
    INEST = 0;
    SDISP = 0;
    TMREQ = 0;
    DDISP = 1;

    for (i = 1; i <= CFG.tskid_max+1; i++)
        memclr((T_TCB NEAR *)&TCB[i], sizeof(T_TCB));

    for (i = 1; i <= CFG.semid_max; i++)
        memclr(&SEM[i], sizeof(T_SEM));

    for (i = 1; i <= CFG.flgid_max; i++)
        memclr(&FLG[i], sizeof(T_FLG));

    for (i = 1; i <= CFG.mbxid_max; i++)
        memclr(&MBX[i], sizeof(T_MBX));

    for (i = 1; i <= CFG.mpfid_max; i++)
        memclr(&MPF[i], sizeof(T_MPF));

    for (i = 1; i <= CFG.mbfid_max; i++)
        memclr(&MBF[i], sizeof(T_MBF));

    for (i = 1; i <= CFG.cycno_max; i++)
        memclr(&CYC[i], sizeof(T_CYC));

    for (i = 1; i <= CFG.almno_max; i++)
        memclr(&ALM[i], sizeof(T_ALM));

    for (i = 1; i <= CFG.tpri_max+1; i++) RDQ[i] = 0;
    for (i = 0; i < CFG.tmrqsz;  i++) TMQ[i] = 0;
    for (i = 0; i < CFG.cycqsz;  i++) CHQ[i] = 0;
    for (i = 0; i < CFG.almqsz;  i++) AHQ[i] = 0;

    TMQMS = (UB)(CFG.tmrqsz - 1);
    CHQMS = (UB)(CFG.cycqsz - 1);
    AHQMS = (UB)(CFG.almqsz - 1);
    i = CFG.tskid_max+1;
    RDQ[0] = (UB)i;
    RDQ[CFG.tpri_max+1] = (UB)i;
    TCB[i].nid = TCB[i].pid = (UB)i;

    return E_OK;
}

#endif
#ifdef OS_CORE
/************************************************/
/* System Start                                 */
/************************************************/

ER syssta(void)
{
    int cnt;

    if (SYSER)
        return SYSER;
    DDISP = 0;
    syssta_sub();

    for (;;)
    {
        TCB[CFG.tskid_max+1].pri = (B)(CFG.tpri_max + 1);
        SDISP = 0;
        dispatch2();

        /* idle task process */

        for (cnt = 0xffff;;)
        {   if (TMREQ)
                break;
            if (cnt != 0)
                cnt--;
        }
        IDLCNT = (UH)(0xffff - cnt);

        /* interval timer process */

        dis_ims();
        do
        {
          #if (CLKBITS>32)
            if (++SYSCK.ltime == 0)
                ++SYSCK.utime;
          #else
            ++SYSCK.ltime;
          #endif
            chk_tmq();
            chk_cyc();
            chk_alm();
        } while (--TMREQ != 0);
    }
}

#endif
#ifdef OS_CORE
/************************************************/
/* Timer Queue Management                       */
/************************************************/

void NEAR chk_tmq(void)
{
    T_TCB NEAR *tcb;
    UB NEAR *tmq;
    ID tskid, end;
    INT sts, pri;
    DLYTIME ltime = (DLYTIME)SYSCK.ltime;

    tmq = &TMQ[(unsigned)ltime & TMQMS];

    for (;;)
    {   tskid = *tmq;
        if (tskid == 0)
            return;
        tcb = &TCB[tskid];
        end = tcb->ptm;
        for (;;)
        {   if (tcb->tmo == ltime)
            {   del_tmq(tcb, tskid);
                sts = tcb->sts;
                tcb->sts = S_RDY;
                if ((sts & 0xf0) == S_DLY)
                    tcb->sp->ercd = E_OK;
                else
                {   tcb->sp->ercd = E_TMOUT;
                    if (sts >= S_FLG)
                        del_que(tcb, tskid);
                }
                if (tcb->sus == 0)
                {   pri = tcb->pri;
                    if (pri < NOWPRI)
                        DELAY = 1;
                    add_que((UB NEAR *)RDQ + pri, tcb, tskid);
                }
                ope_ims();
                break;
            }
            if (tskid == end)
                return;
            tskid = tcb->ntm;
            tcb = &TCB[tskid];
        }
    }
}

#endif
#ifdef OS_CORE
/************************************************/
/* Cyclic Handler Management                    */
/************************************************/

void NEAR chk_cyc(void)
{
    T_CYC NEAR *cyc;
    UB NEAR *chq;
    HNO cycno, end;
    DLYTIME ltime = (DLYTIME)SYSCK.ltime;
  #if (CLKBITS>32)
    H utime = SYSCK.utime;
  #endif

    chq = &CHQ[(unsigned)ltime & CHQMS];

    for (;;)
    {   cycno = *chq;
        if (cycno == 0)
            return;
        cyc = &CYC[cycno];
        end = cyc->ptm;
        for (;;)
        {   if ((ltime == (DLYTIME)cyc->ltime)
          #if (CLKBITS>32)
             && (utime == cyc->utime)
          #endif
               )
            {   del_cycq(cyc, cycno);
                cyc->ltime = cyc->dcyc->cyctim.ltime + ltime;
              #if (CLKBITS>32)
                cyc->utime = cyc->dcyc->cyctim.utime + utime;
                if (cyc->ltime < cyc->dcyc->cyctim.ltime)
                    cyc->utime++;
              #endif
                add_cycq(cyc, cycno);
                ret_ims2();
                if (cyc->cycact & TCY_ON)
                    ((void (FAR *)(void))cyc->dcyc->cychdr)();
                dis_ims();
                break;
            }
            if (cycno == end)
                return;
            cycno = cyc->ntm;
            cyc = &CYC[cycno];
        }
    }
}

#endif
#ifdef OS_CORE
/************************************************/
/* Alarm Handler Management                     */
/************************************************/

void NEAR chk_alm(void)
{
    T_ALM NEAR *alm;
    UB NEAR *ahq;
    HNO almno, end;
    FP almhdr;
    DLYTIME ltime = (DLYTIME)SYSCK.ltime;
  #if (CLKBITS>32)
    H utime = SYSCK.utime;
  #endif

    ahq = &AHQ[(unsigned)ltime & AHQMS];

    for (;;)
    {   almno = *ahq;
        if (almno == 0)
            return;
        alm = &ALM[almno];
        end = alm->ptm;
        for (;;)
        {   if ((ltime == (DLYTIME)alm->ltime)
          #if (CLKBITS>32)
             && (utime == alm->utime)
          #endif
               )
            {   del_almq(alm, almno);
                almhdr = alm->dalm->almhdr;
                alm->dalm = NULL;
                ret_ims2();
                ((void (FAR *)(void))almhdr)();
                dis_ims();
                break;
            }
            if (almno == end)
                return;
            almno = alm->ntm;
            alm = &ALM[almno];
        }
    }
}

#endif
#ifdef CRE_TSK
/************************************************/
/* Create Task                                  */
/************************************************/

ER v3_cre_tsk(ID tskid, const T_CTSK *pk_ctsk)
{
    T_TCB NEAR *tcb;
    B *sp;
    unsigned stksz;
  #if ERC
    int ptn;
    int *p;
  #endif

  #if ERC
    if (INEST)
        return SYSER = E_CTX;
    if (tskid > CFG.tskid_max)
        return SYSER = E_ID;
    if (pk_ctsk->itskpri > CFG.tpri_max)
        return SYSER = E_PAR;
    if (tskid == 0)
        return SYSER = E_ID;
  #endif
    dis_ims();
    tcb = &TCB[tskid];
    if (tcb->ctsk != NULL)
    {   ret_ims2();
        return E_OBJ;
    }
    stksz = pk_ctsk->stksz;
    sp = stk_alloc(stksz);
    if (sp == NULL)
    {   ret_ims2();
        return SYSER = E_NOMEM;
    }
    tcb->sts = 0;
    tcb->wup = 0;
    tcb->sus = 0;
    tcb->ctsk = pk_ctsk;
    tcb->pri = (B)pk_ctsk->itskpri;
    tcb->stk = (T_CTX *)sp;
  #if ERC
    cre_tsk_sub();
  #if (SIZEOF_INT <= 2)
    ptn = (tskid << 8) | tskid;
    stksz /= 2;
  #else
    ptn = (tskid << 24) | (tskid << 16) | (tskid << 8) | tskid;
    stksz /= 4;
  #endif
    p = (int *)sp;
    do { *p++ = ptn; } while (--stksz != 0);
  #endif
    return_ret_ims();
}

#endif
#ifdef STA_TSK
/************************************************/
/* Start Task                                   */
/************************************************/

ER v3_sta_tsk(ID tskid, int stacd)
{
    T_TCB NEAR *tcb;
    T_CTX *ctx;
  #if defined(CPU_86)