/*
*********************************************************************************************************
*                                                 uC/OS
*                                          The Real-Time Kernel
*
*                                              CREATE TASK
*
*                                             Rene Voorberg
*                                             80c52 (Large Model)
*				   Using the COMPASS51 compiler from PLC
*
* File : OS51.C
*********************************************************************************************************
*/
#include "ucos51.h"
//introduce the framepointer to the code
extern near UWORD * _fp;

/*
*********************************************************************************************************
*                                           CREATE A TASK
*
*********************************************************************************************************
*/

UBYTE OSTaskCreate(void (*task)(far void *pd), far void *pdata, far UBYTE *pstk, UBYTE p)
{
	UBYTE *stk;
	UBYTE  err;
	UBYTE *temp;
	UBYTE *temp_sp;
	OS_ENTER_CRITICAL();
	if (OSTCBPrioTbl[p] == (far OS_TCB *)0) {         /* Task must not already exist at this priority  */
	OS_EXIT_CRITICAL();
	//remember the stack grows upward 
	*++pstk   = (UBYTE)((UWORD)pdata >>8 );	/* Push data to xstack       	*/
	temp      = (far UBYTE *)pstk;          /* Save frame pointer           */
	*++pstk   = (UBYTE)(pdata);		/* Push data on xstack       	*/
	/* When pushing the task pointer we need to push LSB first then MSB	*/
	*++pstk   = (UBYTE)(task);		/* Push pointer to task         */
	temp_sp   = (far UBYTE *)pstk;          /* Save stack pointer		*/
	*++pstk   = (UBYTE)((UWORD)task >>8 );	/* Push pointer to task         */
	*++pstk   = 0x00;			/* Push PSW */
	*++pstk   = 0x21;			/* Push A */
	*++pstk   = 0x22;			/* Push B */
	*++pstk   = 0x01;			/* Make R0 00 */
	*++pstk   = 0x02;			/* Make R1 00 */
	*++pstk   = 0x03;			/* Make R2 00 */
	*++pstk   = 0x04;			/* Make R3 00 */
	*++pstk   = 0x05;			/* Make R4 00 */
	*++pstk   = 0x06;			/* Make R5 00 */
	*++pstk   = 0x07;			/* Make R6 00 */
	*++pstk   = 0x08;			/* Make R7 00 */
	*++pstk   = 0x00;			/* Space for dph */
	*++pstk   = 0x00;			/* Space for dpl */
	*++pstk   = (UBYTE)((UWORD)temp>>8);	/* Space for __fp+1 */
	*++pstk   = (UBYTE)(temp);		/* Space for __fp */
	*++pstk   = (UBYTE)((UWORD)temp_sp>>8 );/* Space for __sp+1 */
	*++pstk   = (UBYTE)(temp_sp);		/* Space for __sp */
	*++pstk   = 0x13;			/* Nr of bytes originally on stack that need */
						/* to be copied to internal stack */
	++pstk;					/* far stackpointer points to next free location */
	stk      = (far UBYTE *)pstk;           /* Load stack pointer                            */
	pstk     = (far UBYTE *)temp;           /* Load stack pointer                            */
	err      = OSTCBInit(p, (far void *)stk);      /* Get and initialize a TCB                      */
	if (err == OS_NO_ERR) {
	    if (OSRunning) {                          /* Find highest priority task if tasking         */
	        OSSched();
	    }
	}
	return (err);
	} else {
	OS_EXIT_CRITICAL();
	return (OS_PRIO_EXIST);
	}
	}