core.c

一个多任务实时操作系统

#include "VisbyKernelDef.h"
#include "VisbyDevice.h"
#include "VisbyISR.h"
#include "VisbyCPU.h"

///////////////////// Visby global ////////////////////////////
///////////////////////////////////////////////////////////////
extern void SWI_Handler(void);		//汇编程序调用
extern void TICK_Handler(void);

//Heap Alloc and Free
extern char Image$$RW$$Limit[];
#define STACKSIZE	0xa00								//SVC satck size(do not use user stack)//
#define HEAPEND		(_ISR_STARTADDRESS-STACKSIZE-0x500) // = 0xc7ff000//
uint *pHeapMap = (uint *)Image$$RW$$Limit;
uint heapMapLen;	// = (HEAPEND - (unsigned int)Image$$RW$$Limit) / 4 / 33;
uint *pHeapStart;	// = pHeapMap + heapMapLen;

volatile BOOL Visby_Started = FALSE;
volatile uint Visby_Critical = FALSE;
volatile ullong Visby_Tick = 0;

Visby_Task		*pVisby_IdleTask = NULL;
Visby_Task 		*pVisby_HeadTaskInPrio[6] = {NULL};
Visby_Task 		*pVisby_CurrTask	= NULL;
LPFUNC	   		pVisby_IntHand[VB_ISR_NUM] = {NULL};		//中断服务程序入口地址表
Visby_Device	pVisby_Device[VB_DEV_NUM];					//设备描述表
BOOL			Visby_Resouce[VB_RES_NUM] = {FALSE};		//资源使用情况表

///////////////////////////内核实现////////////////////////////
///////////////////////////////////////////////////////////////
void *heapalloc(uint nbyte)
{
	static BOOL used = FALSE;
	unsigned int i, ndword, start, count;

	VB_ENCRIT;
	if(!used)
	{
		heapMapLen = (HEAPEND - (unsigned int)Image$$RW$$Limit) / 4 / 33;
		pHeapStart = pHeapMap + heapMapLen;
		for(i = 0; i < heapMapLen; i++)
		{
			pHeapMap[i] = 0x00000000;
		}
		used = true;
	}
	ndword = (nbyte >> 2) + ((nbyte & 0x00000003) > 0) + 1;		//Add 1 uint to store the ndword
	count = 0;
	for(i = 0; i < (heapMapLen << 5); i++)
	{
		if(TST(pHeapMap[i >> 5], BIT(i & 0x1F)))
		{
			count = 0;
		}
		else
		{
			if(count == 0)
			{
				start = i;
			}
			count++;
			if(count >= ndword)
			{
				for(i = start; i < start + ndword; i++)
				{
					SET(pHeapMap[i >> 5], BIT(i & 0x1F));
				}
				*(pHeapStart + start) = ndword;
				VB_DECRIT;
				return (void *)(pHeapStart + start + 1);
			}
		}
	}
	VB_DECRIT;
	return NULL;
}

void heapfree(void *ptr)
{
	uint i, ndword, start;
	
	VB_ENCRIT;
	ptr = (void *)((uint *)ptr - 1);
	ndword = *((uint *)ptr);
	start = (uint *)ptr - pHeapStart;
	for(i = start; i < start + ndword; i++)
	{
		CLR(pHeapMap[i >> 5], BIT(i & 0x1F));
	}
	VB_DECRIT;
}

//*****************************************************************
//****************** INT Vector install ***************************
uint Visby_KInstallHandler(uint routine, uint vector)
{
	uint ins, old_ins;
	ins = (routine - vector - 8) >> 2;
	ins = 0xEA000000 | ins;		// OR in 'branch always' code
	old_ins = *((uint *)vector);
	*((uint *)vector) = ins;
	return old_ins;
}

void __irq UndefIns_Handler()
{
	Beep(0x1);
	Uart_Printf("Undefined Instruction.");
	Beep(0x0);
	while(true);
}

void __irq InstAbort_Handler()
{
	Beep(0x1);
	Uart_Printf("Instruction Abort.");
	Beep(0x0);
	while(true);
}

void __irq DataAbort_Handler()
{
	Beep(0x1);
	Uart_Printf("Data Abort.");
	Beep(0x0);
	while(true);
}

//******************************************************************
//****************** Visby Kernel **********************************
void Visby_KStart()
{
	//create idle task
	pVisby_IdleTask = (Visby_Task *)heapalloc(sizeof(Visby_Task));
	if(pVisby_IdleTask == NULL)
	{
		return;
	}
	pVisby_IdleTask->StackEnd = 0;
	pVisby_IdleTask->Priority = PRIO_IDLE;
	pVisby_IdleTask->State = VB_TS_ACTIVE;
	pVisby_IdleTask->pHeadMsg = NULL;
	pVisby_IdleTask->pTailMsg = NULL;
	pVisby_IdleTask->ResumeTick = 0;
	pVisby_IdleTask->WaitMsg = VB_MSG_NULL;
	pVisby_IdleTask->pUsedDev = NULL;
	pVisby_IdleTask->pNext = pVisby_IdleTask;
	pVisby_CurrTask = pVisby_IdleTask;
	
	//安装中断服务程序
	Visby_KInstallHandler((uint)UndefIns_Handler, 	INTVEC_UNDEF);
	Visby_KInstallHandler((uint)SWI_Handler,			INTVEC_SWI);
	Visby_KInstallHandler((uint)InstAbort_Handler,	INTVEC_IABT);
	Visby_KInstallHandler((uint)DataAbort_Handler,	INTVEC_DABT);
	Visby_KInstallHandler((uint)TICK_Handler,			INTVEC_TIMER2);
	
	Visby_KInstallHandler((uint)VB_ISR_Eint0,   	INTVEC_EINT0);
	Visby_KInstallHandler((uint)VB_ISR_Eint1,   	INTVEC_EINT1);    
	Visby_KInstallHandler((uint)VB_ISR_Eint2,   	INTVEC_EINT2);              
	Visby_KInstallHandler((uint)VB_ISR_Eint3,   	INTVEC_EINT3);             
	Visby_KInstallHandler((uint)VB_ISR_Eint4567,	INTVEC_EINT4567);
//	Visby_KInstallHandler((uint)VB_ISR_Inttick, 	INTVEC_TICK);
	Visby_KInstallHandler((uint)VB_ISR_Intzdma0,	INTVEC_ZDMA0);
	Visby_KInstallHandler((uint)VB_ISR_Intzdma1,	INTVEC_ZDMA1);
	Visby_KInstallHandler((uint)VB_ISR_Intbdma0,	INTVEC_BDMA0);
	Visby_KInstallHandler((uint)VB_ISR_Intbdma1,	INTVEC_BDMA1);
	Visby_KInstallHandler((uint)VB_ISR_Wdt,     	INTVEC_WDT);
	Visby_KInstallHandler((uint)VB_ISR_User01,  	INTVEC_UERR01);
	Visby_KInstallHandler((uint)VB_ISR_Timer0,  	INTVEC_TIMER0);
	Visby_KInstallHandler((uint)VB_ISR_Timer1,   	INTVEC_TIMER1);
//	Visby_KInstallHandler((uint)VB_ISR_Timer2,  	INTVEC_TIMER2);
	Visby_KInstallHandler((uint)VB_ISR_Timer3,  	INTVEC_TIMER3);
	Visby_KInstallHandler((uint)VB_ISR_Timer4,  	INTVEC_TIMER4);
	Visby_KInstallHandler((uint)VB_ISR_Timer5,   	INTVEC_TIMER5);
	Visby_KInstallHandler((uint)VB_ISR_Urxd0,    	INTVEC_URXD0);
	Visby_KInstallHandler((uint)VB_ISR_Urxd1,   	INTVEC_URXD1);
	Visby_KInstallHandler((uint)VB_ISR_Iic,		INTVEC_IIC);
	Visby_KInstallHandler((uint)VB_ISR_Sio,     	INTVEC_SIO);
	Visby_KInstallHandler((uint)VB_ISR_Utxd0,   	INTVEC_UTXD0);
	Visby_KInstallHandler((uint)VB_ISR_Utxd1,   	INTVEC_UTXD1);
	Visby_KInstallHandler((uint)VB_ISR_Rtc,     	INTVEC_RTC);
	Visby_KInstallHandler((uint)VB_ISR_Adc,     	INTVEC_ADC);
	                         
	// set prescaler = 1/33
	VB_Timer23Prescale(33);
	// set clock divisor = 2
	VB_MUX_Timer2(TIMER_1_2);
	// set tick time = 1 / (66MHz / 33 / 2) * VB_TICKTIME = 1us * VB_TICKINTV
	VB_TimerCount(2,VB_TICKINTV);
	// update rTCNTB2 and rTCMPB2
	VB_Timer2Update();
	// timer2 auto-reload and start
	VB_Timer2Auto();
	VB_Timer2Start();
}

Visby_Task *Visby_KCreateTask(void (*pentry)(void), byte priority, uint stackSize, uint mode)
{
	Visby_Task *pNewTask;
	pNewTask = (Visby_Task *)heapalloc(sizeof(Visby_Task));
	if(pNewTask == NULL)
	{
		return NULL;
	}
	pNewTask->StackEnd = (uint)heapalloc(stackSize << 2);
	if(pNewTask->StackEnd == NULL)
	{
		heapfree((void *)pNewTask);
		return NULL;
	}
	pNewTask->Regs.SP = pNewTask->StackEnd + (stackSize << 2);
	pNewTask->Regs.PC = (uint)pentry + 4;
	pNewTask->Regs.CPSR = 0x00000010;	//user mode
	pNewTask->Priority = priority;
	pNewTask->State = VB_TS_ACTIVE;
	pNewTask->pHeadMsg = NULL;
	pNewTask->pTailMsg = NULL;
	pNewTask->ResumeTick = 0;
	pNewTask->WaitMsg = VB_MSG_NULL;
	pNewTask->pUsedDev = NULL;
	pNewTask->pNext = pNewTask;
	return pNewTask;
}

void Visby_KAddTask(Visby_Task *pTheTask)
{
	byte priority = pTheTask -> Priority;
	if(pVisby_HeadTaskInPrio[priority] == NULL)
	{
		pVisby_HeadTaskInPrio[priority] = pTheTask;
	}
	else
	{
		pTheTask->pNext = pVisby_HeadTaskInPrio[priority]->pNext;
		pVisby_HeadTaskInPrio[priority]->pNext = pTheTask;
	}
}

void Visby_KDestroyTask(Visby_Task *pTheTask)
{
	Visby_Msg *pMsg;
	for(pMsg = pTheTask->pHeadMsg; pMsg != NULL; pMsg = pMsg->pNext)
	{
		heapfree((void *)pMsg);
	}
	heapfree((void *)(pTheTask->StackEnd));
	heapfree((void *)pTheTask);
}

void Visby_KRemoveTask(Visby_Task *pTheTask)
{
	Visby_Task *pTask;
	if(pTheTask -> pNext == pTheTask)
	{
		pVisby_HeadTaskInPrio[pTheTask->Priority] = NULL;
	}
	else
	{
		pTask = pVisby_HeadTaskInPrio[pTheTask->Priority];
		do
		{
			if(pTask->pNext == pTheTask)
			{
				pTask->pNext = pTheTask->pNext;
				return;
			}
			pTask = pTask->pNext;
		}
		while(pTask != pVisby_HeadTaskInPrio[pTheTask->Priority]);
	}
}

__inline void Visby_KSuspendTask(Visby_Task *pTheTask)
{
	pTheTask->State = VB_TS_SUSPEND;
}

__inline void Visby_KSleepTask(Visby_Task *pTheTask, uint tick)
{
	pTheTask->State = VB_TS_SLEEPING;
	pTheTask->ResumeTick = Visby_Tick + (ullong)tick;
}

__inline void Visby_KResumeTask(Visby_Task *pTheTask)
{
	pTheTask->State = VB_TS_ACTIVE;
}

__inline void Visby_KWaitTask(Visby_Task *pTheTask, uint msgType)
{
	pTheTask->State = VB_TS_WAITMSG;
	pTheTask->WaitMsg = msgType;
}

void Visby_KChgPrio(Visby_Task *pTheTask, byte newPrio)
{
	Visby_KRemoveTask(pTheTask);
	pTheTask->Priority = newPrio;
	Visby_KAddTask(pTheTask);
}

void Visby_KSendMsg(Visby_Task *pTask, Visby_Msg *pMsg)
{
	if(pTask->pHeadMsg == NULL)
	{
		pTask->pHeadMsg = pMsg;
		pTask->pTailMsg = pMsg;
	}
	else
	{
		pTask->pTailMsg->pNext = pMsg;
		pTask->pTailMsg = pMsg;
	}
}

BOOL Visby_KUrgtMsg(Visby_Task *pTask, Visby_Msg *pMsg)
{
	if(pTask->pHeadMsg == NULL)
	{
		pTask->pHeadMsg = pMsg;
		pTask->pTailMsg = pMsg;
	}
	else
	{
		pMsg->pNext = pTask->pHeadMsg;
		pTask->pHeadMsg = pMsg;
	}
	if(pTask->State == VB_TS_WAITMSG && pTask->WaitMsg == pMsg->Type)
	{
		pTask->State = VB_TS_ACTIVE;
		return TRUE;
	}
	return FALSE;
}


//*****************************************************************

BOOL Visby_KernelServer(uint type, uint *pParam)
{
	Visby_Msg *pNewMsg;
	Visby_Task *pNewTask;
	BOOL needSche = FALSE;
	
	switch(type)
	{
	case VB_SWI_NULL:
		break;
	
	case VB_SWI_MSG|SWI_MSG_SEND:
		Visby_KSendMsg((Visby_Task *)pParam[0], (Visby_Msg *)pParam[1]);
		break;
	
	case VB_SWI_MSG|SWI_MSG_URGT:
		needSche = Visby_KUrgtMsg((Visby_Task *)pParam[0], (Visby_Msg *)pParam[1]);
		break;
	
	case VB_SWI_TASK|SWI_TASK_NEW:
		//												 entry,		   priority,	   stackSize,			 mode
		pNewTask = Visby_KCreateTask((void (*)(void))pParam[0], (byte)pParam[1], (uint)pParam[2], (uint)pParam[3]);
		Visby_KAddTask(pNewTask);
		pNewMsg = (Visby_Msg *)heapalloc(sizeof(Visby_Msg));
		pNewMsg->Type = VB_MSG_SYSRTN;
		pNewMsg->pDetail = pNewTask;
		pNewMsg->pNext = NULL;
		Visby_KUrgtMsg(pVisby_CurrTask, pNewMsg);
		break;
	
	case VB_SWI_TASK|SWI_TASK_DEL:
		Visby_KRemoveTask((Visby_Task *)pParam[0]);
		Visby_KDestroyTask((Visby_Task *)pParam[0]);
		needSche = TRUE;
		break;
	
	case VB_SWI_TASK|SWI_TASK_SUS:
		Visby_KSuspendTask((Visby_Task *)pParam[0]);
		needSche = TRUE;
		break;
	
	case VB_SWI_TASK|SWI_TASK_RSM:
		Visby_KResumeTask((Visby_Task *)pParam[0]);
		needSche = TRUE;
		break;
	
	case VB_SWI_TASK|SWI_TASK_SLP:
		Visby_KSleepTask((Visby_Task *)pParam[0], (uint)pParam[1]);
		needSche = TRUE;
		break;
	
	case VB_SWI_TASK|SWI_TASK_WAI:
		Visby_KWaitTask((Visby_Task *)pParam[0], (uint)pParam[1]);
		needSche = TRUE;
		break;

	case VB_SWI_TASK|SWI_TASK_CPR:
		Visby_KChgPrio((Visby_Task *)pParam[0], (byte)pParam[1]);
		break;
//	case ...:
//		...();
//		break;
//	case ...:
//		...();
//		break;
	default:
		break;
	}
	return needSche;
}

void Visby_KernelScheduler()
{
	byte prio;
	Visby_Task *pTask;
	for(prio = PRIO_KERNEL; prio <= PRIO_LOW; prio++)
	{
		if(pVisby_HeadTaskInPrio[prio] == NULL)
		{
			continue;
		}
		pTask = pVisby_HeadTaskInPrio[prio]->pNext;
		do
		{
			switch(pTask->State)
			{
			case VB_TS_ACTIVE:
				pVisby_HeadTaskInPrio[prio] = pTask;
				pVisby_CurrTask = pTask;
				return;
				break;
				
			case VB_TS_SLEEPING:
				if(pTask->ResumeTick <= Visby_Tick)
				{
					pTask->State = VB_TS_ACTIVE;
					pVisby_HeadTaskInPrio[prio] = pTask;
					pVisby_CurrTask = pTask;
					return;
				}
				break;
				
			case VB_TS_SUSPEND:
				break;
				
			case VB_TS_WAITMSG:
				break;
				
			default:
				break;
			}
			pTask = pTask->pNext;
		}while(pTask != pVisby_HeadTaskInPrio[prio]->pNext);
	}
	pVisby_CurrTask = pVisby_IdleTask;
	return;
}