/*
                             *******************
******************************* C SOURCE FILE *******************************
**                           *******************                           **
**                                                                         **
** project      : The C Kernel                                             ** 
** filename     : CKERNEL.C                                                **
** version      : 5                                                        **
** last revised : August 22, 2004                                          **
**                                                                         **
*****************************************************************************
**                                                                         **
** Copyright (c) 1998-2004, P.K. van der Vlugt                             **
** All rights reserved.                                                    **
**                                                                         **
*****************************************************************************

VERSION HISTORY:
----------------

Version     : 1
Date        : June 01, 2003
Revised by  : P.K. van der Vlugt
Description : Original version.

Version     : 2
Date        : December 22, 2003
Revised by  : P.K. van der Vlugt
Description : * Fixed initialization problem where interrupts could be turned 
                on before CK_Run context switches to the first task.

Version     : 3
Date        : February 20, 2004
Revised by  : P.K. van der Vlugt
Description : * Added ck_reent function attribute to all API functions. This
                attribute has to be defined in stddefs.h depending on the
                target environment. For example for Keil this has to be the
                'reentrant' attribute, for reentrant C compilers this can be
                an empty definition.
              * Added ck_reent attribute to idleTaskFunction.

Version     : 4
Date        : April 15, 2004
Revised by  : P.K. van der Vlugt
Description : * Added check on empty ReadyQ in function reschedule. This 
                prevents unnecessary 'round-robin' scheduling, as would happen
                when the the running task has priority 0 and the ReadyQ is 
                empty.

Version     : 5
Date        : August 22, 2004
Revised by  : P.K. van der Vlugt
Description : * Changed module names of lower level modules. These modules are
                now preceded by 'ck' in the module name. Core modules which are
                required for correct compilation now have 'ck' in the module
                name, kernel add-on modules have 'ck_'.
              * Added support for a global version number. Therefore also added 
                the exported variables CK_Version and CK_VersionStr.
*/

#define _CKERNEL_C_SRC

/****************************************************************************/
/**                                                                        **/
/**                     MODULES USED                                       **/
/**                                                                        **/
/****************************************************************************/

/**** Kernel binding ****/
#include "ckbind.h"
/**** C Kernel interface ****/
#include "ckernel.h"
/**** Other modules ****/
#include "ckmbox.h"
#include "ckmsg.h"
#include "ckqueue.h"
#include "ckregion.h"
#include "cksema.h"
#include "stddefs.h"
#include "cktimer.h"
#include "cktcb.h"

/****************************************************************************/
/**                                                                        **/
/**                     DEFINITIONS AND MACROS                             **/
/**                                                                        **/
/****************************************************************************/

/**** The C Kernel global version number ****/
#define CK_VERSION      102
#define CK_VERSION_STR  "V1.02"

/**** General definitions ****/
#define IDLE_PRIORITY   0
#define IDLE_TID        0

/**** Modes used for function ready ****/
#define RESCHED_NO      0
#define RESCHED_YES     1

/**** LEAVE_CRITICAL; prevent interrupts to be turned on before running ****/
#define LEAVE_CRITICAL  if (KernelRunning) END_CRITICAL

/****************************************************************************/
/**                                                                        **/
/**                     TYPEDEFS AND STRUCTURES                            **/
/**                                                                        **/
/****************************************************************************/

/****************************************************************************/
/**                                                                        **/
/**                     PROTOTYPES OF LOCAL FUNCTIONS                      **/
/**                                                                        **/
/****************************************************************************/

static void   kernelTickHandler (void);
static void   reschedule        (void);
static void   ready             (int8u tid, int8u action);
static void   signal            (int8u sid, int8u action);
static int16s send              (int8u mbid, int32u msg, int8u action);
static void   handlePrioQueues  (int8u tid);
static void   queueToReadyQ     (int8u qid);
static void   idleTaskFunction  (void) ck_reent;

/****************************************************************************/
/**                                                                        **/
/**                     EXPORTED VARIABLES                                 **/
/**                                                                        **/
/****************************************************************************/

/**** The C Kernel global version number ****/
const int16u CK_Version      = CK_VERSION;
const int8s  CK_VersionStr[] = CK_VERSION_STR;

/**** Id of TASK_RUNNING ****/
ck_int8u ActiveTask;
/**** OldTask is used by ContextSwitch ****/
ck_int8u OldTask;

/****************************************************************************/
/**                                                                        **/
/**                     GLOBAL VARIABLES                                   **/
/**                                                                        **/
/****************************************************************************/

/**** IdleTask stack ****/
static stacktype IdleStack[IDLE_STACK_SIZE];

/**** Kernel management variables ****/
static ck_int16u RRcount;                   /* Round-robin value */
static ck_int16u PreemptionCount;
static ck_int8u  ReadyQ;
static ck_int8u  SleepQ;
static ck_bool   ISRswitch;                 /* Interrupt switch indication */
static ck_bool   KernelRunning;

/**** Clock variables ****/
static ck_int32u SystemTime;
static ck_int16u SecondCounter;

/****************************************************************************/
/**                                                                        **/
/**                     EXPORTED FUNCTIONS                                 **/
/**                                                                        **/
/****************************************************************************/

/**** KERNEL INITIALIZATION SECTION ****/

/****************************************************************************/
int16s CK_Init (void)
/****************************************************************************/
{
  /**** Turn off all interrupts ****/
  _disable ();

  /**** Initialize main kernel variables ****/
  KernelRunning = FALSE;
  ISRswitch = FALSE;
  RRcount = ROUND_ROBIN;
  SecondCounter = KERNEL_FREQUENCY;
  SystemTime = 0;

  /**** Initialize Kernel Tick hardware ****/
  KernelTickInit ();
  KernelTickSetHandler (kernelTickHandler);

  /**** Initialize queue module and get a ReadyQ ****/
  QueueInit ();
  ReadyQ = QueueAlloc ();
  if (ReadyQ == NR_OF_QUEUES)
    return (CK_NO_QUEUE);
  /**** Request the SleepQ ****/
  SleepQ = QueueAlloc ();
  if (SleepQ == NR_OF_QUEUES)
    return (CK_NO_QUEUE);

  /**** Initialize all further used resource modules ****/
  TcbInit ();
#if NR_OF_SEMAS > 0
  SemaInit ();
#endif
#if NR_OF_MBOXS > 0 && NR_OF_MSGS > 0 
  MsgInit ();
  MboxInit ();
#endif
#if NR_OF_REGIONS > 0
  RegionInit ();
#endif
#if NR_OF_TIMERS > 0
  TimerInit ();
#endif

  /**** Create the Idle Task and make ActiveTask ****/
  ActiveTask = CK_CreateTask (idleTaskFunction, IDLE_PRIORITY,
                              IdleStack, IDLE_STACK_SIZE);
  if (ActiveTask == NR_OF_TASKS)
    return (CK_NO_TCB);

  /**** There is no OldTask yet ****/
  OldTask = NR_OF_TASKS;

  /**** Passed all initializations, return OK ****/
  return (CK_OK);
}

/****************************************************************************/
void CK_Run (void)
/****************************************************************************/
{
  /**** Turn off all interrupts again to be sure ****/
  _disable ();
  /**** Get highest priority task from the ReadyQ ****/  
  ActiveTask = GetLast (ReadyQ);
  OldTask    = NR_OF_TASKS;

  /**** Reset preemption counter ****/
  PreemptionCount = RRcount;

  /**** Set ActiveTask state to running ****/
  Tcb[ActiveTask].state = TASK_RUNNING;
  
  /**** Next call will start the system and will never return ****/
  KernelRunning = TRUE;
  ContextSwitch ();
  /**** Black hole, never to get out here ****/
}


/**** KERNEL TASK FUNCTIONS ****/

/****************************************************************************/
int8u CK_CreateTask (taskptr    taskFunction, int8u  priority,
                     stacktype *stackBasePtr, int16u stackSize) ck_reent
/****************************************************************************/
{
int8u tid;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Request a Task Control Block and check if valid ****/
  tid = TcbAlloc ();
  if (tid < NR_OF_TASKS)
  {
    /**** Set priority fields ****/
    Tcb[tid].prio = priority;
    Tcb[tid].initprio = priority;

    /**** Setup initial stack ****/
    SetupStack (tid, taskFunction, stackBasePtr, stackSize);

    /**** Put in ready list, no reschedule ****/
    ready (tid, RESCHED_NO);
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return new task id ****/
  return (tid);
}

/****************************************************************************/
int16s CK_DeleteTask (int8u id) ck_reent
/****************************************************************************/
{
  /**** Check task id ****/
  if (id >= NR_OF_TASKS || id == IDLE_TID)
    /**** Illegal id or Idle Task Id ****/
    return (CK_ILLEGAL_ID);

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if task already free ****/
  if (Tcb[id].state == TASK_FREE)
  {
    /**** Leave critical section ****/
    LEAVE_CRITICAL;
    /**** Already free ****/
    return (CK_ILLEGAL_STATE);
  }

#if NR_OF_REGIONS > 0
  /**** Check if task has regions locked ****/
  if (Tcb[id].regions > 0)
  {
    /**** Leave critical section ****/
    LEAVE_CRITICAL;
    /**** Can not delete task ! ****/
    return (CK_REGIONS_LOCKED);
  }
#endif

  /**** Check the current task state ****/
  switch (Tcb[id].state)
  {
    case TASK_RUNNING:
      /**** Free the Task Control Block ****/
      TcbFree (id);
      /**** And do a reschedule ****/
      reschedule ();
    break;

    case TASK_SLEEP:
    case TASK_SLEEP_SUSPEND:
      /**** Delete task from delta-list ****/
      DeleteDelta (id);
      /**** Free the Task Control Block ****/
      TcbFree (id);
    break;

    case TASK_WAIT:
#if NR_OF_SEMAS > 0
      /**** Handle semaphore: one task deleted ****/
      Sema[Tcb[id].sid].count++;
#endif
      /**** Fall through ****/
 
    case TASK_RECEIVE:
      /**** Fall through ****/

    case TASK_READY:
      /**** Dequeue task from ready list ****/
      Dequeue (id);
      /**** Fall through ****/

    default:
      /**** Free the Task Control Block ****/
      TcbFree (id);
    break;
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return with success ****/
  return (CK_OK);
}

/****************************************************************************/
int16s CK_SetPriority (int8u id, int8u priority) ck_reent
/****************************************************************************/
{
  /**** Check task id ****/
  if (id >= NR_OF_TASKS || id == IDLE_TID)
    /**** Illegal id or idel Task Id ****/
    return (CK_ILLEGAL_ID);

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if task already free ****/
  if (Tcb[id].state == TASK_FREE)
  {
    /**** Leave critical section ****/
    LEAVE_CRITICAL;
    /**** Already free ****/
    return (CK_ILLEGAL_STATE);
  }

  /**** Set the new priority ****/
  Tcb[id].prio = priority;
  /**** Check task states ****/
  switch (Tcb[id].state)
  {
    case TASK_READY:
      /****Dequeue from the ready list ****/
      Dequeue (id);
      /**** And insert in ready list again ****/
      ready (id, RESCHED_YES);
    break;

    case TASK_RUNNING:
      /**** Check if has to reschedule ****/
      reschedule ();
    break;

    default:
    break;
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return with success ****/
  return (CK_OK);
}

/****************************************************************************/
int8u CK_GetPriority (int8u id) ck_reent
/****************************************************************************/
{
int8u prio;

  /**** Check limit ****/
  if (id >= NR_OF_TASKS)
    return (0);

  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Get the requested priority ****/
  prio = Tcb[id].prio;
  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (prio);
}

/****************************************************************************/
void CK_SetTimeSlice (int16u ticks) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Set new round-robin value ****/
  RRcount = ticks;
  /**** Leave critical section ****/
  LEAVE_CRITICAL;
}

/****************************************************************************/
int16u CK_GetTimeSlice (void) ck_reent
/****************************************************************************/
{
int16u count;

  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Get current round-robin value ****/
  count = RRcount;
  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (count);
}

/****************************************************************************/
int8u CK_GetActiveTask (void) ck_reent
/****************************************************************************/
{
int8u tid;

  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Get ActiveTask id ****/
  tid = ActiveTask;
  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (tid);
}

/****************************************************************************/
void CK_Sleep (int16u ticks) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if already end of sleep ****/
  if (ticks == 0)
  {
    /**** Check for reschedule again ****/
    reschedule ();
    /**** Leave critical section ****/
    LEAVE_CRITICAL;
    return;
  }

  /**** Insert in delta sleep list ****/
  InsertDelta (SleepQ, ActiveTask, ticks);

  /**** Set state to sleep ****/
  Tcb[ActiveTask].state = TASK_SLEEP;

  /**** Force a reschedule ****/
  reschedule ();

  /**** Leave critical section ****/
  LEAVE_CRITICAL;
}

/****************************************************************************/
int16s CK_Suspend (int8u id) ck_reent
/****************************************************************************/
{
  /**** First check limits ****/
  if (id >= NR_OF_TASKS || id == IDLE_TID)
    return (CK_ILLEGAL_ID);

  /**** Enter critical section ****/
  START_CRITICAL;

#if NR_OF_REGIONS > 0
  /**** Check if has regions locked ****/
  if (Tcb[id].regions > 0)
  {
    /**** Leave critical section ****/
    LEAVE_CRITICAL;
    /**** Can not suspend task ! ****/
    return (CK_REGIONS_LOCKED);
  }
#endif

  /**** Check state of task ****/
  switch (Tcb[id].state)
  {
    case TASK_READY:
      /**** Dequeue from ready list ****/
      Dequeue (id);
      /**** Goto suspend state ****/
      Tcb[id].state = TASK_SUSPEND;
      Tcb[id].sdepth = 1;
    break;

    case TASK_RUNNING:
      /**** Goto suspend state ****/
      Tcb[id].state = TASK_SUSPEND;
      Tcb[id].sdepth = 1;
      /**** Give up processor ****/
      reschedule ();
    break;

    case TASK_SUSPEND:
    case TASK_SLEEP_SUSPEND:
      /**** Already suspended, increment suspension depth ****/
      if (Tcb[id].sdepth < 255)
        Tcb[id].sdepth++;
      else
      {
        /**** Leave critical section ****/
        LEAVE_CRITICAL;
        /**** Maximum suspension depth reached ****/
        return (CK_MAX_SUSPENDED);
      }
    break;

    case TASK_SLEEP:
      /**** Goto sleep suspended state ****/
      Tcb[id].state = TASK_SLEEP_SUSPEND;
      Tcb[id].sdepth = 1;
    break;

    default:
      /**** Other states not allowed ****/
      LEAVE_CRITICAL;
      return (CK_ILLEGAL_STATE);
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16s CK_Resume (int8u id) ck_reent
/****************************************************************************/
{
  /**** First check limit ****/
  if (id >= NR_OF_TASKS)
    return (CK_ILLEGAL_ID);

  /**** Enter critical section ****/
  START_CRITICAL;

  switch (Tcb[id].state)
  {
    case TASK_SUSPEND:
      /**** Decrement suspension depth ****/
      Tcb[id].sdepth--;
      /**** When zero reached put in ready list ****/
      if (Tcb[id].sdepth == 0)
        ready (id, RESCHED_YES);
    break;

    case TASK_SLEEP_SUSPEND:
      /**** Decrement suspension depth ****/
      Tcb[id].sdepth--;
      /**** When zero reached put back to sleep ****/
      if (Tcb[id].sdepth == 0)
        Tcb[id].state = TASK_SLEEP;
    break;

    default:
      /**** Other states not allowed ****/
      LEAVE_CRITICAL;
      return (CK_ILLEGAL_STATE);
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
void CK_Yield (void) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Give up processor ****/
  reschedule ();
  /**** Leave critical section ****/
  LEAVE_CRITICAL;
}


#if NR_OF_SEMAS > 0
/**** KERNEL SEMAPHORE FUNCTIONS ****/

/****************************************************************************/
int8u CK_CreateSema (int16s count, int8u mode) ck_reent
/****************************************************************************/
{
int8u sid;

  /**** Check function parameters, Negative count is not allowed ****/
  if (count < 0)
    return (NR_OF_SEMAS);
  
  /**** One of the two modes must be used ****/
  if (mode != PRIO_Q && mode != FIFO_Q)
    return (NR_OF_SEMAS);

  /**** Enter critical section ****/
  START_CRITICAL;
  
  /**** Request the new semaphore ****/
  sid = SemaAlloc ();
  /**** Chek if valid id ****/
  if (sid < NR_OF_SEMAS)
  {
    /**** Setup the initial values ****/
    Sema[sid].count = count;
    Sema[sid].mode  = mode;
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return new semaphore identifier ****/
  return (sid);
}

/****************************************************************************/
int16s CK_DeleteSema (int8u id) ck_reent
/****************************************************************************/
{
int8u qid;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_SEMAS || !Sema[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Get associated queue ****/
  qid = Sema[id].qid;
  /**** Check if not empty ****/
  if (NonEmpty (qid))
  {
    /**** Copy queue to ready queue ****/
    queueToReadyQ (qid);
    /**** Release identifier ****/
    SemaFree (id);
    /**** And reschedule ****/
    reschedule ();
  }
  else
    /**** Release identifier ****/
    SemaFree (id);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16s CK_Wait (int8u id) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_SEMAS || !Sema[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Decrement semaphore count ****/
  Sema[id].count--;
  /**** Check if count becomes negative ****/
  if (Sema[id].count < 0)
  {
    /**** Set ActiveTask to waiting ****/    
    Tcb[ActiveTask].state = TASK_WAIT;
    Tcb[ActiveTask].sid = id;
    /**** Check which queue mode to use ****/
    if (Sema[id].mode == FIFO_Q)
      Enqueue (Sema[id].qid, ActiveTask);
    else
      Insert (Sema[id].qid, ActiveTask, Tcb[ActiveTask].prio);

    /**** Activate a reschedule ****/
    reschedule ();

    /**** Now check if semaphore is not deleted already ****/
    id = Tcb[ActiveTask].sid;
    if (!Sema[id].req)
    {
      /**** Leave critical section ****/
      LEAVE_CRITICAL;
      /**** Was deleted, return error ****/
      return (CK_SEMA_DELETED);
    }
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
bool CK_TestWait (int8u id) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_SEMAS || !Sema[id].req)
  {
    LEAVE_CRITICAL;
    return (FALSE);
  }

  /**** Check if counts available ****/
  if (Sema[id].count <= 0)
  {
    LEAVE_CRITICAL;
    /**** Return no counts available ****/
    return (FALSE);
  }

  /**** Counts available, get count ****/
  CK_Wait (id);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return and indicate success ****/
  return (TRUE);
}

/****************************************************************************/
int16s CK_Signal (int8u id) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_SEMAS || !Sema[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Signal the semaphore and check for reschedule ****/
  signal (id, RESCHED_YES);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16s CK_ISRsignal (int8u id) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_SEMAS || !Sema[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Signal the semaphore, no reschedule ****/
  signal (id, RESCHED_NO);

  /**** Indicate interrupt context ****/
  ISRswitch = TRUE;
  /**** Check for reschedule now ****/
  reschedule ();

  /**** The ISRswitch now indicates if an interrupt context-switch is required ****/
  if (ISRswitch)
  {
    /**** Reset and switch from this context ****/
    ISRswitch = FALSE;
    ISRcontextSwitch ();
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}
#endif


#if NR_OF_MBOXS > 0 && NR_OF_MSGS > 0
/**** KERNEL MESSAGE FUNCTIONS ****/

/****************************************************************************/
int8u CK_CreateMbox (int8u mode) ck_reent
/****************************************************************************/
{
int8u mid;

  /**** Check modes ****/
  if (mode != PRIO_Q && mode != FIFO_Q)
    return (NR_OF_MBOXS);

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Allocate the mailbox id ****/
  mid = MboxAlloc ();
  /**** Check if valid id ****/
  if (mid < NR_OF_MBOXS)
    /**** Set requested mode ****/
    Mbox[mid].mode = mode;

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return new mailbox identifier ****/
  return (mid);
}

/****************************************************************************/
int16s CK_DeleteMbox (int8u id) ck_reent
/****************************************************************************/
{
int8u qid;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_MBOXS || !Mbox[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Get associated task queue ****/
  qid = Mbox[id].qid;
  /**** Check if not empty ****/
  if (NonEmpty (qid))
  {
    /**** Copy queue to ready queue ****/
    queueToReadyQ (qid);
    /**** Free mailbox identifier ****/
    MboxFree (id);
    /**** And reschedule ****/
    reschedule ();
  }
  else
    /**** Free mailbox identifier ****/
    MboxFree (id);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16s CK_Receive (int8u id, int32u *msgPtr) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_MBOXS || !Mbox[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Check if message queue is empty ****/
  if (EmptyMsgQueue (id))
  {
    /**** Change task state to receive ****/
    Tcb[ActiveTask].state  = TASK_RECEIVE;
    Tcb[ActiveTask].mbid   = id;
    Tcb[ActiveTask].msgptr = msgPtr;

    /**** Check queue mode ****/
    if (Mbox[id].mode == FIFO_Q)
      Enqueue (Mbox[id].qid, ActiveTask);
    else
      Insert (Mbox[id].qid, ActiveTask, Tcb[ActiveTask].prio);

    /**** Activate a reschedule ****/
    reschedule ();

    /**** Now check if mailbox is not deleted already ****/
    id = Tcb[ActiveTask].mbid;
    if (!Mbox[id].req)
    {
      /**** Leave critical section ****/
      LEAVE_CRITICAL;
      /**** Was deleted, return error ****/
      return (CK_MBOX_DELETED);
    }
  }
  else
    /**** Messages waiting, dequeue from mailbox ****/
    DequeueMsg (id, msgPtr);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
bool CK_TestReceive (int8u id, int32u *msgPtr) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_MBOXS || !Mbox[id].req)
  {
    LEAVE_CRITICAL;
    return (FALSE);
  }

  /**** Check if messages available ****/
  if (EmptyMsgQueue (id))
  {
    LEAVE_CRITICAL;
    /**** Return no messages available ****/
    return (FALSE);
  }

  /**** Get the message ****/
  CK_Receive (id, msgPtr);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (TRUE);
}

/****************************************************************************/
int16s CK_Send (int8u id, int32u msg) ck_reent
/****************************************************************************/
{
int16s sts;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_MBOXS || !Mbox[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Send the message ****/
  sts = send (id, msg, RESCHED_YES);
  /**** Check if OK ****/
  if (sts != CK_OK)
  {
    LEAVE_CRITICAL;
    /**** Return as failed ****/
    return (CK_FAILED);
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return with OK ****/
  return (CK_OK);
}

/****************************************************************************/
int16s CK_ISRsend (int8u id, int32u msg) ck_reent
/****************************************************************************/
{
int16s sts;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_MBOXS || !Mbox[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Send the message, no reschedule ****/
  sts = send (id, msg, RESCHED_NO);
  /**** Check if OK ****/
  if (sts != CK_OK)
  {
    LEAVE_CRITICAL;
    /**** Return as failed ****/
    return (CK_FAILED);
  }

  /**** Indicate interrupt context ****/
  ISRswitch = TRUE;
  /**** Check for reschedule now ****/
  reschedule ();

  /**** The ISRswitch now indicates if an interrupt context-switch is required ****/
  if (ISRswitch)
  {
    /**** Reset and switch from this context ****/
    ISRswitch = FALSE;
    ISRcontextSwitch ();
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}
#endif


#if NR_OF_REGIONS > 0
/**** KERNEL REGION FUNCTIONS ****/

/****************************************************************************/
int8u CK_CreateRegion (void) ck_reent
/****************************************************************************/
{
int8u rid;

  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Request new region ****/
  rid = RegionAlloc ();
  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return new region identifier ****/
  return (rid);
}

/****************************************************************************/
int16s CK_DeleteRegion (int8u id) ck_reent
/****************************************************************************/
{
int8u qid;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_REGIONS || !Region[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Get the associated queue ****/
  qid = Region[id].qid;
  /**** Check if not empty ****/
  if (NonEmpty (qid))
  {
    /**** Copy queue to ready queue ****/
    queueToReadyQ (qid);
    /**** Release the identifier ****/
    RegionFree (id);
    /**** And reschedule ****/
    reschedule ();
  }
  else
    /**** Release the identifier ****/
    RegionFree (id);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16s CK_Lock (int8u id) ck_reent
/****************************************************************************/
{
int8u tid, qid, prio;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_REGIONS || !Region[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Check if region already locked ****/
  if (Region[id].lock)
  {
    /**** Get task id that locked region ****/
    tid = Region[id].tid;
    /**** Locked by this task ? ****/
    if (tid == ActiveTask)
    {
      LEAVE_CRITICAL;
      return (CK_OK);
    }

    /**** Change task state to waiting for region ****/
    Tcb[ActiveTask].state = TASK_WAIT_REGION;
    Tcb[ActiveTask].rid = id;
    /**** Insert in region's priority queue ****/
    qid = Region[id].qid;
    Insert (qid, ActiveTask, Tcb[ActiveTask].prio);

    /**** Check priority of locked by task ****/
    prio = (int8u) LastKey (qid);
    if (prio > Tcb[tid].prio)
    {
      /**** Set to new priority ****/
      Tcb[tid].prio = prio;
      /**** Indicate raised for restore ****/
      Tcb[tid].raised = TRUE;
      /**** Update priority queues ****/      
      handlePrioQueues (tid);
    }

    /**** Activate a reschedule ****/
    reschedule ();

    /**** Now check if region is not deleted already ****/
    id = Tcb[ActiveTask].rid;
    if (!Region[id].req)
    {
      /**** Leave critical section ****/
      LEAVE_CRITICAL;
      /**** Was deleted, return error ****/
      return (CK_REGION_DELETED);
    }
  }
  else
  {
    /**** Lock the region ****/
    Region[id].lock = TRUE;
    Region[id].tid  = ActiveTask;
    /**** Increment region owned count ****/
    Tcb[ActiveTask].regions++;
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
bool CK_TestLock (int8u id) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_REGIONS || !Region[id].req)
  {
    LEAVE_CRITICAL;
    return (FALSE);
  }

  /**** Check if already locked ****/
  if (Region[id].lock)
  {
    LEAVE_CRITICAL;
    /**** Return already locked indication ****/
    return (FALSE);
  }

  /**** Now lock the region ****/
  CK_Lock (id);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (TRUE);
}

/****************************************************************************/
int16s CK_Unlock (int8u id) ck_reent
/****************************************************************************/
{
int8u qid, tid;
bool  check;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_REGIONS || !Region[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Check if already unlocked ****/
  if (!Region[id].lock)
  {
    LEAVE_CRITICAL;
    return (CK_OK);
  }

  /**** Check if not locked by this task ****/
  if (Region[id].tid != ActiveTask)
  {
    LEAVE_CRITICAL;
    return (CK_FAILED);
  }

  /**** Check if priority was raised ****/
  check = FALSE;
  if (Tcb[ActiveTask].raised)
  {
    /**** Restore old priority ****/
    Tcb[ActiveTask].prio = Tcb[ActiveTask].initprio;
    Tcb[ActiveTask].raised = FALSE;
    check = TRUE;
  }

  /**** Decrement regions owned counter ****/
  Tcb[ActiveTask].regions--;

  /**** Get associated queue ****/
  qid = Region[id].qid;
  /**** And check for tasks waiting ****/
  if (NonEmpty (qid))
  {
    /**** Get first task form prio queue ****/
    tid = GetLast (qid);
    /**** Set new region owner ****/
    Region[id].tid = tid;
    /**** Task owns one more region ****/
    Tcb[tid].regions++;
    /**** Make task ready, wait with reschedule ****/
    ready (tid, RESCHED_NO);
    check = TRUE;
  }
  else
    /**** Unlock region ****/
    Region[id].lock = FALSE;

  /**** Now check if has to reschedule ****/
  if (check)
    reschedule ();

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}
#endif


#if NR_OF_TIMERS > 0
/**** KERNEL TIMER FUNCTIONS ****/

/****************************************************************************/
int8u CK_CreateTimer (void) ck_reent
/****************************************************************************/
{
int8u tid;

  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Request the timer id ****/
  tid = TimerAlloc ();
  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  /**** Return the new timer id ****/
  return (tid);
}

/****************************************************************************/
int16s CK_DeleteTimer (int8u id) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_TIMERS || !Timer[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Check if timer still running ****/
  if (Timer[id].running)
    /**** Delete from delta list ****/
    DeleteDeltaTimer (id);

  /**** Release timer resource ****/
  TimerFree (id);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16s CK_StartTimer (int8u id, int16u count, int8u sid) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_TIMERS || !Timer[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }
  /**** Check if timer already running ****/
  if (Timer[id].running)
  {
    LEAVE_CRITICAL;
    return (CK_TIMER_RUNNING);
  }

  /**** Set minimum value to 1 count ****/
  if (count == 0)
    count = 1;

  /**** Initialize timer variables ****/
  Timer[id].reload  = count;
  Timer[id].count   = count;
  Timer[id].running = TRUE;
  Timer[id].sid     = sid;

  /**** Insert timer in delta list ****/
  InsertDeltaTimer (id);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16s CK_RestartTimer (int8u id) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_TIMERS || !Timer[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }
  /**** Check if timer already running ****/
  if (Timer[id].running)
  {
    LEAVE_CRITICAL;
    return (CK_TIMER_RUNNING);
  }

  /**** Indicate running and insert in delta list again ****/
  Timer[id].running = TRUE;
  InsertDeltaTimer (id);

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16s CK_StopTimer (int8u id) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_TIMERS || !Timer[id].req)
  {
    LEAVE_CRITICAL;
    return (CK_ILLEGAL_ID);
  }

  /**** Do only when running ****/
  if (Timer[id].running)
  {
    /**** Delete from delta list ****/
    DeleteDeltaTimer (id);
    Timer[id].running = FALSE;
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (CK_OK);
}

/****************************************************************************/
int16u CK_GetTimeRemaining (int8u id) ck_reent
/****************************************************************************/
{
int16u count;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Check if valid id ****/
  if (id >= NR_OF_TIMERS || !Timer[id].req)
  {
    LEAVE_CRITICAL;
    return (0xFFFF);
  }

  if (Timer[id].running)
  {
    /**** Delete from list ****/
    DeleteDeltaTimer (id);
    /**** Get the count ****/
    count = Timer[id].count;
    /**** And insert again in delta list ****/
    InsertDeltaTimer (id);
  }
  else
    /**** Get the count ****/
    count = Timer[id].count;

  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (count);
}
#endif


/**** KERNEL CLOCK FUNCTIONS ****/

/****************************************************************************/
void CK_SetTime (int32u systemTime) ck_reent
/****************************************************************************/
{
  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Set new system time ****/
  SystemTime = systemTime;
  /**** Leave critical section ****/
  LEAVE_CRITICAL;
}

/****************************************************************************/
int32u CK_GetTime (void) ck_reent
/****************************************************************************/
{
int32u tempd;

  /**** Enter critical section ****/
  START_CRITICAL;
  /**** Get current system time ****/
  tempd = SystemTime;                
  /**** Leave critical section ****/
  LEAVE_CRITICAL;

  return (tempd);
}

/****************************************************************************/
/**                                                                        **/
/**                     LOCAL FUNCTIONS                                    **/
/**                                                                        **/
/****************************************************************************/

/****************************************************************************/
static void kernelTickHandler (void)
/****************************************************************************/
{
int8u id;
int8u sid = NR_OF_SEMAS;

  /**** Enter critical section ****/
  START_CRITICAL;

  /**** Handle clock ****/
  SecondCounter--;
  if (SecondCounter == 0)
  {
    /**** Second passed ****/
    SecondCounter = KERNEL_FREQUENCY;
    SystemTime++;
  }

#if NR_OF_TIMERS > 0
  /**** Handle TimerQ delta-list ****/
  if (NonEmptyTimerQ ())
  {
    /**** Timer(s) running; get first id ****/
    id = (int8u) FirstTimerId ();
    /**** And decrement it's time ****/
    TimerQ[id].qkey--;

    /**** Check if time has reached 0 ****/
    while (TimerQ[id].qkey == 0)
    {
      /**** Remove form list ****/
      id = GetFirstTimer ();
      /**** Reload value and insert in list again ****/
      Timer[id].count = Timer[id].reload;
      InsertDeltaTimer (id);

#if NR_OF_SEMAS > 0
      /**** Get the associated semaphore ****/
      sid = Timer[id].sid;
      /**** Check if this semaphore is in use ****/
      if (sid < NR_OF_SEMAS && Sema[sid].req)
      {
        /**** Signal semaphore ****/        
        signal (sid, RESCHED_NO);
        /**** Switch from this context ****/
        ISRswitch = TRUE;
      }
#endif
      
      /**** Get new first id from timer list ****/
      id = (int8u) FirstTimerId ();
    }
  }
#endif

  /**** Handle sleeping tasks ****/
  if (NonEmpty (SleepQ))
  {
    /**** Get first task from delta ****/
    id = (int8u) FirstId (SleepQ);
    /**** Decrement it's time ****/
    Q[id].qkey--;
    /**** Remove all keys with 0 ****/
    while (NonEmpty (SleepQ) && (Q[id].qkey == 0))
    {
      Dequeue (id);
      if (Tcb[id].state == TASK_SLEEP_SUSPEND)
        Tcb[id].state = TASK_SUSPEND;
      else
      {
        /**** Make ready again ****/
        ready (id, RESCHED_NO);
        /**** Switch from this context ****/
        ISRswitch = TRUE;
      }

      /**** Get next task from delta ****/
      id = (int8u) FirstId (SleepQ);
    }
  }

  /*** Handle round-robin counting ****/
  if (PreemptionCount > 0)
    PreemptionCount--;
  else
    /**** Switch from this context ****/
    ISRswitch = TRUE;


  /**** Check if needs to reschedule from this context ****/
  if (ISRswitch)
    reschedule ();

  /***** The ISRswitch now indicates if an interrupt context-switch is required ****/
  if (ISRswitch)
  {
    ISRswitch = FALSE;
    ISRcontextSwitch ();
  }

  /**** Leave critical section ****/
  LEAVE_CRITICAL;
}

/****************************************************************************/
static void reschedule (void)
/****************************************************************************/
{
  /**** Interrupts are always assumed to be off !! ****/

  /**** Check if ReadyQ contains tasks ****/
  if (IsEmpty (ReadyQ))
  {
    ISRswitch = FALSE;
    return;
  }
  /**** Check priority of running task against first one in ready queue ****/
  if ((Tcb[ActiveTask].state == TASK_RUNNING) && 
      (LastKey (ReadyQ) < Tcb[ActiveTask].prio))
  {
    ISRswitch = FALSE;
    return;
  }

  /**** ActiveTask still the running task ? ****/
  if (Tcb[ActiveTask].state == TASK_RUNNING)
  {
    /**** Switch required, set state ready and insert in ready queue ****/
    Tcb[ActiveTask].state = TASK_READY;
    Insert (ReadyQ, ActiveTask, Tcb[ActiveTask].prio);
  }

  /**** Set OldTask and ActiveTask; used by context switch ****/
  OldTask    = ActiveTask;
  ActiveTask = GetLast (ReadyQ);

  /**** Reset preemption counter ****/
  PreemptionCount = RRcount;
  
  /**** Make new task running ****/
  Tcb[ActiveTask].state = TASK_RUNNING;

  /**** Next do the context switch ****/
  if (!ISRswitch)
    ContextSwitch ();
}

/****************************************************************************/
static void ready (int8u tid, int8u action)
/****************************************************************************/
{
  /**** Put task in ready list ****/
  Tcb[tid].state = TASK_READY;
  Insert (ReadyQ, tid, Tcb[tid].prio);

  /**** Check if reschedule is desired ****/
  if (action == RESCHED_YES)
    reschedule ();
}

#if NR_OF_SEMAS > 0
/****************************************************************************/
static void signal (int8u sid, int8u action)
/****************************************************************************/
{
int8u qid, tid;

  /**** Check for tasks waiting, and increment sema count ****/
  if ((Sema[sid].count++) < 0)
  {
    /**** Get the associated queue ****/
    qid = Sema[sid].qid;
    
    /**** Get the task id from the queue ****/
    if (Sema[sid].mode == FIFO_Q)
      tid = GetFirst (qid);
    else
      tid = GetLast (qid);

    /**** Make task ready and check reschedule action ****/
    ready (tid, action);
  }
}
#endif

#if NR_OF_MBOXS > 0 && NR_OF_MSGS > 0
/****************************************************************************/
static int16s send (int8u mbid, int32u msg, int8u action)
/****************************************************************************/
{
int8u qid, mid, tid;

  /**** Get the task queue ****/
  qid = Mbox[mbid].qid;
  /**** Any tasks waiting ? ****/
  if (NonEmpty (qid))
  {
    /**** Get first task from queue ****/
    if (Mbox[mbid].mode == FIFO_Q)
      tid = GetFirst (qid);
    else
      tid = GetLast (qid);

    /**** Copy the message ****/
    *(Tcb[tid].msgptr) = msg;
    /**** Make task ready and check reschedule action ****/
    ready (tid, action);
  }
  else
  {
    /**** No tasks waiting, queue the message in the mailbox ****/
    mid = EnqueueMsg (mbid, msg);
    /**** Check for errors ****/
    if (mid >= NR_OF_MSGS)
      return (CK_FAILED);
  }

  return (CK_OK);
}
#endif

#if NR_OF_REGIONS > 0
/****************************************************************************/
static void handlePrioQueues (int8u tid)
/****************************************************************************/
{
int8u id;

  /**** Priority raise is state dependent ****/
  switch (Tcb[tid].state)
  {
    case TASK_READY:
      /**** Dequeue and insert again ****/
      Dequeue (tid);
      Insert (ReadyQ, tid, Tcb[tid].prio);
    break;

    case TASK_WAIT:
#if NR_OF_SEMAS > 0
      /**** Get semaphore identifier ****/
      id = Tcb[tid].sid;
      /**** Check if in priority queue ****/
      if (Sema[id].mode == PRIO_Q)
      { 
        /**** Dequeue and insert again ****/
        Dequeue (tid);
        Insert (Sema[id].qid, tid, Tcb[tid].prio);
      }
#endif
    break;

    case TASK_WAIT_REGION:
      /**** Get region identifier ****/
      id = Tcb[tid].rid;
      /**** Dequeue and insert again ****/
      Dequeue (tid);
      Insert (Region[id].qid, tid, Tcb[tid].prio);
    break;

    case TASK_RECEIVE:
#if NR_OF_MBOXS > 0 && NR_OF_MSGS > 0
      /**** Get mailbox identifier ****/
      id = Tcb[tid].mbid;
      /**** Check if in priority queue ****/
      if (Mbox[id].mode == PRIO_Q)
      {
        /**** Dequeue and insert again ****/
        Dequeue (tid);
        Insert (Mbox[id].qid, tid, Tcb[tid].prio);
      }
#endif
    break;

    default:
      break;
  }
}
#endif

/****************************************************************************/
static void queueToReadyQ (int8u qid)
/****************************************************************************/
{
int8u tid;

  /**** Get task id's from head of queue ****/
  tid = GetFirst (qid);
  /**** Do this for all tasks ****/
  while (tid != LIST_EMPTY)
  {
    /**** Put in ready list ****/
    ready (tid, RESCHED_NO);
    /**** Get next task id ****/    
    tid = GetFirst (qid);
  }
}

/****************************************************************************/
static void idleTaskFunction (void) ck_reent
/****************************************************************************/
{
  forever
    ;
}

/****************************************************************************/
/**                                                                        **/
/**                               EOF                                      **/
/**                                                                        **/
/****************************************************************************/