This file is part of my co-routines framework for small microcontrollers

by Isaac Marino Bavaresco

This is file "C-R.c"

/*============================================================================*/
/*
 Copyright (c) 2014, Isaac Marino Bavaresco
 All rights reserved.

 Redistribution and use in source and binary forms, with or without
 modification, are permitted provided that the following conditions are met:
     * Redistributions of source code must retain the above copyright
       notice, this list of conditions and the following disclaimer.
     * Neither the name of the author nor the
       names of its contributors may be used to endorse or promote products
       derived from this software without specific prior written permission.

 THIS SOFTWARE IS PROVIDED BY THE AUTHOR ''AS IS'' AND ANY
 EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 DISCLAIMED. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
/*============================================================================*/
#include "C-R.h"
/*============================================================================*/

/*----------------------------------------------------------------------------*/
#if         defined USE_POINTERS
/*----------------------------------------------------------------------------*/

#define INVALID_TASK    NULL

#define NEXT(t)         ((t)->Next)
#define PREVIOUS(t)     ((t)->Previous)
#define TIMETOWAKE(t)   ((t)->TimeToWake)
#define TASKFUNC(t)     ((t)->TaskFunction)

#define TASKISVALID(t)  ((t)!=NULL)
#define CONTEXTADDR(i)  (&Tasks[(i)])

/*----------------------------------------------------------------------------*/
#else   /*  defined USE_POINTERS */
/*----------------------------------------------------------------------------*/

#define INVALID_TASK    (-1)

#define NEXT(t)         (Tasks[t].Next)
#define PREVIOUS(t)     (Tasks[t].Previous)
#define TIMETOWAKE(t)   (Tasks[t].TimeToWake)
#define TASKFUNC(t)     (Tasks[t].TaskFunction)

#define TASKISVALID(t)  ((t)<__NumTasks)
#define CONTEXTADDR(i)  (i)

/*----------------------------------------------------------------------------*/
#endif  /*  defined USE_POINTERS */
/*----------------------------------------------------------------------------*/

/*============================================================================*/

volatile tickcount_t    SystemTick = 0;

/*============================================================================*/

ctxtptr_t   ReadyTasks      = 0;
ctxtptr_t   DelayedTasks    = INVALID_TASK;
ctxtptr_t   SuspendedTasks  = INVALID_TASK;

ctxtptr_t   FreeContexts    = INVALID_TASK;

/*============================================================================*/

void InsertTaskIntoReadyList( ctxtptr_t task )
    {
    /* The list of ready tasks is empty... */
    if( ! TASKISVALID( ReadyTasks ))
        {
        /* ...it is a simple insertion. */
        ReadyTasks              = task;
        PREVIOUS( task )        = task;
        NEXT( task )            = task;
        }
    /* There are elements in the list of ready tasks... */
    else
        {
        /* ...we will insert at the end */
        ctxtptr_t Last          = PREVIOUS( ReadyTasks );
        PREVIOUS( task )        = Last;
        NEXT( task )            = ReadyTasks;
        PREVIOUS( ReadyTasks )  = task;
        NEXT( Last )            = task;
        }
    }

/*============================================================================*/

void RemoveTaskFromReadyList( ctxtptr_t task )
    {
    /* This is the only task in the ready list... */
    if( NEXT( task ) == task )
        {
        /* ...so the list becomes empty. */
        ReadyTasks  = INVALID_TASK;
        }
    /* The list has more than one element... */
    else
        {
        /* ...we must unlink the current task from the list. */
        ctxtptr_t   Next, Previous;

        Previous            = PREVIOUS( task );
        Next                = NEXT( task );
        PREVIOUS( Next )    = Previous;
        NEXT( Previous )    = Next;

        ReadyTasks          = Next;
        }

    /* Clean up the "pointers" to avoid possible bugs. */
    NEXT( task )            = INVALID_TASK;
    PREVIOUS( task )        = INVALID_TASK;
    }

/*============================================================================*/

void InsertTaskIntoDelayList( ctxtptr_t task, tickcount_t t )
    {
    TIMETOWAKE( task )  = t;

    /* The list of delayed tasks is empty... */
    if( ! TASKISVALID( DelayedTasks ))
        {
        /* ...it is a simple insertion. */
        DelayedTasks    = task;
        }
    /* The list of delayed tasks is not empty... */
    else
        {
        ctxtptr_t   i, j, Next;

        /*
        ...we must find where in the list the current task
        must be inserted to keep the list ordered by time to
        wake.
        */
        i = DelayedTasks;

        /* Special case, we are inserting before the first item */
        if( (tickcompare_t)( t - TIMETOWAKE( i )) < 0 )
            {
            NEXT( task )        = i;
            PREVIOUS( task )    = INVALID_TASK;
            PREVIOUS( i )       = task;
            DelayedTasks        = task;
            }
        else
            {
            for( j = NEXT( i );
                 TASKISVALID( j ) && (tickcompare_t)( t - TIMETOWAKE( j )) >= 0;
                 i = j, j = NEXT( j ))
                {
                /* Nothing to do here */
                }

            Next                    = NEXT( i );
            NEXT( task )            = Next;
            PREVIOUS( task )        = i;
            if( TASKISVALID( Next ))
                PREVIOUS( Next )    = task;
            NEXT( i )               = task;
            }
        }
    }

/*============================================================================*/

void RemoveTaskFromDelayList( ctxtptr_t task )
    {
    DelayedTasks        = NEXT( DelayedTasks );

    NEXT( task )        = INVALID_TASK;
    PREVIOUS( task )    = INVALID_TASK;
    TIMETOWAKE( task )  = 0;
    }

/*============================================================================*/

void _Sleep( tickcompare_t t )
    {
    unsigned long   Tick;
    ctxtptr_t       CurrentTask = ReadyTasks;

    DISABLEINTERRUPT();
    Tick    = SystemTick;
    ENABLEINTERRUPT();

    RemoveTaskFromReadyList( CurrentTask );

    /* A delay greater than zero means the task wants to sleep...*/
    if( t >= 0 )
        /* ...so we must insert it in the list of delayed tasks. */
        InsertTaskIntoDelayList( CurrentTask, Tick + t );
    /* A delay less than zero means the task wants to be suspended... */
    else
        {
        /* ...let's insert it in the list of suspended tasks. */
        NEXT( CurrentTask ) = SuspendedTasks;
        SuspendedTasks      = CurrentTask;
        }
    }

/*============================================================================*/

ctxtptr_t CreateTask( taskptr_t task )
    {
    ctxtptr_t   ctxt;

    if( !TASKISVALID( FreeContexts ))
        return INVALID_TASK;

    ctxt                = FreeContexts;
    FreeContexts        = NEXT( FreeContexts );

    TASKFUNC( ctxt)     = task;
    NEXT( ctxt )        = INVALID_TASK;

    InsertTaskIntoReadyList( ctxt );

    return ctxt;
    }

/*============================================================================*/

mainreturn_t main( void )
    {
    unsigned int    i;
    tickcount_t     previoust = 0;

    for( i = 0; i < __NumTasks - 1; i++ )
        {
        Tasks[i].TaskFunction   = NULL;
        Tasks[i].Next           = CONTEXTADDR( i + 1 );
        Tasks[i].Previous       = INVALID_TASK;
        Tasks[i].TimeToWake     = 0;
        }
    Tasks[i].TaskFunction   = NULL;
    Tasks[i].Next           = INVALID_TASK;
    Tasks[i].Previous       = INVALID_TASK;
    Tasks[i].TimeToWake     = 0;

    FreeContexts            = CONTEXTADDR( 0 );

    CreateTask( Task1 );

    InitializeTimer();

    while( 1 )
        {
        tickcount_t     t;

        CLEARWATCHDOG();

        DISABLEINTERRUPT();
        t   = SystemTick;
        ENABLEINTERRUPT();

        /* There are sleeping tasks and the system tick has changed */
        if( TASKISVALID( DelayedTasks ) && t != previoust )
            {
            /* Wake all the sleeping tasks that have their delay elapsed. */
            while( TASKISVALID( DelayedTasks )
                    && (taskreturn_t)( TIMETOWAKE( DelayedTasks ) - t ) <= 0 )
                {
                ctxtptr_t   i   = DelayedTasks;

                /*
                The first task to wake will always be the first in the list
                because the list is kept ordered by time to wake.
                */
                RemoveTaskFromDelayList( i );
                InsertTaskIntoReadyList( i );
                }
            previoust   = t;
            }

        /* There are tasks ready to run... */
        if( TASKISVALID( ReadyTasks ))
            {
            /* ...call the current task function */
            if( TASKFUNC( ReadyTasks )() == 0 )
                /* Move to the next task */
                ReadyTasks  = NEXT( ReadyTasks );
            }
        }
    }

/*============================================================================*/