task.c

050117 CD 修正分 iTRON hosroot

/* ------------------------------------------------------------------------- */
/*  HOS 僞僗僋娗棟                                                           */
/*                                   Copyright (C) 1998-2002 by Project HOS  */
/* ------------------------------------------------------------------------- */

#include <itron.h>


/* 僌儘乕僶儖曄悢 */
T_TCB *curtcb;			/* 尰嵼偺僞僗僋偺TCB */
BOOL  dlydsp;			/* 抶墑偝傟偰偄傞僨傿僗僷僢僠偑偁傞偐 */


/* 僞僗僋晹偺弶婜壔 */
void __ini_tsk(void)
{
	int i, j;
	
	curtcb = NADR;
	dlydsp = FALSE;
	
	/* 儗僨傿乕僉儏乕偺弶婜壔 */
	for ( i = 0; i < rdqcnt; i++ )
		rdyque[i].tskcnt = 0;
	
	/* 僗僞僢僋僾乕儖偺弶婜壔 */
	for ( i = 0; i < spcbcnt; i++ ) {
		spcbtbl[i].que.tskcnt = 0;
		for ( j = 0; j < spcbstbl[i].stkcnt - 1; j++ )
			spcbstbl[i].stkhead[spcbstbl[i].stksz * j] = (VH)(j + 1);
		spcbstbl[i].stkhead[spcbstbl[i].stksz * j] = 0xffff;
		spcbtbl[i].freestk = 0;
	}
	
	/* TCB僥乕僽儖偺弶婜壔 */
	for ( i = 0; i < tcbcnt; i++ ) {
		if ( tcbstbl[i].itskpri != 0 )
			tcbtbl[i].tskstat = TTS_DMT;
		else 
			tcbtbl[i].tskstat = 0;	/* NON-EXISTENT */
		tcbtbl[i].que = NADR;
	}
}


/* 僞僗僋奐巒 */
ER sta_tsk(ID tskid, INT stacd)
{
	T_TCB *tcb;
	const T_TCBS *tcbs;
	
#if __ERR_CHECK_LEVEL >= 4
	/* ID僠僃僢僋 */
	if ( tskid <= 0 )
		return E_ID;
	if ( tskid > tcbcnt || tcbtbl[tskid - 1].tskstat == 0 )
		return E_NOEXS;
#endif
	
	tcb  = &tcbtbl[tskid - 1];
	tcbs = &tcbstbl[tskid - 1];
	
	__set_imsk();
	
#if __ERR_CHECK_LEVEL >= 2
	/* 僞僗僋忬懺僠僃僢僋 */
	if ( tcb->tskstat != TTS_DMT ) {
		__res_imsk();
		return E_OBJ;
	}
#endif
	
	/* TCB傪弶婜壔 */
	tcb->sp      = tcbs->isp;
	tcb->tskpri  = tcbs->itskpri;
	tcb->tskstat = TTS_RDY;
	tcb->tskwait = 0;
	tcb->wupcnt  = 0;
	tcb->suscnt  = 0;
	
	/* 僗僞僢僋偑僗僞僢僋僾乕儖偵愝掕偝傟偰偄傞応崌 */
	if ( (UW)tcbs->isp & 0x00000001 ) {
		T_SPCB  *spcb;
		const T_SPCBS *spcbs;
		UW      spcbid;
		VH      *stack;
		char    buf[16];
		
		spcbid = (UW)tcbs->isp >> 1;
		spcb   = &spcbtbl[spcbid];
		spcbs  = &spcbstbl[spcbid];
		
		/* 僗僞僢僋偑嬻偄偰偄側偄応崌偼懸偪 */
		if ( spcb->freestk == 0xffff ) {
			__adt_que(&spcb->que, tcb);
			tcb->tskstat = TTS_WAI;
			tcb->tskwait = TTW_SPL;
			tcb->data    = (VP)stacd;
			
			__res_imsk();
			return E_OK;
		}
		
		
		/* 僗僞僢僋傪僾乕儖偐傜庢摼 */
		stack = &spcbs->stkhead[spcbs->stksz * spcb->freestk];
		spcb->freestk = *stack;
		tcb->sp = stack + spcbs->stksz;
		
		/*
		DWORDToString(buf, tcb->sp);
		LCD_Puts(buf);
		for ( ; ; )
			;
		*/
	}
	
	/* 僗僞僢僋偺弶婜愝掕 */
/*偙偙傪曄峏2002/7/2 by 郷*/
/*	tcb->sp -= 2;*/
	tcb->sp -= 1;
	*(FP *)tcb->sp = (FP)__tskst_entry;	/* 儕僞乕儞傾僪儗僗 */
/*	tcb->sp -= 10;*/
	tcb->sp -= 9;
	*(FP *)tcb->sp = tcbs->task;		/* 僞僗僋奐巒傾僪儗僗 */
	tcb->sp -= 2;
/*	*(--tcb->sp) = (VH)stacd;			/* 弶婜壔僐乕僪 */
	*tcb->sp = (VH)stacd;			/* 弶婜壔僐乕僪 */
	
	/* READY僉儏乕偵捛壛 */
	__adt_que(&rdyque[tcb->tskpri - 1], tcb);
	
	/* 僨傿僗僷僢僠 */
	__tsk_dsp();
	
	__res_imsk();
	
	return E_OK;
}


/* 僗僞僢僋偑僗僞僢僋僾乕儖偺傕偺側傜曉媝 */
void __rel_stp(T_TCB *tcb)
{
	const T_TCBS  *tcbs;
	T_SPCB  *spcb;
	const T_SPCBS *spcbs;
	UH spcbid;
	UH stack;
	
	tcbs = &tcbstbl[curtcb - tcbtbl];
	
	/* 僗僞僢僋僾乕儖傪棙梡偟偰偄傞偐偳偆偐丠 */
	if ( !((UW)tcbs->isp & 0x00000001) )
		return;
	
	/* 棙梡偟偰偄傞僗僞僢僋傪嶼弌 */
	spcbid = (UW)tcbs->isp >> 1;
	spcb   = &spcbtbl[spcbid];
	spcbs  = &spcbstbl[spcbid];
	stack  = ((VH *)tcb->sp - spcbs->stkhead - 1) / spcbs->stksz;
	
	/* 懸偪僞僗僋偑偁傟偽僗僞僢僋傪忳搉 */
	if ( spcb->que.tskcnt > 0 ) {
		T_TCB  *tcbnext;
		const T_TCBS *tcbsnext;
		
		tcbnext  = spcb->que.head;
		tcbsnext = &tcbstbl[tcbnext - tcbtbl];
		__del_que(tcbnext);
		tcbnext->sp = &spcbs->stkhead[(stack + 1) * spcbs->stksz];
		
		/* 僗僞僢僋偺弶婜愝掕 */
/*偙偙傪曄峏2002/7/2 by 郷*/
/*		tcbnext->sp -= 2;*/
		tcbnext->sp -= 1;
		*(FP *)tcbnext->sp = (FP)__tskst_entry;	/* 儕僞乕儞傾僪儗僗 */
/*		tcbnext->sp -= 10;*/
		tcbnext->sp -= 9;
		*(FP *)tcbnext->sp = tcbsnext->task;	/* 僞僗僋奐巒傾僪儗僗 */
		tcbnext->sp -= 2;
/*		*(--tcbnext->sp) = (VH)(INT)tcbnext->data;		/* 弶婜壔僐乕僪 */
		*tcbnext->sp = (VH)(INT)tcbnext->data;		/* 弶婜壔僐乕僪 */
		
		/* READY僉儏乕偵捛壛 */
		__adt_que(&rdyque[tcb->tskpri - 1], tcbnext);
		
		return;
	}
	
	/* 僗僞僢僋僾乕儖偵曉媝 */
	spcbs->stkhead[stack * spcbs->stksz] = spcb->freestk;
	spcb->freestk = stack;
}


/* 僞僗僋廔椆 */
void ext_tsk(void)
{
	const T_TCBS *tcbs;
	
#if __ERR_CHECK_LEVEL >= 4
	/* 僄儔乕僠僃僢僋 */
	if ( sysstat != TSS_TSK ) {
		/* 抳柦揑僄儔乕偩偑儕僞乕儞弌棃側偄 */
		sysstat = TSS_TSK;
	}
#endif
	
	__set_imsk();
	
	if ( curtcb != NADR ) {
		/* 僉儏乕偐傜奜偟DOMANT忬懺偵堏峴 */
		if ( curtcb->que != NADR )
			__del_que(curtcb);
		curtcb->tskstat = TTS_DMT;
		
		/* 僗僞僢僋偑僗僞僢僋僾乕儖偺傕偺側傜曉媝 */
		__rel_stp(curtcb);
		
		curtcb = NADR;
	}
	
	/* 僨傿僗僷僢僠 */
	__tsk_dsp();
}


/* 懠僞僗僋嫮惂廔椆 */
ER  ter_tsk(ID tskid)
{
	T_TCB *tcb;
	
#if __ERR_CHECK_LEVEL >= 4
	/* ID僠僃僢僋 */
	if ( tskid <= 0 )
		return E_ID;
	if ( tskid > tcbcnt || tcbtbl[tskid - 1].tskstat == 0 )
		return E_NOEXS;
#endif
	
	tcb = &tcbtbl[tskid - 1];
	
	__set_imsk();
	
#if __ERR_CHECK_LEVEL >= 2
	/* 僞僗僋忬懺僠僃僢僋 */
	if ( (!(sysstat & TSS_INDP) && tcb == curtcb)
						|| tcb->tskstat == TTS_DMT ) {
		__res_imsk();
		return E_OBJ;
	}
#endif
	
	/* 僞僗僋廔椆 */
	if ( tcb->que != NADR )
		__del_que(tcb);
	tcb->tskstat = TTS_DMT;
	
	/* 僗僞僢僋偑僗僞僢僋僾乕儖偺傕偺側傜曉媝 */
	__rel_stp(tcb);
	
	if ( curtcb == tcb )
		curtcb = NADR;	/* 旕僞僗僋晹偐傜尰嵼幚峴拞僞僗僋傪廔椆偟偨帪 */
	
	/* 僨傿僗僷僢僠 */
	__tsk_dsp();
	
	__res_imsk();
	
	return E_OK;
}


/* 僨傿僗僷僢僠嬛巭 */
ER  dis_dsp(void)
{
#if __ERR_CHECK_LEVEL >= 3
	/* 僄儔乕僠僃僢僋 */
	if ( sysstat & (TSS_INDP | TSS_DINT) )
		return E_CTX;
#endif
	__set_imsk();
	
	sysstat = TSS_DDSP;
	
	__res_imsk();
	
	return E_OK;
}


/* 僨傿僗僷僢僠嫋壜 */
ER  ena_dsp(void)
{
#if __ERR_CHECK_LEVEL >= 3
	/* 僄儔乕僠僃僢僋 */
	if ( sysstat & (TSS_INDP | TSS_DINT) )
		return E_CTX;
#endif
	
	__set_imsk();
	
	/* 抶墑僨傿僗僷僢僠 */
	sysstat = TSS_TSK;
	if ( dlydsp )
		__tsk_dsp();
	
	__res_imsk();
	
	return E_OK;
}


/* 僞僗僋桪愭搙曄峏 */
ER  chg_pri(ID tskid, PRI tskpri)
{
	T_TCB *tcb;
	
#if __ERR_CHECK_LEVEL >= 4
	/* 僷儔儊乕僞乕僠僃僢僋 */
	if ( tskid > tcbcnt )
		return E_NOEXS;
	if ( tskid == TSK_SELF && (sysstat & TSS_INDP) )
		return E_ID;
	if ( tskpri > rdqcnt )
		return E_PAR;
#endif
	
	/* ID -> TCB */
	if ( tskid == TSK_SELF )
		tskid = (curtcb - tcbtbl) + 1;
	tcb = &tcbtbl[tskid - 1];
	
	__set_imsk();
	
#if __ERR_CHECK_LEVEL >= 2
	/* 忬懺僠僃僢僋 */
	if ( tcb->tskstat == TTS_DMT ) {
		__res_imsk();
		return E_OBJ;
	}
#endif
	
	/* 桪愭搙偺曄峏 */
	if ( tskpri == TPRI_INI )
		tskpri = tcbstbl[tskid - 1].itskpri;
	tcb->tskpri = tskpri;
	if ( tcb->tskstat == TTS_RDY || tcb->tskstat == TTS_RUN ) {
		/* 儗僨傿乕僉儏乕偺宷偓懼偊 */
		__del_que(tcb);
		__adt_que(&rdyque[tskpri - 1], tcb);
	}
	
	/* 僨傿僗僷僢僠 */
	__tsk_dsp();
	
	__res_imsk();
	
	return E_OK;
}


/* 儗僨傿乕僉儏乕偺夞揮 */
ER  rot_rdq(PRI tskpri)
{
	T_TCB *tcb;
	
#if __ERR_CHECK_LEVEL >= 4
	/* 僷儔儊乕僞乕僠僃僢僋 */
	if ( tskpri > rdqcnt )
		return E_PAR;
#endif
	
	__set_imsk();
	
	if ( tskpri == TPRI_RUN ) {
		if ( curtcb == NADR ) {
			__res_imsk();
			return E_OK;	/* 僞僗僋偑柍偄応崌偼壗傕偟側偄 */
		}
		tskpri = curtcb->tskpri;
	}
	
	/* 僉儏乕偵僞僗僋偼偁傞偐 */
	if ( rdyque[tskpri - 1].tskcnt == 0 ) {
		__res_imsk();
		return E_OK;
	}
	
	/* 儗僨傿乕僉儏乕夞揮 */
	__rot_que(&rdyque[tskpri - 1]);
	
	/* 昁梫偱偁傟偽僨傿僗僷僢僠傪峴側偆 */
	tcb = rdyque[tskpri - 1].head;
	if ( curtcb->tskpri == tskpri && tcb != curtcb ) {
		if ( sysstat == TSS_TSK ) {
			/* 僨傿僗僷僢僠 */
			curtcb->tskstat = TTS_RDY;
			tcb->tskstat    = TTS_RUN;
			__swc_dsp(tcb);
		}
		else {
			/* 抶墑僨傿僗僷僢僠偺弨旛 */
			dlydsp = TRUE;
		}
	}
	
	__res_imsk();
	
	return E_OK;
}