/*
 * 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: accuracity.c
 * Description: Measures timer accuracity for both relative 
 * and absolute timer specifications.
 *
 * Procedure: 1.- Create a thread.
 *            2.- Program a periodic timer with some period (relative spec) or 
 *            a one-shot timer for absolute specification. 
 *            3.- Let expire the timer n times. If it is absolute, reprogram each time
 *            the timer by adding the period to the absolute time specification.
 *            4.- After n expirations, calculate the teoric time transcurred (n*period)
 *            and the real (end_time -start_time).
 *            5.- Print the error (difference between the teoric time transcurred 
 *            and the real one)
 *
 */

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

pthread_t th;
timer_t timer;

#define TEST_CLOCK CLOCK_REALTIME
struct itimerspec interval;
struct timespec currenttime;
int absflag=1;
int tflags;
int exitflag=0;
int numtimes=1000;
hrtime_t spintime= 10*1000*1000;
hrtime_t inctime = 20*1000*1000;
hrtime_t starttime=0;
hrtime_t endtime=0;
hrtime_t total_time=0;
hrtime_t calctime=0;

/* spinit executes during ntime nanoseconds a loop before returning.*/ 
static int spinit(hrtime_t ntime){
  hrtime_t timenow,timeend;

  if (spintime == 0)
    return 0;
  if ((timenow=clock_gethrtime(CLOCK_REALTIME))<0){

    return -1;
  }
  
  timeend=timenow+ntime;
  while (timenow<timeend)
    if ((timenow=clock_gethrtime(CLOCK_REALTIME))<0){
      rtl_printf("clock_gethrtime fails in handler\n");
      return -1;
    }

  return 0;
}

/* handlers */
static void alarm_handler(int signo){  
  static hrtime_t timesentered=0;
  
  timesentered++;
  
  if (timesentered < numtimes){
    if (inctime < 0 )
      return;
    if (spinit(spintime) == -1){
      rtl_printf("Spin failed in alarm_handler\n");
      pthread_exit(NULL);
    }
    
    if (absflag){
      timespec_add_ns(&interval.it_value,inctime);
      if (timer_settime(timer,tflags,&interval,NULL) < 0){
	rtl_printf("Error. Could not start timer n handler\n");
	pthread_exit(NULL);
      }
    }  
  } else {
    exitflag=1;
  }
}

void *test_routine(void *arg){
  struct sigaction sa;
  rtl_sigset_t sigset;
  struct sched_param p;
  struct timespec res;

  int param=(unsigned) arg;
  
  p . sched_priority =param;
  pthread_setschedparam (pthread_self(), SCHED_FIFO, &p);

  rtl_sigfillset(&sigset);
  rtl_sigdelset(&sigset,RTL_SIGUSR1);
  pthread_sigmask(SIG_SETMASK,&sigset,NULL);
  
  sa.sa_handler=alarm_handler;
  
  if (sigaction(RTL_SIGUSR1,&sa,NULL)<0){
    rtl_printf("Error in sigaction(RTL_SIGUSR2,..);\n");
    return (void *)-1;
  }
  
  if (clock_getres(CLOCK_REALTIME, &res)){
    rtl_printf("Error. Can not get clock resolution");
    pthread_exit(NULL);
  } else {
    rtl_printf("Clock resolution is %d nanoseconds\n",(int)timespec_to_ns(&res));
  }
  
  interval.it_interval.tv_sec=0;
  interval.it_interval.tv_nsec=0;

  if (clock_gettime(CLOCK_REALTIME,&currenttime)<0){
    rtl_printf("Error. Can not get current time");
    pthread_exit(NULL);
  }

  starttime= timespec_to_ns(&currenttime);
  if (absflag){
    rtl_printf("abs time: interrupts: %d at ",numtimes); 
    rtl_printf("%d nsecs ",(int)inctime);
    rtl_printf(", spinning %d nsecs\n",(int)spintime);
    tflags= TIMER_ABSTIME;
    interval.it_value=currenttime;
  } else {
    rtl_printf("rel time: interrupts: %d at ",numtimes);
    rtl_printf("%d nsecs ",(int)inctime);
    rtl_printf(", spinning %d nsecs\n",(int)spintime);
    tflags=0;
    interval.it_value.tv_sec=0;
    interval.it_value.tv_nsec=1;
    interval.it_interval.tv_sec=0;
    interval.it_interval.tv_nsec=(long)inctime;
    //    timespec_normalize(&interval.it_interval);
    
 }

  timespec_add_ns(&interval.it_value,inctime);
  if (timer_settime(timer,tflags,&interval,NULL)<0){
    rtl_printf("Error. Could not start timer\n");
    pthread_exit(NULL);
  }

  for ( ; ; ) {
    sigsuspend(&sigset);
    if (exitflag){
      if (clock_gettime(CLOCK_REALTIME,&currenttime) < 0){
	rtl_printf("Error. Can no get current time");
	pthread_exit(NULL);
      }
      endtime= timespec_to_ns(&currenttime);
      total_time=endtime -starttime;
      calctime= numtimes*inctime;
      //      rtl_printf("calculated time: %\n",(unsigned long)calctime);
      rtl_printf("error:%ld\n",(long)total_time-calctime);
      pthread_exit(NULL);
    }
  }
  
  return (void *)0;

}


int init_module(void){
  sigevent_t evp; 
  int ret=0;

  evp.sigev_notify=SIGEV_SIGNAL;
  evp.sigev_signo=RTL_SIGUSR1;
  ret=timer_create(TEST_CLOCK,&evp,&(timer));
  
  if (!ret) rtl_printf("Timer created succesfully\n");
  else {
    rtl_printf("Error creating timer. errno=%d\n",errno);
    return -1;
  }
  
  pthread_create(&th,NULL,test_routine,(int *) 1);
  
  return ret;
}

void cleanup_module(void) {
  pthread_delete_np (th);
  timer_delete(timer);
}