/**************** os_task.c**************/
/*            MacroStar 1.0a            */
/*          Zhengyuquan,2003.7,Beijing  */
/*           All rights reserved        */
/****************************************/
#include "os_type.h"
#include "os_macIn.h"

#define USER_TASK_NUM (MAX_TASK_NUM-1)

const uint_16 os_maskTable[ ] =
{
	0x8000,0x4000,0x2000,0x1000,0x0800,0x0400,0x0200,0x0100,
    0x0080,0x0040,0x0020,0x0010,0x0008,0x0004,0x0002,0x0001,
	0x0000
};

const uchar os_fastTable[]=
{
255,  7,  6,  6,  5,  5,  5,  5,  4,  4,  4,  4,  4,  4,  4,  4,
  3,  3,  3,  3,  3,  3,  3,  3,  3,  3,  3,  3,  3,  3,  3,  3,
  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,
  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,  2,
  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,
  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,
  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,
  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,  1,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,
};

TCB     os_tcbs[ MAX_TASK_NUM ];	// 10*16=160
PTCB    os_prioToTCB[16];			// 16*2=32
PTCB    os_pCurTCB;

uint_16 os_curPrioMask=0x0001;
uint_16 os_rdyState =0x0001;
uint_16 os_rdyhState=0x0000;
uint_16 os_slpState= 0x0001;

volatile uchar   os_nLayers = 0;
volatile uchar   os_flag    = 0;

#ifdef _DEBUG
uint_16 os_prioState=0xFFFE;
uint_16 os_tcbState =0xFFFE;
#endif

void  STDCALL _FAR os_Exit();
void  STDCALL _FAR os_GetReadyTask( );
extern MSG   STDCALL _FAR os_PeekMessage( );
void interrupt os_Schedule( );

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR LEAVE_CRT_SEG( )
{
	os_nLayers--;
	if( os_flag&0x01)
		os_Schedule( );
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_Initial( )
{
	LOCK_INT( );
	os_nLayers++;
	os_pCurTCB   = &os_tcbs[ USER_TASK_NUM ];
	os_pCurTCB->priority= MAX_PRIORITY_NUM-1;
	os_prioToTCB[MAX_PRIORITY_NUM-1] = os_pCurTCB;
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_StartUp( void _FAR (*OnIdle)( ) )
{
     os_nLayers--;
     UNLOCK_INT( );
     os_Schedule( );
     while( os_PeekMessage() != 0 )
     {
	  os_rdyhState &= 0xFFFE;/*robust code*/
	  if( OnIdle )
	   {
		 (*OnIdle)( );
	   }
	 }
}

/*
************************************************************
*
*
*
************************************************************
*/
void interrupt os_Schedule( )
{
    if( os_nLayers )return;
    os_nLayers++;

	_DX = (int)os_pCurTCB;
	*(int*)(_DX+4) = _SP;
	*(int*)(_DX+6) = _SS;

	 os_GetReadyTask( );
	 _DX = (int)os_pCurTCB;
	 _SP = *(int*)(_DX+4);
	 _DX = *(int*)(_DX+6);
	 _SS = _DX;

	os_nLayers--;
	UNLOCK_INT( );
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_GetReadyTask( )
{
	uint_16 temp;
	uint_16 who;
	do
	{
	  UNLOCK_INT( );
	  os_flag &= 0xFE;
	  if( (temp = os_rdyhState&os_slpState) == 0 )
		temp = os_rdyState&os_slpState;
	  if( (who = os_fastTable[ HIGHBYTE(temp) ]) &0x08 )
		who = os_fastTable[ LOWBYTE(temp) ] | 0x08;
	  LOCK_INT( );
	}while( os_flag & 0x01);
	os_curPrioMask = os_maskTable[who];
	os_pCurTCB = os_prioToTCB[who];
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_CreateTask(TASKPROC task, uchar id, uchar priority, int far* pStack, void far * param)
{
	uint_16 mask = os_maskTable[ priority&0x0f];

	os_ASSERT( os_tcbState&os_maskTable[id], TASK_ID_IS_USED );
	os_ASSERT( os_prioState&mask, TASK_PRIO_IS_USED);

	ENTER_CRITICAL_SEG( );

#ifdef _DEBUG
	os_tcbState &= ~os_maskTable[id];
	os_prioState &= ~mask;
#endif

	os_prioToTCB[ priority&0x0f ] = &os_tcbs[ id ];
	os_rdyState |= mask;
	os_slpState |= mask;
	if( !(priority &0xF0) ) os_rdyhState |= mask;

	 *(int_32*)(pStack-2) = (int_32)param;
	 *(int_32*)(pStack-4) = (int_32)(void far(*)())os_Exit;

	 *(int*)(pStack-5) = 0x0200;
	 *(int_32*)(pStack-7) = (int_32)task;
	 pStack -= 11;
	 *(int*)(pStack-1) = _ES;
	 *(int*)(pStack-2) = _DS;

	 os_tcbs[id].sp = pStack-5;

	 os_tcbs[id].priority = priority&0x0f;
	 os_tcbs[id].msg[0]=0;
	 os_tcbs[id].msg[1]=0;

	 LEAVE_CRITICAL_SEG( );
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_Suspend( )
{
    os_slpState &= ~os_curPrioMask;
	os_Schedule( );
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_Resume(uchar taskId)
{
	os_ASSERT( !(os_tcbState&os_maskTable[taskId]),RESUME_INVALID_TASK);
	LOCK_INT_EX( );
	os_slpState |= os_maskTable[ os_tcbs[taskId].priority ];
	UNLOCK_INT_EX( );
	os_SetSwitchFlag( );
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_RaisePriority( )
{
	os_rdyhState |= os_curPrioMask;
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_LowerPriority( )
{
	os_rdyState  |= os_curPrioMask;/*robust code*/
	os_rdyhState &= ~os_curPrioMask;
	if( os_rdyhState&os_slpState )
	    os_Schedule( );
	else
		os_SetSwitchFlag( );
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_KillTask(uchar taskId)
{
	uint_16 mask = ~os_maskTable[ os_tcbs[taskId].priority ];

	os_WARNING( !(os_tcbState&os_maskTable[taskId]),KILL_INVALID_TASK);

	LOCK_INT( );
	os_slpState &= mask;
	os_rdyState &= mask;
	os_rdyhState &= mask;

#ifdef _DEBUG
	os_tcbState |= os_maskTable[ taskId];
	os_prioState|= ~mask;
#endif
	UNLOCK_INT( );
}

/*
************************************************************
*
*
*
************************************************************
*/
void STDCALL _FAR os_Exit( )
{
	register uint_16 mask = ~os_curPrioMask;
	LOCK_INT( );
#ifdef _DEBUG
	os_tcbState |= os_maskTable[ os_GetCurrentTaskId( ) ];
	os_prioState|= mask;
#endif
	os_slpState &= mask;
	os_rdyState &= mask;
	os_rdyhState&= mask;
	UNLOCK_INT( );
	os_Schedule( );
}