#define OSSCH_C
/******************************************************************************
 *
 *       (C) Copyright 2006, Freescale Semiconductor, Inc. All Rights Reserved.
 *
 *       THIS SOURCE CODE IS CONFIDENTIAL AND PROPRIETARY AND MAY NOT
 *       BE USED OR DISTRIBUTED WITHOUT THE WRITTEN PERMISSION OF
 *       Freescale Semiconductor, Inc.
 *
 * Filename:     $Source: /u/OSEK/src/os2000/src/osesys/src/ossch.c,v $
 * Author:       $Author: c-atroshikhin $
 * Revision:     $Revision: 1.9 $
 *
 * Functions:    
 *
 * History:      Use the CVS command rlog to display revision history
 *               information.
 *
 * Description:  OSEK OS scheduler           
 *
 * Notes:        
 *
 ******************************************************************************/

#if !defined(OSPROPH)
#include "osprop.h"                     /* OS Properties */
#else                                   /* !defined(OSPROPH)    */
#include OSPROPH                        /* OS Properties */
#endif                                  /* !defined(OSPROPH)    */

#include <osapi.h>

/*  Global OS variables */
#if !defined(OSSCHVECTREG)

OSDWORD OsSchedulerVector1;             /* Mask of activated tasks(priorities) */
#if defined(OSEXTPRIORS)
OSDWORD OsSchedulerVector2;             /* Mask of activated tasks(priorities) Extended vector*/
#endif                                  /* defined(OSEXTPRIORS) */

#endif                                  /* !defined(OSSCHVECTREG) */

#if !defined(OSRUNNINGREG)
#if defined(OSECC) || defined(OSRESOURCE) || \
  defined(OSEXTSTATUS) || defined(OSNOFASTTERMINATE)
OSTSKCBPTR OsRunning;                   /* running task (0, if no task is running)  */
#else                                   /* defined(OSECC) || defined(OSRESOURCE) || \*/
/* defined(OSEXTSTATUS) || defined(OSNOFASTTERMINATE) */
OSTASKTYPE OsRunning;                   /* running task */
#endif                                  /* defined(OSECC) || defined(OSRESOURCE) || \*/
/* defined(OSEXTSTATUS) || defined(OSNOFASTTERMINATE) */
#endif                                  /* !defined(OSRUNNINGREG) */

#if defined(OSECC) || defined(OSRESOURCE) || defined(OSCC2) || \
  defined(OSEXTSTATUS) || defined(OSNOFASTTERMINATE)
#if defined(OSRESOURCE) || defined(OSINRES) || defined(OSCC2)
OSTSKCBPTR OsPrioLink[OSNPRIORS];       /* Priority Link table                      */
#endif                                  /* defined(OSRESOURCE) || defined(OSINRES) || defined(OSCC2) \*/
OSJMP_BUF OsTaskJmpBuf[OSNTSKS];        /* Tasks' buffer for long jump (context).   */
#endif                                  /* defined(OSECC) || defined(OSRESOURCE) || defined(OSCC2) || \*/
/* defined(OSEXTSTATUS) || defined(OSNOFASTTERMINATE) */

/*  Scheduler services  */

/*  Private OS Services */

#if defined(OSBCC)
#if defined(OSEXTSTATUS) || defined(OSNOFASTTERMINATE) || defined(OSBCC2)
/***************************************************************************
 * Function :   OSTaskForceDispatch
 *
 * Description: Task dispatcher for BCC1
 *
 * Returns:     none
 *
 * Notes:
 *
 **************************************************************************/
void OSTaskForceDispatch( void )
{
  OSTSKCBPTR osOldRunning;              /* pointer to currently running task CB */

  OSCheckMainStack();                   /* check main stack overflow */
#if defined(OSEXTSTATUS) && defined(OSHOOKERROR)
  if( OsSuspendLevelAll != 0
#if defined(OSISRNORMAL)
   || OsSuspendLevel != 0
#endif                                  /* defined (OSISRNORMAL) */
   )
  {
    OSErrorHook_noPar( E_OS_SYS_ORDER );
  }
#endif                                  /* defined (OSEXTSTATUS) && defined(OSHOOKERROR) */
  osOldRunning = OsRunning;             /* save running task (if any)   */

#if defined(OSHOOKPOSTTASK)
  if( OSTASKID(OsRunning) != OSNULLTASK )
  {
    OSPostTaskHook();                   /* user hook or empty macros */
  }
#endif                                  /* defined(OSHOOKPOSTTASK) */

  if( ( OsRunning = OSGETMAXPRIOTASK ) != osOldRunning )
  {
    if( OSSetJmp( *(osOldRunning->context) ) )
    {
      OSSETBITRUNPRIO();                /* Restore internal resource priotity */
      OSDI();
      OSPreTaskHook();
      return;
    }
    if( (OSRUNNINGSTATUS & OSTCBFIRST) != 0 ) /* if task not running yet */
    {
      OSRUNNINGSTATUS &= (OSBYTE)(~OSTCBFIRST); /* task already running */
      /* Save previous value of the SPRG2 into preempted task context and */
      /* set new SPRG2 value as address of the preempted task context */
      OSSAVESETSPRG2(*(osOldRunning->context));

      OSSETRUNPRIO();                   /* set internal resource priotity, if any       */

      OSCallPreTaskHook();
      OSOrtiResetIdOnExit();            /* set that the caller service is finished  */
      OSEI();
      OSCALLTASK();
      /*          return;                             */
      /* 'return' just to fool optimizer  */
      /* never return at this point */
    }
    OSLOADSPRG2(*(OsRunning->context));
    OSOrtiResetIdOnExit();              /* set that the caller service is finished  */
    OSLongJmp( *(OsRunning->context), 1 ); /* jump directly to the context of preempted task   */

    /* never return at this point */
  }
  OSOrtiResetIdOnExit();                /* set that the caller service is finished  */
  return;
  OSWARNSUPPRESS                        /* For extra CW warning suppress */
}

/***************************************************************************
 * Function : OSTaskTerminateDispatch
 *
 * Description: Reduced analog ща OSTaskForceDispatch()  
 *              for Terminate/ChainTask services.
 *
 * Returns:     never
 *
 * Notes:       Shall be called with disabled system interrupts              
 *
 **************************************************************************/

void OSTaskTerminateDispatch( void )
{

#if defined (OSEXTSTATUS) && defined(OSHOOKERROR)
  if( OsSuspendLevelAll != 0
#if defined (OSISRNORMAL)
   || OsSuspendLevel != 0
#endif                                  /* defined (OSISRNORMAL) */
   )
  {
    OSErrorHook_noPar( E_OS_SYS_ORDER );
  }
#endif                                  /* defined (OSEXTSTATUS) && defined(OSHOOKERROR) */
  OSCheckMainStack();                   /* check main stack overflow */
  OsRunning = OSGETMAXPRIOTASK;

  if( (OSRUNNINGSTATUS & OSTCBFIRST) != 0 ) /* if task not running yet */
  {
    OSRUNNINGSTATUS &= (OSBYTE)(~OSTCBFIRST); /* task already running */
    OSLOADBASICSP();

    OSSETRUNPRIO();                     /* set internal resource priotity, if any       */

    OSCallPreTaskHook();
    OSOrtiResetIdOnExit();              /* set that the caller service is finished  */
    OSEI();
    OSCALLTASK();
    /*          return;                             */
    /* 'return' just to fool optimizer  */
    /* never return at this point */
  }
  OSLOADSPRG2(*(OsRunning->context));
  OSOrtiResetIdOnExit();                /* set that the caller service is finished  */
  OSLongJmp( *(OsRunning->context), 1 ); /* jump directly to the context of preempted task   */
  /* never return at this point */
  /* return; */
  OSWARNSUPPRESS                        /* For extra CW warning suppress */
}

#else                                   /* defined(OSEXTSTATUS) || defined(OSNOFASTTERMINATE) || defined(OSBCC2) */
/***************************************************************************
 * Function :   OSTaskForceDispatch
 *
 * Description: Task dispatcher for BCC1 with FastTerminate
 *
 * Returns:     none
 *
 * Notes:
 *
 **************************************************************************/
void OSTaskForceDispatch( void )
{
#if defined(OSRESOURCE)
  OSTSKCBPTR osOldRunning;              /* pointer to currently running task CB */
#else                                   /* defined(OSRESOURCE) */
  OSTASKTYPE osOldRunning;              /* currently running task   */
#endif                                  /* defined(OSRESOURCE) */

  OSTASKTYPE osChainTask;               /* task to be chain (if any)    */

#if defined(OSINRES)
  OSRUNPRIODEF;                         /* temporary variable for walk around the Diab BUG */
#endif                                  /* defined(OSINRES) */

  OSCheckMainStack();                   /* check main stack overflow */

#if defined (OSEXTSTATUS) && defined(OSHOOKERROR)
  if( OsSuspendLevelAll != 0
#if defined (OSISRNORMAL)
   || OsSuspendLevel != 0
#endif                                  /* defined (OSISRNORMAL) */
   )
  {
    OSErrorHook_noPar( E_OS_SYS_ORDER );
  }
#endif                                  /* defined (OSEXTSTATUS) && defined(OSHOOKERROR) */

  osOldRunning = OsRunning;             /* save running task (if any)   */

#if defined(OSHOOKPOSTTASK)
  if( OSTASKID(OsRunning) != OSNULLTASK )
  {
    OSPostTaskHook();                   /* user hook or empty macros */
  }
#endif                                  /* defined(OSHOOKPOSTTASK) */

  while( (OsRunning = OSGETMAXPRIOTASK) != osOldRunning ) /* cycle to launch all preempting tasks */
  {

    OSRUNPRIOASS;
    OSSETRUNPRIO();                     /* set internal resource priotity, if any       */
#if defined(OSRESOURCE) || defined(OSINRES)
    OSRUNNINGSTATUS |= OSTCBNOSUSPENDED; /* set "nonsuspended" flag */
#endif                                  /* defined(OSRESOURCE) || defined(OSINRES) */

    OSCallPreTaskHook();
    OSOrtiResetIdOnExit();              /* set that the caller service is finished  */
    OSEI();
    osChainTask = OSCALLTASK();         /* execute the task, and get the ID of chained task */
    OSDI();
    OSCheckMainStack();                 /* check main stack overflow */
#if defined (OSEXTSTATUS) && defined(OSHOOKERROR)
    if( OsSuspendLevelAll != 0
#if defined (OSISRNORMAL)
     || OsSuspendLevel != 0
#endif                                  /* defined (OSISRNORMAL) */
     )
    {
      OSErrorHook_noPar( E_OS_SYS_ORDER );
    }
#endif                                  /* defined (OSEXTSTATUS) && defined(OSHOOKERROR) */
#if defined(OSORTICURRENTSERVICEID)
    /* in case of FastTerminate all work of Chain/TerminateTask services    */
    /* is done in the dispatcher, so ORTI trace setting is done here too    */
    if( osChainTask != OSNULLTASK )
    {
      OSOrtiSetServiceWatch( OSServiceId_ChainTask );
      OSOrtiSetOldServiceID( OSServiceId_ChainTask );
    }
    else
    {
      OSOrtiSetServiceWatch( OSServiceId_TerminateTask );
      OSOrtiSetOldServiceID( OSServiceId_TerminateTask );
    }
#endif                                  /* defined(OSORTICURRENTSERVICEID) */
#if defined(OSINRES)
    if( OSCLEARRUNPRIOBITISNECESSARY() )
    {                                   /* if there is no ready task of this prio.  */
      OSTask2Suspend( OSTASKRUNPRIO( OsRunning )); /* then clear bit in OsScheduler         */
    }
    OSRESETRUNPRIO();                   /* Reset internal resource priotity, if any       */
#endif                                  /* defined(OSINRES) */
#if defined(OSRESOURCE) || defined(OSINRES)
    OSRUNNINGSTATUS &= (~OSTCBNOSUSPENDED); /* clear "nonsuspended" flag */
#endif                                  /* defined(OSRESOURCE) || defined(OSINRES) */

    OSREMOVETASK(OsRunning);

    if( osChainTask != OSNULLTASK )     /* if some task needs to be chained */
    {
      OSTask2Ready( osChainTask );      /*then readies chained task */
    }
    OSPostTaskHook();                   /* user hook or empty macros */
  }

  OSRUNPRIOASS;
  OSSETBITRUNPRIO();                    /* restore internal resource priotity */
  OSPreTaskHook();                      /* call user hook           */
  OSOrtiResetIdOnExit();                /* set that the caller service is finished  */

  return;
  OSWARNSUPPRESS                        /* For extra CW warning suppress */
}
#endif                                  /* defined(OSEXTSTATUS) || defined(OSNOFASTTERMINATE) || defined(OSBCC2) */

#endif                                  /* defined(OSBCC)  */

#if defined(OSECC)
/***************************************************************************
 * Function :   OSTaskForceDispatch
 *
 * Description: Task dispatcher for ECC1
 *
 * Returns:     none
 *
 * Notes:
 *
 **************************************************************************/
void OSTaskForceDispatch( void )
{
  OSTSKCBPTR osOldRunning;              /* pointer to currently running task CB */

#if defined (OSEXTSTATUS) && defined(OSHOOKERROR)
  if( OsSuspendLevelAll != 0
#if defined (OSISRNORMAL)
   || OsSuspendLevel != 0
#endif                                  /* defined (OSISRNORMAL) */
   )
  {
    OSErrorHook_noPar( E_OS_SYS_ORDER );
  }
#endif                                  /* defined (OSEXTSTATUS) && defined(OSHOOKERROR) */
  osOldRunning = OsRunning;

#if defined(OSHOOKPOSTTASK)
  if( osOldRunning->taskId != OSNULLTASK )
  {
    OSPostTaskHook();                   /* call user hook               */
  }
#endif                                  /* defined(OSHOOKPOSTTASK) */
  OSCheckMainStack();                   /* check main stack overflow */
  OSCheckStack();                       /* check stack overflow */
  if( ( OsRunning = OSGETMAXPRIOTASK ) != osOldRunning )
  {
    if( OSSetJmp( *(osOldRunning->context) ) ) /* save context of preempted task   */
    {
      OSSETBITRUNPRIO();                /* restore internal resource priotity */
      OSDI();
      OSPreTaskHook();
      /* return point from dispatched tasks */
      OSOrtiResetIdOnExit();            /* set that the caller service is finished  */
      return;
    }

#if OSNXTSKS > 0
    if( (osOldRunning->status & OSTCBEXTENDED) == 0 )
#endif                                  /* OSNXTSKS > 0  */
    {
      /* Save previous value of the SPRG2 into preempted basic task context and */
      /* set new SPRG2 value as address of the preempted task context */
      OSSAVESETSPRG2(*(osOldRunning->context));
    }

    if( (OsRunning->status & OSTCBFIRST) != 0 ) /* if task not running yet */
    {
      /* start-up the task    */
      OsRunning->status &= (OSBYTE)(~OSTCBFIRST); /* task already running */

#if OSNXTSKS > 0
      if( (OsRunning->status & OSTCBEXTENDED) != 0 )
      {
        OSLOADSP(OsRunning->tos);
      }
      else
      {
        if( (osOldRunning->status & OSTCBEXTENDED) != 0 ) /* if preempted task is EXTENDED */
        {
          OSLOADBASICSP();
        }
      }
#endif                                  /* OSNXTSKS > 0  */

      OSSETRUNPRIO();                   /* set internal resource priotity, if any       */

      OSCallPreTaskHook();
      OSOrtiResetIdOnExit();            /* set that the caller service is finished  */
      OSEI();
      OSCALLTASK();                     /* start task (via conventional call or jump)   */
      /*          return;                             */
      /* 'return' just to fool optimizer  */
      /* never return at this point */
    }
#if OSNXTSKS > 0
    if( (OsRunning->status & OSTCBEXTENDED) == 0 )
#endif                                  /* OSNXTSKS > 0  */
    {
      /* Restore previous value of the SPRG2 from the preempted task context */
      OSLOADSPRG2(*(OsRunning->context));
    }
    OSOrtiResetIdOnExit();              /* set that the caller service is finished  */
    OSLongJmp( *(OsRunning->context), 1 ); /* jump directly to the context of preempted task   */

    /* never return at this point */
  }
  OSOrtiResetIdOnExit();                /* set that the caller service is finished  */
  return;
  OSWARNSUPPRESS                        /* For extra CW warning suppress */
}

/***************************************************************************
 * Function :   OSTaskTerminateDispatch
 *
 * Description: Reduced analog of OSTaskForceDispatch()  
 *              for Terminate/ChainTask services.
 *
 * Returns:     never
 *
 * Notes:       Shall be called with disabled system interrupts              
 *
 **************************************************************************/

void OSTaskTerminateDispatch( void )
{

#if defined (OSEXTSTATUS) && defined(OSHOOKERROR)
  if( OsSuspendLevelAll != 0
#if defined (OSISRNORMAL)
   || OsSuspendLevel != 0
#endif                                  /* defined (OSISRNORMAL) */
   )
  {
    OSErrorHook_noPar( E_OS_SYS_ORDER );
  }
#endif                                  /* defined (OSEXTSTATUS) && defined(OSHOOKERROR) */
  OSCheckMainStack();                   /* check main stack overflow */
  OSCheckStack();                       /* check stack overflow */
  OsRunning = OSGETMAXPRIOTASK;

  if( (OsRunning->status & OSTCBFIRST) != 0 ) /* if task not running yet */
  {
    /* start-up the task    */
    OsRunning->status &= (OSBYTE)(~OSTCBFIRST); /* task already running */

#if OSNXTSKS > 0
    if( (OsRunning->status & OSTCBEXTENDED) != 0 )
    {
      OSLOADSP(OsRunning->tos);
    }
    else
#endif                                  /* OSNXTSKS > 0  */
    {
      OSLOADBASICSP();
    }
    OSSETRUNPRIO();                     /* set internal resource priotity, if any       */

    OSCallPreTaskHook();
    OSOrtiResetIdOnExit();              /* set that the caller service is finished  */
    OSEI();
    OSCALLTASK();                       /* start task (via conventional call or jump)   */
    /*          return;                             */
    /* 'return' just to fool optimizer  */
    /* never return at this point */
  }
#if OSNXTSKS > 0
  if( (OsRunning->status & OSTCBEXTENDED) == 0 )
#endif                                  /* OSNXTSKS > 0  */
  {
    OSLOADSPRG2(*(OsRunning->context));
  }

  OSOrtiResetIdOnExit();                /* set that the caller service is finished  */
  OSLongJmp( *(OsRunning->context), 1 ); /* jump directly to the context of preempted task   */
  /* never return at this point */
  /* return; */
  OSWARNSUPPRESS                        /* For extra CW warning suppress */
}
#endif                                  /* defined(OSECC)  */

/*  Public OS Services  */

/***************************************************************************
 * Function : Schedule
 *
 * Description: Yield the processor to the task with higher priority, if any
 *
 * Returns: E_OK                    no errors
 *          E_OS_CALLEVEL (ext.status) call at interrupt level
 *
 * Notes:
 *
 **************************************************************************/

StatusType Schedule( void )
{
#if defined(OSBCC1) && !defined(OSNOFASTERMINATE) && !defined(OSEXTSTATUS) && defined(OSINRES)
  OSRUNPRIODEF;                         /* temporary variable for walk around the Diab BUG */
#endif                                  /* defined(OSBCC1) && !defined(OSNOFASTERMINATE) && !defined(OSEXTSTATUS) \ */
  /* && defined(OSINRES) */

  OSOrtiSetServiceWatch( OSServiceId_Schedule );
#if defined(OSEXTSTATUS)
#if defined(OSISRENTRYEXIT)
  if( OSISRLEVEL != 0 )                 /* Check call level */
  {                                     /* Call at interrupt level is invalid */
    OSRETERROR( E_OS_CALLEVEL, OSServiceId_Schedule, 0 );
  }
#endif                                  /* defined(OSISRENTRYEXIT) */
#if defined(OSRESOURCE)
  if( OsRunning->resources != 0 )
  {                                     /* resource is occupied */
    OSRETERROR( E_OS_RESOURCE, OSServiceId_Schedule, 0 );
  }
#endif                                  /* defined(OSRESOURCE)  */
#if defined(OSRES_SCHEDULER)
  if( (OSRUNNINGSTATUS & OSTCBSCHEDULER ) != 0 )
  {                                     /* resource is occupied */
    OSRETERROR( E_OS_RESOURCE, OSServiceId_Schedule, 0 );
  }
#endif                                  /* defined(OSRES_SCHEDULER) */
#if defined(OSRESOURCEISR)
  if( OsISRLastResource != OSNULLRES)
  {                                     /* resource is occupied */
    OSRETERROR( E_OS_RESOURCE, OSServiceId_Schedule, 0 );
  }
#endif                                  /* defined(OSRESOURCEISR) */
#endif                                  /* defined(OSEXTSTATUS) */
  OSDI();                               /* It is not necessary to save./restore int.mask  because */
  /* this service shall be called with enabled interrupts only */
  OSOrtiSetOldServiceID( OSServiceId_Schedule );
#if defined(OSRES_SCHEDULER)
  if( (OSRUNNINGSTATUS & OSTCBSCHEDULER ) == 0 )
#endif                                  /* defined(OSRES_SCHEDULER) */
  {

#if defined(OSINRES)                    /* this service is not allowed after GetResource! */
    OSRUNPRIOASS;
    if( OSCLEARRUNPRIOBITISNECESSARY() )
    {
      /* if there is no nonsuspended task of this prio    */
      OSTask2Suspend( OSTASKRUNPRIO( OsRunning )); /* then clear bit in OsPrioMask      */
    }
    OSRESETRUNPRIO();
#endif                                  /* defined(OSINRES) */

    if( (OSTASKID(OSGETMAXPRIOTASK) < OSTASKID(OsRunning) )
#if defined(OSISRNORMAL) && defined(OSRESOURCEISR)
     && (OsIsrLevel == 0)               /* Scheduler is called  only in case when */
     /* ISR resources are not occupied */
#endif                                  /* defined(OSISRNORMAL) && defined(OSRESOURCEISR) */
     )
    {
      OSTaskForceDispatch();
    }
#if defined(OSINRES)
    else
    {
      OSSETRUNPRIO();
    }
#endif                                  /* defined(OSINRES) */
  }
  OSOrtiResetIdOnExit();                /* set that the caller service is finished  */
  OSEI();
  return( E_OK );
  OSWARNSUPPRESS                        /* For extra CW warning suppress */
}

