os.c

uHAL for SA1110,INTEL公司出品

/*
 * File: os.c
 *
 * uC/OS Real-time multitasking kernel for the ARM processor.
 *
 * Kernel functions.
 *
 * Created by Jean J. Labrosse. 
 * ARM port by Marco Graziano ([e-mail]).
 *
 */

#include	"ucos.h"
#include	"osdefs.h"
#include	"sa.h"

#pragma         no_check_stack
/* let's stay on the safe side */
#pragma         no_optimise_crossjump
#pragma         no_optimise_multiple_loads
#pragma         no_optimise_cse

/* mapping table to map bit position to bit mask */
uint const OSMapTbl[] = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80 };

/* priority resolution table */
uint const OSUnMapTbl[] = {
	0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
	4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,
};

/* global variables */
uint    OSCtxSwCtr;             /* context switches counter */
uint    OSIdleCtr;              /* idle counter */
uint    OSRunning = 0;              /* kernel running flag */
uint	OSIntNesting;		/* interrupt nesting level */
OS_TCB  *OSTCBCur;              /* current running TCB pointer */
OS_TCB  *OSTCBHighRdy;          /* highest priority TCB ready to run */
OS_TCB  *OSTCBPrioTbl[64];	/* table of pointers to created TCBs */
 
/* local variables */
static	OS_TCB	*OSTCBList;	/* TCBs doubled linked list */
static	uint	OSRdyGrp;	/* ready list group */
static	uint	OSRdyTbl[8];	/* table of ready to run tasks */
static	uint	OSLockNesting;	/* multitasking lock nesting level */
static	OS_TCB	*OSTCBFreeList;	/* free TCBs list */
static	OS_EVENT *OSEventFreeList;	/* free EVENT CB list */
static	OS_Q	*OSQFreeList;		/* free QUEUE CB list */
static	uint	OSTime;			/* system time in ticks */
static	uint	OSIdleStk[OS_IDLE_STK_SIZE];	/* idle task stack */
static	OS_TCB	OSTCBTbl[OS_MAX_TASKS + 1];	/* TCBs table */
static	OS_EVENT OSEventTbl[OS_MAX_EVENTS];	/* EVENT CBs table */
static	OS_Q	OSQTbl[OS_MAX_QS];		/* QUEUE CBs table */


/* idle task */
static void
OSTaskIdle(void *data)
{
	for (;;) {
		ENTER_CRITICAL();
		OSIdleCtr++;
		EXIT_CRITICAL();
	}
} /* OSTaskIdle */

#ifdef EV1110
#define SetLED(n) SAr_WriteHexLedBank(n)
#endif

/* process system tick */
static void
OSTimeTick(void)
{
	OS_TCB	*ptcb = OSTCBList;
	/* define PIDLED if you would like to see the PID leds */
#ifdef	PIDLED
	static int Counter = 0;
	static int Timer = 0;
	extern	void SetLED(uint);
#endif	/* PIDLED */

	timer_reset() ;		/* Restart timer & clear int */

	ENTER_CRITICAL();
	OSTime++;
	EXIT_CRITICAL();

#ifdef	PIDLED
	/* every second change LEDs */
	if (Timer++ == 100) {
		if (++Counter > 0x0F)
			Counter= 0;
		SetLED(Counter);
		Timer = 0;
	}
#endif /* PIDLED */

	/* scan all TCB in list */
	ENTER_CRITICAL(); 
	for ( ; ptcb->OSTCBPrio != OS_LO_PRIO; ptcb = ptcb->OSTCBNext) { 
		if (ptcb->OSTCBDly)
			if (--ptcb->OSTCBDly == 0) {
				/* task is now ready to run */
				OSRdyGrp |= ptcb->OSTCBBitY;
				OSRdyTbl[ptcb->OSTCBY] |= ptcb->OSTCBBitX;
			}
	}
	EXIT_CRITICAL(); 

} /* OSTimeTick */


/* initialize uC/OS */
void	
OSInit(void)
{
	static void __swi(OSEnterSWI) OSEnter(void);
	int	i;

	/* initialize target hardware */
	targetInit();

	/* initialize variables */
	OSTime = 0;
	OSTCBHighRdy = (OS_TCB *)0;
	OSTCBCur = (OS_TCB *)0;
	OSTCBList = (OS_TCB *)0;
	OSIntNesting = 0;
	OSLockNesting = 0;
	OSRunning = FALSE;
	OSRdyGrp = 0;
	OSIdleCtr = 0;
	OSCtxSwCtr = 0;
	for (i = 0; i < 8; i++) 
		OSRdyTbl[i] = 0;
	for (i = 0; i < 64; i++) 
		OSTCBPrioTbl[i] = (OS_TCB *)0;
	for (i = 0; i < OS_MAX_TASKS; i++)
		OSTCBTbl[i].OSTCBNext = &OSTCBTbl[i + 1];
	OSTCBTbl[OS_MAX_TASKS].OSTCBNext = (OS_TCB *)0;
	OSTCBFreeList = &OSTCBTbl[0];
	for (i = 0; i < OS_MAX_EVENTS - 1; i++)
		OSEventTbl[i].OSEventPtr = &OSEventTbl[i + 1];
	OSEventTbl[OS_MAX_EVENTS - 1].OSEventPtr = (OS_EVENT *)0;
	OSEventFreeList = &OSEventTbl[0];
	for (i = 0; i < OS_MAX_QS - 1; i++)
		OSQTbl[i].OSQPtr = &OSQTbl[i + 1];
	OSQTbl[OS_MAX_QS - 1].OSQPtr = (OS_Q *)0;
	OSQFreeList = &OSQTbl[0];

	/* create the idle task */
	OSTaskCreate(OSTaskIdle, (void *)0, 
		(void *)&OSIdleStk[OS_IDLE_STK_SIZE], OS_LO_PRIO);
} /* OSInit */


/* start multitasking */
void	
OSStart(void)
{
	extern void OSStartHighRdy(void);	/* defined in subr.s */
	extern  PFV IRQInstall(int, PFV);
	uint	y, x, p, i;

	/* find the highest priority task */
	y = OSUnMapTbl[OSRdyGrp];
	x = OSUnMapTbl[OSRdyTbl[y]];
	p = (y << 3) + x;

	OSTCBHighRdy = OSTCBPrioTbl[p];

	/* install timer interrupt routine */
	i = (uint)SAir_GetSystemTimerIRQ() ; 
	IRQInstall(i, OSTimeTick) ;    

	OSRunning = 1;
	/* enable interrupts and run the highest priority task */
	OSStartHighRdy();
} /* OSStart */


/* uC/OS scheduler */
static void
OSSched(void)
{
	extern void OSCtxSw(void);	/* defined in subr.s */
	uint	y;

	ENTER_CRITICAL();   
	if (OSLockNesting == 0 && OSIntNesting == 0) {
		/* task scheduling enabled and not interrupt level */
		y = OSUnMapTbl[OSRdyGrp];
		OSTCBHighRdy = OSTCBPrioTbl[(y << 3) + OSUnMapTbl[OSRdyTbl[y]]];
		/* make sure this is not the current running task */
		if (OSTCBHighRdy != OSTCBCur) {
			OSCtxSwCtr++;
			OSCtxSw();	/* context switch */
		}
	}
	EXIT_CRITICAL();   
} /* OSSched */


/* prevent scheduling */
void	
OSSchedLock(void)
{
	ENTER_CRITICAL();
	OSLockNesting++;
	EXIT_CRITICAL();
} /* OSSchedLock */


/* allow scheduling */
void	
OSSchedUnlock(void)
{
	ENTER_CRITICAL();
	if (OSLockNesting == 0) {
		EXIT_CRITICAL();
		return;
	}
	OSLockNesting--;
	if (OSLockNesting == 0 && OSIntNesting == 0) {
		/* scheduling re-enabled and not an ISR */
		EXIT_CRITICAL();
		/* check if a higher priority task became ready */
		OSSched();
	} else
		EXIT_CRITICAL();
} /* OSSchedUnlock */


/* find and initialize a TCB */
static int
OSTCBInit(uint	prio, void *pstk)
{
	OS_TCB	*ptcb;

	ENTER_CRITICAL(); 
	if ((ptcb = OSTCBFreeList) == (OS_TCB *)0) {
		/* no TCB available */
		EXIT_CRITICAL(); 
		return(OS_NO_MORE_TCB);
	}

	OSTCBFreeList = ptcb->OSTCBNext;	/* link to the next free */
	OSTCBPrioTbl[prio] = ptcb;
	ptcb->OSTCBStkPtr = pstk;		/* top of private stack */
	ptcb->OSTCBPrio = prio;			/* task priority */
	ptcb->OSTCBStat = OS_STAT_RDY;		/* task is ready to run */
	ptcb->OSTCBDly = 0;
	ptcb->OSTCBX = prio & 0x07;
	ptcb->OSTCBBitX = OSMapTbl[prio & 0x07];
	ptcb->OSTCBY = prio >> 3;
	ptcb->OSTCBBitY = OSMapTbl[prio >> 3];
	ptcb->OSTCBEventPtr = (OS_EVENT *)0;
	ptcb->OSTCBNext = OSTCBList;
	if (OSTCBList)
		ptcb->OSTCBPrev = ptcb;
	else
		ptcb->OSTCBPrev = (OS_TCB *)0;
	OSTCBList = ptcb;
	OSRdyGrp |= OSMapTbl[prio >> 3];
	OSRdyTbl[prio >> 3] |= OSMapTbl[prio & 0x07];
	EXIT_CRITICAL(); 

	return(OS_NO_ERR);
} /* OSTCBInit */


/* create a task */
int	
OSTaskCreate(PTV task, void *pdata, void *ptsk, uint prio)
{
	uint	*stk;
	int	Ret;

	ENTER_CRITICAL(); 
	if (OSTCBPrioTbl[prio] != (OS_TCB *)0) {
		/* only one task allowed at its priority */
		EXIT_CRITICAL(); 
		Ret = OS_PRIO_EXIST;
	}
	else {
		EXIT_CRITICAL(); 
		stk = (uint *)ptsk;
		/* build a context for the new task */
	   	*--stk = (uint)task;	/* lr */
		*--stk = 0;    		/* r12 */
		*--stk = 0;		/* r11 */
		*--stk = 0;		/* r10 */
		*--stk = 0;		/* r9 */
		*--stk = 0;		/* r8 */
		*--stk = 0;		/* r7 */
		*--stk = 0;		/* r6 */
		*--stk = 0;		/* r5 */
		*--stk = 0;		/* r4 */
		*--stk = 0;		/* r3 */
		*--stk = 0;		/* r2 */
		*--stk = 0;		/* r1 */
		*--stk = (uint)pdata;   /* r0 */
		*--stk = SVC32MODE;  	/* spsr YYY+ */
		*--stk = SVC32MODE;  	/* psr */
		Ret = OSTCBInit(prio, (void *)stk);
		if (Ret == OS_NO_ERR && OSRunning) 
			OSSched();
	}

	return(Ret);
} /* OSCreateTask */


/* delete a task */
int	
OSTaskDel(uint prio)
{
	OS_EVENT	*pevent;
	OS_TCB	*ptcb;

	if (prio == OS_LO_PRIO)
		return(OS_TASK_DEL_IDLE);
	ENTER_CRITICAL();
	if ((ptcb = OSTCBPrioTbl[prio]) == (OS_TCB *)0) {
		EXIT_CRITICAL();
		return(OS_TASK_DEL_ERR);
	}
	/* clear old TCB entry */
	OSTCBPrioTbl[prio] = (OS_TCB *)0;
	/* make task not ready */
	if ((OSRdyTbl[ptcb->OSTCBY] &= ~(ptcb->OSTCBBitX)) == 0)
		OSRdyGrp &= ~(ptcb->OSTCBBitY);
	/* remove from TCB linked list */
	if (ptcb->OSTCBPrev == (OS_TCB *)0) {
		ptcb->OSTCBNext->OSTCBPrev = (OS_TCB *)0;
		OSTCBList = ptcb->OSTCBNext;
	}
	else {
		ptcb->OSTCBPrev->OSTCBNext = ptcb->OSTCBNext;
		ptcb->OSTCBNext->OSTCBPrev = ptcb->OSTCBPrev;
	}
	/* if task waiting on event remove it from event CB */
	if ((pevent = ptcb->OSTCBEventPtr) != (OS_EVENT *)0) 
		if ((pevent->OSEventTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0)
			pevent->OSEventGrp &= ~(ptcb->OSTCBBitY);
	/* return TCB to the free list */
	ptcb->OSTCBNext = OSTCBFreeList;
	OSTCBFreeList = ptcb;
	EXIT_CRITICAL();
	/* find new highest priority task */
	OSSched();

	return(OS_NO_ERR);
} /* OSTaskDel */


/* change task priority */
int	
OSTaskChangePrio(uint oldp, uint newp)
{
	OS_EVENT	*pevent;
	OS_TCB	*ptcb;
	int	rdy;

	ENTER_CRITICAL();
	/* check if new priority not in use */
	if (OSTCBPrioTbl[newp] != (OS_TCB *)0) {
		EXIT_CRITICAL();
		return(OS_PRIO_EXIST);
	}
	/* check if old priority task exists */
	if (OSTCBPrioTbl[oldp] == (OS_TCB *)0) {
		EXIT_CRITICAL();
		return(OS_PRIO_ERR);
	}
	/* remove TCB from old priority */
	OSTCBPrioTbl[oldp] = (OS_TCB *)0;
	/* if task is ready make it not ready */
	if (OSRdyTbl[ptcb->OSTCBY] & ptcb->OSTCBBitX) {
		if ((OSRdyTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0)
			OSRdyGrp &= ~(ptcb->OSTCBBitY);
		rdy = TRUE;
	} else 
		rdy = FALSE;
	/* remove from event list */
	if ((pevent = ptcb->OSTCBEventPtr) != (OS_EVENT *)0)
		if ((pevent->OSEventTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0)
			pevent->OSEventGrp &= ~ptcb->OSTCBBitY;
	/* remove from TCB linked list */
	if (ptcb->OSTCBPrev == (OS_TCB *)0) {
		ptcb->OSTCBNext->OSTCBPrev = (OS_TCB *)0;
		OSTCBList = ptcb->OSTCBNext;
	}
	else {
		ptcb->OSTCBPrev->OSTCBNext = ptcb->OSTCBNext;
		ptcb->OSTCBNext->OSTCBPrev = ptcb->OSTCBPrev;
	}
	EXIT_CRITICAL();
	/* setup task control block with new values */
	ptcb->OSTCBPrio = newp;
	ptcb->OSTCBY = newp >> 3;
	ptcb->OSTCBBitY = OSMapTbl[newp >> 3];
	ptcb->OSTCBX = newp & 0x07;
	ptcb->OSTCBBitX = OSMapTbl[newp & 0x07];
	ENTER_CRITICAL();
	/* make new priority ready to run if old one was ready */
	if (rdy) {
		OSRdyGrp |= ptcb->OSTCBBitY;
		OSRdyTbl[ptcb->OSTCBY] |= ptcb->OSTCBBitX;
	}
	else {
		if (pevent != (OS_EVENT *)0) {
			/* wait for event if old was waiting */
			pevent->OSEventTbl[ptcb->OSTCBY] |= ptcb->OSTCBBitX;
			pevent->OSEventGrp |= ptcb->OSTCBBitY;
		}
	}
	/* place TCB pointer at the new priority */
	OSTCBPrioTbl[newp] = ptcb;
	/* link TCB to the top of the list */
	OSTCBList->OSTCBPrev = ptcb;
	OSTCBList = ptcb;	/* at least the idle task is in the list */
	EXIT_CRITICAL();
	/* run highest priority task ready */
	OSSched();

	return(OS_NO_ERR);
} /* OSTaskChangePrio */


/* delay the current task */
void	
OSTimeDly(uint ticks)
{
	ENTER_CRITICAL();
	/* suspend the current task */
	if ((OSRdyTbl[OSTCBCur->OSTCBY] &= ~OSTCBCur->OSTCBBitX) == 0)
		OSRdyGrp &= ~OSTCBCur->OSTCBBitY;