199 lines
3.9 KiB
C
Executable File
199 lines
3.9 KiB
C
Executable File
#include <sys/threads.h>
|
|
#include <sys/systick.h>
|
|
#include <sys/atomic.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include <avr/sleep.h>
|
|
|
|
int __main_stack_size__ = 0x80;
|
|
extern int __main_stack_size __attribute__((weak, alias("__main_stack_size__")));
|
|
|
|
|
|
extern uint8_t *pSystickSP;
|
|
|
|
/* _threading_threads: Global Thread List
|
|
*
|
|
* listhead: thread->list
|
|
*
|
|
* */
|
|
list_t _threading_threads;
|
|
/* _threading_queues: Thread Queues by Priority (all threads are running)
|
|
*
|
|
* listhead: thread->list_queue
|
|
*
|
|
* */
|
|
list_t _threading_queues[4];
|
|
|
|
/* _threading_periodics: List of all periodically scheduled Threads
|
|
*
|
|
* listhead: thread->list_periodic
|
|
*
|
|
* */
|
|
list_t _threading_periodics;
|
|
|
|
/* _threading_wait: List of all sleeping Threads
|
|
*
|
|
* listhead: thread->list_queue
|
|
*
|
|
* */
|
|
list_t _threading_waiting;
|
|
|
|
|
|
/* _threading_current: Currently active/scheduled Thread */
|
|
THREAD* _threading_current;
|
|
|
|
/* Predefined System Threads... */
|
|
THREAD* _thread_idle;
|
|
THREAD* _thread_main;
|
|
|
|
void main(void *arg);
|
|
void idle(void*);
|
|
|
|
void __ctx_init(void) __attribute__((naked)) __attribute__((section (".init7")));
|
|
void __ctx_init(void)
|
|
{
|
|
uint8_t n;
|
|
|
|
list_init( &_threading_threads );
|
|
list_init( &_threading_waiting );
|
|
list_init( &_threading_periodics );
|
|
|
|
for (n=0;n<4;n++) {
|
|
list_init( &_threading_queues[n] );
|
|
};
|
|
|
|
_thread_main = thread_alloc( main, NULL, __main_stack_size );
|
|
_thread_idle = thread_alloc( idle, NULL, 60 );
|
|
thread_set_priority( _thread_idle, TP_IDLE );
|
|
|
|
};
|
|
|
|
uint8_t* scheduler(uint8_t* oldstack)
|
|
{
|
|
uint8_t n;
|
|
int f;
|
|
|
|
list_t *next = NULL;
|
|
THREAD *next_thread = NULL;
|
|
|
|
if (_threading_current){
|
|
_threading_current->stack.stackpointer = oldstack;
|
|
|
|
f = (int)oldstack - (int)_threading_current->stack.base;
|
|
if ( f < _threading_current->stack.min_free)
|
|
_threading_current->stack.min_free = f;
|
|
|
|
if (_threading_current->remove) {
|
|
unschedule_thread( _threading_current );
|
|
};
|
|
|
|
if (list_next(&_threading_queues[ _threading_current->priority ]) == &(_threading_current->list_queue)){
|
|
list_remove( &(_threading_current->list_queue) );
|
|
list_append( &(_threading_current->list_queue), &_threading_queues[ _threading_current->priority ] );
|
|
};
|
|
};
|
|
|
|
for (n=0;n<4;n++) {
|
|
if (!list_is_empty(&_threading_queues[n])) {
|
|
next = _threading_queues[n].next;
|
|
break;
|
|
};
|
|
};
|
|
|
|
if (next){
|
|
next_thread = list_entry(next,thread_t,list_queue);
|
|
} else {
|
|
next_thread = _thread_idle;
|
|
};
|
|
|
|
next_thread->statistic.scheduled++;
|
|
|
|
_threading_current = next_thread;
|
|
|
|
return next_thread->stack.stackpointer;
|
|
};
|
|
|
|
|
|
void idle(void*arg)
|
|
{
|
|
while (1) {
|
|
/* cli();
|
|
set_sleep_mode(0);
|
|
sleep_enable();
|
|
sei();
|
|
sleep_cpu();
|
|
sleep_disable();
|
|
*/
|
|
};
|
|
};
|
|
|
|
void schedule_thread(THREAD *thread){
|
|
ATOMIC
|
|
|
|
if (thread){
|
|
if (!list_is_empty(&thread->list_queue)) {
|
|
unschedule_thread( thread );
|
|
};
|
|
|
|
if (thread->periodic){
|
|
list_append( &thread->list_periodic, &_threading_periodics );
|
|
};
|
|
|
|
if (thread->wait){
|
|
list_append( &thread->list_queue, &_threading_waiting );
|
|
} else {
|
|
list_append( &thread->list_queue, &_threading_queues[ thread->priority ]);
|
|
};
|
|
|
|
};
|
|
};
|
|
|
|
void unschedule_thread(THREAD *thread){
|
|
ATOMIC
|
|
|
|
if (thread) {
|
|
list_remove( &thread->list_queue );
|
|
list_remove( &thread->list_periodic );
|
|
};
|
|
};
|
|
|
|
/* st_schedule(): Aktualisiert die periodische Threadplanung */
|
|
void st_schedule(void){
|
|
list_t *lh,*tmp;
|
|
THREAD *t;
|
|
|
|
for_each_list_entry_save(lh,tmp,&_threading_waiting) {
|
|
t = list_entry(lh,thread_t,list_queue);
|
|
|
|
if (t->timeout && (t->timeout <= _systick_ticks))
|
|
{
|
|
t->wait = 0;
|
|
t->timeout = 0;
|
|
|
|
schedule_thread( t );
|
|
} else {
|
|
};
|
|
};
|
|
|
|
for_each_list_entry(lh,&_threading_periodics) {
|
|
t = list_entry(lh,thread_t,list_periodic);
|
|
|
|
t->periodic -= _systick_us;
|
|
if (t->periodic <= 0) {
|
|
t->periodic += t->periode;
|
|
if (!t->wait) {
|
|
t->periodic_miss = 1;
|
|
t->statistic.periodic_misses++;
|
|
} else {
|
|
t->wait = 0;
|
|
lh = lh->next;
|
|
|
|
schedule_thread( t );
|
|
}
|
|
};
|
|
};
|
|
|
|
};
|
|
|