forked from haraldwolff/avr-fw-modules
71 lines
1.2 KiB
C
71 lines
1.2 KiB
C
#include <avr/io.h>
|
|
#include <avr/eeprom.h>
|
|
|
|
#include <sys/assert.h>
|
|
#include <sys/errno.h>
|
|
#include <sys/threads.h>
|
|
|
|
#include <hwo/utils.h>
|
|
|
|
#include <stdlib.h>
|
|
|
|
int thread_reset(THREAD *t){
|
|
uint8_t* pstack;
|
|
uint8_t i;
|
|
uint16_t sadr = ((uint16_t)thread_starter); // ret from ctxswitch to start
|
|
|
|
pstack = &t->stack.base[ t->stack.size - 1];
|
|
*(pstack--) = LOW(sadr);
|
|
*(pstack--) = HIGH(sadr);
|
|
|
|
// fill registers with 0
|
|
for (i=0;i<32;i++)
|
|
*(pstack--) = 0;
|
|
|
|
*(pstack--) = 0x80; // initial SREG
|
|
|
|
t->stack.stackpointer = pstack;
|
|
|
|
schedule_thread( t );
|
|
|
|
return ESUCCESS;
|
|
}
|
|
|
|
|
|
THREAD* thread_alloc(threadstart start,void* arg,uint16_t stacksize)
|
|
{
|
|
THREAD* t = malloc(sizeof(THREAD));
|
|
if (t)
|
|
{
|
|
memset(t, 0x00, sizeof( THREAD ) );
|
|
|
|
t->stack.base = malloc( stacksize );
|
|
if (t->stack.base == NULL)
|
|
{
|
|
free(t);
|
|
return NULL;
|
|
};
|
|
|
|
t->stack.size = stacksize;
|
|
t->stack.min_free = stacksize;
|
|
t->start = start;
|
|
t->arg = arg;
|
|
|
|
t->priority = TP_NORMAL;
|
|
|
|
list_init( &(t->list) );
|
|
list_init( &(t->list_queue) );
|
|
list_init( &(t->list_periodic) );
|
|
|
|
list_append( &(t->list), &(_threading_threads) );
|
|
|
|
if (noassert(thread_reset(t))){
|
|
free(t->stack.base);
|
|
free(t);
|
|
return NULL;
|
|
};
|
|
};
|
|
return t;
|
|
};
|
|
|