/*** BeginHeader  */
#ifndef __MCOS_TASK_C
#define __MCOS_TASK_C

// uC/OS-2 version: 1.05

/*** EndHeader */
/*
*********************************************************************************************************
*                                                uC/OS-II
*                                          The Real-Time Kernel
*                                            TASK MANAGEMENT
*
*                          (c) Copyright 1992-2001, Jean J. Labrosse, Weston, FL
*                                           All Rights Reserved
*
* File : OS_TASK.C
* By   : Jean J. Labrosse
*********************************************************************************************************
*/

/*** BeginHeader OSTaskChangePrio, OSTaskCreate, OSStkLogAddr, OSTaskCreateExt, OSTaskDel, OSTaskDelReq,
                 OSTaskResume, OSTaskStkChk, OSTaskSuspend, OSTaskQuery */
#if OS_TASK_CHANGE_PRIO_EN > 0
INT8U  OSTaskChangePrio (INT8U oldprio, INT8U newprio);
#endif

#if OS_TASK_CREATE_EN > 0
INT8U  OSTaskCreate (void (*task)(), void *pdata, INT16U stk_size, INT8U prio);
#endif

#if OS_TASK_CREATE_EXT_EN > 0
INT8U  OSTaskCreateExt (void (*task)(), void *pdata, INT8U prio, INT16U id, INT16U stk_size, void *pext, INT16U opt);
#endif

#if OS_TASK_DEL_EN > 0
INT8U  OSTaskDel (INT8U prio);
INT8U  OSTaskDelReq (INT8U prio);
#endif

#if OS_TASK_SUSPEND_EN > 0
INT8U  OSTaskResume (INT8U prio);
INT8U  OSTaskSuspend (INT8U prio);
#endif

#if OS_TASK_CREATE_EXT_EN > 0
INT8U  OSTaskStkChk (INT8U prio, OS_STK_DATA *pdata);
#endif

#if OS_TASK_QUERY_EN > 0
INT8U  OSTaskQuery (INT8U prio, OS_TCB *pdata);
#endif

/*** EndHeader */

#if OS_TASK_CREATE_EN > 0
INT8U _OSTaskCreate (void (*task)(), void *pdata, INT32U LogAddr, INT8U prio);
#endif

#if OS_TASK_CREATE_EXT_EN > 0
INT8U _OSTaskCreateExt(void (*task)(), void *pdata, INT8U prio, INT16U id, INT32U LogAddr, void *pext, INT16U opt);
#endif

INT16U OSStkLogAddr(INT32U LogAddr, INT16U PhysAddr);
INT32U OSStkPhysAddr(INT32U LogAddr);

/*
*********************************************************************************************************
*                                        CHANGE PRIORITY OF A TASK
*********************************************************************************************************
*/

/* START FUNCTION DESCRIPTION ********************************************
OSTaskChangePrio                                            <UCOS.LIB>

SYNTAX:    		INT8U OSTaskChangePrio (INT8U oldprio, INT8U newprio);

DESCRIPTION:	This function allows a task's priority to be changed
					dynamically.  Note that the new priority MUST be available.

PARAMETER1:    The old priority

PARAMETER2:    The new priority

RETURN VALUE:  OS_NO_ERR        if the call was successful
               OS_PRIO_INVALID  if the priority specified is higher that
               					  the maximum allowed
                                (i.e. >= OS_LOWEST_PRIO)
               OS_PRIO_EXIST    if the new priority already exist.
               OS_PRIO_ERR      there is no task with the specified OLD
               					  priority (i.e. the OLD task does not
                                exist).

END DESCRIPTION **********************************************************/


#if OS_TASK_CHANGE_PRIO_EN > 0
nodebug
INT8U  OSTaskChangePrio (INT8U oldprio, INT8U newprio)
{
#if OS_CRITICAL_METHOD == 3                      /* Allocate storage for CPU status register           */
    OS_CPU_SR    cpu_sr;
#endif

#if OS_EVENT_EN > 0
    auto  OS_EVENT    *pevent;
#endif

    auto OS_TCB      *ptcb;
    auto INT8U        x;
    auto INT8U        y;
    auto INT8U        bitx;
    auto INT8U        bity;


#if OS_ARG_CHK_EN > 0
    if ((oldprio >= OS_LOWEST_PRIO && oldprio != OS_PRIO_SELF)  ||
         newprio >= OS_LOWEST_PRIO) {
        return (OS_PRIO_INVALID);
    }
#endif
    OS_ENTER_CRITICAL();
    if (OSTCBPrioTbl[newprio] != (OS_TCB *)0) {                 /* New priority must not already exist */
        OS_EXIT_CRITICAL();
        return (OS_PRIO_EXIST);
    } else {
        OSTCBPrioTbl[newprio] = (OS_TCB *)1;                    /* Reserve the entry to prevent others */
        OS_EXIT_CRITICAL();
        y    = newprio >> 3;                                    /* Precompute to reduce INT. latency   */
        bity = OSMapTbl[y];
        x    = newprio & 0x07;
        bitx = OSMapTbl[x];
        OS_ENTER_CRITICAL();
        if (oldprio == OS_PRIO_SELF) {                          /* See if changing self                */
            oldprio = OSTCBCur->OSTCBPrio;                      /* Yes, get priority                   */
        }
        if ((ptcb = OSTCBPrioTbl[oldprio]) != (OS_TCB *)0) {    /* Task to change must exist           */
            OSTCBPrioTbl[oldprio] = (OS_TCB *)0;                /* Remove TCB from old priority        */
            if ((OSRdyTbl[ptcb->OSTCBY] & ptcb->OSTCBBitX) != 0x00) {  /* If task is ready make it not */
                if ((OSRdyTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0x00) {
                    OSRdyGrp &= ~ptcb->OSTCBBitY;
                }
                OSRdyGrp    |= bity;                            /* Make new priority ready to run      */
                OSRdyTbl[y] |= bitx;
#if OS_EVENT_EN > 0
            } else {
                if ((pevent = ptcb->OSTCBEventPtr) != (OS_EVENT *)0) { /* Remove from event wait list  */
                    if ((pevent->OSEventTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0) {
                        pevent->OSEventGrp &= ~ptcb->OSTCBBitY;
                    }
                    pevent->OSEventGrp    |= bity;              /* Add new priority to wait list       */
                    pevent->OSEventTbl[y] |= bitx;
                }
#endif
            }
            OSTCBPrioTbl[newprio] = ptcb;                       /* Place pointer to TCB @ new priority */
            ptcb->OSTCBPrio       = newprio;                    /* Set new task priority               */
            ptcb->OSTCBY          = y;
            ptcb->OSTCBX          = x;
            ptcb->OSTCBBitY       = bity;
            ptcb->OSTCBBitX       = bitx;
            OS_EXIT_CRITICAL();
            OS_Sched();                                         /* Run highest priority task ready     */
            return (OS_NO_ERR);
        } else {
            OSTCBPrioTbl[newprio] = (OS_TCB *)0;                /* Release the reserved prio.          */
            OS_EXIT_CRITICAL();
            return (OS_PRIO_ERR);                               /* Task to change didn't exist         */
        }
    }
}
#endif

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

/* START FUNCTION DESCRIPTION ********************************************
OSTaskCreate                                                <UCOS.LIB>

SYNTAX: 	char OSTaskCreate(void (*task)(),void *pdata,INT16U stk_size,
                           char priority);

DESCRIPTION:  This function is used to have uC/OS-II manage the execution
of a task. Tasks can either be created prior to the start of multitasking
or by a running task.  A task cannot be created by an ISR.

PARAMETER1: pointer to task function
PARAMETER2: pointer to an optional data area which can be used to
pass parameters to the task when the task first executes.  Where the task
is concerned it thinks it was invoked and passed the argument 'pdata' as
follows:

    void Task (void *pdata){for (;;) { Task code ;}  }

PARAMETER3:  size of task's stack in bytes.

PARAMETER4: the task's priority.  A unique priority ( 0-62 ) MUST be
assigned to each task and the lower the number, the higher the priority.
(63 is for idle task)

RETURN VALUE: OS_NO_ERR        if the function was successful.
              OS_PRIO_EXIT     if the task priority already exist
                               (each task MUST have a unique priority).
              OS_PRIO_INVALID  if the priority specified is higher than
              						 the maximum allowed (i.e. >= OS_LOWEST_PRIO)

END DESCRIPTION **********************************************************/

#if (OS_TASK_CREATE_EN > 0)
nodebug
INT8U OSTaskCreate (void (*task)(), void *pdata, INT16U stk_size, INT8U prio)
{
    auto INT32U LogAddr;

    OSSchedLock();
    LogAddr = _Alloc_Stack(stk_size);
    OSSchedUnlock();
    if(!LogAddr)
    {
        exception(-ERR_BADSTACKALLOC);
        exit(-ERR_BADSTACKALLOC);
    }
    return(_OSTaskCreate (task, pdata, LogAddr, prio));
}
#endif


#if (OS_TASK_CREATE_EN > 0)

/*
 * Modified by Tom Collins <tom@installco.com> so OSTaskDel will
 * work when OS_TASK_CREATE_EXT_EN is undefined and OSTaskCreate is used
 * instead of OSTaskCreateExt.
 */

nodebug
INT8U _OSTaskCreate (void (*task)(), void *pdata, INT32U LogAddr, INT8U prio)
{
    auto INT16U  psp;
    auto INT16U  stk_size;
    auto INT16U  pbos;
    auto INT8U   err;
    auto INT32U  PhysAddr;

    if (prio > OS_LOWEST_PRIO) {             // Make sure priority is within allowable range
        return (OS_PRIO_INVALID);
    }

    OS_ENTER_CRITICAL();
    if (OSTCBPrioTbl[prio] == (OS_TCB *)0) { // Make sure task doesn't already exist at this priority
        OSTCBPrioTbl[prio] = (OS_TCB *)1;    // Reserve the priority to prevent others from doing ...
                                             // ... the same thing until task is created.
        OS_EXIT_CRITICAL();

        // OSTaskStkInit requires the 20-bit physical address for the stack
        PhysAddr = OSStkPhysAddr(LogAddr);
        stk_size = _Get_Stack_Size(LogAddr);  // BIOS may have allocated a stack larger than requested
        psp = (INT16U)OSTaskStkInit(task, pdata, (INT16U)PhysAddr, (INT8U)(PhysAddr >> 16), 0);
        // OS_TCBInit requires the 16-bit logical address for the stack
        psp = OSStkLogAddr(LogAddr, psp);
        pbos = ((INT16U)LogAddr - stk_size) + 1;
        err = OS_TCBInit(prio, psp, pbos, (INT8U)(LogAddr >> 16), 0, stk_size, (void *)0, 0);

        if (err == OS_NO_ERR) {
            OS_ENTER_CRITICAL();
            OSTaskCtr++;                                   // Increment the #tasks counter
            OS_EXIT_CRITICAL();
            if (OSRunning) {                 // Find highest priority task if multitasking has started
                OS_Sched();
            }
        } else {
            OS_ENTER_CRITICAL();
            OSTCBPrioTbl[prio] = (OS_TCB *)0;// Make this priority available to others
            OS_EXIT_CRITICAL();
        }
        return (err);
    } else {
        OS_EXIT_CRITICAL();
        return (OS_PRIO_EXIST);
    }
}
#endif

/* START _FUNCTION DESCRIPTION ********************************************
OSStkPhysAddr                                               <UCOS.LIB>

SYNTAX: 	INT32U OSStkPhysAddr(INT32U PackedAddr);

DESCRIPTION: Given the 32-bit value returned from Alloc_Stack
(32..24 = ~seg, 23..16 = seg, 15..0 = logical offset), this function will
return the 20-bit physical address of a task’s stack.

PARAMETER1: 32-bit value returned from Alloc_Stack.

RETURN VALUE: 20-bit physical address of a task’s stack.

END DESCRIPTION **********************************************************/

nodebug INT32U OSStkPhysAddr(INT32U LogAddr)
{
   auto INT32U retval;

   // move segment value stored in LogAddr so that it
   // occupies bits 19..12
   retval = (LogAddr >> 4) & 0x000ff000;
   // Add in the logical offset, producing 20-bit physical address.
   retval += (INT16U)LogAddr;
   return retval;
}

/* START _FUNCTION DESCRIPTION ********************************************
OSStkLogAddr                                                <UCOS.LIB>

SYNTAX: 	INT16U OSStkLogAddr(INT32U LogAddr, INT16U PhysAddr);

DESCRIPTION: Given the 32-bit value returned from Alloc_Stack (which
contains the 16-bit logical offset) and the lower 16 bits of the physical
address returned from OSTaskStkInit, which returns (lower 16-bits of the
physical address - the number of bytes initially pushed onto the task’s
stack), returns the logical offset into the segment where the top of the
stack is located.  This assumes that fewer than 4096 bytes were initally
pushed onto the stack.

PARAMETER1: 32-bit value returned from Alloc_Stack.

PARAMETER2: Lower 16 bits of physical address returned from OSTaskStkInit.

RETURN VALUE: 16-bit logical offset into stack segment.

END DESCRIPTION **********************************************************/

nodebug INT16U OSStkLogAddr(INT32U LogAddr, INT16U PhysAddr)
{
    auto INT16U offset;
    // This offset assumes that fewer than 4096 bytes where initially
    // pushed onto the task's stack (a very safe assumption)
    offset = (INT16U)(0x0fff & LogAddr) - (0x0fff & PhysAddr);
    return (INT16U) LogAddr - offset;
}


/*
*********************************************************************************************************
*                                     CREATE A TASK (Extended Version)
*********************************************************************************************************
*/

/* START FUNCTION DESCRIPTION ********************************************
OSTaskCreateExt                                             <UCOS.LIB>

SYNTAX:        INT8U OSTaskCreateExt (void   (*task)(),
                                      void    *pdata,
                                      INT8U    prio,
                                      INT16U   id,
                                      INT16U   stk_size,
                                      void    *pext,
                                      INT16U   opt);

DESCRIPTION:	This function is used to have uC/OS-II manage the creation
					of a task.  Tasks can either be created prior to the start
					of multitasking or by a running task.  A task cannot be
					created by an ISR.  This function is similar to
               OSTaskCreate()	except that it allows additional information
               about a task to be specified.

PARAMETER1:    Pointer to the task's code

PARAMETER2:    Pointer to an optional data area which can be used to pass
               parameters to the task when the task first executes.  Where
               the task is concerned it thinks it was invoked and passed
               the argument 'pdata' as follows:

                           void Task (void *pdata)
                           {
                               for (;;) {
                                   Task code;
                               }
                           }

PARAMETER3:    The task's priority.  A unique priority MUST be assigned to
					each task and the lower the number, the higher the priority.

PARAMETER4:    The task's ID (0..65535)

PARAMETER5:    Size of the stack in number of elements.  If OS_STK is set
					to INT8U, 'stk_size' corresponds to the number of bytes
					available.  If OS_STK is set to INT16U, 'stk_size' contains
					the number of 16-bit entries available.  Finally, if OS_STK
					is set to INT32U, 'stk_size' contains the number of 32-bit
					entries available on the stack.

PARAMETER6:    Pointer to a user supplied memory location which is used as
					a TCB extension.  For example, this user memory can hold the
					contents of floating-point registers during a context
               switch, the time each task takes to execute, the number of
               times the task has been switched-in, etc.

PARAMETER7:    Contains additional information (or options) about the
					behavior of the task.  The LOWER 8-bits are reserved by
					uC/OS-II while the upper 8 bits can be application specific.

RETURN VALUE:  OS_NO_ERR        if the function was successful.
               OS_PRIO_EXIT     if the task priority already exist
                                (each task MUST have a unique priority).
               OS_PRIO_INVALID  if the priority specified is higher than
               					  the maximum allowed (i.e. > OS_LOWEST_PRIO)
END DESCRIPTION **********************************************************/

#if OS_TASK_CREATE_EXT_EN > 0
nodebug INT8U OSTaskCreateExt (void (*task)(), void *pdata, INT8U prio, INT16U id,
                               INT16U stk_size, void *pext, INT16U opt)
{
    auto INT32U LogAddr;

    OSSchedLock();
    LogAddr = _Alloc_Stack(stk_size);
    OSSchedUnlock();
    if(!LogAddr)
    {
        exception(-ERR_BADSTACKALLOC);
        exit(-ERR_BADSTACKALLOC);
    }
    return( _OSTaskCreateExt(task, pdata, prio, id, LogAddr, pext, opt));
}

nodebug
INT8U _OSTaskCreateExt (void   (*task)(),
                       void    *pdata,
                       INT8U    prio,
                       INT16U   id,
                       INT32U   LogAddr,
                       void    *pext,
                       INT16U   opt)
{
    auto INT16U   psp;
    auto INT8U    err;
    auto INT16U   i;
    auto INT16U   pbos;
    auto INT16U   stk_size;
    auto INT32U   PhysAddr;

    if (prio > OS_LOWEST_PRIO) {                    // Make sure priority is within allowable range
        return (OS_PRIO_INVALID);
    }
    OS_ENTER_CRITICAL();


    if (OSTCBPrioTbl[prio] == (OS_TCB *)0) {        // Make sure task doesn't already exist at this priority
        OSTCBPrioTbl[prio] = (OS_TCB *)1;           // Reserve the priority to prevent others from doing ...
                                                    // ... the same thing until task is created.
        OS_EXIT_CRITICAL();

        PhysAddr = OSStkPhysAddr(LogAddr);
        stk_size = _Get_Stack_Size(LogAddr);       // BIOS may have allocated a stack larger than requested
        if (opt & OS_TASK_OPT_STK_CHK) {          // See if stack checking has been enabled
            if (opt & OS_TASK_OPT_STK_CLR) {      // See if stack needs to be cleared
                                                  // Yes, fill the stack with zeros
                // PhysAddr is the last address of the stack (e.g., ...FF), so it's
                // necessary to add 1 to get to the starting address of the stack.
                xmemset(PhysAddr - stk_size + 1, 0, stk_size);
            }
        }

        // Initialize the task's stack
        psp = (INT16U)OSTaskStkInit(task, pdata, (INT16U)PhysAddr, (INT8U)(PhysAddr >> 16), opt);
        psp = OSStkLogAddr(LogAddr, psp);
        pbos = ((INT16U)LogAddr - stk_size) + 1;
        err = OS_TCBInit(prio, psp, pbos, (INT8U)(LogAddr >> 16), id, stk_size, pext, opt);

        if (err == OS_NO_ERR) {
            OS_ENTER_CRITICAL();
            OSTaskCtr++;                            // Increment the #tasks counter
            OS_EXIT_CRITICAL();
            if (OSRunning) {                        // Find highest priority task if multitasking has started
                OS_Sched();
            }
        } else {
            OS_ENTER_CRITICAL();
            OSTCBPrioTbl[prio] = (OS_TCB *)0;       // Make this priority available to others
            OS_EXIT_CRITICAL();
        }
        return (err);

    } else {
        OS_EXIT_CRITICAL();
        return (OS_PRIO_EXIST);
    }
}
#endif

/*
*********************************************************************************************************
*                                            DELETE A TASK
*********************************************************************************************************
*/

/* START FUNCTION DESCRIPTION ********************************************
OSTaskDel                                                   <UCOS.LIB>

SYNTAX:    		INT8U OSTaskDel (INT8U prio);

DESCRIPTION:	This function deletes a task.  The calling task can delete
               itself by its own priority number.  The deleted task is
               returned to the dormant state and can be re-activated by
               creating the deleted task again.

PARAMETER1:    The priority of the task to delete.  Note that you can
               explicitely delete the current task without knowing its
               priority level by setting 'prio' to OS_PRIO_SELF.

RETURN VALUE:  OS_NO_ERR           if the call was successful
               OS_TASK_DEL_IDLE    if attempting to delete uC/OS-II's idle
               						  task
               OS_PRIO_INVALID     if the priority specified is higher than
               						  the maximum allowed (i.e.
               						  >= OS_LOWEST_PRIO) or, OS_PRIO_SELF not
               						  specified.
               OS_TASK_DEL_ERR     if the task to delete does not exist
               OS_TASK_DEL_ISR     if attempting to delete a task from an
                                   ISR

Notes:        1) To reduce interrupt latency, OSTaskDel() 'disables' the
					  task:
                    a) by making it not ready
                    b) by removing it from any wait lists
                    c) by preventing OSTimeTick() from making the task
                       ready to run.
                 The task can then be 'unlinked' from the miscellaneous
                 structures in uC/OS-II.
              2) The function OSDummy() is called after OS_EXIT_CRITICAL()
              	  because, on most processors, the next instruction
                 following the enable interrupt instruction is ignored.
                 You can replace OSDummy() with a macro that basically
                 executes a NO OP (i.e. OS_NOP()).  The NO OP macro would
                 avoid the execution time of the function call and return.
              3) An ISR cannot delete a task.
              4) The lock nesting counter is incremented because, for a
              	  brief instant, if the current task is being deleted, the
              	  current task would not be able to be rescheduled because
                 it is removed from the ready list.  Incrementing the
                 nesting counter prevents another task from being schedule.
                 This means that the ISR would return to the current task
                 which is being deleted.  The rest of the deletion would
                 thus be able to be completed.

END DESCRIPTION **********************************************************/

#if OS_TASK_DEL_EN > 0
nodebug
INT8U  OSTaskDel (INT8U prio)
{
#if OS_CRITICAL_METHOD == 3                      /* Allocate storage for CPU status register           */
    auto OS_CPU_SR     cpu_sr;
#endif

#if OS_EVENT_EN > 0
    auto OS_EVENT     *pevent;
#endif
#if (OS_VERSION >= 251) && (OS_FLAG_EN > 0) && (OS_MAX_FLAGS > 0)
    auto OS_FLAG_NODE pnode;
#endif
    auto OS_TCB       *ptcb;
    auto int          dealloc_ret;

    if (bios_intnesting > 0) {                                  /* See if trying to delete from ISR    */
        return (OS_TASK_DEL_ISR);
    }
#if OS_ARG_CHK_EN > 0
    if (prio == OS_IDLE_PRIO) {                                 /* Not allowed to delete idle task     */
        return (OS_TASK_DEL_IDLE);
    }
    if (prio >= OS_LOWEST_PRIO && prio != OS_PRIO_SELF) {       /* Task priority valid ?               */
        return (OS_PRIO_INVALID);
    }
#endif
    OS_ENTER_CRITICAL();
    if (prio == OS_PRIO_SELF) {                                 /* See if requesting to delete self    */
        prio = OSTCBCur->OSTCBPrio;                             /* Set priority to delete to current   */
    }
    if ((ptcb = OSTCBPrioTbl[prio]) != (OS_TCB *)0) {                /* Task to delete must exist      */
        if ((OSRdyTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0x00) {  /* Make task not ready            */
            OSRdyGrp &= ~ptcb->OSTCBBitY;
        }
#if OS_EVENT_EN > 0
        pevent = ptcb->OSTCBEventPtr;
        if (pevent != (OS_EVENT *)0) {                          /* If task is waiting on event         */
            if ((pevent->OSEventTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0) { /* ... remove task from */
                pevent->OSEventGrp &= ~ptcb->OSTCBBitY;                        /* ... event ctrl block */
            }
        }
#endif
#if (OS_VERSION >= 251) && (OS_FLAG_EN > 0) && (OS_MAX_FLAGS > 0)
        if(ptcb->OSTCBFlagNode != 0) {                          /* If task is waiting on event flag    */
            xmem2root(&pnode, ptcb->OSTCBFlagNode, sizeof(pnode));
            OS_FlagUnlink(&pnode);                               /* Remove from wait list               */
        }
#endif
        ptcb->OSTCBDly  = 0;                                    /* Prevent OSTimeTick() from updating  */
        ptcb->OSTCBStat = OS_STAT_RDY;                          /* Prevent task from being resumed     */
		if (OSLockNesting < 255) {
            OSLockNesting++;
		}
        OS_EXIT_CRITICAL();                                     /* Enabling INT. ignores next instruc. */
        OS_Dummy();                                             /* ... Dummy ensures that INTs will be */
        OS_ENTER_CRITICAL();                                    /* ... disabled HERE!                  */
		if (OSLockNesting > 0) {
            OSLockNesting--;
		}
        OSTaskDelHook(ptcb);                                    /* Call user defined hook              */
        OSTaskCtr--;                                            /* One less task being managed         */
        OSTCBPrioTbl[prio] = (OS_TCB *)0;                       /* Clear old priority entry            */
        if (ptcb->OSTCBPrev == (OS_TCB *)0) {                   /* Remove from TCB chain               */
            ptcb->OSTCBNext->OSTCBPrev = (OS_TCB *)0;
            OSTCBList                  = ptcb->OSTCBNext;
        } else {
            ptcb->OSTCBPrev->OSTCBNext = ptcb->OSTCBNext;
            ptcb->OSTCBNext->OSTCBPrev = ptcb->OSTCBPrev;
        }
        ptcb->OSTCBNext = OSTCBFreeList;                        /* Return TCB to free TCB list         */
        OSTCBFreeList   = ptcb;
        dealloc_ret =
              _Dealloc_Stack((long)(((long)ptcb->OSTCBStkSeg << 16) |
                      (ptcb->OSTCBStkBottom + (ptcb->OSTCBStkSize - 1))));
        OS_EXIT_CRITICAL();
        if(!dealloc_ret)
        {
            // stack deallocation failed
            exception(ERR_BADSTACKDEALLOC);
            exit(ERR_BADSTACKDEALLOC);
        }
        OS_Sched();                                             /* Find new highest priority task      */
        return (OS_NO_ERR);
    }
    OS_EXIT_CRITICAL();
    return (OS_TASK_DEL_ERR);
}
#endif

/*
*********************************************************************************************************
*                                    REQUEST THAT A TASK DELETE ITSELF
*********************************************************************************************************
*/

/* START FUNCTION DESCRIPTION ********************************************
OSTaskDelReq                                                <UCOS.LIB>

SYNTAX:    		INT8U OSTaskDelReq (INT8U prio);

DESCRIPTION:	This function notifies a task to delete itself, and sees if
					a task requested that the current task delete itself.

PARAMETER1:    The priority of the task to request the delete from.

RETURN VALUE:  OS_NO_ERR          if the task exists and the request has
											 been registered
               OS_TASK_NOT_EXIST  if the task has been deleted.  This allows
               						 the caller to know whether the request has
               						 been executed.
               OS_TASK_DEL_IDLE   if requesting to delete uC/OS-II's idle
               						 task
               OS_PRIO_INVALID    if the priority specified is higher than
               						 the maximum allowed
                                  (i.e. >= OS_LOWEST_PRIO)
               						 or, OS_PRIO_SELF is not specified.
               OS_TASK_DEL_REQ    if a task (possibly another task)
                                  requested that the running task be
                                  deleted.

END DESCRIPTION **********************************************************/

#if OS_TASK_DEL_EN > 0
nodebug
INT8U  OSTaskDelReq (INT8U prio)
{
#if OS_CRITICAL_METHOD == 3                      /* Allocate storage for CPU status register           */
    auto OS_CPU_SR  cpu_sr;
#endif
    auto BOOLEAN    stat;
    auto INT8U      err;
    auto OS_TCB    *ptcb;


#if OS_ARG_CHK_EN > 0
    if (prio == OS_IDLE_PRIO) {                                 /* Not allowed to delete idle task     */
        return (OS_TASK_DEL_IDLE);
    }
    if (prio >= OS_LOWEST_PRIO && prio != OS_PRIO_SELF) {       /* Task priority valid ?               */
        return (OS_PRIO_INVALID);
    }
#endif
    if (prio == OS_PRIO_SELF) {                                 /* See if a task is requesting to ...  */
        OS_ENTER_CRITICAL();                                    /* ... this task to delete itself      */
        stat = OSTCBCur->OSTCBDelReq;                           /* Return request status to caller     */
        OS_EXIT_CRITICAL();
        return (stat);
    }
    OS_ENTER_CRITICAL();
    if ((ptcb = OSTCBPrioTbl[prio]) != (OS_TCB *)0) {           /* Task to delete must exist           */
        ptcb->OSTCBDelReq = OS_TASK_DEL_REQ;                    /* Set flag indicating task to be DEL. */
        err               = OS_NO_ERR;
    } else {
        err               = OS_TASK_NOT_EXIST;                  /* Task must be deleted                */
    }
    OS_EXIT_CRITICAL();
    return (err);
}
#endif

/*
*********************************************************************************************************
*                                        RESUME A SUSPENDED TASK
*********************************************************************************************************
*/

/* START FUNCTION DESCRIPTION ********************************************
OSTaskResume                                                <UCOS.LIB>

SYNTAX:    		INT8U OSTaskResume (INT8U prio);

DESCRIPTION:	This function is called to resume a previously suspended
               task. This is the only call that will remove an explicit
               task suspension.

PARAMETER1:    The priority of the task to resume.

RETURNS:       OS_NO_ERR                if the requested task is resumed
               OS_PRIO_INVALID          if the priority specified is higher
               								 than the maximum allowed
               								 (i.e. >= OS_LOWEST_PRIO)
               OS_TASK_RESUME_PRIO      if the task to resume does not
                                        exist
               OS_TASK_NOT_SUSPENDED    if the task to resume has not been
               								 suspended

END DESCRIPTION **********************************************************/

#if OS_TASK_SUSPEND_EN > 0
nodebug
INT8U  OSTaskResume (INT8U prio)
{
#if OS_CRITICAL_METHOD == 3                      /* Allocate storage for CPU status register           */
    auto OS_CPU_SR  cpu_sr;
#endif
    auto OS_TCB    *ptcb;

#if OS_ARG_CHK_EN > 0
    if (prio >= OS_LOWEST_PRIO) {                               /* Make sure task priority is valid    */
        return (OS_PRIO_INVALID);
    }
#endif
    OS_ENTER_CRITICAL();
    if ((ptcb = OSTCBPrioTbl[prio]) == (OS_TCB *)0) {           /* Task to suspend must exist          */
        OS_EXIT_CRITICAL();
        return (OS_TASK_RESUME_PRIO);
    }
    if ((ptcb->OSTCBStat & OS_STAT_SUSPEND) != 0x00) {                     /* Task must be suspended   */
        if (((ptcb->OSTCBStat &= ~OS_STAT_SUSPEND) == OS_STAT_RDY) &&      /* Remove suspension        */
             (ptcb->OSTCBDly  == 0)) {                                     /* Must not be delayed      */
            OSRdyGrp               |= ptcb->OSTCBBitY;                     /* Make task ready to run   */
            OSRdyTbl[ptcb->OSTCBY] |= ptcb->OSTCBBitX;
            OS_EXIT_CRITICAL();
            OS_Sched();
        } else {
            OS_EXIT_CRITICAL();
        }
        return (OS_NO_ERR);
    }
    OS_EXIT_CRITICAL();
    return (OS_TASK_NOT_SUSPENDED);
}
#endif

/*
*********************************************************************************************************
*                                             STACK CHECKING
*********************************************************************************************************
*/

/* START FUNCTION DESCRIPTION ********************************************
OSTaskStkChk                                                <UCOS.LIB>

SYNTAX:        INT8U OSTaskStkChk (INT8U prio, OS_STK_DATA *pdata);

DESCRIPTION:	This function is called to check the amount of free memory
               left on the specified task's stack.

PARAMETER1:    The task priority

PARAMETER2:    Pointer to a data structure of type OS_STK_DATA.

RETURN VALUE:  OS_NO_ERR           upon success
               OS_PRIO_INVALID     if the priority you specify is higher
               						  that the maximum allowed
               						  (i.e. > OS_LOWEST_PRIO) or, OS_PRIO_SELF
               						  not specified.
               OS_TASK_NOT_EXIST   if the desired task has not been created
               OS_TASK_OPT_ERR     if OS_TASK_OPT_STK_CHK NOT specified
               						  when the task was created

END DESCRIPTION **********************************************************/

#if OS_TASK_CREATE_EXT_EN > 0
nodebug
INT8U OSTaskStkChk (INT8U prio, OS_STK_DATA *pdata)
{
#if OS_CRITICAL_METHOD == 3                      /* Allocate storage for CPU status register           */
    auto OS_CPU_SR  cpu_sr;
#endif
    auto OS_TCB  *ptcb;
    auto INT16U   bottom;
    auto INT16U   free;
    auto INT16U   stksize;
    auto INT32U   physaddr;
    auto INT8U    upper_nibble;

    pdata->OSFree = 0;                                   // Assume failure, set to 0 size
    pdata->OSUsed = 0;
    if (prio > OS_LOWEST_PRIO && prio != OS_PRIO_SELF) { // Make sure task priority is valid
        return (OS_PRIO_INVALID);
    }
    OS_ENTER_CRITICAL();
    if (prio == OS_PRIO_SELF) {                          // See if check for SELF
        prio = OSTCBCur->OSTCBPrio;
    }
    ptcb = OSTCBPrioTbl[prio];
    if (ptcb == (OS_TCB *)0) {                           // Make sure task exist
        OS_EXIT_CRITICAL();
        return (OS_TASK_NOT_EXIST);
    }
    if ((ptcb->OSTCBOpt & OS_TASK_OPT_STK_CHK) == 0) {     // Make sure stack checking option is set
        OS_EXIT_CRITICAL();
        return (OS_TASK_OPT_ERR);
    }
    free = 0;
    stksize = (INT16U)ptcb->OSTCBStkSize;
    bottom = ptcb->OSTCBStkBottom;
    physaddr = ptcb->OSTCBStkSeg;
    OS_EXIT_CRITICAL();
    physaddr = (physaddr << 16) | bottom;
    physaddr = OSStkPhysAddr(physaddr);                  // Get 20-bit physical address of bottom of stack
    upper_nibble = (INT8U)(physaddr >> 16);              // Get upper nibble of physical address
    physaddr = (INT16U)(physaddr & 0x0000ffff);          // Get lower 16 bits of physical address
                                                         // Compute the number of zero entries on the stk
    #asm
       ld	hl,@sp+upper_nibble
		 add	hl,sp
       ld	a,(hl)			;a has upper nibble of physical address
       ld	hl,@sp+physaddr
		 add	hl,sp
		 ld	hl,(hl)
       ld	ix,hl				;ix has lower 16 bits of physical address
       ld   bc,0
       ld	de,0
     _ucos_stkchkbegin:
       ldp  hl,(ix)        		;load contents of stack at a:ix into hl
       sbc  hl,bc          		;check to see if 0 is at location
       jr	nz,_ucos_stkchkdone	;if not zero, done
       inc  de             		;not done, increment free count
       inc  ix             		;go to next memory location
       jr   _ucos_stkchkbegin    ;check next memory location
     _ucos_stkchkdone:
       ld 	hl,@sp+free    ;load free count from de into free
		 add	hl,sp
		 ld	(hl),e
		 inc	hl
		 ld	(hl),d
    #endasm

    pdata->OSFree = free;                                // Number of free bytes on the stack
    pdata->OSUsed = stksize - free;                         // Compute number of bytes used on the stack
    return (OS_NO_ERR);
}
#endif


/*
*********************************************************************************************************
*                                            SUSPEND A TASK
*********************************************************************************************************
*/

/* START FUNCTION DESCRIPTION ********************************************
OSTaskSuspend                                               <UCOS.LIB>

SYNTAX: 			INT8U OSTaskSuspend (INT8U prio);

DESCRIPTION:	This function is called to suspend a task.  The task can be
               the calling task if the priority passed to OSTaskSuspend()
               is the priority of the calling task or OS_PRIO_SELF.  This
               function should be used with great care.  If a task is
               suspended that is waiting for an event (i.e. a message, a
               semaphore, a queue ...) the task will be prevented from
               running when the event arrives.

PARAMETER1:    The priority of the task to suspend.  If OS_PRIO_SELF is
					specified, the calling task will suspend itself and
					rescheduling will occur.

RETURN VALUE:  OS_NO_ERR                if the requested task is suspended
               OS_TASK_SUSPEND_IDLE     if attempting to suspend the idle
               								 task (not allowed).
               OS_PRIO_INVALID          if the priority specified is higher
               								 than the maximum allowed (i.e.
               								 >= OS_LOWEST_PRIO) or,
               								 OS_PRIO_SELF is not specified .
               OS_TASK_SUSPEND_PRIO	    if the task to suspend does not
                                        exist

END DESCRIPTION **********************************************************/

#if OS_TASK_SUSPEND_EN > 0
nodebug
INT8U  OSTaskSuspend (INT8U prio)
{
#if OS_CRITICAL_METHOD == 3                      /* Allocate storage for CPU status register           */
    auto OS_CPU_SR  cpu_sr;
#endif
    auto BOOLEAN    self;
    auto OS_TCB    *ptcb;


#if OS_ARG_CHK_EN > 0
    if (prio == OS_IDLE_PRIO) {                                 /* Not allowed to suspend idle task    */
        return (OS_TASK_SUSPEND_IDLE);
    }
    if (prio >= OS_LOWEST_PRIO && prio != OS_PRIO_SELF) {       /* Task priority valid ?               */
        return (OS_PRIO_INVALID);
    }
#endif
    OS_ENTER_CRITICAL();
    if (prio == OS_PRIO_SELF) {                                 /* See if suspend SELF                 */
        prio = OSTCBCur->OSTCBPrio;
        self = TRUE;
    } else if (prio == OSTCBCur->OSTCBPrio) {                   /* See if suspending self              */
        self = TRUE;
    } else {
        self = FALSE;                                           /* No suspending another task          */
    }
    if ((ptcb = OSTCBPrioTbl[prio]) == (OS_TCB *)0) {           /* Task to suspend must exist          */
        OS_EXIT_CRITICAL();
        return (OS_TASK_SUSPEND_PRIO);
    }
    if ((OSRdyTbl[ptcb->OSTCBY] &= ~ptcb->OSTCBBitX) == 0x00) { /* Make task not ready                 */
        OSRdyGrp &= ~ptcb->OSTCBBitY;
    }
    ptcb->OSTCBStat |= OS_STAT_SUSPEND;                         /* Status of task is 'SUSPENDED'       */
    OS_EXIT_CRITICAL();
    if (self == TRUE) {                                         /* Context switch only if SELF         */
        OS_Sched();
    }
    return (OS_NO_ERR);
}
#endif

/*
*********************************************************************************************************
*                                            QUERY A TASK
*********************************************************************************************************
*/

/* START FUNCTION DESCRIPTION ********************************************
OSTaskQuery                                                 <UCOS.LIB>

SYNTAX:  		INT8U OSTaskQuery (INT8U prio, OS_TCB *pdata);

DESCRIPTION:	This function is called to obtain a copy of the desired
					task's TCB.

RETURN VALUE:  OS_NO_ERR       if the requested task is suspended
               OS_PRIO_INVALID if the priority you specify is higher than
                               the maximum allowed (i.e. >= OS_LOWEST_PRIO)
                               or, OS_PRIO_SELF is not specified .
               OS_PRIO_ERR     if the desired task has not been created

END DESCRIPTION **********************************************************/

#if OS_TASK_QUERY_EN > 0
nodebug
INT8U  OSTaskQuery (INT8U prio, OS_TCB *pdata)
{
#if OS_CRITICAL_METHOD == 3                      /* Allocate storage for CPU status register           */
    OS_CPU_SR  cpu_sr;
#endif
    auto OS_TCB    *ptcb;


#if OS_ARG_CHK_EN > 0
    if (prio > OS_LOWEST_PRIO && prio != OS_PRIO_SELF) {   /* Task priority valid ?                    */
        return (OS_PRIO_INVALID);
    }
#endif
    OS_ENTER_CRITICAL();
    if (prio == OS_PRIO_SELF) {                            /* See if suspend SELF                      */
        prio = OSTCBCur->OSTCBPrio;
    }
    if ((ptcb = OSTCBPrioTbl[prio]) == (OS_TCB *)0) {      /* Task to query must exist                 */
        OS_EXIT_CRITICAL();
        return (OS_PRIO_ERR);
    }
    memcpy(pdata, ptcb, sizeof(OS_TCB));                   /* Copy TCB into user storage area          */
    OS_EXIT_CRITICAL();
    return (OS_NO_ERR);
}
#endif

/*** BeginHeader  */
#endif
/*** EndHeader */