/**
 *         >>>> krnl.h <<<<
 * my own small KeRNeL adapted for Arduino
 *
 * this version adapted for Arduino
 *
 * (C) 2012
 * Version see krnl.h
 * Jens Dalsgaard Nielsen <jdn@es.aau.dk>
 * http://www.control.aau.dk/~jdn
 * Section of Automation & Control
 * AAU SATLAB
 * Aalborg University,
 * Denmark
 *
 * "THE BEER-WARE LICENSE"
 * <jdn@es.aau.dk> wrote this file. As long as you retain this notice you
 * can do whatever you want with this stuff. If we meet some day, and you think
 * this stuff is worth it, you can buy me a beer in return :-) or if you are real happy then ...
 * single malt will be well received :-)
 *
 * /Jens
 *
 *****************************************************************************************
 */
 
#include "krnl.h"

#if (KERNEL_VRS != 108)
#error "KERNELVERSION NOT UPDATED in krnl.c /JDN"
#endif

//#include <avr/interrupt.h>
//#include <avr/io.h>
//#include <HardwareSerial.h>
#if not defined (NULL)
#define NULL 0x0000
#endif
 
/***** KeRNeL variables *****/
struct k_t task_pool[K_TASK];	 
struct k_t sem_pool[K_SEM];	 

#ifdef MSGQ
struct k_msg_t send_pool[K_MSG];
#endif

struct k_t AQ,			// activeQ
main_el;

struct k_t
*pAQ,
*pDmy, 			        // ptr to dummy task descriptor
*pRun; 				// ptr to running task

/* counters for created KeRNeL items */
char nr_task = 0, 
nr_sem = 0,
nr_send = 0;


volatile char k_running = 0;  // no running

// counters for allocated tasks/semaphores/timer semaphores

volatile char k_err_cnt = 0;				// every time an error occurs cnt is incr by one

/***** QOPS *****
 * double chained list
 * ordered acc to prio
 * prio 0 in front
 * qhead has prio QHEAD_PRIO which no element must have
 * NB: no DI/EI you must do it prio to call !
 * might not be most efficient but easy to understand :-)
 */

void enQ(struct k_t * Q, struct k_t * el) {
  el->next = Q;
  el->pred = Q->pred;
  Q->pred->next = el;
  Q->pred = el;
}

struct k_t * deQ(struct k_t * el) {
  el->pred->next = el->next;
  el->next->pred = el->pred;
  return (el);
}

void prio_enQ(struct k_t * Q, struct k_t * el) {
  char prio = el->prio;

  Q = Q->next; // bq first elm is Q head itself

  while (Q->prio <= prio) {
    Q = Q->next;
  }

  el->next = Q;
  el->pred = Q->pred;
  Q->pred->next = el;
  Q->pred = el;
}

/***** ende QOPS *****/
 
/***** HW DEPENDENT PART FOR TASKS *****/ 


int k_unused_stak(struct k_t *t) {  
  int i;
  volatile char *p;
  p = (char *)(t->cnt1);
  i = 0;

  while (*(p + i) == STAK_HASH) // cnt hash codes on stak == amount of unused stak pr dft :-)
    i++; 
  return (i);
}


// following for having a clean piece of code wher the PUSHREGS and POPREGS + iret
// surrounds the body
// the interrupt attribute is for ensuring that it is compiled and generated as
// a function and not as inline code.
// Without the interrupt is does not work on windows due to "wrong" parms to the compiler
// Linux vrs is clean and works
 
 void   __attribute__((naked,noinline))  ki_task_shift(void) {
  PUSHREGS();   // push task regs on stak so we are rdy to task shift
   #ifdef DMYBLINK
  digitalWrite(13,LOW);
  #endif

  if (pAQ->next == pRun) // need to change task ?
    goto exitt;

  pRun->sp_lo = SPL;  // save stak ptr
  pRun->sp_hi = SPH;

  pRun = pAQ->next; 

  SPL = pRun->sp_lo;  // restablish stk ptr
  SPH = pRun->sp_hi;
exitt:
  POPREGS();  // restore regs
  reti();     // and do a reti NB this also enables interrupt !!!
 }


struct k_t * k_crt_task(void (*pTask)(void), char prio, char *pStk, int stkSize) {
  struct k_t *pT;

  int i;
  char *s;

  if (k_running)
    return (NULL);
    
  if (DMY_PRIO < prio) {
    pT = NULL;
    goto badexit;
  }

  if (K_TASK <= nr_task) {
    goto badexit;
  }

  pT = task_pool + nr_task; // lets take a task descriptor
  nr_task++;

  pT->cnt2 = 0; // no time out running on you for the time being
  
  pT->cnt1 = (int) (pStk);

  for (i = 0; i < stkSize; i++)   // put hash code on stak to be used by k_unused_stak()
    pStk[i] = STAK_HASH;  

// http://dev.bertos.org/doxygen/frame_8h_source.html
// now we are goigg to precook stak
  s = pStk + stkSize - 1;  // now we point on top of stak
  *(s--) = 0x00;                     // 1 byte safety distance
  *(s--) = lo8(pTask);     //  so top now holds address of function which is code body for task
  *(s--) = hi8(pTask);

  // 2560 use 3 byte for call/ret addresses 
  #if defined (__AVR_ATmega2560__)
  *(s--) = EIND;// EIND; // eind best guess : 3 byte addresses !!!
  #endif

  *(s--) = 0x00; // r1
  *(s--) = 0x00; // r0
  *(s--) = 0x00; // sreg
  
 //1280 and 2560 need to save rampz reg just in case
 #if defined (__AVR_ATmega2560__) || defined (__AVR_ATmega1280__)
    *(s--) = RAMPZ; // best guess 
    *(s--) = EIND; // best guess 
 #endif
  
  for (i=0; i < 30; i++) //r2-r31 = 30 regs  
    *(s--) = 0x00;
    
  pT->sp_lo = lo8(s);   // now we just need to save stakptr in thread descriptor
  pT->sp_hi = hi8(s);
  pT->prio = prio;

  prio_enQ(pAQ, pT); // and put task in active Q

  return (pT); // shall be index to task descriptor

badexit: 
  k_err_cnt++;
  return (NULL);
}

/***** ENDE HW DEPENDANT AROUND TASK *****/
 
 char k_set_prio(char prio) {
   if (!k_running)
     return (-1);
     
   if (( prio < 0) || (DMY_PRIO <= prio) ) // not legal value my friend
     return (-2);
     
   DI();
   pRun->prio  = prio;
   prio_enQ(pRun,deQ(pRun) );
   ki_task_shift();
   EI();
   return (0);
 }

/***** Semaphore ops ******/

struct k_t * k_crt_sem(char init_val, int maxvalue) {
  struct k_t *sem;

  if (k_running) 
    return (NULL);
 
  if ((init_val < 0) || (maxvalue < 0)) {
    goto badexit;
  }
 
  if (K_SEM <= nr_sem) {
    goto badexit;
  }

  sem = sem_pool + nr_sem;
  nr_sem++;

  sem->cnt2 = 0 ; // no timer running
  sem->next = sem->pred = sem;
  sem->prio = QHD_PRIO;
  sem->cnt1 = init_val;
  sem->maxv=maxvalue;

  return (sem);

badexit: 
  k_err_cnt++;
   return (NULL);
}


char k_set_sem_timer(k_t * sem, int val) {

  if (val < 0)
    return (-1); // bad value

  DI();
  sem->cnt2 = sem->cnt3 = val; // if 0 then timer is not running - so 
  EI();                       // there is no k_stop_sem_timer fct.

  return (0);
}

 
char ki_signal(struct k_t * sem) {

  if (sem->maxv <= sem->cnt1)
    return (-1);

  sem->cnt1++;  // Salute to Dijkstra
  
  if (sem->cnt1 <= 0) {
    sem->next->cnt2 = 0; // return code == ok
    prio_enQ(pAQ, deQ(sem->next));
  }
  
  return (0);
}

char k_signal(struct k_t * sem) {
  volatile char res;

  DI();
  
  res = ki_signal(sem);
  
  if (res == 0)
    ki_task_shift();

  EI();

  return (res);
}


char k_wait(struct k_t * sem, int timeout) {
 
  DI();

  if (0 < sem->cnt1) {   // do by any chance be so lucky that we do not need to wait ?
    sem->cnt1--;  // Salute to Dijkstra
    EI();
    return (0);
  }
  
  if (timeout == -1) {  // no luck and we dont want to wait so just bye bye
    EI();
    return (-2); // do not want to wait - sorry
  }
  
  // from here we want to wait
  pRun->cnt2 = timeout; // if 0 then wait forever
  
  if (timeout)
    pRun->cnt3 = (int)sem; // nasty keep ref to semaphore so we can be removed if timeout occurs

  sem->cnt1--; // Salute to Dijkstra
  
  enQ(sem, deQ(pRun));
  ki_task_shift(); // do enable ISR on return

  EI();
  
  return (char)(pRun->cnt2); // 0: ok, -1: timeout 
}


int ki_semval(struct k_t * sem){
int i;
	DI(); // bq atomic
	i = sem->cnt1;
	EI();
	return i;
}
/*****  ende semaphore operations *****/

/***** msg Q operations *****/

char ki_send(struct k_msg_t *pB, void *el) {
  int i;
  char *pSrc, *pDst;

  if (pB->nr_el <= pB->cnt) { // room for a putting new msg in Q ?
    return (-1);  // nope
  }

  pB->cnt++;
  
  pSrc = (char *)el;

  pB->w++;
  if (pB->nr_el <= pB->w)  // simple wrap around
    pB->w = 0;

  pDst = pB->pBuf + (pB->w * pB->el_size); // calculate where we shall put msg in ringbuf

  for (i=0; i < pB->el_size; i++) { // copy to Q
    *(pDst++) = *(pSrc++);
  }

  ki_signal(pB->sem);  // indicate a new msg is in Q

  return (0);
}

char k_send(struct k_msg_t *pB, void *el) {
  char res;
 
  DI();
  
  res = ki_send(pB,el);

  if (res == 0)
    ki_task_shift();

  EI();

  return (res);
}


// there is no ki_receive - why is left to the reader
// hint: not good idea to block in an ISR ...
char k_receive(struct k_msg_t *pB, void *el,int timeout) {
  int i;
  char *pSrc, *pDst;

  DI();
  
  if (k_wait(pB->sem,timeout) == 0) { 
    
    DI(); // bq k_wait enables interrupt

    pDst = (char *)el;
    pB->r++;
    pB->cnt--; // got one

    if (pB->nr_el <= pB->r)
      pB->r = 0;

    pSrc = pB->pBuf+pB->r*pB->el_size;

    for (i=0; i < pB->el_size; i++) {
      *(pDst++) = *(pSrc++);
    }
  
    EI();    
    return (0) ; // yes
  } // if k_wait
  
  return (-1); // nothing for you my friend
}


struct k_msg_t * k_crt_send_Q(int nr_el, int el_size, void *pBuf) {
  struct k_msg_t *pMsg;

  if (k_running)
    return (NULL);
  
  if (K_MSG <= nr_send)  
    goto errexit;
  
   if (K_SEM <= nr_sem)  
     goto errexit;
      
  pMsg = send_pool + nr_send;
  nr_send++;

  pMsg->sem = k_crt_sem(0,nr_el);
 
  if (pMsg->sem == NULL) 
    goto errexit;

  pMsg->pBuf = (char*)pBuf;
  pMsg->r=pMsg->w = -1;
  pMsg->el_size = el_size;
  pMsg->nr_el = nr_el;
  
  return (pMsg);
  
  errexit:
  k_err_cnt++;
  return (NULL);
}

/***** end mesg Q operations *****/
 

/***** Timer section *****
* The KeRNeL Timer is driven by timer2
*/

 
/*
 * Install the Interrupt Service Routine (ISR) for Timer2 overflow.
 * This is normally done by writing the address of the ISR in the
 * interrupt vector table but conveniently done by using ISR()  
 */

/* Timer2 reload value, globally available */
 
volatile unsigned int tcnt2;
volatile int fakecnt,fakecnt_preset;

int tmr_indx;
k_t * pEl;
 
 
ISR(TIMER2_OVF_vect, ISR_NAKED) {
// no local vars ?!?  ! I think
    PUSHREGS();
   
   TCNT2 = tcnt2;     // Reload the timer

   fakecnt--;
   if (0 < fakecnt) // how often shall we run KeRNeL timer code ?
     goto exitt;
   fakecnt = fakecnt_preset; // now it's time for doing RT stuff
   
  #ifdef DMYBLINK
  digitalWrite(13,LOW);
  #endif

  if (!k_running)
    goto exitt;
    
  pEl = sem_pool; // check timers on semaphores - they are cyclic
  for (tmr_indx = 0; tmr_indx < nr_sem; tmr_indx++) {
    if (0 < pEl->cnt2) { // suspended or not
      pEl->cnt2--;    
      if (pEl->cnt2 <= 0) {
        pEl->cnt2 = pEl->cnt3; // preset again
        ki_signal(pEl); //issue a signal to the semaphore
      }
    }
    pEl++;
  }
  
  pEl = task_pool;   // Chk timers on tasks - they are one shoot
  for (tmr_indx = 0; tmr_indx < nr_task; tmr_indx++) {
    if (0 < pEl->cnt2) { // timer ?
      pEl->cnt2--;    
      if (pEl->cnt2 == 0) { // timeout my friend
        pEl->cnt2 = -1; // error timeout
        prio_enQ(pAQ,deQ(pEl));
        ((struct k_t *)(pEl->cnt3))->cnt1++; // bq we have leaved this semQ  
      }
    }
    pEl++;
  }

  prio_enQ(pAQ, deQ(pRun)); // round robbin
   
  K_CHG_STAK();
 
exitt:
  POPREGS();
  reti();
 }

 
int k_start(int tm) {
  
  if (k_err_cnt)
	return -1;
   
  DI();
  // http://arduinomega.blogspot.dk/2011/05/timer2-and-overflow-interrupt-lets-get.html
  // Inspiration from  http://popdevelop.com/2010/04/mastering-timer-interrupts-on-the-arduino/ 
  //TIMSK2 &= ~(1 << TOIE2);  // Disable the timer overflow interrupt while we're configuring 
  //TCCR2B &= ~(1 << WGM22);
  
  #if defined(__AVR_ATmega32U4__)
  // 32u4 have no intern/extern clock source register
  #else
  ASSR &= ~(1 << AS2);       // Select clock source: internal I/O clock 32u4 does not have this facility
  #endif

  if (0 < tm) {
  TIFR2 = 0x00; 
  TCCR2B = 0x00; //silencio from this timer
  TCCR2A &= ~((1 << WGM21) | (1 << WGM20));  //Configure timer2 in normal mode (pure counting, no PWM etc.) 
  TCCR2B |= (1 << CS22) | (1 << CS21) | (1 << CS20);   // Set prescaler to CPU clock divided by 1024 See p162 i atmega328
  TIMSK2 &= ~(1 << OCIE2A);  //Disable Compare Match A interrupt enable (only want overflow) 
  TIMSK2 = 0x01; //HACK ?
  TCCR2A = 0x00; // normal
  
   /* for your memory
   *  We need to calculate a proper value to load the timer counter.
   * The following loads the value 131 into the Timer 2 counter register
   * The math behind this is:
   * (CPU frequency) / (prescaler value) = 16000000/1024= 15625 Hz ~= 64us.
   * 100Hz = 10msec
   * 10000usec / 64us = 156.25
   * MAX(uint8) + 1 - 156 = 100;
   * JDN
   * 100 Hz ~ 100
   * tm in msec ->
   * cnt =  tm*1000/64
   * ex: 10 msec: 10000/64 =156
   * 
   * some timer reg values: 
   * 1msec: 240 5: 178  10: 100   15: 22
   */
  tcnt2 = 240; // 1 msec as basic heart beat
  
  // lets set divider for timer ISR
  if (tm <= 0)
    fakecnt = fakecnt_preset = 10; // 10 msec
  else
    fakecnt = fakecnt_preset = tm;
     
  TCNT2 = tcnt2;   // Finally load end enable the timer 
  TIMSK2 |= (1 << TOIE2);
  }
  k_running = 1; // 1
  pRun = &main_el; // just for ki_task_shift
  DI();
  ki_task_shift(); // bye bye from here
  EI();
  while (1); // you will never come here
  return (0); // ok
}

/***** ende timer section *****/

void k_round_robbin(void) {
  // reinsert running task in activeQ if round robbin is selected
  DI();

  prio_enQ(pAQ, deQ(pRun));
  ki_task_shift();

  EI();
}


/***** INIT PART INCL DUMMY TASK *****/

char dmy_stk[DMY_STK_SZ];
 
void dmy_task(void) {
  #ifdef DMYBLINK
int tgl = 0;
  pinMode(13,OUTPUT); 
  #endif
 
  while (1) {
    #ifdef DMYBLINK
    digitalWrite(13, tgl == 0 ? HIGH : LOW);  
    tgl = ~tgl;
    digitalWrite(13,HIGH);
    #endif
  }
}

int k_init(void) {

  pAQ = &AQ;
  pAQ->next = pAQ->pred = pAQ;
  pAQ->prio = QHD_PRIO;
 
  pDmy = k_crt_task(dmy_task, DMY_PRIO, dmy_stk, DMY_STK_SZ);

  return (0); // ok
}

/***** for test of amount of free memory *****/
 
int freeRam(void) {
  extern int __heap_start, *__brkval;
  int v;
  return ((int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval));
}

/***** ende for test of amount of free memory *****/

/***** stak usage ****/

#ifdef STAK_USAGE

void k_stak_usage(void)
{
  int i,s;
  char ss[11];
   Serial.println(" ");
   for (i=0 ; i< nr_task; i++) {
     itoa(i,ss,10);
     Serial.print("task"); Serial.print(ss); Serial.print(": ");
     s = k_unused_stak(task_pool+i);
     itoa(s,ss,10);
     Serial.println(ss);
  } 
}

#else
void k_stak_usage(void)
{
}
#endif