/*
 * POSIX.4 Timers test program
 *
 * Written by J. Vidal
 * Copyright (C) Dec, 2002 OCERA Consortium.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License
 * published by the Free Software Foundation version 2.
 *
 * Name: test.c 
 * Description: This program test RTLinux POSIX.4 timers real-time constraints.
 *              Provides support for testing both CLOCK_REALTIME & CLOCK_MONOTONIC.
 *              Also for system clock on mode ONESHOT or PERIODIC deppending on the 
 *              defines.
 *
 * Procedure: 1.- Create a user-defined number of tasks. 
 *            2.- Set the system clock on mode ONESHOT or PERIODIC.
 *            3.- Schedule them, using RTLinux API or timers & signals.
 *            4.- Analyse test chronograms. 
 *
 *            5.- NOTE: You will observe that the scheduler result using the RTLinux API
 *                and timers + signals, is basically the same.
 *                Also you will observe that test chronogram for timers based on 
 *                CLOCK_MONOTONIC and system clock on mode periodic is not synchronous.
 *                This is OK, since in RTLinux CLOCK_MONOTONIC with system clock on mode 
 *                periodic is different from CLOCK_REALTIME (the one used by the scheduler
 *                and test chronogram)
 *
 * Author: J. Vidal <jvidal@disca.upv.es>
 *
 *
 */

#include <rtl.h>
#include <pthread.h>
#include <time.h>
#include <signal.h>

#define NTASKS 3
#define ONEMILISEC (long long)1000*1000
#define PERIODIC_WITH_TIMERS
//#define CLOCK_MODE_PERIODIC
#define MY_SIGNAL RTL_SIGUSR1
#define MY_CLOCK CLOCK_REALTIME
//#define MY_CLOCK CLOCK_MONOTONIC


pthread_t thread[NTASKS];
#ifdef  PERIODIC_WITH_TIMERS
timer_t timer[NTASKS];
#endif
long long start_time=0;

void waste_time(int param){
  int j,k,l,m=0;
  
  // Consume time

    for (j=0;j<(19-6*param);j++)
    for (k=0;k<50;k++)
    for (l=0;l<100*(NTASKS-param+1);l++)
    m=m+j-k+l;

}

#ifdef  PERIODIC_WITH_TIMERS
void timer_fun(int sig){
  int i,param=sig-MY_SIGNAL;
  static int count=0;
#ifdef TEST_DEBUG
  for (i=0;i<2;i++) rtl_printf("\n");
  rtl_printf("Timer handler called for timer:%d\n",timer[param]->id);
  rtl_printf("Timer period ns:%d\n",(unsigned int)timer[param]->expires.it_interval);
  rtl_printf("Timer signal:%d\n",timer[param]->signal.sigev_signo);
  rtl_printf("Count %d\n",count++);
  for (i=0;i<2;i++) rtl_printf("\n");
#endif
    waste_time(param);
}


void timer_intr(int sig){
  timer_fun(sig);
}
#endif

void *start_routine(void *arg)
{
  struct sched_param p;
#ifdef  PERIODIC_WITH_TIMERS
  struct itimerspec new_setting;
  struct sigaction sa;
  int signal=0;
  int err=0;
#endif

  int i;

  long period=0;
  int param=(unsigned) arg;
  
  p . sched_priority =param;
  pthread_setschedparam (pthread_self(), SCHED_FIFO, &p);
  
  if (param!=(NTASKS-1))
    period=(long)10*ONEMILISEC + 10*((long)param*ONEMILISEC) + param*ONEMILISEC;
  else
    period=(long)5*ONEMILISEC+(long)1000*param;
  
  
#ifdef  PERIODIC_WITH_TIMERS
  sa.sa_handler=timer_intr;
  sa.sa_mask=0;
  sa.sa_flags=0;
  
  if ((signal=MY_SIGNAL+param)> RTL_MAX_SIGNAL) signal=RTL_MAX_SIGNAL;

  if ((err=sigaction(signal,&sa,NULL))<0 ){
    rtl_printf("sigaction(MY_SIGNAL,&sa,NULL) FAILING, err:%d.\n",err);
    pthread_exit(NULL);
  }
  
  new_setting.it_interval.tv_sec=0;
  new_setting.it_interval.tv_nsec=period;
  timespec_normalize(&new_setting.it_interval);
  new_setting.it_value.tv_sec=0;
  new_setting.it_value.tv_nsec=start_time+period;
  timespec_normalize(&new_setting.it_value);
  
  err=timer_settime(timer[param],0,&new_setting,NULL);
  rtl_printf("timer_settime(timer[%d],0,&new_setting,&old_setting) returns %d\n",param,err);
#else
  pthread_make_periodic_np (pthread_self(), start_time+period ,period );
  // rtl_printf("Periodic thread starts at %d\n",(int)gethrtime());
  while (1) {    
#ifdef TEST_DEBUG
    for (i=0;i<2;i++) rtl_printf("\n");
    rtl_printf("I am a periodic thread number:%d. \n",param);
    for (i=0;i<2;i++) rtl_printf("\n");
#endif
    waste_time(param);
    pthread_wait_np();
	
  }
#endif
#ifdef  PERIODIC_WITH_TIMERS
    pthread_suspend_np(pthread_self());
#endif 
    
    pthread_exit(NULL);
    return 0;
}

int init_module(void) {
  int i,ret;
#ifdef  PERIODIC_WITH_TIMERS  
  sigevent_t signal; 
#endif
#ifdef CLOCK_MODE_PERIODIC
  hrtime_t period=100*1000; // 100 microseconds.
  clockid_t cl=rtl_getschedclock();
  rtl_setclockmode(cl,RTL_CLOCK_MODE_PERIODIC,period); 
#endif

#ifdef  PERIODIC_WITH_TIMERS    
  for (i=0;i<NTASKS;i++){    
    signal.sigev_notify=SIGEV_SIGNAL;
    if ((signal.sigev_signo=MY_SIGNAL+i)>=RTL_MAX_SIGNAL) 
      signal.sigev_signo=RTL_MAX_SIGNAL-1;
    ret=timer_create(MY_CLOCK,&signal,&(timer[i]));
    
    if (!ret) rtl_printf("Timer %d created succesfully\n",i);
    else {
      rtl_printf("Error creating timer %d. errno=%d\n",i,errno);
      return -1;
    }
  }  
  
#endif
    
  start_time=100*ONEMILISEC;

  // Threads creation.
  for (i=0;i<NTASKS;i++)
    pthread_create (&(thread[i]), NULL, start_routine,(void *) i); 
  
  return 0;
}

void cleanup_module(void) {
  int i;
  
#ifdef  PERIODIC_WITH_TIMERS
  for (i=0;i<NTASKS;i++)
    timer_delete(timer[i]);
#endif
  
  for (i=0;i<NTASKS;i++)
    pthread_delete_np (thread[i]);
    
}