int
k_mut_ceil_enter (struct k_t *sem, int timeout)
{
int retval;
DI ();
// if ceiling_prio < 0 then its a normal wait call
if (sem->ceiling_prio < 0) {
retval = ki_wait (sem, timeout); // could call k_wait but just extra fct call
EI ();
return retval;
}
if (pRun->prio < sem->ceiling_prio) { // I have higher priority than ceiling :-(
EI ();
return CEILINGFAIL;
}
// now we play imm ceiling protocol
sem->saved_prio = pRun->prio; // do im ceiling
pRun->prio = sem->ceiling_prio; // dont need to reinsert in AQ bq ceil prio is higher or equal to mine and Im already in front of AQ
prio_enQ (pAQ, deQ (pRun)); // resinsert me in AQ acc to nwe(old) priority
retval = ki_wait (sem, timeout);
// coming back interrupt is still disabled !
// chk if we did get semaphore
if (retval < 0) { // NOPE we did not
pRun->prio = sem->saved_prio; // reset to my old priority
prio_enQ (pAQ, deQ (pRun)); // reinsert me in AQ acc to nwe(old) priority
ki_task_shift (); // bq maybe started task has higher prio than me
}
EI ();
return retval; // 0(has waited),1(straight through) : ok, -1: timeout
}
ISR (TIMER2_OVF_vect, ISR_NAKED)
{
// no local vars ! I think
PUSHREGS ();
TCNT2 = tcnt2; // Reload the timer
if (!k_running) // obvious
goto exitt;
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
// It looks maybe crazy to go through all semaphores and tasks
// but
// you may have 3-4 tasks and 3-6 semaphores in your code
// so...
// and - it's a good idea not to init krnl with more items (tasks/Sem/msg descriptors than needed)
pE = sem_pool; // Semaphore timer - check timers on semaphores - they may be cyclic
for (tmr_indx = 0; tmr_indx < nr_sem; tmr_indx++) {
if (0 < pE->cnt2) { // timer on semaphore ?
pE->cnt2--; // yep decrement it
if (pE->cnt2 <= 0) { // timeout ?
pE->cnt2 = pE->cnt3; // preset again - if cnt3 == 0 and >= 0 the rep timer
ki_signal (pE); //issue a signal to the semaphore
}
}
pE++;
}
pE = task_pool; // Chk timers on tasks - they may be one shoot waiting
for (tmr_indx = 0; tmr_indx < nr_task; tmr_indx++) {
if (0 < pE->cnt2) { // timer active on task ?
pE->cnt2--; // yep so let us do one down count
if (pE->cnt2 <= 0) { // timeout ? ( == 0 )
pE->cnt2 = -1; // indicate timeout inis semQ
prio_enQ (pAQ, deQ (pE)); // to AQ
}
}
pE++;
}
prio_enQ (pAQ, deQ (pRun)); // round robbin
K_CHG_STAK ();
exitt:
POPREGS ();
RETI ();
}
int
ki_signal (struct k_t *sem)
{
DI (); // just in case
if (sem->cnt1 < sem->maxv) {
sem->cnt1++; // Salute to Dijkstra
if (sem->cnt1 <= 0) {
sem->next->cnt2 = 0; // return code == ok
prio_enQ (pAQ, deQ (sem->next));
return (0); // task was waiting
}
return (1); // just delivered a signal - no task was waiting
}
if (SEM_MAX_VALUE > sem->clip) {
sem->clip++;
}
// here we are on bad clip failure no signal takes place
// signal is lost !!!
#ifdef KRNLBUG
k_sem_clip (sem->nr, sem->clip);
#endif
return (-1);
}
int
k_set_prio (char prio)
{
int i;
if (!k_running) {
return (-2);
}
DI ();
if ((prio <= 0) || (DMY_PRIO <= prio)) // not legal value my friend
{
EI ();
return (-1);
}
i = pRun->prio;
pRun->prio = prio;
prio_enQ (pAQ, deQ (pRun));
ki_task_shift ();
EI ();
return (i);
}
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 ();
}
void
chg_Q_pos (struct k_t *el)
{
// not mature
#ifdef PRIOINHERITANCE
struct k_t *q;
q = el->next;
while (q->prio < QHD_PRIO) // lets find Q head
q = q->next;
prio_enQ (q, el); // resinsert
#endif
}
char
k_mutex_leave (struct k_t *sem)
{
volatile char res;
DI ();
pRun->prio = (char) (pRun->maxv); // back to org prio
prio_enQ (pAQ, deQ (pRun)); // chg pos in AQ acc to prio
res = ki_signal (sem);
if (res == 0)
ki_task_shift ();
EI ();
return (res);
}
int
ki_signal (struct k_t *sem)
{
if (sem->maxv <= sem->cnt1){
if (32000 > sem->clip)
sem->clip++;
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);
}
int
k_prio_signal (struct k_t *sem, char prio)
{
int res;
DI ();
res = ki_signal (sem);
// set prio
pRun->prio = prio;
prio_enQ (pAQ, deQ (pRun));
ki_task_shift ();
EI ();
return (res);
}
int
k_init (int nrTask, int nrSem, int nrMsg)
{
if (k_running) // are you a fool ???
{
return (-666);
}
k_task = nrTask + 1; // +1 due to dummy
k_sem = nrSem + nrMsg + 1; // due to that every msgQ has a builtin semaphore
k_msg = nrMsg + 1; // to align so first user msgQ has index 1
nr_send++; // to align so we waste one but ... better equal access
task_pool = (struct k_t *) malloc (k_task * sizeof (struct k_t));
sem_pool = (struct k_t *) malloc (k_sem * sizeof (struct k_t));
send_pool = (struct k_msg_t *) malloc (k_msg * sizeof (struct k_msg_t));
// we dont accept any errors
if ((task_pool == NULL) || (sem_pool == NULL) || (send_pool == NULL)) {
k_err_cnt++;
goto leave;
}
// init AQ as empty double chained list
pAQ = &AQ;
pAQ->next = pAQ->pred = pAQ;
pAQ->prio = QHD_PRIO;
// crt dummy
//pDmy = k_crt_task (dummy_task, DMY_PRIO, dmy_stk, DMY_STK_SZ);
pmain_el = task_pool;
pmain_el->nr = 0;
pmain_el->cnt2 = pmain_el->cnt3 = 0;
nr_task++;
pmain_el->prio = DMY_PRIO; // main is dummy
prio_enQ (pAQ, pmain_el);
pSleepSem = k_crt_sem (0, 2000);
leave:
return k_err_cnt;
}
int
k_mut_ceil_leave (struct k_t *sem)
{
int res;
DI ();
if (sem->ceiling_prio < 0) { // just std signal
return k_signal (sem);
}
res = ki_signal (sem); // 1: ok no task to AQ, 0: ok task to AQ
// coming back interrupt is still disabled !
pRun->prio = sem->saved_prio; // reset to my old priority
prio_enQ (pAQ, deQ (pRun)); // resinsert me in AQ acc to nwe(old) priority
ki_task_shift (); // bq maybe started task has higher prio than me
EI ();
return (res);
}
int
k_set_prio (char prio)
{
if (!k_running)
return (-1);
DI ();
if ((prio <= 0) || (DMY_PRIO <= prio)) {
// not legal value my friend
EI ();
return (-2);
}
pRun->prio = prio;
prio_enQ (pAQ, deQ (pRun));
ki_task_shift ();
EI ();
return (0);
}
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) || ((prio <= 0) || (DMY_PRIO < prio))
|| (k_task <= nr_task)) {
goto badexit;
}
pT = task_pool + nr_task; // lets take a task descriptor
pT->nr = nr_task;
nr_task++;
pT->cnt2 = 0; // no time out running on you for the time being
pT->cnt3 = 0; // no time out semaphore
pT->cnt1 = (int) (pStk); // ref to my stack
// stack paint :-)
for (i = 0; i < stkSize; i++) // put hash code on stak to be used by k_unused_stak()
{
pStk[i] = STAK_HASH;
}
s = pStk + stkSize - 1; // now we point on top of stak
*(s--) = 0x00; // 1 byte safety distance :-)
// an interrupt do only push PC on stack by HW - can be 2 or 3 bytes
// depending of 368/.../1280/2560
#ifdef BACKSTOPPER
pT->pt = pTask;
*(s--) = lo8 (jumper); // so top now holds address of function
*(s--) = hi8 (jumper); // which is code body for task
#else
*(s--) = lo8 (pTask); // so top now holds address of function
*(s--) = hi8 (pTask); // which is code body for task
#endif
// NB NB 2560 use 3 byte for call/ret addresses the rest only 2
#if defined (__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)
*(s--) = EIND; // best guess : 3 byte addresses !!! or just 0
#endif
// r1 is the socalled zero value register
// see https://gcc.gnu.org/wiki/avr-gcc
// can tmp be non zero (multiplication etc)
*(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__) || defined (__AVR_ATmega1284P__) || defined(__AVR_ATmega2561__)
*(s--) = RAMPZ; // best guess 0x3b
// obsolete JDN *(s--) = EIND; // best guess
#endif
#if defined (__AVR_ATmega2560__) || defined (__AVR_ATmega1280__) || defined(__AVR_ATmega2561__)
*(s--) = EIND; // best guess 0x3c
#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 DEPENDENT PART - ENDE
pT->prio = prio; // maxv for holding org prio for inheritance
pT->maxv = (int) prio;
prio_enQ (pAQ, pT); // and put task in active Q
return (pT);
badexit:
k_err_cnt++;
return (NULL);
}
ISR (KRNLTMRVECTOR, ISR_NAKED) // naked so we have to supply with prolog and epilog (push pop stack of regs)
{
PUSHREGS (); // no local vars ! I think
// JDN NASTY FOR TIMING ANALYSIS ONLY
// PORTB |=0x10;
wdt_reset ();
#if (KRNLTMR == 0)
// we have overtaken the millis timer so we do it by hand
timer0_millis += MILLIS_INC;
timer0_fractt += FRACT_INC;
if (timer0_fractt >= FRACT_MAX) {
timer0_fractt -= FRACT_MAX;
timer0_millis += 1;
}
timer0_overflow_count++;
#else
TCNTx = tcntValue; // Reload the timer
#endif
if (!k_running) {
goto exitt;
}
if (1 < k_tick_size) {
fakecnt--;
if (fakecnt <= 0) {
fakecnt = k_tick_size;
} else {
goto exitt; // no service
}
}
k_millis_counter += k_tick_size; // my own millis counter
// the following may look crazy: to go through all semaphores and tasks
// but you may have 3-4 tasks and 3-6 semaphores in your code
// so - seems to be efficient :-)
// so - it's a good idea not to init krnl with more items
// (tasks/Sem/msg descriptors than needed)
pE = sem_pool; // Semaphore timer - check timers on semaphores
for (tmr_indx = 0; tmr_indx < nr_sem; tmr_indx++) {
if (0 < pE->cnt2) // timer on semaphore ?
{
pE->cnt2--; // yep decrement it
if (pE->cnt2 <= 0) // timeout ?
{
pE->cnt2 = pE->cnt3; // preset again - if cnt3 == 0 and >= 0 the rep timer
ki_signal (pE); //issue a signal to the semaphore
}
}
pE++;
}
pE = task_pool; // Chk timers on tasks - they may be one shoot waiting
for (tmr_indx = 0; tmr_indx < nr_task; tmr_indx++) {
if (0 < pE->cnt2) // timer active on task ?
{
pE->cnt2--; // yep so let us do one down count
if (pE->cnt2 <= 0) // timeout ? ( == 0 )
{
((struct k_t *) (pE->cnt3))->cnt1++; // leaving sem so adjust semcount on sem
prio_enQ (pAQ, deQ (pE)); // and rip task of semQ and insert in activeQ
pE->cnt2 = -1; // indicate timeout in this semQ
}
}
pE++;
}
prio_enQ (pAQ, deQ (pRun));
K_CHG_STAK ();
exitt:
//JDN NASTY FOR MEA ONLY onn UNO pin8
//PORTB &=0xef;
POPREGS ();
RETI ();
}
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 ((prio <= 0 ) || (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
// HW_DEP_START
// inspiration from http://dev.bertos.org/doxygen/frame_8h_source.html
// and http://www.control.aau.dk/~jdn/kernels/krnl/
// now we are going to precook stak
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;
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 the 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; // maxv for holding org prio for inheritance
pT->maxv = (int) 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);
}
ISR (KRNLTMRVECTOR, ISR_NAKED)
{
// no local vars ! I think
PUSHREGS ();
TCNTx = tcntValue; // Reload the timer
if (!k_running) { // obvious
goto exitt;
}
fakecnt--; // for very slow k_start values
//bq timer cant run so slow (8 bit timers at least)
if (0 < fakecnt) { // how often shall we run KeRNeL timer code ?
goto exitt;
}
fakecnt = fakecnt_preset; // now it's time for doing RT stuff
k_millis_counter += k_tick_size; // my own millis counter
// the following may look crazy: to go through all semaphores and tasks
// but you may have 3-4 tasks and 3-6 semaphores in your code
// so - seesm to be efficient :-)
// so - it's a good idea not to init krnl with more items
// (tasks/Sem/msg descriptors than needed)
pE = sem_pool; // Semaphore timer - check timers on semaphores - they may be cyclic
for (tmr_indx = 0; tmr_indx < nr_sem; tmr_indx++) {
if (0 < pE->cnt2) { // timer on semaphore ?
pE->cnt2--; // yep decrement it
if (pE->cnt2 <= 0) { // timeout ?
pE->cnt2 = pE->cnt3; // preset again - if cnt3 == 0 and >= 0 the rep timer
ki_signal (pE); //issue a signal to the semaphore
}
}
pE++;
}
pE = task_pool; // Chk timers on tasks - they may be one shoot waiting
for (tmr_indx = 0; tmr_indx < nr_task; tmr_indx++) {
if (0 < pE->cnt2) { // timer active on task ?
pE->cnt2--; // yep so let us do one down count
if (pE->cnt2 <= 0) { // timeout ? ( == 0 )
ki_signal ((struct k_t *) (pE->cnt3));
pE->cnt2 = -1; // indicate timeout in this semQ
}
}
pE++;
}
if (krnl_preempt_flag) {
prio_enQ (pAQ, deQ (pRun)); // round robbin
K_CHG_STAK ();
}
exitt:
POPREGS ();
RETI ();
}
