Colony

/**

 * @file scheduler.c

 * @brief Scheduler

 *

 * Implementation of functions for scheduler

 *

 * @author Colony Project, CMU Robotics Club

 * Based on avrOS and 18348 Lab9 code

 **/

#include "scheduler.h"

#include "time.h"

static uint8_t STACK[MAXTASKS][STACKSIZE];

static uint8_t nactive_tasks = 1;

static uint8_t current_task = 0; //Default to main.

//Internal functions 

void task_terminate(void);

//Scheduler gets called from the interrupt.

void scheduler(void);

void create_launch_stack(uint8_t task);

void store_task(void);

void restore_task(void); 

void restore_next_task(void); 

typedef struct PCB_t

{

        void (*exec)(void);

        uint8_t* sp;

        char running;

        uint16_t period; //Interval in clock cycles.

        uint32_t next;

} PCB_t;

PCB_t PCB[MAXTASKS + 1];

void scheduler_init() { }

void task_terminate(void) { 

        PCB[current_task].running = 0;

        yield();

}

void yield() {

        //TODO Actually implement.

        for(;;){}

}

int register_task(void (*exec)(void), uint16_t period)

{

        if(nactive_tasks >= MAXTASKS) { 

                return -1;

        }

        PCB[nactive_tasks].exec = exec;

        PCB[nactive_tasks].period = period;

        PCB[nactive_tasks].next = rtc_get() + period;

        PCB[nactive_tasks].running = 0;        

        //Don't need to initialize SP, it will get done later.

        nactive_tasks++;

        return nactive_tasks - 1;

}

//Create a fresh launch stack. 

void create_launch_stack(uint8_t task) {

        uint8_t* sp = &STACK[task][STACKSIZE - 1];

        //Put task terminate  and the task to execute on the stack.

        *(sp--) = (uint8_t)(uint16_t) *task_terminate;

        *(sp--) = (uint8_t)(uint16_t) *task_terminate >> 8;

        *(sp--) = (uint8_t)(uint16_t) *PCB[nactive_tasks].exec;

        *(sp--) = (uint8_t)(uint16_t) *PCB[nactive_tasks].exec >> 8;

        //This is going to get me in trouble,

        //but store_task already does everything else we need

        //(since we only care about void-void functions)

        store_task();        

}

void scheduler(void){

        static uint8_t task_i = 1;

        uint8_t i;

        int current_time = rtc_get();

        //Loop over registered tasks, like in round robin order.

        for(i = nactive_tasks; i > 0; i--) { 

                if(                !PCB[task_i].running

                        &&   PCB[task_i].next <= current_time) { 

                        current_task = task_i;        

                        PCB[task_i].next += PCB[task_i].period;

                        create_launch_stack(task_i);

                        task_i++;

                        break;

                }

                if(         PCB[task_i].running ) {

                        current_task = task_i;

                        task_i++

                        break;

                }

                //Loop back to 0 if necessary.

                task_i++;

                if(task_i >= nactive_tasks) 

                        task_i = 1;

        }

        //If no task was selected to run,

        if(i == 0) { 

                //Return to main.

                current_task = 0;        

        }

}

void store_task(void) { 

        //Store all state for this task.

        /* store general purpose registers */

        asm volatile( \

                 "push r31 \n" \

                   "push r30 \n" \

            "push r29 \n" \

                "push r28 \n" \

                "push r27 \n" \

                "push r26 \n" \

                "push r25 \n" \

                "push r24 \n" \

                "push r23 \n" \

                "push r22 \n" \

                "push r21 \n" \

                "push r20 \n" \

                "push r19 \n" \

                "push r18 \n" \

                "push r17 \n" \

                "push r16 \n" \

                "push r15 \n" \

                "push r14 \n" \

                "push r13 \n" \

                "push r12 \n" \

                "push r11 \n" \

                "push r10 \n" \

                "push r9 \n" \

                "push r8 \n" \

                "push r7 \n" \

                "push r6 \n" \

                "push r5 \n" \

                "push r4 \n" \

                "push r3 \n" \

                "push r2 \n" \

                "push r1 \n" \

                "push r0 \n");

         //Store status register.

        asm volatile( \

                  "in r0, __SREG__ \n" \

                   "push r0 \n");

        //Store sp.

        uint16_t sploc = (uint16_t)&(PCB[current_task].sp);

        asm volatile( \

                "in r0, __SP_L__ \n" \

                   "st Z+, r0 \n" \

            "in r0, __SP_H__ \n" \

                "st Z, r0 \n"  : : "e" (sploc));

}

//Figure out which task to run next, then run it.

void restore_task(void) { 

        //Again, we never return.

        asm volatile( \

                 "pop r0 \n" \

                 "pop r0 \n");

        /* restore stack pointer */

        uint16_t sp = (uint16_t)PCB[current_task].sp;

        asm volatile( \

                "out __SP_L__, %A0 \n" \

                "out __SP_H__, %B0 \n" : : "d" (sp));

        /* restore status register */

         asm volatile( \

                  "pop r0 \n" \

                   "out __SREG__, r0 \n");

         /* restore general purpose registers */

         asm volatile( \

                 "pop r0 \n" \

                 "pop r1 \n" \

                "pop r2 \n" \

                "pop r3 \n" \

                "pop r4 \n" \

                "pop r5 \n" \

                "pop r6 \n" \

                "pop r7 \n" \

                "pop r8 \n" \

                "pop r9 \n" \

                "pop r10 \n" \

                "pop r11 \n" \

                "pop r12 \n" \

                "pop r13 \n" \

                "pop r14 \n" \

                "pop r15 \n" \

                "pop r16 \n" \

                "pop r17 \n" \

                "pop r18 \n" \

                "pop r19 \n" \

                "pop r20 \n" \

                "pop r21 \n" \

                "pop r22 \n" \

                "pop r23 \n" \

                "pop r24 \n" \

                "pop r25 \n" \

                "pop r26 \n" \

                "pop r27 \n" \

                "pop r28 \n" \

                "pop r29 \n" \

                "pop r30 \n" \

                "pop r31 \n");

         /* start process and enable interrupts */

         //Note that either create_launch_stack or a timer interrupt

         //already has stored the value to load into PC

         asm volatile("reti \n");

}

void restore_next_task(void) { 

        //Again, we never return.

        asm volatile( \

                 "pop r0 \n" \

                 "pop r0 \n");

        PCB[current_task].running = 0;

        scheduler();

        PCB[current_task].running = 1;

        restore_task();

}

//I need a timer to do the rest.  Thinking about stealing it from rtc.

//ISR needs to 

//a) store_task

//b) restore_next_task.