crashtest-r0ket/firmware/basic/idle.c

53 lines
888 B
C
Raw Normal View History

#include <sysinit.h>
#include "basic/basic.h"
#include "lcd/print.h"
QUEUE the_queue;
volatile uint32_t _timectr=0;
/**************************************************************************/
void work_queue(void){
void (*elem)(void);
int start;
if (the_queue.qstart == the_queue.qend){
__asm volatile ("WFI");
return;
};
start=the_queue.qstart;
start=(start+1)%MAXQENTRIES;
elem=the_queue.queue[start].callback;
the_queue.qstart=start;
elem();
};
void idle_queue(uint32_t ms){
int end=_timectr+ms/10;
do {
if (the_queue.qstart == the_queue.qend){
__asm volatile ("WFI");
}else{
work_queue();
};
} while (end >_timectr);
};
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;
};