crashtest-r0ket/firmware/basic/idle.c

78 lines
1.2 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
2011-07-26 22:11:30 +00:00
#error "foo"
2011-07-25 06:50:33 +00:00
extern uint32_t simTimeCounter();
#define _timectr (simTimeCounter())
#endif
/**************************************************************************/
void work_queue(void){
void (*elem)(void);
int start;
if (the_queue.qstart == the_queue.qend){
2011-07-26 22:11:30 +00:00
#ifdef __arm__
__asm volatile ("WFI");
2011-07-26 22:11:30 +00:00
#else
delayms(10);
2011-07-24 20:14:06 +00:00
#endif
return;
};
start=the_queue.qstart;
start=(start+1)%MAXQENTRIES;
elem=the_queue.queue[start].callback;
the_queue.qstart=start;
elem();
};
2011-07-24 14:03:19 +00:00
void delayms_queue(uint32_t ms){
int end=_timectr+ms/10;
do {
if (the_queue.qstart == the_queue.qend){
2011-07-26 22:11:30 +00:00
#ifdef __arm__
__asm volatile ("WFI");
2011-07-26 22:11:30 +00:00
#else
delayms(10);
2011-07-24 20:14:06 +00:00
#endif
}else{
work_queue();
};
} while (end >_timectr);
};
2011-07-24 14:03:19 +00:00
void delayms_power(uint32_t ms){
ms+=_timectr;
2011-07-24 14:03:19 +00:00
do {
2011-07-26 22:11:30 +00:00
#ifdef __arm__
__asm volatile ("WFI");
2011-07-26 22:11:30 +00:00
#else
delayms(10);
2011-07-24 20:14:06 +00:00
#endif
} 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].callback=new;
the_queue.qend=end;
return 0;
};