/* **********************************************************************
 * *	                                                                *
 * *             Routines for Process Scheduling/Suspension             *
 * *                                                                    *
 * **********************************************************************
 * File: libs/user/schedule.c
 *
 *              COPYRIGHT (c) 1994, 1996, 1999, 2003  BY
 *         SIGMET INCORPORATED, WESTFORD MASSACHUSETTS, U.S.A.
 * 
 * THIS SOFTWARE IS FURNISHED UNDER A LICENSE AND MAY BE USED AND  COPIED
 * ONLY  IN  ACCORDANCE WITH  THE  TERMS  OF  SUCH  LICENSE  AND WITH THE
 * INCLUSION OF THE ABOVE COPYRIGHT NOTICE.  THIS SOFTWARE  OR  ANY OTHER
 * COPIES  THEREOF MAY NOT BE PROVIDED OR OTHERWISE MADE AVAILABLE TO ANY
 * OTHER PERSON.  NO TITLE TO AND OWNERSHIP OF  THE  SOFTWARE  IS  HEREBY
 * TRANSFERED.
 *
 *   schedule_ef()          Schedule an event flag to be set in the future
 *   cancel_schedule_ef()   Cancel the above
 */
#include <stdio.h>
#include <string.h>
#include <sys/param.h>
#include <signal.h>
#include <math.h>
#include <errno.h>
#include <time.h>
#include <sys/time.h>

#ifdef LINUX
#define INCLUDE_SCHED_PROTOTYPES 1
#define INCLUDE_SYSUTSNAME_PROTOTYPES 1
#include <sched.h>
#include <sys/utsname.h>
#endif

#define INCLUDE_SYSTIME_PROTOTYPES 1
#include "sigtypes.h"
#include "user_lib.h"

static void schedule_ef_kernel( SINT4 imilli_a );
static void schedule_ef_sig_handler(int dummy);

/* ------------------------------
 * Kernel of Unix Timer Operations
 */
#define LAST_CALL_SNOOZE       (1)
#define LAST_CALL_SCHEDULE_EF  (2)
#define LAST_CALL_SCHEDULE_CBF (3)
static volatile UINT1 schedule_flag_c = 0 ;

#define SNOOZE_EF_NONE 0x00FFFF00
static volatile UINT4 snooze_ef_c = SNOOZE_EF_NONE ;


static void schedule_ef_sig_handler(int dummy)
{
  if( snooze_ef_c != SNOOZE_EF_NONE )
    {
      sig_setef( snooze_ef_c ) ;
      snooze_ef_c = SNOOZE_EF_NONE ;
    }
  schedule_flag_c = 0 ;
}

static void schedule_ef_kernel( SINT4 imilli_a )
{
  struct sigaction action ;
  sigset_t sigalrmset ; struct itimerval timer ;

  if( imilli_a <= 0 ) return ;

  /* Set up a signal handler to catch the real-time ALARM signal.
   */
  action.sa_handler = schedule_ef_sig_handler ;
  sigemptyset( &action.sa_mask ) ;
  action.sa_flags = SA_RESTART ;
  if (schedule_flag_c) sig_signal_va( USER_SCH_CONFLICT, 2, "SCHEDULE_EF",
				      schedule_flag_c );
  schedule_flag_c = LAST_CALL_SCHEDULE_EF;
  if( sigaction( SIGALRM, &action, NULL ) != 0 )
    {
      sig_signal( errno_to_message_va( errno, 1, "schedule_ef_kernel()" ) ) ;
      return ;
    }
  sigemptyset( &sigalrmset ) ;
  sigaddset( &sigalrmset, SIGALRM ) ;

  /* Set an interval timer to expire at a later time.
   */
  timer.it_value.tv_sec  =         imilli_a / 1000 ;
  timer.it_value.tv_usec = 1000 * (imilli_a % 1000) ;
  timer.it_interval.tv_sec  = 0 ;
  timer.it_interval.tv_usec = 0 ;

  if( setitimer( ITIMER_REAL, &timer, NULL ) != 0 )
    {
      sig_signal( errno_to_message_va( errno, 1, "schedule_ef_kernel()" ) ) ;
      return ;
    }
 
  /* ======= Allow Alarm to fire later ========
   */
  if( sigprocmask( SIG_UNBLOCK, &sigalrmset, NULL ) != 0 )
    {
      sig_signal( errno_to_message_va( errno, 1, "schedule_ef_kernel()" ) ) ;
      return ;
    }
}

/* ------------------------------
 * Schedule an event flag to fire some time in the future.
 */
void schedule_ef
  (UINT4 ief_a,
   SINT4 imilli_a )
{
  if ( imilli_a > 0)
    {
      snooze_ef_c  = ief_a ;
      schedule_ef_kernel(imilli_a) ;  
    }
  else
    {
      sig_setef( ief_a ) ;
    }
}

/* ------------------------------
 * Cancel a scheduled event flag firing from a previous call to
 * schedule_ef() or schedule_cbf().
 */
void cancel_schedule_ef( void )
{
  struct itimerval timer ;

  timer.it_value.tv_sec     = 0 ;
  timer.it_value.tv_usec    = 0 ;
  timer.it_interval.tv_sec  = 0 ;
  timer.it_interval.tv_usec = 0 ;
  if( setitimer( ITIMER_REAL, &timer, NULL ) != 0 ) {
    sig_signal( errno_to_message_va( errno, 1, "cancel_schedule_ef()" ) ) ;
  }
  snooze_ef_c = SNOOZE_EF_NONE ;
  schedule_flag_c = 0 ;
}

/* ------------------------------
 * Schedule a call back function to be called periodically.
 */
void schedule_cbf( void (*callback_a)(int), SINT4 imilli_a )
{
  struct sigaction action ;
  sigset_t sigalrmset ; struct itimerval timer ;

  if( imilli_a <= 0 ) return ;

  /* Set up a signal handler to catch the real-time ALARM signal.
   */
  action.sa_handler = callback_a;
  sigemptyset( &action.sa_mask ) ;
  action.sa_flags = SA_RESTART;

  if (schedule_flag_c) sig_signal_va( USER_SCH_CONFLICT, 2, "SCHEDULE_CBF",
				      schedule_flag_c );
  schedule_flag_c = LAST_CALL_SCHEDULE_CBF;

  if( sigaction( SIGALRM, &action, NULL ) != 0 )
    {
      sig_signal( errno_to_message_va( errno, 1, "schedule_cbf()" ) ) ;
      return ;
    }
  sigemptyset( &sigalrmset ) ;
  sigaddset( &sigalrmset, SIGALRM ) ;

  /* Set an interval timer to expire at a later time.
   */
  timer.it_value.tv_sec  =         imilli_a / 1000 ;
  timer.it_value.tv_usec = 1000 * (imilli_a % 1000) ;
  timer.it_interval = timer.it_value ;

  if( setitimer( ITIMER_REAL, &timer, NULL ) != 0 )
    {
      sig_signal( errno_to_message_va( errno, 1, "schedule_cbf()" ) ) ;
      return ;
    }
 
  /* ======= Allow Alarm to fire later ========
   */
  if( sigprocmask( SIG_UNBLOCK, &sigalrmset, NULL ) != 0 )
    {
      sig_signal( errno_to_message_va( errno, 1, "schedule_cbf()" ) ) ;
      return ;
    }
}

/* **********************************
 * *                                *
 * *  Short and Signal-less Sleeps  *
 * *                                *
 * **********************************
 */

static void sig_microSleep_int( SINT4 iMicroSeconds_a ) ;
static UINT4 calibrate_delay( void ) ;

volatile UINT4 delayLoopNonOptimized_c ; /* Not optimized away */
#define DELAYLOOP( COUNT ) { \
  SINT4 _ii = (COUNT) ; while( _ii-- >= 0 ) delayLoopNonOptimized_c++ ; \
}

/* ------------------------------
 * Convenient cover for the nanosleep() system call.  This method of
 * sleeping is generally preferable because it is implemented in the
 * kernel without the use of signals.  For threaded applications, this
 * is really the only proper way to sleep.
 */
void sig_microSleep( SINT4 iMicroSeconds_a )
{
  /* Break very long waits into smaller chunks to avoid overflows in
   * subsequent tick calculations.
   */
  SINT4 iRemainMicroSeconds = iMicroSeconds_a ;
  while( iRemainMicroSeconds > 0 ) {
    SINT4 iChunk = MIN( 5000000, iRemainMicroSeconds ) ;
    sig_microSleep_int( iChunk ) ; iRemainMicroSeconds -= iChunk ;
  }
}
static void sig_microSleep_int( SINT4 iMicroSeconds_a )
{
  /* Handle short waits differently from longer ones.  This is similar
   * to Linux 2.4 nanosleep() in the special case of processes that
   * are scheduled with realtime priority.  The code here unifies this
   * behavior on all platforms and for any type of process scheduling.
   */
  if( iMicroSeconds_a <= 500 ) {
    /* Short delays are handled by spinning.  Calibrate the delay loop
     * for this machine on the first time through.
     */
    static UINT4 oneMsDelay_c = 0 ; UINT4 iReqDelay ;
    if( oneMsDelay_c == 0 ) oneMsDelay_c = calibrate_delay() ;
    iReqDelay = NINT( 1E-3 * ((FLT4)iMicroSeconds_a) * (FLT4)oneMsDelay_c ) ;
    DELAYLOOP( iReqDelay ) ;
  }
  else {
    /* Long waits are handled by going to sleep.  Calculate the delay
     * that we need to request in order to get closest to the desired
     * delay.  The nanosleep() system call is horribly nonuniform from
     * one platform to the next, so this code is conditional.
     */
    struct timespec interval, remainder ;
    SINT4 iReqMicroSeconds = iMicroSeconds_a ; /* Obvious default */
    {
#ifdef LINUX
      /* The 2.6 kernel uses a 1000Hz clock interrupt and does not
       * give special treatment to realtime processes.  However, it
       * seems to have quirky not-quite-exactly-1ms behavior too.
       * Realtime oddities below 2ms in the 2.4 kernel need to be
       * handled, hence we need to test our priority each time.
       */
      struct utsname name ; MESSAGE istatus = sig_uname( &name ) ;
      if( istatus != SS_NORMAL ) sig_signal( istatus ) ;
      if( 0 == strncmp( name.release, "2.4", 3 ) ) {
	struct sched_param proc_param = {0} ;    /* For older 2.4 kernels we */
	(void)sched_getparam( 0, &proc_param ) ; /* need to know our priority. */
	iReqMicroSeconds = MAX( 0, iMicroSeconds_a - 10000 ) ;
	if( proc_param.sched_priority )
	  iReqMicroSeconds = MAX( 2001, iReqMicroSeconds ) ;
      } else {
	iReqMicroSeconds = MAX( 0, iMicroSeconds_a -  1001 ) ;
	iReqMicroSeconds -= (iMicroSeconds_a / 6000) ; /* Uggghhh!! */
      }
#endif
    }
    /* Setup the requested delay (in microseconds), and execute the
     * nanosleep() system call with suitable handling of interrupted
     * early returns.
     */
    interval.tv_sec  =   iReqMicroSeconds / 1000000 ;
    interval.tv_nsec = ( iReqMicroSeconds % 1000000 ) * 1000 ;

    while( nanosleep( &interval, &remainder ) == -1 ) {
      if( errno == EINTR ) interval = remainder ;
      else {
	sig_signal( errno_to_message_va( errno, 1, "sig_microSleep()" ) ) ;
	break ;
      }
    }
  }
}

/* Internal routine to calibrate the 'spin' delay.
 */
#define DELAY_MEASURE_MS      10
#define DELAY_TRIAL_LOOPS   5000

static UINT4 calibrate_delay( void )
{
  SINT4 iCount=0 ; UINT4 iTick1, iTick2 ;

  iTick1 = iGetBtimeTVal( NULL ) ;
  while( (iTick2 = iGetBtimeTVal( NULL ) ) == iTick1 ) {} ;
  
  do { DELAYLOOP(DELAY_TRIAL_LOOPS) ; iCount++ ; }
  while( ((iTick1 = iGetBtimeTVal( NULL )) - iTick2) < DELAY_MEASURE_MS ) ;

  return( (iCount * DELAY_TRIAL_LOOPS) / DELAY_MEASURE_MS ) ;
}

/* ------------------------------
 * Routine to sleep in such a way that we wakeup at regular intervals,
 * and those intervals do not drift over long periods of time.  The
 * fLastWakeupSec parameter keeps track of the (ideal) time that the
 * process last woke up, and can be used both to monitor and to
 * initialize the progress.  The parameter MUST be initialized prior
 * to the first call, but can be done creatively as follows:
 *
 *   To wakeup every second on the second, initialize fLastWakeupSec
 *   to 0.0 and call with an argument of 1000000.
 *
 *   To wakeup every second on the half second, initialize to 0.5 and
 *   call with an argument of 1000000.
 *
 * If you don't particularly care about the absolute position of your
 * regular time ticks, simply initialize to zero.
 */
void sig_periodicSleep( SINT4 iMicroSeconds_a, FLT8 *fLastWakeupSec_a )
{
  struct timeval tval ;
  if( gettimeofday( &tval, NULL ) < 0 ) {
    sig_signal( errno_to_message_va( errno, 1, "sig_periodicSleep()" ) ) ;
  } else {
    /* Convert current time to floating seconds, and verify that the
     * requested period is greater than 0ms.  The reason to convert to
     * miliseconds is to keep full resolution within the 48-bit FLT8
     * mantissa.
     */
    FLT8 fNowSec = ((FLT8)tval.tv_sec) + ( 1E-6 * (FLT8)tval.tv_usec ) ;
    SINT4 iMiliSeconds = ( (iMicroSeconds_a + 500) / 1000 ) ;
    if( iMiliSeconds > 0 ) {
      FLT8 fPeriodSec = ( 1E-3 * (FLT8)iMiliSeconds ) ;
      FLT8 fWaitSec = ( ( fPeriodSec + *fLastWakeupSec_a ) - fNowSec ) ;

      if( ( fWaitSec < (-0.5 * fPeriodSec) ) ||
	  ( fWaitSec > ( 1.5 * fPeriodSec) ) ) {
	/* We're either too late (and have missed our next scheduled
	 * return time), or way too early (more than a tick time still
	 * left to go).  Recalculate the wait so that we'll land on
	 * the next regular tick.  This is also the code that handles
	 * initial entry with a zero time, or any other bogus input.
	 */
	FLT8 fInts = ceil( ( fNowSec - *fLastWakeupSec_a ) / fPeriodSec ) ;
	*fLastWakeupSec_a += ( fInts * fPeriodSec ) ;
	sig_microSleep( NINT( 1E6 * ( *fLastWakeupSec_a - fNowSec ) ) ) ;
      } else {
	/* If all is well, simply sleep the expected amount of time
	 * and bump the wakeup in a regular manner.  This is the
	 * normal code that will not wander in the long term.
	 */
	*fLastWakeupSec_a += fPeriodSec ;
	sig_microSleep( NINT( 1E6 * fWaitSec ) ) ;
      }
    }
    /* Always leave the exit time quantized to the nearest milisecond.
     * This prevents FLT8 quantization from leading to long term drift
     * of the time slots.
     */
    *fLastWakeupSec_a = 1E-3 * floor( 0.5 + (1E3 * *fLastWakeupSec_a) ) ;
  }
}
