#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;
};