ORIGINAL CSRTOS
http://www.circuitcellar.com/avr2004/DA3650.html
DO NOT USE- GCC WILL
NOT KNOW THAT 'CALL SAVED'
REGISTERS WILL BE CHANGED
WHEN TASK SWITCHING


// csRTOS.c 2-MAY-04 Atmel/Circuit Cellar contest entry number A3650

// Multitasking with small AVRs - a cooperative multitasking demo
//  for the ATMEL STK-200 breadboard.
// This demo code uses I/O specific to the STK-200 breadboard, but
//  it should be easy to write similar code for any ATMEL micro.

// This cooperative single-stack RTOS (csRTOS) is coded mostly
//  in C. Smaller RAM requirements and faster task switching can be
//  achieved by coding the C library routines  <setJump.h>
//  and <longjmp.h> in assembly language, as is done here.

// This code was written and tested using the free WinAVR C compiler.
// It should work with any standard C compiler. With slight modifications
//    the code has been run using the Codevision and Imagecraft compilers.
// Warning: The Codevision compiler has many bugs.

// CSRTOS requires two simple rules to work:

//  1) Operating calls that may result in task switches can occur only
//      at the top level of a task. That is, you cannot make os calls
//      from subroutines called within a task.

//  2) Local variables of a task subroutine are not preserved across
//      os calls. If you need to preserve the value of a local, declare
//      it to be "static" (or perhaps "register", see comments below).

// The following psuedo-code shows some things that work and some that
//  don't.

/*

void aFunc(void) {
    ...
    osCall(something);  // THIS WILL NOT WORK - os calls can be made
                        // only at the task level, never from routines
                        // called by task code (except for the few os calls
                        // that do not result in task switches).
    ...
}

void aTask(void) {  // All tasks are declared like this.
    int i;          // A variable local to the task. It is OK
                    // to have local variables, but their values
                    // are not preserved across os calls.
    osTASK_INIT;    // All tasks must start with this.
    for(;;) {       // All tasks should start with this.
        ...
        for (i = 0; i < 5; i++) osCALL(something); // DOES NOT WORK!
                    // To make this call work, define i to be
                    //  static. Above should be: static int i;
        ...
        aFunc();    // This does not work! See the comments above.
    }
}

*/

#define I32 long int
#define U32 unsigned long int
#define U8 unsigned char
#define U16 unsigned int

#include <avr/io.h>
#include <avr/signal.h>
#include <string.h>
#include <avr/pgmspace.h>

// Task related declarations go here. Tasks are subroutines that are
//  declared to return void, with no parameters. They are written to
//  never return. Each task has a number, 0 through 7 in this
//  implementation. It would be very easy to allow for 16 tasks
//  instead of 8.

//  The priority of a task is fixed at compile time, and is equal to
//  the task number. Lower numbers correspond to higher priority.

// The tasks are listed here, in no particular order.

void blinkTask(void);
void clockTask(void);
void randomBlinkTask(void);
void adcTask(void);
void setClockTask(void);

// The task numbers are given by an enum.
// It is vital that the order of the task addresses in startAdr[nTasks]
//   is the same as the order they are listed in the enum.
// NOTE: the last entry in the enum is "nTasks" - it is NOT a task,
//  but serves to define the number of tasks.

enum {blinkTaskNumber,clockTaskNumber,randomBlinkTaskNumber,adcTaskNumber,\
    setClockTaskNumber,nTasks};

// There are three arrays of constants that are stored in flash to save ram:
//    1) a byte array of powers of 2
//    2) a word array of powers of 10
//    3) a word array of task starting addresses

// In an ideal world these would be declared to be const arrays, and the compiler would put them
//    in rom or flash. In the real world some compilers (including WinAVR) require something
// extra to get constants into flash. Check the assembly language generated by your compiler.
// Some compilers will put this both in flash AND in ram - a waste of memory.


const U8 two2nArray[] PROGMEM = {1,2,4,8,16,32,64,128};
                              // used to convert a task number
                              // to the corresponding bit in rtr.
const U16 ten2nArray[] PROGMEM = {0,1,10,100,1000,10000}; // used by int2ascii conversion


// To initialize tasks (see main() below) it is useful to have a rom array giving the
// starting address of each tasks. List the tasks in the same order as in the enum.

const int startAdr[nTasks] PROGMEM = {(int)blinkTask,(int)clockTask,(int)randomBlinkTask,\
    (int)adcTask,(int)setClockTask};

// Semaphores are defined here.  Eash semaphore uses two bytes: one
//  has a bit set for each task waiting for the semaphore, and the
//  other contains the number of the task that owns the semaphore.
//  If the semaphore is available, the owner byte will be 255.
// Semaphores are numbered sequentially from zero.

enum {CRTSEMA,nSemas};    // The semaphore number for each semaphore.
                          // As for the task enumeration, the last entry
                          // is not a semaphore, but defines the number
                          // of semaphores.

U8 wantSema[nSemas];      // One bit high for each task waiting for the sema.
U8 semaOwner[nSemas];     // Task number of sema owner, or 255 if sema is available.

// Global variables used by CSRTOS:

volatile U8 rtr;            // If a task is ready to run, a bit corresponding to
                              //  the task number is set in rtr. If you want to
                              //  allow up to 16 tasks, use a U16 variable for rtr.

volatile U8 thisTask;      // The number of the currently running task.

U8 ticks[nTasks];             // If a task is suspended for ticks,
                              //  the corresponding variable will be non-zero.
                              // With a tick period of 20 mSec, a task can be
                              //  delayed for up to 5 seconds. If longer delays
                              //  are required, simply call osWAIT() several
                              //  times (perhaps in a for() loop - but be sure to
                            //  use a "static" loop variable).

U32 clockTicks;                // for keeping clock time
U8 taskSpace[nTasks*3];        // Three bytes of status information are kept for each
                            //    suspended task
U8 secs,mins,hrs;
U8 clockSet;                 // 1 to set minutes, 2 to set hours
char crtBuf[20];              // A buffer for characters to send to CRT.
U8 crtBufN;                   // The number of the next character to send.
PGM_P romAdr;                // used to access bytes stored in flash

U8 two2n(U8 n) {
    romAdr = &(two2nArray[n]);
    return pgm_read_byte(romAdr);
}

U16 ten2n(U8 n) {
    romAdr = (char*)&(ten2nArray[n]);
    return pgm_read_word(romAdr);
}

// Macros for operating system calls.  If a higher priority task is ready to run,
//    an os call will result in the suspension of the calling task and the resumption
//    of the higher priority one. Exceptions: osSET_RTR and osCLEAR_RTR do not result
//    in a task switch.

#define osWAIT(ticks) if (saveTask(getAdr()) == 0) __wait(ticks)
#define osTASK_INIT if(saveTask(getAdr()) == 0) return
#define osYIELD if(saveTask(getAdr()) == 0) __schedule()
#define osSUSPEND if(saveTask(getAdr()) == 0) __wait(0)
#define osCLEAR_RTR(task) rtr = rtr & ~two2n(task)
#define osSET_RTR(task) rtr |= two2n(task)
#define osGET_SEMA(semaNumber) if(saveTask(getAdr()) == 0) __getSema(semaNumber)
#define osRELEASE_SEMA(semaNumber) if(saveTask(getAdr()) == 0) __releaseSema(semaNumber)

// Task switching occurs only when a task makes certain os calls. After an os call the highest
//  priority task that is ready to run (rtr) is resumed.
// The os calls give the following functionality:

// osWAIT(ticks) suspends the calling task for some number of ticks. In this implementation
//    there are 64 ticks per second, or about 16 mSec per tick.
//  osTASK_INIT must be called for each task, before starting multitasking. The initialization
//    code in main() takes care of this.
// osYIELD resumes the highest priority task that is rtr. The rtr status of the calling
//  task is not changed - so if it is the highest priority rtr it immediately resumes.
// osSUSPEND makes the calling task not rtr. Usually it will be made rtr at some future
//  time by an interrupt service routine or by another task.
// osCLEAR_RTR(task) clears the rtr bit of the given task, but DOES NOT call the scheduler
//  for a task switch.
// osSET_RTR(task) sets the rtr bit of the given task, but DOES NOT call the scheduler
//  for a task switch.
// osGET_SEMA(number) makes the calling task the owner of the given semaphore. If the semaphore
//  is not available the task is suspended. When the semaphore becomes available the highest
//  priority task that is waiting for it is set rtr. As with most os calls, a call to this
//  routine results in a task switch if a higher priority task is rtr.
// osRELEASE_SEMA(number) releases the semaphore, which results in another task being set
//  rtr if another task is waiting for the semaphore. Of course a task should not release
//  a semaphore unless it owns it - no error checking is done for this condition.

//      REDEFINING setJump(), longjmp(), and jmp_buf: NOT REQUIRED!

//    csRTOS can be written entirely in c, using the above routines from the standard c library.
//        For smaller code size and greater speed, this demo uses similar routines with different
//        names, coded in assembly language.

// The redefined setJump() and longjmp are called saveTask() and restoreTask().
// Coding in assembly language gives smaller code and faster task switching, because setJump()
//  et al save more registers than is necessary. If you want to code in assembly for some micro,
//  take a look at the code generated by the compiler for the standard routines, and use that
//  as a template.

// NOTE: here is a detail that will be of interest only if you really want to completely
//  understand how all this works. If you use setJump() et al, it is possible to preserve
//  the values of local variables across OS calls if the variables are "register" variables.
// This is because setJump() saves all the necessary registers. However, the compiler is
//  not required to actually put variables defined as register in registers, so you must
//  look at the code generated to see if they are (or just write some little code to test).
//  This technique may save some space, because "static" variables will use up some RAM,
//  and "register" variables will not (except for the jmp_buf space that is required for
//  the registers whether they are used or not).
// As a further complication, many compilers will keep a few local variables in registers
//  even if they are not declared to be register variables. You can ignore this if you want,
//  it just means that some local variables may be preserved across os calls.

//                  OS FUNCTIONS

// These functions are normally not called from user code, except via the supplied os macros.

// The schedule function finds the highest priority task that is rtr, and resumes it.
// If no task is rtr, the code waits until one is. In real application
//  code you may want to put the CPU in a low power mode when no task is rtr.

// You can use a 256 byte array to more quickly find the highest priority task that is
//    ready to run (if you have enough eeprom or flash space available).
// If you want to allow 16 tasks, use a U16 for rtr, and look at the low order byte of
//  rtr first. If it is zero, look at the high order byte.

U16 getAdr(void) {
    return (U16)(taskSpace+3*thisTask);    // Returns the address of the first byte of
                                            //    the space used to store a task's state info.
}
U8 saveTask(U16 adr) {                // saves data stack pointer and return address
    asm("mov r26,r24");            // get address of current task
    asm("mov r27,r25");

    asm("pop r24");                // get return address
    asm("pop r25");
    asm("push r25");                // put RA back on return stack
    asm("push r24");
    asm("st x+,r24");                // save RA in taskSpace array
    asm("st x+,r25");
    asm("st x+,r28");                // save data stack pointer
    return 0;                        //
};

U8 restoreTask(U16 adr) {            // restores data stack pointer and return address
    asm("mov r26,r24");            // get address of current task
    asm("mov r27,r25");            // this will always be zero on AT90S4433
    asm("ldi r28,0xDF");            // reset return stack pointer
    asm("out 0x3D,r28");
    asm("ld r24,x+");                // get return address
    asm("ld r25,x+");
    asm("push r25");                // note: could use an indirect jump here
    asm("push r24");
    asm("ld r28,x+");                // get data stack pointer
    return 1;
}

void __schedule(void) {
U8 osTemp;
    while(rtr == 0) {};               // could put CPU in low power mode here
    osTemp = rtr;
    thisTask = 0;
    while ((osTemp & 1) == 0) {     // find highest priority
        osTemp >>= 1;
        thisTask += 1;
    }

//    thisTask = lsBit[rtr];          // Find the highest priority task that is rtr. Do this if
                                    //    the array lsBit[256] is used.
    restoreTask(getAdr());             // Resume the new task.

}

// Suspend the calling task for some number of ticks.
// Calling this with nTicks=0 will result in a
//  suspension that will not be resumed by ticks,
//  but can only be resumed by an ISR or another
//  task setting the rtr bit.

void __wait(U8 nTicks) {
    osCLEAR_RTR(thisTask);
    ticks[thisTask] = nTicks;
    __schedule();
}

void __getSema(U8 semaNumber) {    // Aquire a semaphore, or suspend.
    if (semaOwner[semaNumber] == 255) semaOwner[semaNumber] = thisTask;
    else {
        wantSema[semaNumber] |= two2n(thisTask);
        osCLEAR_RTR(thisTask);
    }
    __schedule();
}

void __releaseSema(U8 semaNumber) {
    U8 temp,osTemp;
    temp = two2n(thisTask);
    wantSema[semaNumber] &= ~temp;
    osTemp = wantSema[semaNumber];
    if (osTemp != 0) {            // someone else wants the sema
        temp = 0;
        while ((osTemp & 1) == 0) {     // find highest priority
        osTemp >>= 1;
        temp += 1;
    }
        semaOwner[semaNumber] = temp;
        osSET_RTR(temp);
    }
    else semaOwner[semaNumber] = 255;
    __schedule();
}

///////// ISR CODE /////////////////


SIGNAL (SIG_INTERRUPT0) {        // Press the switch on external interrupt 0 to bump the minutes.
     clockSet = 1;
     osSET_RTR(setClockTaskNumber);
}

SIGNAL (SIG_INTERRUPT1) {        // Press the switch on external interrupt 1 to bump the hours.
     clockSet = 2;
     osSET_RTR(setClockTaskNumber);
}

SIGNAL (SIG_ADC){                // This isr just clears the ADC interrupt flag.

    osSET_RTR(adcTaskNumber);
}
SIGNAL (SIG_OVERFLOW0) {        // Timer0 overflows every 4 mSec.
    static U8 delay;
    U8 i;

    if((clockTicks+=4096) >= 1000000) {

        osSET_RTR(clockTaskNumber);
    }
    if ((delay -= 1) == 0) {        // Every 20 mSec update the wait timers for tasks that are waiting.
        for (i = 0; i < nTasks; i++) {
            if (ticks[i]) {        // this task is counting
                ticks[i] -= 1;
                if (ticks[i]==0) {    // this task is now ready to run.
                    osSET_RTR(i);
                }
            }
        delay = 5;  // about 20.5 mS total delay
        }
    }
}

SIGNAL (SIG_UART_TRANS) {        // UART transmitter ISR
    char toSend;
    if ((toSend = crtBuf[crtBufN++])) {    // If there is another non-zero character in the
        UDR = toSend;                        //    buffer, send it.
    }
    else {    // done sending zero-terminated crtBuf[]
        osSET_RTR(semaOwner[CRTSEMA]);        // Whomever had the CRT semaphore can run again now.
    }
}

//////// End of ISR code //////////////


// Extra bonus!!! The following psuedo-random number generator is of high statistical quality
//    and does not require any multiplies or divides, so it is fast on small micros.
//    An article describing it should appear in Circuit Cellar magazine soon after
//    the Atmel design contest is over.

#define M 0x7FFFFFFF  // 2^31-1, the modulus used by the psuedo-random
                      //  number generator prng().
I32 prng(I32 x) {     // a good random number generator; call with 1 <= x <= M-1
    x = (x >> 16) + ((x << 15) & M)
        - (x >> 21) - ((x << 10) & M);
    if (x < 0) x += M;
    return x;
}

void int2ascii(U16 x, U8 width, char* buffer, U8 slz)  { // converts ints to ascii.
                                // This code does not require multiplies or divides.

    while (width > 0) {
        *buffer = '0';
        while (x >= ten2n(width)) {
            *buffer += 1;
            x -= ten2n(width);
        }
        if(slz) {
            if ((*buffer == '0') && (width > 1)) *buffer = ' ';
            else slz = 0;
        }
           width -= 1;
           buffer += 1;
      }
    *buffer++ = 10;
    *buffer++ = 13;
    *buffer++ = 0;
}


void blinkTask(void) {        // Just about the simplest possible task.
    osTASK_INIT;
    for(;;) {
        PORTB &= !0x01;        // Turn on the LED on PORTB.0
        osWAIT(15);         // Wait about 0.3 seconds
        PORTB |= 0x01;        // Turn off the LED on PORTB.0
        osWAIT(85);         // Wait about 1.7 secs
    }
}

void setClockTask(void) {    // Set the clock. When incrementing minutes the
     osTASK_INIT;            //    seconds are set to zero.
     for(;;) {
          if (clockSet & 1) {
             secs = 0;
            mins += 1;
            if(mins > 59) mins = 0;
         }
         if (clockSet & 2) {
             hrs += 1;
            if(hrs > 23) hrs = 0;
        }

        osWAIT(10);           // wait 200 ms for debounce
        GIFR = 0;             // clear external interrupt flags
        osSUSPEND;
     }
}

void clockTask(void) {        // Write the time to the CRT and blink an LED.
    osTASK_INIT;
    for(;;) {
        clockTicks -= 1000000;
        PORTB ^= 0x02;                // toggle the PORTB.1 LED
        secs += 1;                    // NOTE: this task blinks the LED at "exactly"
        if (secs > 59) {            // a two second period, while blinkTask blinks at
            mins += 1;                // a period of approximately 2 seconds.
            secs -= 60;
            if (mins > 59) {
                hrs += 1;
                mins -= 60;
            }
        }
        osGET_SEMA(CRTSEMA);          // suspend until crt semaphore is avail
        int2ascii(hrs,2,crtBuf,0); // last param: include leading zeros
        crtBuf[2] = ':';
        int2ascii(mins,2,&crtBuf[3],0);
        crtBuf[5] = ':';
        int2ascii(secs,2,&crtBuf[6],0);
        crtBufN = 1;                // index of next char to send
        UDR = crtBuf[0];            // send the first char
        osSUSPEND;                    // wait for crt ISR to set rtr when finished sending
        osRELEASE_SEMA(CRTSEMA);
        osSUSPEND;
    }
}

void randomBlinkTask(void) {    //Blinks the LED on PORTB.2 at psuedo-random intervals
    static I32 prn = 1;        // An initial value, 1 <= prn < 2^31-1
    osTASK_INIT;
    for(;;) {
        PORTB &= ~4;         // turn PORTB.2 LED on
        prn = prng(prn);
        osWAIT((U8)(prn & 0x3F) + 1);        // Wait 4 mSec to about 0.25 seconds
        PORTB |= 4;         // turn LED off
        prn = prng(prn);
        osWAIT((U8)(prn & 0x3F)+1);
    }
}

void adcTask(void) {        // Measures the band gap voltage, nominally 1.22 volts.
    U16 result;
    U8 low;
    osTASK_INIT;
    for(;;) {
        osCLEAR_RTR(thisTask);
        ADCSR |= 0x40;         // start adc conversion
        osYIELD;          // don't do suspend because isr may make rtr before suspend executes!
        low = ADCL;
        result = ADCH;
        result = (result << 8) + low;
        osGET_SEMA(CRTSEMA);
        //low = 0;    // NOTE: strcpy does not work with codevision compiler
        //crtBuf[0] = 'B';
        //crtBuf[1] = 'G';
        //crtBuf[2] = '=';
        strcpy(crtBuf,"BG = ");
        result <<= 1;         // 2 mv per count with 2.046 volt reference. Adjust the pot
                            //    on the STK200 board for a reference of 2.046 volts.
        int2ascii(result,4,&crtBuf[3],0);
         crtBufN = 1;                // index of next char to send
        UDR = crtBuf[0];            // send the first char
        osSUSPEND;
        osRELEASE_SEMA(CRTSEMA);
        osWAIT(150);    // wait approx 3 seconds
    }
}

int main(void)
{

PORTB=0x07; // turn off the first 3 LEDs
DDRB=0x07;

// Port D initialization

DDRD=0x02;        // uart xmtr

// Timer/Counter 0 initialization
// Clock source: System Clock
// Clock value: 62.500 kHz
TCCR0=0x03;
TIMSK = 0x02; // enables timer0 overflow int

// UART initialization
// Communication Parameters: 8 Data, 1 Stop, No Parity
// UART Transmitter: On
// UART Baud rate: 9600
UBRR = 25;        // 9600 baud with 4 MHz xtal
UCSRB = 0x48;    // enable uart xmtr and ints

// ADC initialization

ADMUX = 0x40; // select ADC band-gap reference
ADCSR = 0x8E; // Enable ADC & ints, clock / 64 = 62.5 KHz

MCUCR = 0x0A; // external interrupts on falling edge
GIMSK = 0xC0; // enable external interruputs

for (thisTask = 0; thisTask < nSemas; thisTask++) { // using thisTask for a different purpose here
    //wantSema[thisTask] = 0;            // Nobody wants the semaphore. NOTE: not required as
     semaOwner[thisTask] = 0xFF;            //    c initializes variables to zero.
}

for (thisTask = 0; thisTask < nTasks; thisTask++) {
    romAdr = (char*)&(startAdr[thisTask]); // get address of task address array in flash
    romAdr = (void*)pgm_read_word(romAdr);    // get starting address of thisTask from flash
    ((U8 (*)(void)) romAdr)();            // call the task to initialize it
}

osSET_RTR(blinkTaskNumber);                // Set some tasks ready to run.
osSET_RTR(randomBlinkTaskNumber);
osSET_RTR(adcTaskNumber);

asm("sei");                               // enable interrupts.

__schedule();                             // go start multitasking.

}