/********************************************************************************************************
*												uC/OS-II
*											The Real-Time Kernel
*												CORE FUNCTIONS
*
*							(c) Copyright 1992-2002, Jean J. Labrosse, Weston, FL
*											All Rights Reserved
*
* File : OS_CORE.C
* By   : Jean J. Labrosse
********************************************************************************************************/

#ifndef  OS_MASTER_FILE
#define  OS_GLOBALS
#include "includes.h"
#endif

/********************************************************************************************************
*							  MAPPING TABLE TO MAP BIT POSITION TO BIT MASK
*
* Note: Index into table is desired bit position, 0..7
*	   Indexed value corresponds to bit mask
********************************************************************************************************/

INT8U  const  OSMapTbl[]   = {0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80};

/********************************************************************************************************
*									   PRIORITY RESOLUTION TABLE
*
* Note: Index into table is bit pattern to resolve highest priority
*	   Indexed value corresponds to highest priority bit position (i.e. 0..7)
********************************************************************************************************/
INT8U  const  OSUnMapTbl[] = {
		0, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x00 to 0x0F								*/
		4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x10 to 0x1F								*/
		5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x20 to 0x2F								*/
		4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x30 to 0x3F								*/
		6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x40 to 0x4F								*/
		4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x50 to 0x5F								*/
		5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x60 to 0x6F								*/
		4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x70 to 0x7F								*/
		7, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x80 to 0x8F								*/
		4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0x90 to 0x9F								*/
		5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0xA0 to 0xAF								*/
		4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0xB0 to 0xBF								*/
		6, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0xC0 to 0xCF								*/
		4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0xD0 to 0xDF								*/
		5, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0,			/* 0xE0 to 0xEF								*/
		4, 0, 1, 0, 2, 0, 1, 0, 3, 0, 1, 0, 2, 0, 1, 0			/* 0xF0 to 0xFF								*/
};

/********************************************************************************************************
*										FUNCTION PROTOTYPES
********************************************************************************************************/
static  void  OS_InitEventList(void);
static  void  OS_InitMisc(void);
static  void  OS_InitRdyList(void);
static  void  OS_InitTaskIdle(void);
static  void  OS_InitTaskStat(void);
static  void  OS_InitTCBList(void);

/********************************************************************************************************
*											INITIALIZATION
* Description: This function is used to initialize the internals of uC/OS-II and MUST be called prior to
*			  creating any uC/OS-II object and, prior to calling OSStart().
* Arguments  : none
* Returns	: none
********************************************************************************************************/
void  OSInit (void)
{
#if OS_VERSION >= 204
	OSInitHookBegin();											// Call port specific initialization code
#endif

	OS_InitMisc();												// Initialize miscellaneous variables

	OS_InitRdyList();											// Initialize the Ready List
	OS_InitTCBList();											// Initialize the free list of OS_TCBs
	OS_InitEventList();											// Initialize the free list of OS_EVENTs

#if (OS_VERSION >= 251) && (OS_FLAG_EN > 0) && (OS_MAX_FLAGS > 0)
	OS_FlagInit();												// Initialize the event flag structures
#endif

#if (OS_MEM_EN > 0) && (OS_MAX_MEM_PART > 0)
	OS_MemInit();												// Initialize the memory manager
#endif

#if (OS_Q_EN > 0) && (OS_MAX_QS > 0)
	OS_QInit();													// Initialize the message queue structures
#endif

	OS_InitTaskIdle();											// Create the Idle Task
#if OS_TASK_STAT_EN > 0
	OS_InitTaskStat();											// Create the Statistic Task
#endif

#if OS_VERSION >= 204
	OSInitHookEnd();											// Call port specific init. code
#endif
}
/********************************************************************************************************
*											  ENTER ISR
*
* Description: This function is used to notify uC/OS-II that you are about to service an interrupt
*			  service routine (ISR).  This allows uC/OS-II to keep track of interrupt nesting and thus
*			  only perform rescheduling at the last nested ISR.
*
* Arguments  : none
*
* Returns	: none
*
* Notes	  : 1) This function should be called ith interrupts already disabled
*			  2) Your ISR can directly increment OSIntNesting without calling this function because
*				 OSIntNesting has been declared 'global'.  
*			  3) You MUST still call OSIntExit() even though you increment OSIntNesting directly.
*			  4) You MUST invoke OSIntEnter() and OSIntExit() in pair.  In other words, for every call
*				 to OSIntEnter() at the beginning of the ISR you MUST have a call to OSIntExit() at the
*				 end of the ISR.
*			  5) You are allowed to nest interrupts up to 255 levels deep.
*			  6) I removed the OS_ENTER_CRITICAL() and OS_EXIT_CRITICAL() around the increment because
*				 OSIntEnter() is always called with interrupts disabled.
********************************************************************************************************/
void OSIntEnter (void)
{
	if (OSRunning == TRUE) {
		if (OSIntNesting < 255) {
			OSIntNesting++;						// Increment ISR nesting level
		}
	}
}

/********************************************************************************************************
*												EXIT ISR
*
* Description: This function is used to notify uC/OS-II that you have completed serviving an ISR.  When
*			   the last nested ISR has completed, uC/OS-II will call the scheduler to determine whether
*			   a new, high-priority task, is ready to run.
*
* Arguments	: none
*
* Returns	: none
*
* Notes		: 1) You MUST invoke OSIntEnter() and OSIntExit() in pair.  In other words, for every call
*				 to OSIntEnter() at the beginning of the ISR you MUST have a call to OSIntExit() at the
*				 end of the ISR.
*			  2) Rescheduling is prevented when the scheduler is locked (see OS_SchedLock())
********************************************************************************************************/
void  OSIntExit (void)
{
#if OS_CRITICAL_METHOD == 3								// Allocate storage for CPU status register
	OS_CPU_SR  cpu_sr;
#endif

	if (OSRunning == TRUE) {
		OS_ENTER_CRITICAL();
		if (OSIntNesting > 0) {							// Prevent OSIntNesting from wrapping
			OSIntNesting--;
		}
		if ((OSIntNesting == 0) && (OSLockNesting == 0)) {	//* Reschedule only if all ISRs complete
			OSIntExitY	= OSUnMapTbl[OSRdyGrp];			// ... and not locked.
			OSPrioHighRdy = (INT8U)((OSIntExitY << 3) + OSUnMapTbl[OSRdyTbl[OSIntExitY]]);
			if (OSPrioHighRdy != OSPrioCur) {			// No Ctx Sw if current task is highest rdy
				OSTCBHighRdy  = OSTCBPrioTbl[OSPrioHighRdy];
				OSCtxSwCtr++;							// Keep track of the number of ctx switches
				OSIntCtxSw();							// Perform interrupt level ctx switch
			}
		}
		OS_EXIT_CRITICAL();
	}
}
/********************************************************************************************************
*											PREVENT SCHEDULING
*
* Description: This function is used to prevent rescheduling to take place.  This allows your application
*			  to prevent context switches until you are ready to permit context switching.
* Arguments  : none
* Returns	: none
* Notes	  : 1) You MUST invoke OSSchedLock() and OSSchedUnlock() in pair.  In other words, for every
*				 call to OSSchedLock() you MUST have a call to OSSchedUnlock().
********************************************************************************************************/
#if OS_SCHED_LOCK_EN > 0
void  OSSchedLock (void)
{
#if OS_CRITICAL_METHOD == 3							// Allocate storage for CPU status register
	OS_CPU_SR  cpu_sr;
#endif

	if (OSRunning == TRUE) {						// Make sure multitasking is running
		OS_ENTER_CRITICAL();
		if (OSLockNesting < 255) {					// Prevent OSLockNesting from wrapping back to 0
			OSLockNesting++;						// Increment lock nesting level
		}
		OS_EXIT_CRITICAL();
	}
}
#endif

/********************************************************************************************************
*											ENABLE SCHEDULING
*
* Description: This function is used to re-allow rescheduling.
* Arguments  : none
* Returns	: none
* Notes	  : 1) You MUST invoke OSSchedLock() and OSSchedUnlock() in pair.  In other words, for every
*				 call to OSSchedLock() you MUST have a call to OSSchedUnlock().
********************************************************************************************************/
#if OS_SCHED_LOCK_EN > 0
void  OSSchedUnlock (void)
{
#if OS_CRITICAL_METHOD == 3										// Allocate storage for CPU status register
	OS_CPU_SR  cpu_sr;
#endif

	if (OSRunning == TRUE) {									// Make sure multitasking is running
		OS_ENTER_CRITICAL();
		if (OSLockNesting > 0) {								// Do not decrement if already 0
			OSLockNesting--;									// Decrement lock nesting level
			if ((OSLockNesting == 0) && (OSIntNesting == 0)) {	// See if sched. enabled and not an ISR
				OS_EXIT_CRITICAL();
				OS_Sched();										// See if a HPT is ready
			} else {
				OS_EXIT_CRITICAL();
			}
		}
		else {
			OS_EXIT_CRITICAL();
		}
	}
}
#endif

/********************************************************************************************************
*											START MULTITASKING
*
* Description: This function is used to start the multitasking process which lets uC/OS-II manages the
*			   task that you have created.  Before you can call OSStart(), you MUST have called OSInit()
*			   and you MUST have created at least one task.
*
* Arguments	: none
*
* Returns	: none
*
* Note		: OSStartHighRdy() MUST:
*				a) Call OSTaskSwHook() then,
*				b) Set OSRunning to TRUE.
*				c) Load the context of the task pointed to by OSTCBHighRdy.
*				d_ Execute the task.
********************************************************************************************************/
void OSStart (void)
{
	INT8U y;
	INT8U x;

	if (OSRunning == FALSE) {
		y = OSUnMapTbl[OSRdyGrp];					// Find highest priority's task priority number
		x = OSUnMapTbl[OSRdyTbl[y]];
		OSPrioHighRdy = (INT8U)((y << 3) + x);
		OSPrioCur = OSPrioHighRdy;
		OSTCBHighRdy = OSTCBPrioTbl[OSPrioHighRdy];	// Point to highest priority task ready to run
		OSTCBCur = OSTCBHighRdy;
		OSStartHighRdy();							// Execute target specific code to start task
	}
}

/********************************************************************************************************
*										STATISTICS INITIALIZATION
*
* Description: This function is called by your application to establish CPU usage by first determining
*			  how high a 32-bit counter would count to in 1 second if no other tasks were to execute
*			  during that time.  CPU usage is then determined by a low priority task which keeps track
*			  of this 32-bit counter every second but this time, with other tasks running.  CPU usage is
*			  determined by:
*
*											 OSIdleCtr
*				 CPU Usage (%) = 100 * (1 - ------------)
*											OSIdleCtrMax
*
* Arguments  : none
* Returns	: none
********************************************************************************************************/
#if OS_TASK_STAT_EN > 0
void  OSStatInit (void)
{
#if OS_CRITICAL_METHOD == 3						// Allocate storage for CPU status register
	OS_CPU_SR  cpu_sr;
#endif	

	OSTimeDly(2);								// Synchronize with clock tick
	OS_ENTER_CRITICAL();
	OSIdleCtr	= 0L;							// Clear idle counter
	OS_EXIT_CRITICAL();
	OSTimeDly(OS_TICKS_PER_SEC);				// Determine MAX. idle counter value for 1 second
	OS_ENTER_CRITICAL();
	OSIdleCtrMax = OSIdleCtr;					// Store maximum idle counter count in 1 second
	OSStatRdy	= TRUE;
	OS_EXIT_CRITICAL();
}
#endif

/********************************************************************************************************
*										PROCESS SYSTEM TICK
*
* Description: This function is used to signal to uC/OS-II the occurrence of a 'system tick' (also known
*			   as a 'clock tick').  This function should be called by the ticker ISR but, can also be
*			   called by a high priority task.
*
* Arguments  : none
*
* Returns	: none
********************************************************************************************************/
void OSTimeTick (void)
{
#if OS_CRITICAL_METHOD == 3								// Allocate storage for CPU status register
	OS_CPU_SR  cpu_sr;
#endif	
	OS_TCB	*ptcb;

	OSTimeTickHook();									// Call user definable hook
#if OS_TIME_GET_SET_EN > 0   
	OS_ENTER_CRITICAL();								// Update the 32-bit tick counter
	OSTime++;
	OS_EXIT_CRITICAL();
#endif
	if (OSRunning == TRUE) {	
		ptcb = OSTCBList;								// Point at first TCB in TCB list
		while (ptcb->OSTCBPrio != OS_IDLE_PRIO) {		// Go through all TCBs in TCB list
			OS_ENTER_CRITICAL();
			if (ptcb->OSTCBDly != 0) {					// Delayed or waiting for event with TO
				if (--ptcb->OSTCBDly == 0) {			// Decrement nbr of ticks to end of delay
					if ((ptcb->OSTCBStat & OS_STAT_SUSPEND) == OS_STAT_RDY) {	// Is task suspended?
						OSRdyGrp			   |= ptcb->OSTCBBitY;	// No,  Make task R-to-R (timed out)
						OSRdyTbl[ptcb->OSTCBY] |= ptcb->OSTCBBitX;