/**
 *         >>>> 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 "krnlstat.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;							// for main(loop) for parking only

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

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

volatile char k_running = 0;  				// no running

volatile char k_err_cnt = 0;	// every time an error occurs cnt is incr by one
// KeRNeL will NOT start if != 0 !!!

/***** 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 :-)
 */

/**
 * @brief   Insert an element in end of double chain list
 * @post    Nothing special
 * @note    Queue head need to be initialized prior to ...
 *
 * @param[in] Q	Queue reference
 * @param[in] el Element to enter Q
 * @param[out] nothing
 * @return  nothing
 *
 */
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

// HW_DEP_START
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 !!!
}
// HW_DEP_ENDE

/**
 * Prepare task
 * HW dependent part between HW_DEP_START and HW_DEP_ENDE
 */
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;

	// HW_DEP_START
	// 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
	*(s--) = hi8(pTask);			// which is code body for task

	// NB  NB 2560 use 3 byte for call/ret addresses rest only 2
#if defined (__AVR_ATmega2560__)
	*(s--) = 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
	pT->sp_hi = hi8(s); 			// in thread descriptor
	//HW_DE_ENDE

	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) { 				//	lucky that we do not need to wait ?
		sem->cnt1--;  					// Salute to Dijkstra
		EI();
		return (0);
	}

	if (timeout == -1) {  				// no luck, dont want to wait so bye bye
		EI();
		return (-2);
	}

	// 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(); 					// call enables interrupt on return

	EI();

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

int ki_semval(struct k_t * sem) {
	int i;
	DI();
	// bq we ant it to be 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 and my own 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 5msec: 178  10msec: 100   15msec: 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(int dmy1, int dmy2, int dmy3) {

	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 *****/