dispatch.c

基于东南大学开发的SEP3203的ARM7中的所有驱动

/****************************************************************
 *  Copyright (C) Asic Center. 2001
 *  All Rights Reserved
 *
 *  Filename : DISPATCH.C
 *  Function : dispatch tasks
 *  Revision :
 *		2002/4/17	PESSIA	Create this file
 *****************************************************************/
#include <stdio.h>
#include "Ros33.h"		//gfd
#include "pwrctrl.h"  //gfd
#include "internal.h"
#include "M68328.h"     //gfd

UB g_ubIntNestCnt;
UB g_ubSysStat;
UB g_blDelay;
UW g_cpsr2r0;
UW g_psr_sysclk;

void release_dispatch( void );
void int_dispatch( void );
void schedule( void );
void ret_int( void );
void tmr_handler( void );
void ret_tmr( void );
void sys_clk( void );

void release_dispatch( void )
{
	//for future reference
}

void int_dispatch(void )
{
	/* save PSR into stack */
	//PUSH_PSR;
	//for the caller of the int_dispatch has already save the PSR. (gfd::hxf)
	
	
	/* check system status */
	if ( g_ubSysStat & (TSS_LOC|TSS_DDSP|TSS_INDP) )
	{
		/* set delayed dispatch flag */
		g_blDelay |= 0x01;
		return;
	}
	
	/* build minimum context structure ( T_SAVEDREG_MIN ) */
	PUSH_CALLEE_SAVED_REG;/* save callee-saved registers */
	PUSH_NULL;/* push null into stack for return value */

	/* save current task's stack point  :  g_pCurTsk->uwSP = sp */
	SAVE_CURTSK_SP;
	
	/* clear delayed dispatch flag */
	g_blDelay &= 0x0;

	/* change current task's state to join in disptaching */
	g_pCurTsk->ubStatus |= TTS_RDY;
	
	schedule();

	/* we will never get here. otherwise, system panic */
PANIC_LOOP:
	goto PANIC_LOOP;
}

void schedule( void )
{
	/******************************************
	  * the two local variables can not be in local stack,  *
	  * so, their type should be register or static.           *
	  * BUT, because we restore next task's own stack   *
	  * to switch to it while not depending on current sp. *
	  * so, we can do use current stack.                        *
	  ******************************************/
	register T_TSKCB *p_tsk;
	register T_READYQUEUE *p_readyqueue;

	p_readyqueue = g_sReadyQueue;
SET_P_TSK:
	p_tsk = ( T_TSKCB * )p_readyqueue->pNxtTsk;
	
	/* search next running task */
	do
	{
		/* check whether the ready queue has tasks or not */
		if ( (T_TSKCB *)p_readyqueue == p_tsk )
		{
			/* no task,  then search next ready queue */
			p_readyqueue++;
			goto SET_P_TSK;	/* to reuse same code */
			printf("this ready queue is null\n");
			//continue;
		}
		else
		{	
			/* find a task,  then check its status */
			if ( p_tsk->ubStatus & TTS_SUS )
			{
				/* it is suspending, then goto next task */
				p_tsk = ( T_TSKCB * )(p_tsk->pNxtTsk);
				//continue;
			}
			else
				/* we find a ready task */
				break;
		}
	}
	while ( 1 );

	/* this task is next running task */
	p_tsk->ubStatus = TTS_RUN;
	g_pCurTsk = p_tsk;

#if 0
	/* power saving code:
	  * while system runs in DOZE-MODE or SLEEP-MODE, 
	  * some wake-up event (RTC INT, for example) happens
	  * and causes current task switched to an application-level
	  * task, system should be back to NORMAL-MODE. */
	if( power_saving_flag && ( cur_pcs == PCS_DOZE || cur_pcs == PCS_SLEEP ))
	{
	#define APPLICATION_LEVEL_TASK_PRIORITY 5
		if( p_tsk->bPriority >= APPLICATION_LEVEL_TASK_PRIORITY )
			set_pcs( PCS_NORMAL );
	}
#endif 		//end for power saving		
	/* restore this task's running-environment */
	RESTORE_CURTSK_SP; /* restore this task's sp */
	if ( p_tsk->ubIntinfo & 0x01)
	{
		p_tsk->ubIntinfo &= 0;
		/* the task was interrupted last time,
		  * now, restore all common registers. */
		POP_ALL_COMM_REG;
		INT_RETI;			//add by gfd
	}
	else
	{
		GET_RETURN_VALUE_FROM_STACK;
		/* the task was put into wait-queue last time,
		  * now, restore all callee-saved registers. */
		POP_CALLEE_SAVED_REG;
		RETI;		//gfd pop r4 and r14 to pc
	}

	/* switch to the task */
	//RETI;		//by gfd

	/* we will never get here. otherwise, system panic */
PANIC_LOOP:
	goto PANIC_LOOP;
}

void ret_int( void )
{
	/**********************************************
	  *                   ALL IN CRITICAL SECTION
	  *              except some code in tmr_handler()
	  **********************************************/
	ENTER_CRITICAL_SECTION;
	
	/* this function will never back to its caller */
	//POP_NULL;		//gfd
	POP_NULL;		//pop r4
	POP_NULL;		//pop r14 (in ret_int function)
	
	/* *****************************************
	  * save interrupted task's environment.                 *
	  * this operation reduces some efficiency, because *
	  * these registers may be poped again at once.      * 
	  ******************************************/
	//PUSH_ALL_COMM_REG;
	
	/* if interrupt nest exists,  exit ret_int and 
	  * continue to handle nested interrupt . */
	if ( --g_ubIntNestCnt )
	{
		/* pop them only after a little time. 
		  * efficiency is reduced */
		POP_ALL_COMM_REG;
		INT_RETI;
	}
	
	/* check Ready Handler Queue */
	if ( (T_READYHNDR*)&g_sReadyHndr != \
		(T_READYHNDR*)(g_sReadyHndr.pNxtReadyHndr) )
		tmr_handler();

	/* change system status and check it */
	g_ubSysStat &= ~TSS_INDP;
	if ( g_ubSysStat & (TSS_LOC|TSS_DDSP) )
		goto not_dispatch;

	/* check delayed dispatch */
	if ( !(g_blDelay & 0x01) )
		goto not_dispatch;

	/* clear delayed dispatch flag */
	g_blDelay &= 0x0;

	/* save current task's stack point  :  g_pCurTsk->uwSP = sp */
	POP_NULL;		//pop r3
	POP_NULL;		//pop r14 (in ENT_INT_XXX)
	SAVE_CURTSK_SP;
	/* show current task was interruped */
	g_pCurTsk->ubIntinfo |= 0x01;
	/* change current task's state to join in disptaching */
	g_pCurTsk->ubStatus |= TTS_RDY;
	
	schedule();
	
not_dispatch:
	////POP_ALL_COMM_REG;
	//INT_RETI;	//while use "RETI;"  in c33
	////RETI;
	POP_NULL;
	POP_NULL;
	POP_ALL_COMM_REG;
	INT_RETI;
	return;
}

void tmr_handler( void )
{
	register T_HNDRCB *p_readyhndr;
	register T_HNDRCB *p_nextreadyhndr;

	/******************************************************
	  *                           STILL IN CRITICAL SECTION
	  ******************************************************/
	/* get next ready timer handler */
	p_readyhndr = (T_HNDRCB *)(g_sReadyHndr.pNxtReadyHndr);
	
	do
	{
		/* check this ready handler's state */
		if ( p_readyhndr->ubHndrStat & HNDR_CYC )
		{
			/* CYC timer*/
			/* if this handler is off, delete it from ready-handler-queue */
			if ( p_readyhndr->cycact == TCY_OFF )
				goto RET_TMR;
		}
		else
			/* ALARM timer*/
			/* clear alarm handler status */
			p_readyhndr->ubHndrStat &= ~HNDR_ALM;

		g_pCurHndr = p_readyhndr;
		
		/* increase interrupt nest count, in order to disable dispatch */
		g_ubIntNestCnt++;

		/******************************************
	  	 *                  OUT OF CRITICAL SECTION
	  	 *    BUT, current system status is TSS_INDP, so 
	  	 *    no dispatch will happen during timer handler's
	  	 *    process.
	  	 ******************************************/
		EXIT_CRITICAL_SECTION;
		
		/* call its handler */
		p_readyhndr->fpPC();

		/******************************************
	  	 *                  BACK TO CRITICAL SECTION
	  	 ******************************************/
		ENTER_CRITICAL_SECTION;

		g_ubIntNestCnt--;
		
	RET_TMR:
		/* save next ready timer handler */
		p_nextreadyhndr = (T_HNDRCB *) \
			(((T_READYHNDR *)p_readyhndr)->pNxtReadyHndr);
		
		/* delete this handler from ready handler queue */
		vfnDelInitMember((T_NODE*)p_readyhndr);
		
		if ( p_readyhndr->ubHndrStat & HNDR_ALM )
			goto CHECK_NEXT_HANDLER;
		else //if ( p_readyhndr->ubHndrStat & HNDR_CYC )
		{
			/* recalculate CYC timer's next ready time, and 
		  	 * add it to time handler queue (not ready queue) */
			p_readyhndr->utime = g_sSysTime.utime + p_readyhndr->Iniutime;
			if ( g_sSysTime.ltime > g_sSysTime.ltime + p_readyhndr->Iniltime )
				p_readyhndr->utime ++;
			p_readyhndr->ltime = g_sSysTime.ltime + p_readyhndr->Iniltime;

			/* add member to time handler queue */
			vfnAddTimeHndr((T_NODE*)p_readyhndr);
		}

	CHECK_NEXT_HANDLER:
		/* get next ready timer handler */
		p_readyhndr = p_nextreadyhndr;
		
	}/* if there are no ready handler, return */
	while (  (T_READYHNDR *)p_readyhndr != (T_READYHNDR *)&g_sReadyHndr );
}

void ret_tmr( void )
{
	//NULL
}

void sys_clk( void )
{
	register T_TIMEOUTTSK *p_timeouttsk;
	register T_TSKCB *p_tsk;
	register T_TIMEHNDR *p_tmrhndr;
	register T_TIMEHNDR *p_nxttmrhndr;
	
	//PUSH_PSR;				//gfd
	/**************************************************
	  *                           IN CRITICAL SECTION
	  **************************************************/
	//ENTER_CRITICAL_SECTION;		//gfd
	g_psr_sysclk = ent_cri();		//gfd add

	/* change system time */
	if ( g_sSysTime.ltime > g_sSysTime.ltime + g_uhIntTime )
		g_sSysTime.utime ++;
	g_sSysTime.ltime += g_uhIntTime;

	/* check timeout task queue */
	p_timeouttsk = (T_TIMEOUTTSK *)g_sTimeOutTsk.pNxtTimeOutTsk;
	while ( (T_TIMEOUTTSK *)&g_sTimeOutTsk != p_timeouttsk )
	{
		/* get task control block pointer */
		//	p_tsk = (T_TSKCB *)\
		//		((UW)p_timeouttsk - (UW)(((T_TSKCB *)0)->pNxtTmoTsk));
		p_tsk = (T_TSKCB *)((UW)p_timeouttsk-8);
		
		/* check whether time is out or not */
		if ( p_tsk->utime <=  g_sSysTime.utime && p_tsk->ltime <=  g_sSysTime.ltime  )
		{
			p_tsk->ubStatus &= ~TTS_WAI;
			
			/* save return value to stack */
			if ( p_tsk->ubWaitStat == TTW_DLY )
				((T_SAVEDREG_MIN *)(p_tsk->uwSP))->returnvalue = E_OK;  //gfd
			else
				((T_SAVEDREG_MIN *)(p_tsk->uwSP))->returnvalue = E_TMOUT;  //gfd
			p_tsk->ubWaitStat = 0; /* clear wait status */

			/* get next timeout task */
			p_timeouttsk = (T_TIMEOUTTSK*)p_timeouttsk->pNxtTimeOutTsk;
			
			/* delete this task from time out queue, and add it to ready queue */
			vfnDelInitMember((T_NODE*)&(p_tsk->pNxtTmoTsk));
    			vfnDelAddMember((T_NODE*)&g_sReadyQueue[(unsigned char)p_tsk->bPriority], (T_NODE*)p_tsk);
    			/* set delay dispatch flag */
    			g_blDelay |= 0x01;
		}
		else
			break;
	}

	/* check timer handler queue */
	p_tmrhndr = (T_TIMEHNDR *)g_sTimeHndr.pNxtTimeHndr;
	while ( (T_TIMEHNDR *)&g_sTimeHndr !=  p_tmrhndr )
	{
		/* check whether time is out or not */
		if ( ((T_HNDRCB*)p_tmrhndr)->utime <=  g_sSysTime.utime 
			&&  ((T_HNDRCB*)p_tmrhndr)->ltime <=  g_sSysTime.ltime  )
		{
			/* save next timer handler */
			p_nxttmrhndr = (T_TIMEHNDR *)p_tmrhndr->pNxtTimeHndr;
			
			/* delete this handler from timer queue, and 
			  * add it to ready timer handler queue */
			vfnDelAddMember((T_NODE*)&g_sReadyHndr, (T_NODE*)p_tmrhndr);

			/* get next timer handler */
			p_tmrhndr = p_nxttmrhndr;
		}
		else
			break;
	}

	/* restore PSR, and perhaps exit critical section(decided by origin PSR) */
	//POP_PSR;	//GFD
	ret_cri(g_psr_sysclk);
}