os.c

uHAL for SA1110,INTEL公司出品

	/* load number of ticks in TCB */
	OSTCBCur->OSTCBDly = ticks;
	EXIT_CRITICAL();
	/* find a new task to run */
	OSSched();
} /* OSTimeDly */


/* set system time */
void	
OSTimeSet(uint ticks)
{
	ENTER_CRITICAL();
	OSTime = ticks;
	EXIT_CRITICAL();
} /* OSTimeSet */


/* get system time */
uint	
OSTimeGet(void)
{
	return(OSTime);
} /* OSTimeGet */


/* create a semaphore */
OS_EVENT*
OSSemCreate(int value)
{
	OS_EVENT	*pevent;

	ENTER_CRITICAL();
	/* get next available event CB */
	pevent = OSEventFreeList;
	/* update free list */
	if (OSEventFreeList)
		OSEventFreeList = (OS_EVENT *)OSEventFreeList->OSEventPtr;
	EXIT_CRITICAL();
	if (pevent == (OS_EVENT *)0)
		return((OS_EVENT *)0);	/* ran out of event CB */
	if (value < 0) {
		/* semaphore cannot start negative */
		ENTER_CRITICAL();
		/* return event CB back to the free list */
		pevent->OSEventPtr = (void *)OSEventFreeList;
		OSEventFreeList = pevent;
		EXIT_CRITICAL();
		return((OS_EVENT *)0);
	}
	/* set semaphore values */
	pevent->OSEventCnt = value;
	pevent->OSEventGrp = 0;
	pevent->OSEventTbl[0] = 0;
	pevent->OSEventTbl[1] = 0;
	pevent->OSEventTbl[2] = 0;
	pevent->OSEventTbl[3] = 0;
	pevent->OSEventTbl[4] = 0;
	pevent->OSEventTbl[5] = 0;
	pevent->OSEventTbl[6] = 0;
	pevent->OSEventTbl[7] = 0;

	return(pevent);
} /* OSSemCreate */


static void
OSEventTaskResume(OS_EVENT *pevent)
{
	uint	x, y, bitx, bity, p;
	OS_TCB	*ptcb;

	ENTER_CRITICAL(); 
	/* find highest priority task pending on event */
	y = OSUnMapTbl[pevent->OSEventGrp];
	bity = OSMapTbl[y];
	x = OSUnMapTbl[pevent->OSEventTbl[y]];
	bitx = OSMapTbl[x];
	p = (y << 3) + x;
	/* remove task from waiting list */
	if ((pevent->OSEventTbl[y] &= ~bitx) == 0)
		pevent->OSEventGrp &= ~bity;
	/* pointer to new task */ 
	ptcb = OSTCBPrioTbl[p];
	/* reset timeout */
	ptcb->OSTCBDly = 0;
	/* task is ready to run */
	ptcb->OSTCBStat = OS_STAT_RDY;
	/* unlink control block */
	ptcb->OSTCBEventPtr = (OS_EVENT *)0;
	/* put task in ready to run list */
	OSRdyGrp |= bity;
	OSRdyTbl[y] |= bitx;
	EXIT_CRITICAL(); 
} /* OSEventTaskResume */


/* signal semaphore */
uint		
OSSemPost(OS_EVENT *pevent)
{
	ENTER_CRITICAL();
	if (INT_MAX == pevent->OSEventCnt) {
		/* semaphore overflow */
		EXIT_CRITICAL();
		return(OS_SEM_OVF);
	}
	if (pevent->OSEventCnt++ < 0) {
		/* negative semaphore means tasks pending */
		if (pevent->OSEventGrp) {
			EXIT_CRITICAL();  
			/* resume highest priority task pending on semaphore */
			OSEventTaskResume(pevent);
			OSSched();
		}
	} else
		EXIT_CRITICAL();

	return(OS_NO_ERR);
} /* OSSemPost */


/* wait semaphore */
void		
OSSemPend(OS_EVENT *pevent, uint timeout, uint *err)
{
	ENTER_CRITICAL();
	if (pevent->OSEventCnt-- > 0) {
		/* resource available */
		EXIT_CRITICAL();
		*err = OS_NO_ERR;
		return;
	}
	/* resource not available, sleep on semaphore */
	OSTCBCur->OSTCBStat = OS_STAT_SEM;
	OSTCBCur->OSTCBDly = timeout;
	/* suspend task */
	if ((OSRdyTbl[OSTCBCur->OSTCBY] &= ~OSTCBCur->OSTCBBitX) == 0)
		OSRdyGrp &= ~OSTCBCur->OSTCBBitY;
	pevent->OSEventTbl[OSTCBCur->OSTCBY] |= OSTCBCur->OSTCBBitX;
	pevent->OSEventGrp |= OSTCBCur->OSTCBBitY;
	EXIT_CRITICAL();
	/* find next task ready to run */
	OSSched();
	ENTER_CRITICAL();
	if (OSTCBCur->OSTCBStat == OS_STAT_SEM) {
		/* still pending on semaphore: timeout */
		if ((pevent->OSEventTbl[OSTCBCur->OSTCBY] &= 
						~OSTCBCur->OSTCBBitX) ==0)
			pevent->OSEventGrp &= ~OSTCBCur->OSTCBBitY;
		OSTCBCur->OSTCBStat = OS_STAT_RDY;
		/* no longer waiting for event */
		OSTCBCur->OSTCBEventPtr = (OS_EVENT *)0;
		EXIT_CRITICAL();
		/* signal timeout */
		*err = OS_TIMEOUT;
	}
	else {
		OSTCBCur->OSTCBEventPtr = (OS_EVENT *)0;
		EXIT_CRITICAL();
		*err = OS_NO_ERR;
	}
} /* OSSemPend */


/* create mailbox */
OS_EVENT*
OSMboxCreate(void *msg)
{
	OS_EVENT	*pevent;

	ENTER_CRITICAL();
	/* get next available event CB */
	pevent = OSEventFreeList;
	/* update free list */
	if (OSEventFreeList)
		OSEventFreeList = (OS_EVENT *)OSEventFreeList->OSEventPtr;
	EXIT_CRITICAL();
	if (pevent == (OS_EVENT *)0)
		return((OS_EVENT *)0);	/* ran out of event CB */
	/* set mailbox values */
	pevent->OSEventPtr = msg;
	pevent->OSEventGrp = 0;
	pevent->OSEventTbl[0] = 0;
	pevent->OSEventTbl[1] = 0;
	pevent->OSEventTbl[2] = 0;
	pevent->OSEventTbl[3] = 0;
	pevent->OSEventTbl[4] = 0;
	pevent->OSEventTbl[5] = 0;
	pevent->OSEventTbl[6] = 0;
	pevent->OSEventTbl[7] = 0;

	return(pevent);
} /* OSMBoxCreate */


/* post mbox message */
uint		
OSMboxPost(OS_EVENT *pevent, void *msg)
{
	ENTER_CRITICAL();
	/* make sure mailbox doesn't contain a message */
	if (pevent->OSEventPtr != (void *)0) {
		EXIT_CRITICAL();
		return(OS_MBOX_FULL);
	}
	/* place message in mailbox */
	pevent->OSEventPtr = msg;
	/* see if any task is pending on the mailbox */
	if (pevent->OSEventGrp) {
		EXIT_CRITICAL();
		/* resume highest priority task pending on mailbox */
		OSEventTaskResume(pevent);
		OSSched();
	} else
		EXIT_CRITICAL();

	return(OS_NO_ERR);
} /* OSMboxPost */


/* pend for mbox message */
void*
OSMboxPend(OS_EVENT *pevent, uint timeout, uint *err)
{
	void	*msg;

	ENTER_CRITICAL();
	/* check if the mailbox already has a message */
	if ((msg = pevent->OSEventPtr) != (void *)0) {
		/* clear the mailbox */
		pevent->OSEventPtr = (void *)0;
		EXIT_CRITICAL();
		*err = OS_NO_ERR;
		/* return mailbox content */
		return(msg);
	}
	/* resource not available, sleep on mailbox */
	OSTCBCur->OSTCBStat = OS_STAT_MBOX;
	OSTCBCur->OSTCBDly = timeout;
	/* suspend task */
	if ((OSRdyTbl[OSTCBCur->OSTCBY] &= ~OSTCBCur->OSTCBBitX) == 0)
		OSRdyGrp &= ~OSTCBCur->OSTCBBitY;
	pevent->OSEventTbl[OSTCBCur->OSTCBY] |= OSTCBCur->OSTCBBitX;
	pevent->OSEventGrp |= OSTCBCur->OSTCBBitY;
	EXIT_CRITICAL();
	/* find next task ready to run */
	OSSched();
	ENTER_CRITICAL();
	if (OSTCBCur->OSTCBStat == OS_STAT_MBOX) {
		/* still pending on mailbox: timeout */
		if ((pevent->OSEventTbl[OSTCBCur->OSTCBY] &=
                                                ~OSTCBCur->OSTCBBitX) == 0)
                        pevent->OSEventGrp &= ~OSTCBCur->OSTCBBitY;
                OSTCBCur->OSTCBStat = OS_STAT_RDY;
                /* no longer waiting for event */
                OSTCBCur->OSTCBEventPtr = (OS_EVENT *)0;
		/* set message content to null */
		msg = (void *)0;
                EXIT_CRITICAL();
                /* signal timeout */
                *err = OS_TIMEOUT;
	}
	else {
		/* message received */
		msg = pevent->OSEventPtr;
		/* clear the mailbox */
		pevent->OSEventPtr = (void *)0;
		EXIT_CRITICAL();
		*err = OS_NO_ERR;
	}

	return(msg);
} /* OSMBoxPend */


/* create a queue */
OS_EVENT*
OSQCreate(void **start, uint size)
{
	OS_EVENT	*pevent;
	OS_Q		*pq;

	ENTER_CRITICAL();
	/* get next available event CB */
	pevent = OSEventFreeList;
	/* update free list */
	if (OSEventFreeList)
		OSEventFreeList = (OS_EVENT *)OSEventFreeList->OSEventPtr;

	if (pevent == (OS_EVENT *)0) {
		/* ran out of event CB */
		EXIT_CRITICAL();
		return((OS_EVENT *)0);	
	}
	
	pq = OSQFreeList;
	if (OSQFreeList != (OS_Q *)0)
		OSQFreeList = OSQFreeList->OSQPtr;
	
	if (pq == (OS_Q *)0) {
		/* ran out of queue CB return event CB to free list */
		pevent->OSEventPtr = (void *)OSEventFreeList;
		OSEventFreeList = pevent;
		EXIT_CRITICAL();
		return((OS_EVENT *)0);
	}

	EXIT_CRITICAL();
	/* initialize the queue */
	pq->OSQStart = start;
	pq->OSQEnd = &start[size];
	pq->OSQIn = start;
	pq->OSQOut = start;
	pq->OSQSize = size;
	pq->OSQEntries = 0;
	pevent->OSEventPtr = pq;
	pevent->OSEventGrp = 0;
	pevent->OSEventTbl[0] = 0;
	pevent->OSEventTbl[1] = 0;
	pevent->OSEventTbl[2] = 0;
	pevent->OSEventTbl[3] = 0;
	pevent->OSEventTbl[4] = 0;
	pevent->OSEventTbl[5] = 0;
	pevent->OSEventTbl[6] = 0;
	pevent->OSEventTbl[7] = 0;

	return(pevent);
} /* OSQCreate */


/* post a message to a queue */
uint		
OSQPost(OS_EVENT *pevent, void *msg)
{
	OS_Q	*pq;

	ENTER_CRITICAL();
	pq = (OS_Q*)(pevent->OSEventPtr);
	/* make sure the queue is not full */
	if (pq->OSQEntries >= pq->OSQSize) {
		EXIT_CRITICAL();
		return(OS_Q_FULL);
	}
	/* insert message into queue */
	*pq->OSQIn++ = msg;
	pq->OSQEntries++;
	if (pq->OSQIn == pq->OSQEnd)
		/* wrap around In pointer if reached end */
		pq->OSQIn = pq->OSQStart;

	/* see if any task is pending on the mailbox */
	if (pevent->OSEventGrp) {
		EXIT_CRITICAL();
		/* resume highest priority task pending on mailbox */
		OSEventTaskResume(pevent);
		OSSched();
	} else
		EXIT_CRITICAL();

	return(OS_NO_ERR);
} /* OSQPost */


/* pend for message from a queue */
void*
OSQPend(OS_EVENT *pevent, uint timeout, uint *err)
{
	void	*msg;
	OS_Q	*pq;

	ENTER_CRITICAL();
	pq = (OS_Q *)(pevent->OSEventPtr);
	/* check if the queue already has a message */
	if (pq->OSQEntries != 0) {
		/* extract the message */
		msg = *pq->OSQOut++;
		/* update message counter */
		pq->OSQEntries--;
		/* wrap around Out pointer if reached end */
		if (pq->OSQOut == pq->OSQEnd)
			pq->OSQOut = pq->OSQStart;
		EXIT_CRITICAL();
		*err = OS_NO_ERR;
		return(msg);
	}
	/* no messages available, sleep on the queue */
	OSTCBCur->OSTCBStat = OS_STAT_Q;
	OSTCBCur->OSTCBDly = timeout;
	/* suspend task */
        if ((OSRdyTbl[OSTCBCur->OSTCBY] &= ~OSTCBCur->OSTCBBitX) == 0)
                OSRdyGrp &= ~OSTCBCur->OSTCBBitY;
        pevent->OSEventTbl[OSTCBCur->OSTCBY] |= OSTCBCur->OSTCBBitX;
        pevent->OSEventGrp |= OSTCBCur->OSTCBBitY;
        EXIT_CRITICAL();
        /* find next task ready to run */
        OSSched();
        ENTER_CRITICAL();
	if (OSTCBCur->OSTCBStat == OS_STAT_Q) {
		/* still pending on queue: timeout */
		if ((pevent->OSEventTbl[OSTCBCur->OSTCBY] &=
                                                ~OSTCBCur->OSTCBBitX) == 0)
                        pevent->OSEventGrp &= ~OSTCBCur->OSTCBBitY;
		OSTCBCur->OSTCBStat = OS_STAT_RDY;
		/* no longer waiting for event */
		OSTCBCur->OSTCBEventPtr = (OS_EVENT *)0;
		/* set message content to null */
		msg = (void *)0;
		EXIT_CRITICAL();
		/* signal timeout */
		*err = OS_TIMEOUT;
	}
	else {
		/* message received */
		msg = *pq->OSQOut++;
		/* update message counter */
		pq->OSQEntries--;
		/* wrap around Out pointer if reached end */
		if (pq->OSQOut == pq->OSQEnd)
			pq->OSQOut = pq->OSQStart;
		EXIT_CRITICAL();
		*err = OS_NO_ERR;
	}
	
	return(msg);
} /* OSQPend */


/* exit from interrupt */
int
OSIntExit(void)
{
	uint	y;

	ENTER_CRITICAL();	/* balanced by restoring PSR in IRQTrap */
	if (--OSIntNesting == 0 && OSLockNesting == 0) {
		/* reschedule if all ISR completed and not locked */
		y = OSUnMapTbl[OSRdyGrp];
		OSTCBHighRdy = OSTCBPrioTbl[(y << 3) + OSUnMapTbl[OSRdyTbl[y]]];

		if (OSTCBHighRdy == OSTCBCur) {
			/* the highest priority task is the current task */
			return(FALSE);
		}
		else {
			/* signal preemption to IRQTrap */
			return(TRUE);
		}
	}
	return(FALSE);
} /* OSIntExit */