#include "includes.h"

/*
********************************************************************************
*                     INITIALIZE A TASK'S STACK
*
* Description:
*	This function is called by either OSTaskCreate() or OSTaskCreateExt() to
*	initialize the stack frame of the task being created. This function is
*	highly processor specific.
*
* Arguments:
*	task - 	is a pointer to the task code
*
*	pdata -	is a pointer to a user supplied data area that will be passed to the
*			 task when the task first executes.
*
*	ptos - 	is a pointer to the top of stack. It is assumed that 'ptos' points to
*			the highest valid address on the stack.
*
*	opt - 	specifies options that can be used to alter the behavior of 
			OSTaskStkInit(). (see uCOS_II.H for OS_TASK_OPT_???).
*
* Returns:
*	Always returns the location of the new top-of-stack' once the processor 
*	registers have been placed on the stack in the proper order.
*
* Note(s):
*	Interrupts are enabled when your task starts executing. You can change this 
* 	by setting the SREG to 0x00 instead. In this case, interrupts would be 
*	disabled upon task startup. The application code would be responsible for 
*	enabling interrupts at the beginning of the task code. You will need to 
*	modify OSTaskIdle() and OSTaskStat() so that they enable interrupts. Failure
*	to do this will make your system crash! The AVR return stack is placed 64 
*	bytes above the start of the (512 byte) stack. This provides 32 levels of 
*	function call nesting which should be more than enough for most applications
*	(see the "read.me" file for more info).
*
********************************************************************************
*/
OS_STK *OSTaskStkInit (void (*task)(void *pd),
						 void *pdata, OS_STK *ptos, INT16U stk_size){
    INT8U  *stk;
    INT8U  *stks;		// Temp. variable used for setting up AVR hardware stack
    INT16U tmp;


    stk     = (INT8U *)ptos;
	// AVR return stack ("hardware stack")
    //from IAR C
    //stks    = (INT8U *)(ptos)-(OS_TASK_DEF_STK_SIZE-64);/* AVR return stack ("hardware stack")          */
    //from ICC AVR C
    //stks    = (INT8U *)(ptos)-(OS_TASK_DEF_STK_SIZE-32);
    stks    = (INT8U *)(ptos)-(stk_size - OS_HARDWARE_STACK_SIZE);
#if OS_STRICT
    if((0==task)||(ptos==0)||((INT16U)ptos<stk_size)||(stk_size<OS_HARDWARE_STACK_SIZE+40))
	  OSError(0);
#endif	
	// the function address has an extra level of indirection. Fetch the
    // actual address.
    //    
    tmp = *(INT16U const *)task;
   
	// 36 Bytes
    *stks-- = (INT8U)tmp;	//Put task start address on top of hardware stack
    *stks-- = (INT8U)(tmp >> 8);

    *stk-- = (INT8U)0x00;		// R0  = 0x00
    *stk-- = (INT8U)0x01; 
    *stk-- = (INT8U)0x02;
    *stk-- = (INT8U)0x03;
    *stk-- = (INT8U)0x04;
    *stk-- = (INT8U)0x05;
    *stk-- = (INT8U)0x06;
    *stk-- = (INT8U)0x07;
    *stk-- = (INT8U)0x08;
    *stk-- = (INT8U)0x09;
    *stk-- = (INT8U)0x10;
    *stk-- = (INT8U)0x11;
    *stk-- = (INT8U)0x12;
    *stk-- = (INT8U)0x13;
    *stk-- = (INT8U)0x14;
    *stk-- = (INT8U)0x15;
    tmp    = (INT16U)pdata;
    *stk-- = (INT8U)tmp;	//Simulate call to function with argument
    *stk-- = (INT8U)(tmp >> 8);
    *stk-- = (INT8U)0x18;
    *stk-- = (INT8U)0x19;
    *stk-- = (INT8U)0x20;
    *stk-- = (INT8U)0x21;
    *stk-- = (INT8U)0x22;
    *stk-- = (INT8U)0x23;
    *stk-- = (INT8U)0x24;
    *stk-- = (INT8U)0x25;
    *stk-- = (INT8U)0x26;
    *stk-- = (INT8U)0x27;
	// the heap pointer Y=R29:R28 will not be stored
    *stk-- = (INT8U)0x30;
    *stk-- = (INT8U)0x31;
    *stk-- = (INT8U)0x3B;
    *stk-- = (INT8U)0x80;	//SREG = Interrupts enabled

    tmp    = (INT16U)(stks);
    *stk-- = (INT8U)(tmp >> 8);
    *stk   = (INT8U)(tmp);
    return ((void *)stk);
}