Colony

/**

 * @file scheduler.c

 * @brief Scheduler

 *

 * Implementation of functions for scheduler

 * Currently relies on avr-gcc's interrupt call convention.

 *

 * @author Colony Project, CMU Robotics Club

 * Based on avrOS and 18348 Lab9 code

 **/

#include <avr/interrupt.h>

#include <util/delay.h>

#include "scheduler.h"

#define DEBUG

#ifdef DEBUG

#include <serial.h>

#endif //DEBUG

static uint8_t STACK[MAXTASKS + 1][STACKSIZE];

static uint8_t nactive_tasks = 1;

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

//Internal functions

static void task_terminate(void);

//TODO Use a context struct instead of pushing onto the stack.

//so we don't need to worry so much about stack overflow.

typedef struct context_t {

        uint8_t reg[32];

        uint8_t sreg;

} context_t;

//Define some PCB's

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];

//Keep track of global time.

static uint32_t current_time = 0;

unsigned int time_now() {

        return current_time;

}

void scheduler_init() {

        //Set the counter to 0.

        TCNT3 = 0;

        //Clear timer on compare (CTC) mode.

        TCCR3B |= _BV(WGM32);

        //Set a prescalar of 8. 8 / (8MHz) = 1us per tick.

        TCCR3B |= _BV(CS31);

        //Trigger an interrupt every 16th second for now.

        OCR3A = 0xF424;

        //Enable Output Compare A Interrupt. (Begins immediately).

          ETIMSK |= _BV(OCIE3A);

#ifdef DEBUG

        usb_init();

#endif //DEBUG

        sei();

}

static void task_terminate(void) {

#ifdef DEBUG

        usb_puts("In task terminate!.\n\r");

#endif //DEBUG

        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 = current_time + period;

        PCB[nactive_tasks].running = 0;

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

        nactive_tasks++;

        return nactive_tasks - 1;

}

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

SIGNAL(TIMER3_COMPA_vect) {

        static uint8_t task_i = 1;

        volatile uint8_t** sploc;

        volatile uint8_t* sp;

        uint8_t i;

#ifdef DEBUG

        char test_buf[128];

#endif //DEBUG

        //Store sp.

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

        asm volatile( \

                "in r0, __SP_L__ \n" \

                   "st %a0+, r0 \n" \

            "in r0, __SP_H__ \n" \

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

#ifdef DEBUG

        sprintf(test_buf, "current_task = %d, current_time = %d, sp = %x\n\r", current_task, current_time, sploc);

        usb_puts(test_buf);

#endif //DEBUG

        //The current task may no longer be running when we exit.

        PCB[current_task].running = 0;

        //Keep track of the number of 1/16 second's that have passed.

        current_time++;

        /******** scheduler ********/

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

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

                //Launch a new task if it is due to run.

                if(        !PCB[task_i].running

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

                        task_i++;

                        current_task = task_i;

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

                        /***** create launch stack ***/

#ifdef DEBUG

                        usb_puts("Switching tasks..... (cross fingers)\n\r");

#endif //DEBUG

                        sp = &(STACK[task_i][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;

                        //The current state of the registers is what the tasks will

                        //see on entry (shouldn't matter).

                        asm volatile( \

                                "out __SP_L__, %A0 \n" \

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

#ifdef DEBUG

                        sprintf(test_buf, "Switching tasks,  sp = %x\n\r", sp);

                        usb_puts(test_buf);

#endif //DEBUG

                         /* start process and enable interrupts */

                         asm volatile("reti \n");

                        break;

                }

                //Continue an old task.

                if(PCB[task_i].running) {

                        current_task = task_i;

                        //Reset the stack pointer.

                        sp = PCB[current_task].sp;

                        asm volatile( \

                                "out __SP_L__, %A0 \n" \

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

                        task_i++;

                        break;

                }

                //Loop back to 1 if necessary.

                task_i++;

                if(task_i >= nactive_tasks)

                        task_i = 1;

        }

        //If no task was selected to run, go to main

        if(i == 0) {

#ifdef DEBUG

                usb_puts("Resetting main.\n\r");

#endif //DEBUG

                //Set the SP to the original stack.

                sp = PCB[current_task].sp;

                asm volatile( \

                        "out __SP_L__, %A0 \n" \

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

                current_task = 0;

        }

        //Set the running task.*/

        PCB[current_task].running = 1;

        //avr-gcc inserts all of the necessary pops and the reti here.

}