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 ?
if (pB->lost_msg < 10000)
pB->lost_msg++;
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);
}
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
k_signal (struct k_t *sem)
{
int res;
DI ();
res = ki_signal (sem);
if (res == 0)
ki_task_shift ();
EI ();
return (res);
}
int
k_signal (struct k_t *sem)
{
int res;
DI ();
res = ki_signal (sem); // 1: ok no task to AQ, 0: ok task to AQ
if (res == 0) {
ki_task_shift (); // bq maybe started task has higher prio than me
}
EI ();
return (res);
}
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
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);
}
char
ki_send (struct k_msg_t *pB, void *el)
{
int i;
char *pSrc, *pDst;
if (pB->nr_el <= pB->cnt) {
// nope - no room for a putting new msg in Q ?
if (pB->lost_msg < SEM_MAX_VALUE) {
pB->lost_msg++;
}
#ifdef KRNLBUG
k_send_Q_clip (pB->nr, pB->lost_msg);
#endif
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++);
}
return (ki_signal (pB->sem)); // indicate a new msg is in Q
}
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);
}
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 ();
}
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 ();
}
