/*
primitive multitasking example for NXP's LPC1788 using IAR IDE

it contains only two tasks.
Both tasks are only simple LED flashes in infinite loops.
tasks are preemted always from one to the other
it is done by SysTick interrupt handler which switches the context

1/ interrupt is set up
2/ stack for task 2 created
3/ main enters task 1 so task 1 uses stack originally used by main program

4/the tasks are regulary interupted and switched to the other one

*/
#include <nxp/iolpc1788.h>


#define LED1 (1<<13)
#define LED2 (1<<18)

#define STACK_SIZE 50

unsigned int *task1SP;
unsigned int *task2SP;
unsigned int current_task;

//stack for task 2
unsigned int stack2 [STACK_SIZE];

/*change LED state at given IO pin*/
void flash_LED(unsigned int led){
  
  //check pin state and change it
  if(FIO1PIN & led){
    FIO1CLR = led;
  }
  else{
    FIO1SET = led;
  }
  
}

/*demo task 1 flash LED1 */
void task1(){
  
  while(1){
    
    //wait
    for(int i=0;i<1000000;i++){
    }
    
    flash_LED(LED1);

  } 
}

/*demo task 2 flash LED2 */
void task2(){
  
  while(1){
    
    //wait
    for(int i=0;i<1000000;i++){
    }
    
    flash_LED(LED2);
  } 
}

/*basic hw setup */
void hw_init(){
  // diodes at P1.13 a 15 as output
  FIO1DIR = (1<<13|1<<18);
  
  //SYSTICK set up
  //enable + interupt + cpu freq
  STCTRL = (0x7);
  
  // value to count-down for systick reloadREG = (fcpu * T) - 1
  STRELOAD = 0x1D31D;//0xFFFFFE 0xB67753 0x123F22 0x1D31D //max 1s 100ms 10ms
  
}

/*this function sets up a stack for task2. This is the most difficult part.

contents of the stack from the top adress

PSR - manual copy from stack of task 1 ;) (Program Status Register)
PC - adress of task2 (Program Counter)
LR - to be poped into LR (Link Register)
R12 - to be poped into
(R3 to R0) - to be poped into
EXC_RETURN value - so processor knows that it returns from an interrupt
0x0 - from top to this point values are pushed autmatically when entering an 
      interrupt and poped back when leaving interrupt

(R4 to R11) - values to be pushed after context switch

*/
void create_task2(){
  
  //init whole array to zeros
  for(int i=0;i<STACK_SIZE;i++){
    stack2[i]=0;
  }
  //target task2SP to end of array stack2
  //so it can grow downwards
  task2SP = &stack2[STACK_SIZE-1];
  
  //set the stack formation all registers with correct values of PC LR etc
  //comment out =0 (already set)
  *task2SP = 0x81000000; //PSR ?
  task2SP--;
  *task2SP = (unsigned int) ((void *) task2); //PC task2()

  task2SP = task2SP-7;
  *task2SP = 0xFFFFFFF9; //EXC_RETURN
  
  task2SP = task2SP-9;
  //now task2SP targets to end of stack of task 2
  //ready to be switched into
}

void main(){
  
  //create stack formation for task 2
  create_task2();
  
  //start with task 1
  current_task = 1;
  
  //set up systick and other hw
  hw_init();
  
  //and go into the first task
  task1();
  
}


/* set Stack Pointer */
void set_sp(void *x){
  
  asm("MOV SP, R0");
  
}
/* get Stack Pointer */
void* get_sp(){
  
  asm("MOV R0, SP");
  
  //return value is in R0
}

//SysTick_Handler function run by the interrupt
//my simple scheduler
//switch between task 1 and 2
void SysTick_Handler(){
  
  //1 stack what is not automatically stacked
  asm("PUSH {R4-R11}");
  
  switch (current_task){
    
  case 1:
    //2 save current stack pointer
    task1SP = get_sp();
    
    //3 get SP of another task
    set_sp(task2SP);
    current_task=2;
    break;
    
  case 2:
    //2 save current stack pointer
    task2SP = get_sp();
    
    //3 get SP of another task
    set_sp(task1SP);
    current_task=1;
    break;
  }
  
  //4 restore the stack
  asm("POP {R4-R11}");
  
}