os.c

ucos on sa1200 from ixp1200 EB

/*
 * 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]).
 *
 */
/*
 * Dan M. White - 10/13/00 - changed ossched for bug #1112 (cpsr not saved)
 *
 */
#include "os.h"
#include "interrupts.h"
#include "platform.h"

#ifdef __cplusplus
extern "C" {
#endif

    void OSStartHighRdy(void);  /* defined in subr.s */
    void OSCtxSw(void);         /* defined in subr.s */

#ifdef __cplusplus
}
#endif

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

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


/* idle task */
static void
OSTaskIdle(void* IGNORE)
{
    unsigned int intFlagsSave;

    for (;;) {
        intFlagsSave = ENTER_CRITICAL();
        OSIdleCtr++;
        EXIT_CRITICAL(intFlagsSave);
    }
} /* OSTaskIdle */


/* initialize uC/OS */
void    
OSInit(void)
{
    int i;

    /* initialize variables */
    OSTime = 0;
    OSTCBHighRdy = (OS_TCB *)0;
    OSTCBCur = (OS_TCB *)0;
    OSTCBList = (OS_TCB *)0;
    OSLockNesting = 0;
    OSRunning = UCOS_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, OS_IDLE_STK_SIZE);
} /* OSInit */


/* start multitasking */
void    
OSStart(void)
{
    uint    y, x, p;

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

    OSTCBHighRdy = OSTCBPrioTbl[p];

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


/* uC/OS scheduler */
void
OSSched(void)
{
    unsigned int intFlagsSave;
    uint    y;
    uint    psr;

    intFlagsSave = ENTER_CRITICAL();   
    psr = SAr_GetCPSR(); /* dmw (bug 1112) */
    if (OSLockNesting == 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++;
            SAr_PutCPSR(psr); /* dmw (bug 1112) */
            OSCtxSw();  /* context switch */
        }
        else
        {
            SAr_PutCPSR(psr); /* dmw (bug 1112) */
        }
    }
    else
    {
        SAr_PutCPSR(psr); /* dmw (bug 1112) */
    }
    EXIT_CRITICAL(intFlagsSave);
} /* OSSched */


/* prevent scheduling */
void    
OSSchedLock(void)
{
    unsigned int intFlagsSave;

    intFlagsSave = ENTER_CRITICAL();
    OSLockNesting++;
    EXIT_CRITICAL(intFlagsSave);
} /* OSSchedLock */


/* allow scheduling */
void    
OSSchedUnlock(void)
{
    unsigned int intFlagsSave;

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


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

    intFlagsSave = ENTER_CRITICAL(); 
    if ((ptcb = OSTCBFreeList) == (OS_TCB *)0) {
        /* no TCB available */
        EXIT_CRITICAL(intFlagsSave); 
        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->OSTCBStkTop = stkTop;
    ptcb->OSTCBStkBottom = stkBottom;
    /* link TCB to the top of the list */
    ptcb->OSTCBNext = OSTCBList;
    if (OSTCBList)
        OSTCBList->OSTCBPrev = ptcb;
    ptcb->OSTCBPrev = (OS_TCB*)0;
    OSTCBList = ptcb;
    /* put task in ready to run list */
    OSRdyGrp |= OSMapTbl[prio >> 3];
    OSRdyTbl[prio >> 3] |= OSMapTbl[prio & 0x07];
    EXIT_CRITICAL(intFlagsSave); 

    return(OS_NO_ERR);
} /* OSTCBInit */


/* create a task */
int 
OSTaskCreate(PTV task, void *pdata, void *pstk, uint prio, uint stkSize)
{
    unsigned int intFlagsSave;
    uint *stk;
    uint  *stkBottom;
    int Ret;
    unsigned char* stkPtr;
    int ii;

    /* initialized with special pattern for usage check. */
    stkPtr = (unsigned char*)pstk - stkSize;
    for (ii = 0; ii < stkSize; ii++)
        stkPtr[ii] = 0xee;

    intFlagsSave = ENTER_CRITICAL(); 
    if (OSTCBPrioTbl[prio] != (OS_TCB *)0) {
        /* only one task allowed at its priority */
        EXIT_CRITICAL(intFlagsSave); 
        Ret = OS_PRIO_EXIST;
    }
    else {
        EXIT_CRITICAL(intFlagsSave); 
        stk = (uint *)pstk;
        /* stack boundary checker */
        *--stk = MAGIC_WORD3;
        *--stk = MAGIC_WORD2;
        *--stk = MAGIC_WORD1;
        *--stk = MAGIC_WORD0;
        stkBottom = stk;

        /* 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 */
        *--stk = SVC32MODE;     /* psr */
        Ret = OSTCBInit(    prio,
                            (void *)stk,
                            (void*)stkBottom,
                            (void*)((char*)pstk - stkSize));
        if (Ret == OS_NO_ERR && OSRunning) 
            OSSched();
    }

    return(Ret);
} /* OSCreateTask */


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

    if (prio == OS_LO_PRIO)
        return(OS_TASK_DEL_IDLE);
    intFlagsSave = ENTER_CRITICAL();
    if ((ptcb = OSTCBPrioTbl[prio]) == (OS_TCB *)0) {
        EXIT_CRITICAL(intFlagsSave);
        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(intFlagsSave);
    /* find new highest priority task */
    OSSched();

    return(OS_NO_ERR);
} /* OSTaskDel */


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

    intFlagsSave = ENTER_CRITICAL();
    /* check if new priority not in use */
    if (OSTCBPrioTbl[newp] != (OS_TCB *)0) {
        EXIT_CRITICAL(intFlagsSave);
        return(OS_PRIO_EXIST);
    }
    /* check if old priority task exists */
    if ((ptcb = OSTCBPrioTbl[oldp]) == (OS_TCB *)0) {
        EXIT_CRITICAL(intFlagsSave);
        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 = UCOS_TRUE;
    } else 
        rdy = UCOS_FALSE;
    /* remove from event list */
    if ((pevent = ptcb->OSTCBEventPtr) != (OS_EVENT *)0)
        if ((pevent->OSEventTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0)
            pevent->OSEventGrp &= ~ptcb->OSTCBBitY;
    /* 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];
    /* 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;
    EXIT_CRITICAL(intFlagsSave);
    /* run highest priority task ready */
    OSSched();

    return(OS_NO_ERR);
} /* OSTaskChangePrio */


/* process system tick */
void
OSTimeTick(void)
{
    unsigned int intFlagsSave;
    OS_TCB  *ptcb;

    intFlagsSave = ENTER_CRITICAL();
    OSTime++;
    /* scan all TCB in list */
    for (ptcb = OSTCBList; 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;
                ptcb->OSTCBStat = OS_STAT_RDY;
                taskReSched = 1; /* it causes task re-schedule later */
            }
    }
    EXIT_CRITICAL(intFlagsSave); 
} /* OSTimeTick */

/* delay the current task */
void    
OSTimeDly(uint ticks)
{
    unsigned int intFlagsSave;
    intFlagsSave = ENTER_CRITICAL();
    /* suspend the current task */
    if ((OSRdyTbl[OSTCBCur->OSTCBY] &= ~OSTCBCur->OSTCBBitX) == 0)
        OSRdyGrp &= ~OSTCBCur->OSTCBBitY;
    /* load number of ticks in TCB */
    OSTCBCur->OSTCBDly = ticks;
    OSTCBCur->OSTCBStat = OS_STAT_SLEEP;
    EXIT_CRITICAL(intFlagsSave);
    /* find a new task to run */
    OSSched();
} /* OSTimeDly */


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


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