Delta Queue - Embedded Scheduler

I am trying to implement a simple delta queue on a small Cortex-M3 to schedule some tasks in the future. I have constructed something but I don't think its very elegant (I don't write code often). It also seems a little flaky perhaps due to incorrect use of the volatile specifier.

#include "deltaqueue.h"
#include "debug.h"
#include "interrupt.h"

//*****************************************************************************
//
// Define NULL, if not already defined.
//
//*****************************************************************************
#ifndef NULL
#define NULL                    ((void *)0)
#endif

//! Delta queue structure encapsulating a complete process entry into the queue
typedef struct dq{
    struct dq * psPrev;             //Address of previous queue entry
    struct dq * psNext;             //Address of next queue entry
    unsigned long ulDelta;          //Delta ticks (ticks relative to the next/previous process)          
    tProcessObject sProcess;        //Process to be executed
} tDeltaQueueObject;


//! Contains the maximum number of processes in the queue at any one time (health indicator).
static unsigned long g_ulMaximumProcesses=0;
//! Contains the current number of processes in the queue (health indicator).
static unsigned long g_ulCurrentProcesses=0;
//! Contains the current number of executed processes (health indicator).
static unsigned long g_ulExecutedProcesses=0;
//! Contains the total number of processes scheduled since initialized (health indicator).
static unsigned long g_ulTotalProcesses=0;

//! Contains the accumulated tick count.
static volatile unsigned long g_ulSchedulerTickCount;
//! Simple counter used to generate process IDs.
static unsigned long g_ulPID=1;
//! Pointer to the first sleeping process.
static tDeltaQueueObject * volatile psSleeping;
//! Pointer to the processes ready for execution.
static tDeltaQueueObject * psReady;
//! Pointer to an available slot in the queue.
static tDeltaQueueObject * psAvailable;
//! Queue of processes.
static tDeltaQueueObject sDeltaQueue[QUEUE_MAX];


unsigned long SchedulerElapsedTicksCalc(unsigned long, unsigned long);
unsigned long GetProcessID(void);
tDeltaQueueObject * FreeEntry(void);

//****************************************************************************
//
//! Initializes the scheduler.
//!
//! This function resets the queue pointers.
//!
//! \return None.
//
//****************************************************************************
void SchedulerInit(void){

    //Initialize queue pointers
    psAvailable=&sDeltaQueue[0];
    psSleeping=psAvailable;
    psReady=psAvailable;

}


//****************************************************************************
//
//! Inserts supplied process into the queue.
//!
//! This function iterates the queue starting the sleep pointer and looks for
//! the insert location based on the supplied delay. As this is a delta queue,
//! the delay is decremented by the sleeping process' delta until a the delay
//! is less than that of the sleeping process. This then becomes the insertion
//! point. If there are no sleeping processes then the process is inserted 
//! after the last ready process. If there are no sleeping processes or ready
//! processes then it's inserted and becomes the sole sleeping process.
//!
//! \param pf is the process to execute after the supplied delay.
//! \param ulDelay is the number of ticks to wait before executing the supplied
//! process.
//!
//! \return Process ID of inserted process or zero if unable to insert.
//
//****************************************************************************
unsigned long SchedulerInsert(void (*pf)(void),unsigned long ulDelay){

    static unsigned long ulBeginCount; 
    static unsigned long ulEndCount;   

    ASSERT(psSleeping);
    ASSERT(psAvailable);    

    //Pick off current systick count to calculate execution time
    ulBeginCount=(*((volatile unsigned long *)(NVIC_ST_CURRENT)));

    //CRITICAL SECTION BEGIN
    IntMasterDisable();    

    //Begin iterating at the current sleep pointer
    tDeltaQueueObject * p=(void *)psSleeping;
    tDeltaQueueObject * q;

    //Adjust health indicators
    g_ulTotalProcesses++;
    if(++g_ulCurrentProcesses>g_ulMaximumProcesses)
        g_ulMaximumProcesses=g_ulCurrentProcesses;    

    //Loop through each sleeping process starting at the current
    //sleep pointer and ending when the next pointer of any is 
    //equivalent to the available pointer
    while(p!=psAvailable){

        //If the delay is greater than the current queue item delay,
        //compute the delta for the inserted process and move on
        if(p->ulDelta <= ulDelay){
            ulDelay-=p->ulDelta;   
        }
        //Otherwise, this is the point to insert the new process
        else{   

            //Insert the new process before the current queue entry
            q=FreeEntry();
            ASSERT(q);  //TODO: Exit gracefully when no room
            q->psNext=p;
            q->psPrev=p->psPrev;      

            //Adjust previous and next pointers on each side of the new process
            p->psPrev->psNext=q;
            p->psPrev=q;

            //Set deltas for inserted queue entry and the supplied queue entry 
            p->ulDelta-=ulDelay;
            q->ulDelta=ulDelay;

            //Set the function pointer for the new process and obtain a unique
        开发者_JAVA技巧    //process ID
            q->sProcess.pf=pf;
            q->sProcess.ulPID=GetProcessID();             

            //Adjust the sleep pointer if the insert 
            //happens before it
            if(p==psSleeping)
                psSleeping=q;

            //CRITICAL SECTION END
            IntMasterEnable();       

            //Pick off current systick count to calculate execution time
            ulEndCount=(*((volatile unsigned long *)(NVIC_ST_CURRENT)));

            return q->sProcess.ulPID;
        }

        //Move to next
        p=p->psNext;

    }

    //If here, the list is either empty or the delay is larger than the 
    //sum of all the delays in the queue and so it should be appended 
    //to the end of the queue
    psAvailable->ulDelta = ulDelay;
    psAvailable->sProcess.pf=pf;
    psAvailable->sProcess.ulPID=GetProcessID();      
    q=psAvailable;

    //Increment the available pointer 
    psAvailable=FreeEntry();
    ASSERT(psAvailable);
    psAvailable->psPrev=q;
    q->psNext=psAvailable;
    psAvailable->psNext=NULL;

    //CRITICAL SECTION END
    IntMasterEnable();       

    //Pick off current systick count to calculate execution time
    ulEndCount=(*((volatile unsigned long *)(NVIC_ST_CURRENT)));

    return q->sProcess.ulPID; 
}

//****************************************************************************
//
//! Runs any processes which are ready for execution.
//!
//! This function is usually called in the main loop of the application
//! (anywhere NOT within an interrupt handler). It will iterate the queue
//! and execute any processes which are not sleeping (delta is zero).
//!
//! \return None.
//
//****************************************************************************
void SchedulerRunTask(void){

    tDeltaQueueObject * p;

    ASSERT(psReady);

    //Run tasks until we bump up against the sleeping tasks
    while(psReady!=psSleeping){

        //Adjust health indicators
        g_ulCurrentProcesses--;
        g_ulExecutedProcesses++;

    //Execute task      
    if(psReady->sProcess.pf)
            (psReady->sProcess.pf)();

        p=psReady->psNext;

    //Clear task
    psReady->sProcess.pf=NULL;
        psReady->sProcess.ulPID=0;
    psReady->psNext=NULL;
        psReady->psPrev=NULL;
        psReady->ulDelta=0;

        //Increment ready pointer
    psReady=p;

    }   
}

//****************************************************************************
//
//! Manages sleeping processes in the queue.
//!
//! This function is to be called by the system tick interrupt (at a given 
//! interval). When called, the sleeping tasks' delta is decremented and the
//! sleep pointer is adjusted to point at the next sleeping task (if changed). 
//!
//! \return None.
//
//****************************************************************************
void SchedulerTick(void){

    ASSERT(psSleeping);

    //Increment tick counter
    g_ulSchedulerTickCount++;

    //Adjust sleeping task (never roll past zero)
    if(psSleeping->ulDelta)
        psSleeping->ulDelta--;

    //Push the sleep pointer until a non-zero delta.
    //Multiple processes can expire on one tick.
    while(!psSleeping->ulDelta && psSleeping!=psAvailable){
        psSleeping=psSleeping->psNext;
    }

}

//****************************************************************************
//
//! Searches the queue for a free slot.
//!
//! This function iterates the entire queue looking for an open slot.
//!
//! \return Pointer to the next free DeltaQueueObject or 0 if no free space
//! available.
//
//****************************************************************************
tDeltaQueueObject * FreeEntry(){

    unsigned long i;

    //Iterate entire queue
    for(i=0; i<QUEUE_MAX; i++){

        //Look for a free slot by examining the contents
        if(!(sDeltaQueue[i].psNext) && !(sDeltaQueue[i].psPrev) && !(sDeltaQueue[i].sProcess.ulPID) && !(sDeltaQueue[i].ulDelta) && !(sDeltaQueue[i].sProcess.pf))
            return &sDeltaQueue[i];
    }

    //If we are here, there are no free spots in the queue
    ASSERT(1);
    return NULL;

}

//****************************************************************************
//
//! Produces a unique process ID.
//!
//! This function simply returns the next PID available.
//!
//! \todo Keep a list of unexpired PIDs so that it can be guaranteed unique
//! must have before creating remove function
//!
//! \return A unique process ID.
//
//****************************************************************************
unsigned long GetProcessID(void){

    //PID can never be zero, catch this case
    if(!g_ulPID)
        g_ulPID=1;    

    return g_ulPID++;
}

The idea behind what I have is there exists a static buffer which is filled with delta queue objects. Each delta queue object has pointers to the previous/next delta queue object, a relative delay to the previous task and some process information (process ID and function pointer). There are 3 global pointers, the ready pointer, sleep pointer and the available pointer. The ready pointer points to a list of tasks to be executed. The sleep pointer to a list of tasks which are... well... asleep and not ready to execute. The available pointer basically points to the end where there is an available slot. These pointers only move forward. When one is pushed up against another, that 'sub-queue' is empty. For example, when the ready pointer equals the sleep pointer, there are no ready tasks.

So, an example might look something like:

Initially the pointers look like so..

Pointers    Slot #    Delta  
RP,SP,AP -> Slot 1    0

A task is inserted with a delay of 50ms and the queue now looks like...

Pointers    Slot #    Delta  
RP,SP    -> Slot 1    50
AP       -> Slot 2    0

A few ticks go by and another task is inserted with a delay of 10ms...

Pointers    Slot #    Delta  
RP,SP    -> Slot 3    10
         -> Slot 1    38
AP       -> Slot 2    0

Twenty ticks then go by and we have...

Pointers    Slot #    Delta  
RP       -> Slot 3    0
SP       -> Slot 1    18
AP       -> Slot 2    0

SchedulerTick() is called by the systick interrupt at a rate of 1ms. SchedulerRun() is called from the main loop of the application (when it's not doing anything else) so that my systick interrupt is very short. SchedulerInsert() is called as needed to schedule a task.

So, that's where I was headed with the code above. Now, my problems...

1) I specified psSleeping as a volatile pointer because it is modified in the SchedulerTick(). I am confident that it's needed but is my usage correct? Is the pointer declared volatile or is what it's pointed to declared volatile.

2) The SchedulerTick() and SchedulerRun() functions are pretty straight forward but the SchedulerInsert() has become quite messy. Most of the mess is due to the fact that an inserted task can be placed before the sleep pointer meaning SchedulerTick() is no longer exclusively writing to it and so I have to disable the interrupts while I do that. Moreover, there seems to be some bug in the insert (presumably) which causes the SchedulerTick() to grind to a halt in the while loop because psAvailable is never reached. This bug occurs very seldom... I can't repeat it while stepping through. Perhaps it's related to the volatile declaration?

Any thoughts?

---

My suggestion is that you reconsider whether you truly need to do any actual list processing from within the interrupt handler.

As near as I can tell you can achive a similar result by merely tracking the elapsed ticks and using them to wake up sleeping tasks anywhere you previously would have accessed the sleep tail pointer outside of interrupts.

E.g. something along these lines:

// Only bumb the tick counter from within interrupts
void SchedulerTick(void) {
    g_ulSchedulerTickCount++;
}

// Use the number of elapsed ticks since the last call wake up processes for execution.
// Returns the first task that's still sleeping
tDeltaQueueObject *SchedulerStillSleeping(void) {
    static unsigned long lastTick;
    unsigned long currentTick = g_ulSchedulerTickCount;
    signed long elapsedTicks = currentTick - lastTick;
    lastTick = currentTick;

    for(; psSleeping != psAvailable; psSleeping = psSleeping->psNext) {
        if(psSleeping->ulDelta > elapsedTicks)
            psSleeping->ulDelta -= elapsedTicks;
            break;
        }
        elapsedTicks -= psSleeping->ulDelta;
        psSleeping->ulDelta = 0;
    }
    return psSleeping;
}

// Reassess the set of sleeping processes by calling the StillSleeping function anywhere
// you would previously have polled the list head
void SchedulerRunTask(void) {
    while(psReady != SchedulerStillSleeping()) {
        .
        .
        .
    }
}

unsigned long SchedulerInsert(...) {
    .
    .
    .
    tDeltaQueueObject *p = SchedulerStillSleeping();
    while(p != psAvailable) {
        .
        .
        .
    }
}