127 lines
2.4 KiB
C
127 lines
2.4 KiB
C
#include <sysinit.h>
|
|
|
|
#include "basic/basic.h"
|
|
#include "lcd/print.h"
|
|
|
|
QUEUE the_queue;
|
|
#ifdef __arm__
|
|
volatile uint32_t _timectr=0;
|
|
#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;
|
|
};
|
|
};
|
|
/* 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){
|
|
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;
|
|
};
|
|
|
|
void delayms_queue(uint32_t ms){
|
|
int end=_timectr+ms/SYSTICKSPEED;
|
|
do {
|
|
if (the_queue.qstart == the_queue.qend){
|
|
WFI;
|
|
}else{
|
|
work_queue();
|
|
};
|
|
} while (end >_timectr);
|
|
};
|
|
|
|
void delayms_power(uint32_t ms){
|
|
ms/=SYSTICKSPEED;
|
|
ms+=_timectr;
|
|
do {
|
|
WFI;
|
|
} while (ms >_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].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;
|
|
};
|