Bare minimum preemptive multitasking code for arduino

arduino-multitasking-bare-minimum.ino

typedef struct {
  uint8_t taskStatus = 0;
  uint16_t R_SP;
} Regs;

const size_t MAX_TASKS = 2;
Regs tasks[MAX_TASKS];
uint8_t currentTask = 0;

void setup() {
  
  uint8_t sreg = SREG;
  Serial.begin(9600);
  
  //set current task to 0 - the initial task. Note that the only way for it to be killed is if someone sets its taskStatus to 0 or replaces it with different task
  currentTask = 0;
  
  //set up a new task
  
  //stack of the new task begins 256 bytes after the end of the first task stack
  uint8_t* stack = (uint8_t*)(RAMEND - 256);
  
  *(uint16_t*)(stack-1) = (uint16_t) (void*) terminateTask;//return address of the task - terminate it if it ever returns
  stack -= 2;
  *(uint16_t*)(stack-1) = (uint16_t) (void*) task1;//the task - return address of the interrupt
  stack -= 2;
  for(int i = 0; i < 33; i++) {//33 registers saved in the stack
    *stack = (uint8_t) 0;
    if(i == 1) {
      //SREG, hopefully. TODO: is it even needed?
      *stack = sreg;
    }
    stack--;
  }
  stack--;//TODO: figure out why it doesn't work without it. By all my logic it shouldn't be needed
  tasks[1].R_SP = (uint16_t) stack;
  
  tasks[0].taskStatus = 1;
  tasks[1].taskStatus = 1;//enable both tasks
  timer2_setup();//set up timer to enable multitasking
  
  pinMode(12, OUTPUT);
}

void loop() {
  digitalWrite(12, HIGH);
  delay(780);
  digitalWrite(12, LOW);
  delay(780);
}

void task1() {
  while(true) {
    digitalWrite(13, HIGH);
    delay(700);
    digitalWrite(13, LOW);
    delay(700);
    Serial.println("test");
  }
}

void terminateTask() {
  tasks[currentTask].taskStatus = 0;
  while(true);
}

void timer2_setup() {
  noInterrupts();
  // Setup Timer2 overflow to fire every 8ms (125Hz)
  //   period [sec] = (1 / f_clock [sec]) * prescale * (255-count)
  //                  (1/16000000)  * 1024 * (255-130) = .008 sec


  TCCR2B = 0x00;        // Disable Timer2 while we set it up

  TCNT2  = 130;         // Reset Timer Count  (255-130) = execute ev 125-th T/C clock
  TIFR2  = 0x00;        // Timer2 INT Flag Reg: Clear Timer Overflow Flag
  TIMSK2 = 0x01;        // Timer2 INT Reg: Timer2 Overflow Interrupt Enable
  TCCR2A = 0x00;        // Timer2 Control Reg A: Wave Gen Mode normal
  TCCR2B = 0x07;        // Timer2 Control Reg B: Timer Prescaler set to 1024
  interrupts();
}

//handle timer 2 overflow, naked interrupt
ISR(TIMER2_OVF_vect, ISR_NAKED) {
  //save state of current task.
  asm volatile(
    "push r31\n\t"//push all general purpose registers registers
    "in r31, __SREG__\n\t"//save SREG
    "push r31\n\t"
    "push r30\n\t"
    "push r29\n\t"
    "push r28\n\t"
    "push r27\n\t"
    "push r26\n\t"
    "push r25\n\t"
    "push r24\n\t"
    "push r23\n\t"
    "push r22\n\t"
    "push r21\n\t"
    "push r20\n\t"
    "push r19\n\t"
    "push r18\n\t"
    "push r17\n\t"
    "push r16\n\t"
    "push r15\n\t"
    "push r14\n\t"
    "push r13\n\t"
    "push r12\n\t"
    "push r11\n\t"
    "push r10\n\t"
    "push r9\n\t"
    "push r8\n\t"
    "push r7\n\t"
    "push r6\n\t"
    "push r5\n\t"
    "push r4\n\t"
    "push r3\n\t"
    "push r2\n\t"
    "push r1\n\t"
    "clr r1\n\t"//compiler expects this
    "push r0\n\t"
  ::);
  register uint16_t stack = SP;
  tasks[currentTask].R_SP = stack;
  
  findTask();
  //restore stack of new task
  stack = tasks[currentTask].R_SP;
  SP = stack;
  
  //Restore registers of new tasks
  asm volatile(
    "pop r0\n\t"
    "pop r1\n\t"
    "pop r2\n\t"
    "pop r3\n\t"
    "pop r4\n\t"
    "pop r5\n\t"
    "pop r6\n\t"
    "pop r7\n\t"
    "pop r8\n\t"
    "pop r9\n\t"
    "pop r10\n\t"
    "pop r11\n\t"
    "pop r12\n\t"
    "pop r13\n\t"
    "pop r14\n\t"
    "pop r15\n\t"
    "pop r16\n\t"
    "pop r17\n\t"
    "pop r18\n\t"
    "pop r19\n\t"
    "pop r20\n\t"
    "pop r21\n\t"
    "pop r22\n\t"
    "pop r23\n\t"
    "pop r24\n\t"
    "pop r25\n\t"
    "pop r26\n\t"
    "pop r27\n\t"
    "pop r28\n\t"
    "pop r29\n\t"
    "pop r30\n\t"
    "pop r31\n\t"
    "out __SREG__, r31\n\t"
    "pop r31\n\t"
    "reti\n\t"
  ::);
};

void findTask() {
  do {
    currentTask++;
    if(currentTask >= MAX_TASKS) {
      currentTask = 0;
    }
  } while(!tasks[currentTask].taskStatus);
}