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


#define  OS_CORE_GLOBALS
#include "INCLUDES.H"

/*
*********************************************************************************************************
*                                         LOCAL GLOBAL VARIABLES
*********************************************************************************************************
*/
                                       
static  INT8U  xdata      OSIntExitY;               /* Variable used by 'OSIntExit' to prevent using locals */

static  OS_STK xdata      OSTaskIdleStk[OS_TASK_IDLE_STK_SIZE];       /* Idle       task stack              */

#if     OS_TASK_STAT_EN
static  OS_STK xdata      OSTaskStatStk[OS_TASK_STAT_STK_SIZE];      /* Statistics task stack               */
#endif

static  OS_TCB xdata      OSTCBTbl[OS_MAX_TASKS + OS_N_SYS_TASKS];   /* Table of TCBs                       */

/*$PAGE*/
/*
*********************************************************************************************************
*                              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 code 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 code 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
};

/*$PAGE*/
/*
*********************************************************************************************************
*                             MAKE TASK READY TO RUN BASED ON EVENT OCCURING
*
* Description: This function is called by other uC/OS-II services and is used to ready a task that was
*              waiting for an event to occur.
*
* Arguments  : pevent    is a pointer to the event control block corresponding to the event.
*
*              msg       is a pointer to a message.  This pointer is used by message oriented services
*                        such as MAILBOXEs and QUEUEs.  The pointer is not used when called by other
*                        service functions.
*
*              msk       is a mask that is used to clear the status byte of the TCB.  For example,
*                        OSSemPost() will pass OS_STAT_SEM, OSMboxPost() will pass OS_STAT_MBOX etc.
*
* Returns    : none
*
* Note       : This function is INTERNAL to uC/OS-II and your application should not call it.
*********************************************************************************************************
*/
#if  (OS_Q_EN && (OS_MAX_QS >= 2)) || OS_MBOX_EN || OS_Sem_EN
void  OSEventTaskRdy (OS_EVENT *pevent, void *msg, INT8U msk)reentrant
{
    OS_TCB *ptcb;
    INT8U   x;
    INT8U   y;
    INT8U   bitx;
    INT8U   bity;
    INT8U   prio;


    y    = OSUnMapTbl[pevent->OSEventGrp];            /* Find highest prio. task waiting for message   */
    bity = OSMapTbl[y];
    x    = OSUnMapTbl[pevent->OSEventTbl[y]];
    bitx = OSMapTbl[x];
    prio = (INT8U)((y << 3) + x);                     /* Find priority of task getting the msg         */
    if ((pevent->OSEventTbl[y] &= ~bitx) == 0) {      /* Remove this task from the waiting list        */
        pevent->OSEventGrp &= ~bity;
    }
    ptcb                 =  OSTCBPrioTbl[prio];       /* Point to this task's OS_TCB                   */
    ptcb->OSTCBDly       =  0;                        /* Prevent OSTimeTick() from readying task       */
    ptcb->OSTCBEventPtr  = (OS_EVENT *)0;             /* Unlink ECB from this task                     */
#if (OS_Q_EN && (OS_MAX_QS >= 2)) || OS_MBOX_EN
    ptcb->OSTCBMsg       = msg;                       /* Send message directly to waiting task         */
#else
    msg                  = msg;                       /* Prevent compiler warning if not used          */    
#endif    
    ptcb->OSTCBStat     &= ~msk;                      /* Clear bit associated with event type          */
    if (ptcb->OSTCBStat == OS_STAT_RDY) {             /* See if task is ready (could be susp'd)        */
        OSRdyGrp        |=  bity;                     /* Put task in the ready to run list             */
        OSRdyTbl[y]     |=  bitx;
    }
}
#endif
/*$PAGE*/
/*
*********************************************************************************************************
*                                   MAKE TASK WAIT FOR EVENT TO OCCUR
*
* Description: This function is called by other uC/OS-II services to suspend a task because an event has
*              not occurred.
*
* Arguments  : pevent   is a pointer to the event control block for which the task will be waiting for.
*
* Returns    : none
*
* Note       : This function is INTERNAL to uC/OS-II and your application should not call it.
*********************************************************************************************************
*/
#if  (OS_Q_EN && (OS_MAX_QS >= 2)) || OS_MBOX_EN || OS_Sem_EN
void  OSEventTaskWait (OS_EVENT *pevent)reentrant
{
    OSTCBCur->OSTCBEventPtr = pevent;            /* Store pointer to event control block in TCB        */
    if ((OSRdyTbl[OSTCBCur->OSTCBY] &= ~OSTCBCur->OSTCBBitX) == 0) {      /* Task no longer ready      */
        OSRdyGrp &= ~OSTCBCur->OSTCBBitY;
    }
    pevent->OSEventTbl[OSTCBCur->OSTCBY] |= OSTCBCur->OSTCBBitX;          /* Put task in waiting list  */
    pevent->OSEventGrp                   |= OSTCBCur->OSTCBBitY;
}
#endif
/*$PAGE*/
/*
*********************************************************************************************************
*                              MAKE TASK READY TO RUN BASED ON EVENT TIMEOUT
*
* Description: This function is called by other uC/OS-II services to make a task ready to run because a
*              timeout occurred.
*
* Arguments  : pevent   is a pointer to the event control block which is readying a task.
*
* Returns    : none
*
* Note       : This function is INTERNAL to uC/OS-II and your application should not call it.
*********************************************************************************************************
*/
#if  (OS_Q_EN && (OS_MAX_QS >= 2)) || OS_MBOX_EN || OS_Sem_EN
void  OSEventTO (OS_EVENT *pevent)reentrant
{
    if ((pevent->OSEventTbl[OSTCBCur->OSTCBY] &= ~OSTCBCur->OSTCBBitX) == 0) {
        pevent->OSEventGrp &= ~OSTCBCur->OSTCBBitY;
    }
    OSTCBCur->OSTCBStat     = OS_STAT_RDY;       /* Set status to ready                                */
    OSTCBCur->OSTCBEventPtr = (OS_EVENT *)0;     /* No longer waiting for event                        */
}
#endif
/*$PAGE*/
/*
*********************************************************************************************************
*                                 INITIALIZE EVENT CONTROL BLOCK'S WAIT LIST
*
* Description: This function is called by other uC/OS-II services to initialize the event wait list.
*
* Arguments  : pevent    is a pointer to the event control block allocated to the event.
*
* Returns    : none
*
* Note       : This function is INTERNAL to uC/OS-II and your application should not call it.
*********************************************************************************************************
*/
#if  (OS_Q_EN && (OS_MAX_QS >= 2)) || OS_MBOX_EN || OS_Sem_EN
void  OSEventWaitListInit (OS_EVENT *pevent)reentrant
{
    INT8U i;
    
    
    pevent->OSEventGrp = 0x00;                   /* No task waiting on event                           */
    for (i = 0; i < OS_EVENT_TBL_SIZE; i++) {
        pevent->OSEventTbl[i] = 0x00;
    }
}
#endif
/*$PAGE*/
/*
*********************************************************************************************************
*                                             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)
{
    INT16U i;


    OSTime        = 0L;                                    /* Clear the 32-bit system clock            */
    OSIntNesting  = 0;                                     /* Clear the interrupt nesting counter      */
    OSLockNesting = 0;                                     /* Clear the scheduling lock counter        */
#if OS_TASK_CREATE_EN  || OS_TASK_CREATE_EXT_EN || OS_TASK_DEL_EN
    OSTaskCtr     = 0;                                     /* Clear the number of tasks                */
#endif
    OSRunning     = FALSE;                                 /* Indicate that multitasking not started   */
    OSIdleCtr     = 0L;                                    /* Clear the 32-bit idle counter            */
#if OS_TASK_STAT_EN && OS_TASK_CREATE_EXT_EN
    OSIdleCtrRun  = 0L;
    OSIdleCtrMax  = 0L;
    OSStatRdy     = FALSE;                                 /* Statistic task is not ready              */
#endif
    OSCtxSwCtr    = 0;                                     /* Clear the context switch counter         */
    OSRdyGrp      = 0;                                     /* Clear the ready list                     */
    for (i = 0; i < OS_RDY_TBL_SIZE; i++) {
        OSRdyTbl[i] = 0;
    }
                
    OSPrioCur     = 0;
    OSPrioHighRdy = 0;                                           
    OSTCBHighRdy  = (OS_TCB *)0;                                 /* TCB Initialization                 */
    OSTCBCur      = (OS_TCB *)0;
    OSTCBList     = (OS_TCB *)0;
    for (i = 0; i < (OS_LOWEST_PRIO + 1); i++) {                 /* Clear the priority table           */
        OSTCBPrioTbl[i] = (OS_TCB *)0;
    }
    for (i = 0; i < (OS_MAX_TASKS + OS_N_SYS_TASKS - 1); i++) {  /* Init. list of free TCBs            */
        OSTCBTbl[i].OSTCBNext = &OSTCBTbl[i + 1];
    }
    OSTCBTbl[OS_MAX_TASKS + OS_N_SYS_TASKS - 1].OSTCBNext = (OS_TCB *)0;    /* Last OS_TCB             */
    OSTCBFreeList                                         = &OSTCBTbl[0];

#if OS_MAX_EVENTS >= 2
    for (i = 0; i < (OS_MAX_EVENTS - 1); i++) {            /* Init. list of free EVENT control blocks  */
        OSEventTbl[i].OSEventPtr = (OS_EVENT *)&OSEventTbl[i + 1];
    }
    OSEventTbl[OS_MAX_EVENTS - 1].OSEventPtr = (OS_EVENT *)0;
    OSEventFreeList                          = &OSEventTbl[0];    
#endif

#if OS_Q_EN && (OS_MAX_QS >= 2)
    OSQInit();                                             /* Initialize the message queue structures  */
#endif

#if OS_MEM_EN && OS_MAX_MEM_PART >= 2
    OSMemInit();                                           /* Initialize the memory manager            */
#endif    

#if OS_STK_GROWTH == 1
    #if OS_TASK_CREATE_EXT_EN
    OSTaskCreateExt(OSTaskIdle, 
                    (void *)0,                                 /* No arguments passed to OSTaskIdle()  */
                    &OSTaskIdleStk[OS_TASK_IDLE_STK_SIZE - 1], /* Set Top-Of-Stack                     */
                    OS_IDLE_PRIO,                              /* Lowest priority level                */
                    OS_TASK_IDLE_ID,
                    &OSTaskIdleStk[0],                         /* Set Bottom-Of-Stack                  */
                    OS_TASK_IDLE_STK_SIZE, 
                    (void *)0,                                 /* No TCB extension                     */
                    OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR);/* Enable stack checking + clear stack  */
    #else
    OSTaskCreate(OSTaskIdle, (void *)0, &OSTaskIdleStk[OS_TASK_IDLE_STK_SIZE - 1], OS_IDLE_PRIO);
    #endif