crashtest-r0ket/firmware/basic/idle.c

127 lines
2.4 KiB
C
Raw Normal View History

#include <sysinit.h>
#include "basic/basic.h"
#include "lcd/print.h"
QUEUE the_queue;
2011-07-26 22:11:30 +00:00
#ifdef __arm__
volatile uint32_t _timectr=0;
2011-07-25 06:50:33 +00:00
#else
extern uint32_t simTimeCounter();
#define _timectr (simTimeCounter())
#endif
/**************************************************************************/
uint8_t work_queue_minimal(void){
int start;
if (the_queue.qstart == the_queue.qend){
return 0;
};
start=the_queue.qstart;
start=(start+1)%MAXQENTRIES;
if(the_queue.queue[start].type == QT_NORMAL){
void (*elem)(void);
elem=the_queue.queue[start].u.callback;
the_queue.qstart=start;
elem();
return 0;
}else if(the_queue.queue[start].type == QT_PLUS){
uint8_t (*elem)(uint8_t);
uint8_t state=the_queue.queue[start].state;
elem=the_queue.queue[start].u.callbackplus;
state=elem(state);
if(state==QS_END){
the_queue.qstart=start;
return 0;
}else{
the_queue.queue[start].state=state;
return 1;
};
};
2011-08-04 23:33:48 +00:00
/* NOTREACHED */
return 0;
};
void work_queue(void){
if (the_queue.qstart == the_queue.qend){
WFI;
return;
};
while(work_queue_minimal());
};
uint8_t delayms_queue_plus(uint32_t ms, uint8_t final){
2011-08-04 23:33:48 +00:00
int ret=0;
int end=_timectr+ms/SYSTICKSPEED;
do {
if (the_queue.qstart == the_queue.qend){
WFI;
}else{
ret=work_queue_minimal();
};
} while (end >_timectr);
if(ret && final){
while(work_queue_minimal());
};
return ret;
};
2011-07-24 14:03:19 +00:00
void delayms_queue(uint32_t ms){
2011-07-26 22:25:24 +00:00
int end=_timectr+ms/SYSTICKSPEED;
do {
if (the_queue.qstart == the_queue.qend){
WFI;
}else{
work_queue();
};
} while (end >_timectr);
};
2011-07-24 14:03:19 +00:00
void delayms_power(uint32_t ms){
2011-07-26 22:25:24 +00:00
ms/=SYSTICKSPEED;
ms+=_timectr;
2011-07-24 14:03:19 +00:00
do {
WFI;
} while (ms >_timectr);
2011-07-24 14:03:19 +00:00
};
int push_queue(void (*new)(void)){
int end;
end=the_queue.qend;
end=(end+1)%MAXQENTRIES;
if(end == the_queue.qstart) // Queue full
return -1;
the_queue.queue[end].u.callback=new;
the_queue.queue[end].type=QT_NORMAL;
the_queue.qend=end;
return 0;
};
int push_queue_plus(uint8_t (*new)(uint8_t)){
int end;
end=the_queue.qend;
end=(end+1)%MAXQENTRIES;
if(end == the_queue.qstart) // Queue full
return -1;
the_queue.queue[end].u.callbackplus=new;
the_queue.queue[end].type=QT_PLUS;
the_queue.queue[end].state=QS_START;
the_queue.qend=end;
return 0;
};