DVD

开发平台：
Others

SAMPScheduler.c：源码内容
							#include "Config.h"		// Global Configuration - do not remove!
// *****************************************************************************
// * Define debug messages condition
// * has to be define before "SysDefs.h"
// *****************************************************************************
#if D_SUPPORT_SACD // ZKR GLSACD
//#define _DEBUG_SCHEDULER
#ifdef _DEBUG_SCHEDULER
#define _DEBUG_TASK_SWITCH
#endif
#include <embedded.h>
#include "IncludeSysDefs.h"
#include "PlaycoreCoremaincoremain.h"
#include "Playcoretimingtiming.h"
#include "KerneluITRONrtos.h"
#include "KernelKer_API.h"
#include "KernelEventDef.h"
#include "PlaycoreScPadScPadAlloc.h"
#include "CPUtimefunc.h"
#include "Playcorecoremaincoregdef.h"
#include "PlaycoreSAMPschedulerSAMPScheduler.h"
#include "decoderlow_leveldec_ll_reg.h"
extern int SACD_Navigator(HDLR_OP Op, EVENT Event, void *Param);
//JERRYC_2004AUG24
extern UINT32 Ker_GetCoreStackBottom(void);
typedef void (_far *LPSACDTIMERPROC)(void);
#ifdef dbg_printf
#undef dbg_printf
	#define dbg_printf(sMsg)		printf ## sMsg
#endif
// *****************************************************************************
// * SACD task scheduler
// * Implemented features: TASKSWITCH,SEMAPHORES,MAILBOXES
// *****************************************************************************
// *****************************************************************************
// * Constant definition
// *****************************************************************************
//definition of allocated memory in SDRAM for tasks stack
#define SDRAM_STACK_SIZE				2048ul   //BYTE
#define SDRAM_STACK_BASE				(SC_CLIPSDIR1_LIST_ADDRESS << 1)
//staticly allocated resources definition
#define NUMBER_OF_TASKS					6
#define NUMBER_OF_SEMAPHORES			13
#define NUMBER_OF_MAILBOXES			8
//ID of scheduler task
#define TASK_ID_SCHEDULER				NUMBER_OF_TASKS
//definition of uninitialized semaphor counter
#define SEMAPHORE_UNINITIALIZED		0xFFFF
// *****************************************************************************
// * External referencies
// *****************************************************************************
extern void SAMP_API_Handler (UINT32 param);
// *****************************************************************************
// * Global variable
// *****************************************************************************
int SchedulerID = -1;
//keeps id of active task
static IDENT currentTask;
#define CUR_TASK currentTask
//initial level of core tasks stack (offset part only)
static UINT16 initialStack;
#define INITIAL_STACK initialStack
#define TASK (gns.sacd.task)
#define SEMA (gns.sacd.semaphore)
#define MB (gns.sacd.mailbox)
// *****************************************************************************
// * Function prototypes
// *****************************************************************************
static IDENT SchedulerGetReadyTask (void);
static SAMP(ErrCode) TaskSwitch (void);
static int SAMP_Scheduler (HDLR_OP Op, EVENT Event, void *Param);
// *****************************************************************************
// * TASKS handling implementation
// *****************************************************************************
// *****************************************************************************
// * Name: OS_TaskCreate
// * Purpose: Create and start a new task
// * Input: Priority 	Priority of the task.
// * 			Stacksize 	Stack size required by the task.
// * 			Entry 		The function that gets control when the new task starts
// * 							execution.
// * Output: *Id 			Unique identification number for the created task.
// * Returns: SAMP(ERR_OK) 	Operation successful.
// * 			  ERR_UNKNOWN Unknown internal failure.
// * Description: Create a new task with the given priority and stack size and
// * start it at the entry point. The task Id is returned. There are eight
// * different task priorities required by SA-MP. They are specified by the
// * integer numbers 0, 1, 2, ... 7 where 0 is the lowest possible priority
// * and 7 is the highest. Note that these priorities must be mapped by the OSA
// * to some suitable set of priorities for the underlying operating system.
// * Tasks running at priority 0 will have a higher priority than the idle task
// * of the underlying operating system.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_TaskCreate) (PRIORITY Priority, size_t Stacksize, void(*Entry)(void),IDENT *Id)
{
	TASK_CONTEXT taskContext;
   TCB *taskPtr;
#ifdef _DEBUG_SCHEDULER
	dbg_printf (("SCHEDULER(%i) TaskCreate",CUR_TASK));
//   SAMP(TH_Send)("SCHEDULER ", "TaskCreate by:", "%u", CUR_TASK);
#endif
	//test if the priority doesnot exccedes number of tasks
	if (Priority >= NUMBER_OF_TASKS) return SAMP(ERR_UNKNOWN);
	//assume that number of tasks is equal to number of priorities minus one
   //the id sets the level of priority, higher id - lower priority
   *Id = NUMBER_OF_TASKS - Priority - 1;
	taskPtr = &(TASK[*Id]);
   if (taskPtr->Status != TASK_NOT_STARTED) return SAMP(ERR_UNKNOWN);
#ifdef _DEBUG_SCHEDULER
	dbg_printf (("(%i)n",*Id));
//   SAMP(TH_Send)("SCHEDULER ", "TaskCreate new:", "%u", *Id);
#endif
   //initialize task stack in SDRAM
	taskContext.task = Entry;
   //initializa first stack offset after task switch
   taskPtr->StackOff = INITIAL_STACK - sizeof(TASK_CONTEXT);
   wai_sem(SEM_DRAM_ID);
   {
    UINT8 * addr = (UINT8 *)(&taskContext);
    if(sizeof(TASK_CONTEXT)%2) //odd
     addr -= 1;
    I49_WriteDRAMDataNSW (taskPtr->SDRAMStackAddr,(UINT16*)addr,(sizeof(TASK_CONTEXT) + 1)/2);
   }
   sig_sem(SEM_DRAM_ID);
	//change task status
	taskPtr->Status = TASK_STATUS_READY;
   return TaskSwitch ();
}
// *****************************************************************************
// * Name: OS_TaskDelete
// * Purpose: Stop and delete a task.
// * Input: Id 		Identification number of the task as was returned on
// * 						OS_TaskCreate().
// * Returns: 	SAMP(ERR_OK) Operation successful.
// * 				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: See purpose.
// * Note: The task Id will be invalidated by this function. A new call to
// * OS_TaskCreate() for the same entry-function may or may not return the same
// * value.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_TaskDelete) (IDENT Id)
{
	dbg_printf (("SCHEDULER(%i) TaskDelete",CUR_TASK));
   dbg_printf (("(%i)n",Id));
//   SAMP(TH_Send)("SCHEDULER ", "TaskDelete ID:", "%u", Id);
	//cannot delete task that is curently running
	if (CUR_TASK == Id) return SAMP(ERR_UNKNOWN);
   //invalidate the task
	TASK[Id].Status = TASK_NOT_STARTED;
   return SAMP(ERR_OK);
}
// *****************************************************************************
// * SEMAPHORES implementation
// *****************************************************************************
// *****************************************************************************
// * Name: OS_SemCreate
// * Purpose: Create a new counting semaphore.
// * Input: Units 	The initial amount of units in the semaphore.
// * Ouput: *Id 	  Unique identification number for the created semaphore.
// * Returns: 	SAMP(ERR_OK) Operation successful.
// * 				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Precondition: (Units=0(Units=1)
// * Description: Create a new counting semaphore and return the semaphore Id.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_SemCreate) (UNITS Units,IDENT *Id)
{
	IDENT id;
#ifdef _DEBUG_SCHEDULER
	dbg_printf (("SCHEDULER(%i) SemaphoreCreate",CUR_TASK));
//   SAMP(TH_Send)("SCHEDULER ", "SemaphoreCreate by task:", "%u", CUR_TASK);
#endif
	for (id = 0; id < NUMBER_OF_SEMAPHORES; id++)
   {
		if (SEMA[id] == SEMAPHORE_UNINITIALIZED)
      {
      	SEMA[id] = Units;
         *Id = id;
#ifdef _DEBUG_SCHEDULER
         dbg_printf (("(%i)n",id));
#endif
      	return SAMP(ERR_OK);
      }
   }
   dbg_printf (("Errorn"));
//   SAMP(TH_Send)("SCHEDULER ", "SemaphoreCreate:", "%s", "Errorn");
   return SAMP(ERR_UNKNOWN);
}
// *****************************************************************************
// * Name: OS_SemDelete
// * Purpose: Delete a semaphore.
// * Input: Id 	Identification number of the semaphore as was returned
// * 					on OS_SemCreate().
// * Returns: 	SAMP(ERR_OK) Operation successful.
// * 				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: Delete the semaphore associated with Id.
// * Note: The semaphore Id will be invalidated by this function.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_SemDelete)(IDENT Id)
{
	dbg_printf (("SCHEDULER(%i) SemaphoreDelete",CUR_TASK));
   dbg_printf (("(%i)n",Id));
//   SAMP(TH_Send)("SCHEDULER ", "SemaphoreDelete: task - sem", "%u%u", CUR_TASK, Id);
   if (Id >= NUMBER_OF_SEMAPHORES) return SAMP(ERR_UNKNOWN);
	SEMA[Id] = SEMAPHORE_UNINITIALIZED;
   return SAMP(ERR_OK);
}
// *****************************************************************************
// * Name: OS_SemP
// * Purpose: Take a unit from a semaphore.
// * Input:	Id 		Identification number of the semaphore.
// * 		  	Timeout 	The maximum time to wait for a unit to become available.
// * Returns: SAMP(ERR_OK) 	Operation successful.
// * 			  ERR_FAILED	No unit available (after waiting).
// * 			  SAMP(ERR_UNKNOWN) 	Unknown internal failure.
// * Description: Obtain a unit from the semaphore associated with Id but don't
// * wait more than Timeout centiseconds for it. If a unit is available, take it
// * and return. If Timeout is OS_FOREVER, wait until an unit becomes
// * available. If Timeout is 0, return immediately. If the time-out elapses
// * while waiting for a unit, return ERR_FAILED. Waiting tasks are queued by
// * priority, that is the highest priority task waiting will get the next
// * available unit.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_SemP) (IDENT Id,TIMEOUT Timeout)
{
   STATUS_REG stReg;
   TCB *taskPtr = &(TASK[CUR_TASK]);
#ifdef _DEBUG_SCHEDULER
   //dbg_printf (("S%i SG",CUR_TASK));
   //dbg_printf (("%in",Id));
//   SAMP(TH_Send)("SCHEDULER ", "SemaphoreGet: task - sem", "%u%u", CUR_TASK, Id);
#endif
   //test if the semaphor is already initialized
   if (SEMA[Id] == SEMAPHORE_UNINITIALIZED) return SAMP(ERR_UNKNOWN);
   //test if the id is correct
   if (Id >= NUMBER_OF_SEMAPHORES) return SAMP(ERR_UNKNOWN);
	do
   {
	   //test if semaphore unit available
		stReg = InterruptDisable ();
		if (SEMA[Id])
	   {
	    	SEMA[Id]--;
		   set_SR (stReg);
//         dbg_printf (("SCHEDULER(%i) SemaphoreReceivedn",CUR_TASK));  
//         SAMP(TH_Send)("SCHEDULER ", "SemaphoreReceived: task:", "%u", CUR_TASK);
		   return SAMP(ERR_OK);
	   }
	   //semmaphore unit not available at this moment
   
   	//test if need to wait for the semaphor
	 	if (!Timeout)
      {
		   set_SR (stReg);
      	return SAMP(ERR_FAILED);
      }
      //change task status
		taskPtr->Status = TASK_STATUS_WAITING_FOR_SEMAPHORE;
   	//save semaphore id, that is the current task waiting for
		taskPtr->SemaphoreID = Id;
      set_SR (stReg);
		//return execution to task scheduler and wait semaphore unit for to arrive
   }
   while (TaskSwitch () == SAMP(ERR_OK));
   //if we get here there was error in the scheduler
   return SAMP(ERR_FAILED);
}
// *****************************************************************************
// * Name: OS_SemV
// * Purpose: Release a unit to a semaphore.
// * Input: Id 	Identification number of the semaphore.
// * Returns: 	SAMP(ERR_OK) 		Operation successful.
// * 				SAMP(ERR_FAILED)	No unit available after waiting.
// * 				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: Release a unit to semaphore associated with Id.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_SemV) (IDENT Id)
{
   STATUS_REG stReg;
#ifdef _DEBUG_SCHEDULER
	//dbg_printf (("S%i SS",CUR_TASK));
   //dbg_printf (("%in",Id));
//   SAMP(TH_Send)("SCHEDULER ", "SemaphoreSet: task - sem:", "%u%u", CUR_TASK, Id);
#endif
   //test if the semaphor is already initialized
   if (SEMA[Id] == SEMAPHORE_UNINITIALIZED) return SAMP(ERR_UNKNOWN);
   //test if the id is correct
   if (Id >= NUMBER_OF_SEMAPHORES) return SAMP(ERR_UNKNOWN);
	stReg = InterruptDisable ();   
   //increment semaphor count
  	SEMA[Id]++;
   //test if needs to release task from wait state
   // if (releaseTask)
   {
		TCB *taskPtr;
      
      //test if there is task waiting for the semaphore
      for (taskPtr = TASK; taskPtr < (TASK + NUMBER_OF_TASKS); taskPtr++)
      {
         if ((taskPtr->Status == TASK_STATUS_WAITING_FOR_SEMAPHORE) && 
             (taskPtr->SemaphoreID == Id))
         {
            //release task from wait state
            taskPtr->Status = TASK_STATUS_READY;
            set_SR (stReg);
            return TaskSwitch ();
         }
      }
   }
   
   set_SR (stReg);
   return SAMP(ERR_OK);
}
// *****************************************************************************
// * Name: OS_MboxCreate
// * Purpose: Create a new mailbox.
// * Input: Capacity 	Maximum number of messages that the maibox can
// * 							contain.
// * Output: *Id 			Unique identification number for the created mailbox.
// * Returns: 	SAMP(ERR_OK) 		 Operation successful.
// * 				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: Create a new mailbox with size Capacity and return the mailbox
// * Id.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_MboxCreate) (CAPACITY Capacity,IDENT *Id)
{
	MAILBOX *mailboxPtr;
   IDENT id;
   if (Capacity > MAX_NUM_OF_MESSAGE)
   {
     Capacity = MAX_NUM_OF_MESSAGE;
   }
#ifdef _DEBUG_SCHEDULER
	dbg_printf (("SCHEDULER(%i) MailboxCreate",CUR_TASK));
//   SAMP(TH_Send)("SCHEDULER ", "MailboxCreate: task:", "%u", CUR_TASK);
#endif
	//search for available mailbox
	for (id = 0; id < NUMBER_OF_MAILBOXES; id++)
   {
   	mailboxPtr = &(MB[id]);
   	//test if mailbox available
		if (mailboxPtr->MessageQueue == NULL)
      {
      	//allocate memory for the mailbox on heap
      	mailboxPtr->MessageQueue = (MESSAGE*)(SACD_GLOBALS.mailbox_buff[id]); //(MESSAGE*)malloc(MESSAGE_LENGTH * Capacity);
			//if no memory available return error
         if (mailboxPtr->MessageQueue == NULL)
         {
         	printf ("Fatal! Error no memory availablen");
//            SAMP(TH_Send)("SCHEDULER ", "MailboxCreate: ", "%s", "Error no memory availablen");
         	return SAMP(ERR_UNKNOWN);
         }
			//initialize mailbox
         memset (mailboxPtr->MessageQueue,0,MESSAGE_LENGTH * Capacity);
         mailboxPtr->Capacity = Capacity;
      	mailboxPtr->MessageCount = 0;
         mailboxPtr->WritePointer = 0;
         mailboxPtr->ReadPointer = 0;
         *Id = id;
			
#ifdef _DEBUG_SCHEDULER
         dbg_printf (("(%i)n",id));
//         SAMP(TH_Send)("SCHEDULER ", "MailboxCreate: id:", "%u", id);
#endif
      	return SAMP(ERR_OK);
      }
   }
   //return error - no mailbox is available
	dbg_printf (("Errorn"));
//   SAMP(TH_Send)("SCHEDULER ", "MailboxCreate: id:", "%s", "Errorn");
   return SAMP(ERR_UNKNOWN);
}
// *****************************************************************************
// * Name: OS_MboxDelete
// * Purpose: Delete a mailbox.
// * Input: Id 	Identification number of the mailbox to be deleted.
// * Returns: 	SAMP(ERR_OK) 		Operation successful.
// * 				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: Delete the mailbox associated with Id.
// * Note: The mailbox Id will be invalidated by this function.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_MboxDelete) (IDENT Id)
{
	dbg_printf (("SCHEDULER(%i) MailboxDelete",CUR_TASK));
   dbg_printf (("(%i)n",Id));
//   SAMP(TH_Send)("SCHEDULER ", "MailboxDelete: task - box", "%u%u", CUR_TASK, Id);
	//test validity of mailbox
	if (Id >= NUMBER_OF_MAILBOXES) return SAMP(ERR_UNKNOWN);
   //release memory allocated for the mailbox
   if (MB[Id].MessageQueue != NULL)
   {
//    free (MB[Id].MessageQueue);
    MB[Id].MessageQueue = NULL;
   }
   return SAMP(ERR_OK);
}
// *****************************************************************************
// * Name: MessageSend
// * Pupose: Send a message to a mailbox.
// * Input: Id 		Identification number of the mailbox.
// * 			*Ptr 		Pointer to the start of the message.
// * 			RelTask	0 - task switch not allowed in this function
// * 						other - task switch can happen
// * Returns: 	SAMP(ERR_OK) Operation successful.
// * 				SAMP(ERR_INVALID_PARAM) Size exceeds maximum message size (12 bytes).
// * 				SAMP(ERR_FAILED) Capacity of mailbox exceeded.
// *				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: Send a message of Size bytes starting at Ptr to the mailbox
// * associated with Id.
// *****************************************************************************
SAMP(ErrCode) MessageSend (IDENT Id, void * Ptr,size_t Size, BOOL ReleaseTask)
{
	STATUS_REG stReg;
	MAILBOX *mb = &(MB[Id]);
	TCB *taskPtr;
#ifdef _DEBUG_SCHEDULER
//   dbg_printf (("SCHEDULER(%i) MessageSend(%i)n",CUR_TASK,Id));
   dbg_printf (("S%i MS",CUR_TASK));
   dbg_printf (("%in", Id));
//   SAMP(TH_Send)("SCHEDULER ", "MessageSend: task - box", "%u%u", CUR_TASK, Id);
#endif
   //test if mailbox is initialized already
   if (mb->MessageQueue == NULL) return SAMP(ERR_UNKNOWN);
   //test validity of mailobox ID
   if (Id >= NUMBER_OF_MAILBOXES) return SAMP(ERR_UNKNOWN);
   //test message length
   if (Size > MESSAGE_LENGTH) return SAMP(ERR_INVALID_PARAM);
	stReg = InterruptDisable ();
   //test if mailbox is full
	if (mb->MessageCount >= mb->Capacity)
   {
	   set_SR (stReg);
   	return SAMP(ERR_FAILED);
   }
   //add message to mailbox
#ifdef _DEBUG_SCHEDULER
   dbg_printf (("WP%in",mb->WritePointer));
   dbg_printf (("MC%in",mb->MessageCount));
#endif
   memcpy (mb->MessageQueue[mb->WritePointer].Message, Ptr, Size);
   mb->MessageCount++;
   if (mb->WritePointer == (mb->Capacity - 1))
   {
     mb->WritePointer = 0;
   }
   else
   {
     mb->WritePointer++;
   }
   //test if there is task waiting for this message
   for (taskPtr = TASK; taskPtr < (TASK + NUMBER_OF_TASKS); taskPtr++)
   {
      if ((taskPtr->Status == TASK_STATUS_WAITING_FOR_MESSAGE) && 
          (taskPtr->MailboxID == Id))
      {
      	//release the task from sleep state
       	taskPtr->Status = TASK_STATUS_READY;
		   set_SR (stReg);
         //test if task switch is allowed
 	      if (ReleaseTask) return TaskSwitch ();
         //send event in case all task are in sleep state
			ie_send_ex(IE_CORE_SAMP_WAKE_UP, (void *)(0));
     	   return SAMP(ERR_OK);
      }
   }
   set_SR (stReg);
   return SAMP(ERR_OK);
}
// *****************************************************************************
// * Name: OS_SendMsg
// * Pupose: Send a message to a mailbox.
// * Input: Id 		Identification number of the mailbox.
// * 			*Ptr 		Pointer to the start of the message.
// * Returns: 	SAMP(ERR_OK) Operation successful.
// * 				SAMP(ERR_INVALID_PARAM) Size exceeds maximum message size (12 bytes).
// * 				SAMP(ERR_FAILED) Capacity of mailbox exceeded.
// * 				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: Send a message of Size bytes starting at Ptr to the mailbox
// * associated with Id.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_SendMsg) (IDENT Id,void *Ptr,size_t Size)
{
	return MessageSend (Id,Ptr,Size,1);
}
// *****************************************************************************
// * Name: OS_ISendMsg
// * Pupose: Send a message from an ISR to a mailbox.
// * Input: Id 		Identification number of the mailbox.
// * 			Ptr 		Pointer to the start of the message.
// * Returns: 	SAMP(ERR_OK) Operation successful.
// * 				SAMP(ERR_INVALID_PARAM) Size exceeds maximum message size (12 bytes).
// * 				SAMP(ERR_FAILED) Capacity of mailbox exceeded.
// * 				SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: Send a message of Size bytes starting at Ptr to the mailbox
// * associated with Id.
// * The behaviour of this function is identical to OS_SendMsg() but is intended
// * to be called by interrupt service routines only. Interrupt service routines
// * are not allowed to call any other OSA functions.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_ISendMsg) (IDENT Id,void *Ptr,size_t Size)
{
	return MessageSend (Id,Ptr,Size,0)	;
}
// *****************************************************************************
// * Name: OS_RecvMsg
// * Purpose: Receive a message from a mailbox.
// * Input: Id Identification number of the mailbox.
// * Timeout Maximum time to wait for a message to become
// * available.
// * Ptr Pointer to buffer where message will be stored.
// * Size Size of this buffer in bytes.
// * Output: *Msgsize Actual number of bytes stored in the buffer.
// * Returns: SAMP(ERR_OK) Operation successful.
// * SAMP(ERR_FAILED)No message available (after waiting).
// * SAMP(ERR_UNKNOWN) Unknown internal failure.
// * Description: Receive a message from the mailbox Id but don't wait more than
// * Timeout centiseconds for it. If a message is available, the message is
// * copied to the data area of Size bytes starting at Ptr. The actual message
// * size is returned in Msgsize if it is known to the system. Otherwise,
// * Msgsize will be set to Size.
// * If Timeout is OS_FOREVER, wait until an message becomes available. If
// * Timeout is 0, return immediately. If the time-out elapses while waiting for
// * a message, return SAMP(ERR_FAILED).
// * Note: Waiting tasks are queued by priority, that is the highest priority
// * task waiting will get the next available message.
// *****************************************************************************
SAMP(ErrCode) SAMP(OS_RecvMsg) (IDENT Id,TIMEOUT Timeout,void *Ptr,size_t Size,size_t *Msgsize)
{
	STATUS_REG stReg;
	MAILBOX *mb = &(MB[Id]);
   TCB *taskPtr = &(TASK[CUR_TASK]);
//   UINT16 loopcount = 0;
#ifdef _DEBUG_SCHEDULER
   //dbg_printf (("SCHEDULER(%i) MessageGet(%i)n",CUR_TASK,Id));
   dbg_printf (("S%i MG",CUR_TASK));
   dbg_printf (("%in",Id));
//   SAMP(TH_Send)("SCHEDULER ", "MessageGet: task - box", "%u%u", CUR_TASK, Id);
#endif
   //test if mailbox is initialized already
   if (mb->MessageQueue == NULL) return SAMP(ERR_UNKNOWN);
   //test validity of mailobox ID
   if (Id >= NUMBER_OF_MAILBOXES) return SAMP(ERR_UNKNOWN);
   //size of the message
   *Msgsize = Size;
   do
   {
#if 0
   //sacd todo: check later.
      if(TASK_ID_SCHEDULER == CUR_TASK && loopcount > 1000)
      {
	      taskPtr->Status = TASK_STATUS_READY;
      	return SAMP(ERR_OK);
      }
      loopcount++;
#endif
      //test if message available in the mailbox
      stReg = InterruptDisable ();
      if (mb->MessageCount)
      {
#ifdef _DEBUG_SCHEDULER
         dbg_printf (("RP%in",mb->ReadPointer));
         dbg_printf (("MC%in",mb->MessageCount));
#endif
      	//message available decrement the counter and copy message to dest
         memcpy (Ptr, mb->MessageQueue[mb->ReadPointer].Message, Size);
         mb->MessageCount--;
#if 1
         if (mb->ReadPointer == (mb->Capacity - 1))
         {
           mb->ReadPointer = 0;
         }
         else
         {
           mb->ReadPointer++;
         }
#endif
         set_SR (stReg);
#ifdef _DEBUG_SCHEDULER
         //dbg_printf (("SCHEDULER(%i) MessageReceivedn",CUR_TASK));
         dbg_printf (("S%i MR",CUR_TASK));
         dbg_printf (("%in",Id));
//         SAMP(TH_Send)("SCHEDULER ", "MessageReceived: task:", "%u", CUR_TASK);
#endif
         return SAMP(ERR_OK);
      }
	   //message not available in mailbox at this moment
   	//test if need to wait for the message
	 	if (!Timeout)
      {
	      set_SR (stReg);
      	return SAMP(ERR_FAILED);
      }
      //change task status
      taskPtr->Status = TASK_STATUS_WAITING_FOR_MESSAGE;
      //save mailbox id and message pointer, that is the current task waiting for
      taskPtr->MailboxID = Id;
      set_SR (stReg);
		//return execution to task scheduler and wait for message to arrive
   }
   while (TaskSwitch () == SAMP(ERR_OK));
   //if we get here there was error in the scheduler
   return SAMP(ERR_FAILED);
}
// *****************************************************************************
// * Purpose          : get task ID of ready task with highest priority
// * Input Parameters : none
// * Return Value     : SAMP(ERR_OK) - the task switch is succesful
// * Description		: the function checks TCB of all tasks and select the task
// *						  with highest priority and ready status
// * Comments		 	:
// *****************************************************************************
static IDENT SchedulerGetReadyTask (void)
{
	TCB *taskPtr;
	IDENT taskID;
	//return the READY task with the highest priority
	for (taskPtr = TASK, taskID = 0; taskPtr < (TASK + NUMBER_OF_TASKS); taskPtr++,taskID++)
   {
   	if (taskPtr->Status == TASK_STATUS_READY) return taskID;
   }
   return TASK_ID_SCHEDULER;
}
// *****************************************************************************
// * Purpose          : switch execution of task
// * Input Parameters : none
// * Return Value     : none
// * Description		: the function will save stack of current task to SDRAM,
// *						  than download stack of next ready task from SDRAM and
// *						  return execution to new task
// * Comments		 	: the si,di registers has to be preserved, it is requirment
// *						  of the compiler
// *****************************************************************************
static SAMP(ErrCode) TaskSwitch (void)
{
	//has to be static variable, so the address will not change
	//get id of ready task with highest priority
   static TCB *taskPtr;
	static IDENT id;
#ifdef _DEBUG_TASK_SWITCH
	//static UINT32 s_time,e_time;
#endif
   static UINT8 * addr;
	_SI = _DI = 0x0000;
   id = SchedulerGetReadyTask ();
#ifdef _DEBUG_TASK_SWITCH
   //dbg_printf (("*** TaskSwitch %i->%in",CUR_TASK,id));
   dbg_printf (("TS %i->",CUR_TASK));
   dbg_printf (("%in",id));
//   SAMP(TH_Send)("SCHEDULER ", "TaskSwitch: task - id", "%u%u", CUR_TASK, id);
#endif
   if (CUR_TASK == id) return SAMP(ERR_OK);
	taskPtr = &(TASK[CUR_TASK]);
   //force compiler to save si and di registers on stack at the beggining of TaskSwitch function
   //save offset of stack pointer to TCB of current task
   taskPtr->StackOff = _SP;
#ifdef _DEBUG_TASK_SWITCH
	//dbg_printf (("  * Old stack: SDRAM %lx, Stack %Fp, Num %in",taskPtr->SDRAMStackAddr, MK_FP (_SS,taskPtr->StackOff),initialStack - taskPtr->StackOff));
//   SAMP(TH_Send)("SCHEDULER ", "TaskSwitch: task - id", "%u%u", CUR_TASK, id);
	//s_time = gen_timer ();
#endif
	//save stack of current task to SDRAM
   wai_sem(SEM_DRAM_ID);
   {
    addr = MK_FP (_SS,taskPtr->StackOff);
    if((INITIAL_STACK - taskPtr->StackOff)%2) //odd
     addr -= 1;
   I49_WriteDRAMDataNSW (taskPtr->SDRAMStackAddr,(UINT16 *)addr,(INITIAL_STACK - taskPtr->StackOff + 1)/2);
   }
   //change the current task to new task
   CUR_TASK = id;
   taskPtr = &(TASK[id]);
   //reload stack pointer with value from new task
   _SP = taskPtr->StackOff;
   //from this point the code is using stack space of new task
	//restore the stack of the new task
   {
    addr = MK_FP (_SS,taskPtr->StackOff);
    if((INITIAL_STACK - taskPtr->StackOff)%2) //odd
     addr -= 1;
	 I49_ReadDRAMDataNSW (taskPtr->SDRAMStackAddr,(UINT16 *)addr,(INITIAL_STACK - taskPtr->StackOff + 1)/2);
   }
   sig_sem(SEM_DRAM_ID);
#ifdef _DEBUG_TASK_SWITCH
     //e_time = gen_timer();
	  //dbg_printf (("  * New stack: SDRAM %lx, Stack %Fp, Num %in",taskPtr->SDRAMStackAddr, MK_FP (_SS,taskPtr->StackOff),initialStack - taskPtr->StackOff));
	  //dbg_printf (("  * Task switch time: %liusn",e_time - s_time));
#endif
   return SAMP(ERR_OK);
}
/*
Purpose: get the current elapsed time in centi seconds since startup.
*/
SAMP(ErrCode) SAMP(OS_GetTime)(UINT32 *Ticks)
{
   *Ticks = timing_get_clock()/10000;
   return SAMP(ERR_OK);
}
/*
Purpose: arm logical timer to invoke callback on every interval.
Return: SAMP(ERR_OK), succesful.
        SAMP(ERR_FAILED), no free timer available.
        SAMP(ERR_INVALID_PARAM), Ticks is 0 or OS_FOREVER.
*/
SAMP(ErrCode) SAMP(OS_TimerEvery)(UINT32 Ticks, void (*Callback)(void), IDENT *Id)
{
   if(0 == Ticks || OS_FOREVER == Ticks)
    return SAMP(ERR_INVALID_PARAM);
	*Id = timer_service_create((LPTIMERPROC)Callback, Ticks*10,
						 TIMER_ENABLED | TIMER_SAMP);
   //*Id = timer_service_create((LPTIMERPROC)Callback, Ticks*1000,
	//					 TIMER_ENABLED);
   if(NULL == *Id)
    return SAMP(ERR_FAILED);
   else
    return SAMP(ERR_OK);
}
SAMP(ErrCode) SAMP(OS_TimerCancel)(IDENT Id)
{
   if( TRUE == timer_service_delete(Id))
    return SAMP(ERR_OK);
   else
    return SAMP(ERR_UNKNOWN);
}
// *****************************************************************************
// * Purpose          : Initialize task scheduler
// * Input Parameters : None
// * Return Value     : None
// * Description		: The function sets states of all scedular resources
// * to initial state and assignes SDRAM memory space for each task stack
// *****************************************************************************
static void Constructor (void)
{
   UINT8 id;
  //set current task to scheduler
//JERRYC_2004AUG24
INITIAL_STACK = Ker_GetCoreStackBottom();	//set the stack to the end of the SRAM memory
   CUR_TASK = TASK_ID_SCHEDULER;
	//initialize tasks including the scheduler
   for (id = 0; id < NUMBER_OF_TASKS + 1; id++)
   {
   	TASK[id].Status = TASK_NOT_STARTED;
	   TASK[id].SDRAMStackAddr = SDRAM_STACK_BASE + (UINT32)id * (SDRAM_STACK_SIZE/2);
   }
   //initialize semaphors
   for (id = 0;id < NUMBER_OF_SEMAPHORES;id++)
   {
   	SEMA[id] = SEMAPHORE_UNINITIALIZED;
   }
   //initialize mailboxes
   for (id = 0;id < NUMBER_OF_MAILBOXES;id++)
   {
   	MB[id].MessageQueue = NULL;
   }
}
// *****************************************************************************
// * Purpose          : Dealocate all resources used by scheduler
// * Input Parameters : None
// * Return Value     : None
// * Description		: Release heap memory allocated by mailboxes if not done
// * before
// *****************************************************************************
static void Destructor (void)
{
   IDENT id;
   //delete the timers otherwise core task mail box will get overflowed with timer events.
//	timer_service_delete_all_samp_timers(); //sacd todo: check if timers are not needed any more at this time.
   SAMP_SchedulerRemove();
	//release memory allocated for mailboxes
   for (id = 0;id < NUMBER_OF_MAILBOXES;id++) SAMP(OS_MboxDelete) (id);
   //samp already does the clean up.
//   for (id = 0;id < NUMBER_OF_SEMAPHORES;id++) SAMP(OS_SemDelete) (id);
//   for (id = 0;id < NUMBER_OF_TASKS;id++) SAMP(OS_TaskDelete) (id);
}
// *****************************************************************************
// * Purpose          : instal SAMP scheduler
// * Input Parameters : NA
// * Return Value     : even handler id
// * 							-1 - no even hadler position available
// * Description      : see purpose
// *****************************************************************************
int SAMP_SchedulerInstall (void)
{
   if(SchedulerID == -1)
   {
	 SchedulerID = install_core_event_handler( SAMP_Scheduler );
#ifndef TEST_HARNASS
    SACD_GLOBALS.nav_handler_id = install_core_event_handler( SACD_Navigator );
#endif
   }
   return SchedulerID;
}
// *****************************************************************************
// * Purpose          : remove SAMP scheduler
// * Input Parameters : NA
// * Return Value     : even handler id
// * 							-1 - no even hadler position available
// * Description      : see purpose
// *****************************************************************************
void SAMP_SchedulerRemove (void)
{
	if (SchedulerID != -1)
   {
   	//remove_core_event_handler (SchedulerID); //ask Jerry!!!
#ifndef TEST_HARNASS
   	remove_core_event_handler (SACD_GLOBALS.nav_handler_id);
#endif
   	SchedulerID = -1;
   }
}
// *****************************************************************************
// * Purpose          : task scheduler implementation
// * Input Parameters : Op		handler operation code
// *							Event	event type
// *                    Param	parameter
// * Return Value     : zero
// * Description		: the handler will initialize all scheduler resources upon
// * instalation and release memory if not done in the code. The handler manages
// * following events:
// * 		IE_CORE_SAMP_API - request to call API function. It is processed by
// * supplied SAMP_API_Handler function
// *		IE_CORE_SAMP_WAKE_UP - send during OS_ISendMsg to allow task switch in
// * case of all tasks are in sleep state waiting for message or semaphor
// *****************************************************************************
static int SAMP_Scheduler (HDLR_OP Op, EVENT Event, void *Param)
{
	switch (Op)
	{
	case HDLR_ENTER:
   	Constructor();
      //if(gpSAMP_API_param == NULL)
      //gpSAMP_API_param = malloc(8 * sizeof(UINT32));
      //ASSERT(gpSAMP_API_param != NULL)
		break;
	case HDLR_EXIT:
   	Destructor();
      //if(gpSAMP_API_param != NULL)
      //{
      // free(gpSAMP_API_param);
      // gpSAMP_API_param =NULL;
      // }
		break;
	case HDLR_EVENT:
		switch (Event)
      {
        	case IE_CORE_SAMP_API:
         	//the API function calls occur inside of this function
         	SAMP_API_Handler ((UINT32)Param);
        		break;
         case IE_CORE_SAMP_WAKE_UP:
         	//this message is send by OS_ISendMsg in order to allow task switch
            //in case all task are in wait state
				TaskSwitch ();
            break;
         case IE_CORE_SAMP_TIMER_EVERY:
         {
          LPSACDTIMERPROC cb = (LPSACDTIMERPROC)Param;
          cb();
          break;
         }
         default:
		      //dbg_printf(("SAMP_Nav: Unexpected Event %xn",Event & 0xFF));
            break;
         }   
         break;
	default:			
		dbg_printf((" SAMP_Nav: Unknown Operation requested %dn", Op));
		break;
	}
	return 0;
}
#endif // D_SUPPORT_SACD

						