// main.c
// SIMPLE COOPERATIVE MULTITASKING SYSTEM
// Harold Hallikainen
// harold@hallikainen.org
// December 2006
// This is a test of a SIMPLE cooperative multitasker. For each task, the system keeps a copy of the
// stack pointer and a copy of the stack.
// Initialization -
// Each task starts with a call of InitTask(n) where n is the task number. The task follows the InitTask
// call with any other required initialization, then enters a loop. InitTask saves the return address into
// the task in the SavedStack array. It then pops the return address off the stack so we return to main()
// instead of the function that called InitTask(). This allows us to continue setting up other tasks in main().
// Running-
// After each task has been initialized, main calls StartMultiTask(). This loads the stack with the values
// stored for task 0 by InitTask(). 
// Task Switching-
// Whenever a task is waiting for something (typically input, perhaps waiting for an output device to be ready),
// the loop contains a call of NextTask(). NextTask saves the current stack, loads the stack for the next task,
// then returns into that function where IT had called NextTask. Note that this return may be several 
// function call levels down. It need not be in the original task loop, but, instead, in a function called
// by that loop. This is makes multitasking much simpler than use of state machines for each task and function
// within that task.
// Variables and Function Parameters
// If a particular function contains a NextTask call, local variables and parameters to the function MUST
// be static. This keeps them from being located on the software stack (as automatic variables would be).
// Automatic variables and function parameters are lost during a task switch. In addition, the software stack
// may undeflow or overflow if you do a task switch while automatic variables or parameters exist. However,
// functions that do NOT include a task switch may use automatic parameters and functions since these are
// destroyed before the NextTask is called.
 
#include <p18f8627.h>
#include "types.h"
 
#define MaxTask 3
 
// Function Prototypes
void InitTask(static uint8_t TaskNum); 	// Save return address for this function to continue,
				//   then throw it out, dropping to main()
void StartMultiTask(void); 		// loads stack with initial data for task 0, then returns to task 0.
void NextTask(void); 			// save current stack and load stack for next task
void task0(void);
void task1(void);
void task2(void);
void task3(void);
uint8_t Task3SubFunction(static uint8_t n); // testing a subfunction
uint8_t Task3subSub(uint8_t n);
 
#pragma udata MultiTask 		// generally requires more than 256 bytes or ram, so put in bigger section
union{
    uint24_t u24b; 			// access as 24 bit unsigned int
    uint8_t u8b[sizeof(uint24_t)]; 	// or array of bytes
}SavedStack[MaxTask+1][31]; 		// Each stack entry is 3 bytes and there are 31 of them in each stack.
// MaxTask is highest task number. Since they start at zero, dimension to MaxTask+1 
 
static uint8_t SavedStackPointer[MaxTask+1]; // stack pointer saved for a particular task
#pragma udata 			// let linker allocate rest of stuff
 
void main(void){
    task0(); 
    task1();
    task2();
    task3(); 			// init each task
    StartMultiTask(); 		// go run multitask
}
 
void InitTask(static uint8_t TaskNum){
// Call from within a task. Puts the Saves the return address in SavedStack[TaskNum][1] so a later call
// of NextTask will continue from this address. Uses static parameter to avoid use of parameter stack which
// would be messed up when we do a return to main instead of calling function.
    INTCONbits.GIEH=0; 		// disable high priority interrupts
    INTCONbits.GIEL=0; 		// and low priority interrupts while messing with stack
    SavedStackPointer[TaskNum]=STKPTR; 	// save the current stack pointer
    SavedStack[TaskNum][STKPTR & 0b11111].u24b=TOS; // save current tos, which is where this function should start
    _asm POP _endasm; 		// remove return address so we drop back to main()
    INTCONbits.GIEL=1; 		// interrupts ok now
    INTCONbits.GIEH=1;
} 
 
void StartMultiTask(void){
// loads stack with initial data for task 0, then returns to task 0.
    INTCONbits.GIEH=0; 		// disable high priority interrupts
    INTCONbits.GIEL=0; 		// and low priority interrupts while messing with stack
    STKPTR=0; 			// start with clear stack pointer
    while(STKPTR<=SavedStackPointer[0]){
        //TOS=SavedStack[0][STKPTR & 0b11111]; // restore stack value - use below code to avoid movff to tos
        WREG=SavedStack[0][STKPTR & 0b11111].u8b[0];
        TOSL=WREG;
        WREG=SavedStack[0][STKPTR & 0b11111].u8b[1];
        TOSH=WREG;
        WREG=SavedStack[0][STKPTR & 0b11111].u8b[2];
        TOSU=WREG;
        _asm PUSH _endasm; 		// and increment stack pointer
    }
    _asm POP _endasm; 		// get rid of extra stack pointer increment at end 
    INTCONbits.GIEL=1; 		// interrupts ok now
    INTCONbits.GIEH=1;
} 
 
 
void NextTask(void){
// save away the current stack, load the one for the next task, then exit
static uint8_t TaskNum=0; 
    INTCONbits.GIEH=0; 		// disable high priority interrupts
    INTCONbits.GIEL=0; 		// and low priority interrupts while messing with stack
    SavedStackPointer[TaskNum]=STKPTR; 	// save the current stack pointer
    while(STKPTR & 0b11111){ 		// while stuff still on the stack
        SavedStack[TaskNum][STKPTR & 0b11111].u24b=TOS; // save current tos
       _asm POP _endasm; 		// decrement the stack pointer
    }
    TaskNum++; 			// on to next task
    if(TaskNum>MaxTask)
    TaskNum=0; 			// roll over if past last
    while(STKPTR<=SavedStackPointer[TaskNum]){
        //TOS=SavedStack[TaskNum][STKPTR & 0b11111]; // restore stack value - use below code to avoid movff to tos
        WREG=SavedStack[TaskNum][STKPTR & 0b11111].u8b[0];
        TOSL=WREG;
        WREG=SavedStack[TaskNum][STKPTR & 0b11111].u8b[1];
        TOSH=WREG;
        WREG=SavedStack[TaskNum][STKPTR & 0b11111].u8b[2];
        TOSU=WREG;
        _asm PUSH _endasm; 		// and increment stack pointer
     }
    _asm POP _endasm; 		// get rid of extra stack pointer increment at end 
    INTCONbits.GIEL=1; 		// interrupts ok now
    INTCONbits.GIEH=1;
    } 
 
 
 
void task0(void){
static uint8_t n=0;
    InitTask(0); 			// set up return address for here
    while(1){ 			// task loop
        ClrWdt();
        n++;
        NextTask();
    } // end while
} // end function 
 
void task1(void){
static uint8_t n=0;
    InitTask(1); 			// set up return address for here
    while(1){ 			// task loop
        ClrWdt();
        n++;
        NextTask();
    } // end while
}
 
void task2(void){
static uint8_t n=0;
    InitTask(2); 			// set up return address for here
    while(1){ 			// task loop
        ClrWdt();
        n++;
        NextTask();
    } // end while
}