#define _XOPEN_SOURCE 600
#define _XOPEN_SOURCE_EXTENDED
#define _POSIX_C_SOURCE 199506L
#define _REENTRANT
#include <stdlib.h>
#include <stdio.h>
#include <time.h>
#include <sys/time.h>
#include <errno.h>
#include <pthread.h>
#include <limits.h>
#include <string.h>

#define CHECK(CALL) if (CALL) { perror (#CALL); exit (-1); } 

/* This example is very sloppy about checking and recovering
   from failed system calls.  Essentially, we are assuming
   everything goes right.  One sloppy detail is that we are
   not distinguishing functions that return -1 for failure
   and set errno from functions that return the errno value
   for failure instead of setting errno.  Another sloppy
   detail is that with Linux threads exiting the process
   does not kill off all the threads; a bunch of threads
   may be left running.
 */

#define SECOND_AS_NS 1000000000
#define PERIOD_AS_NS 50000000
#define START_OFFSET_AS_NS 500000000
#define TEST_ITERATIONS 10
#define MIN_TICKS_TO_PERIOD 5

pthread_t thread;
struct timespec start_time;

pthread_mutex_t M;
pthread_cond_t C;

volatile int running = 0, bisecting = 1, missed_deadline = 0;
volatile struct timespec stop_time;
volatile int load_parm;

void
add_tl (volatile struct timespec *l, long r) {
    l->tv_nsec += r;
    if (l->tv_nsec > SECOND_AS_NS) {
        l->tv_nsec -= SECOND_AS_NS;
        l->tv_sec++;
    }    
}

void
subtract_tl (volatile struct timespec *l, long r) {
    l->tv_nsec -= r;
    if (l->tv_nsec < 0) {
        l->tv_nsec += SECOND_AS_NS;
        l->tv_sec--;
    }    
}

long
subtract_tt (struct timespec *l, struct timespec *r) {
    long t;
    t = l->tv_sec - r->tv_sec;
    t = t * SECOND_AS_NS + (l->tv_nsec - r->tv_nsec);
    return t;
}

int
less_tt (volatile struct timespec *l, volatile struct timespec *r) {
    if (l->tv_sec < r->tv_sec) return 1;
    if (l->tv_sec > r->tv_sec) return 0;
    if (l->tv_nsec < r->tv_nsec) return 1;
    return 0;
}



#define PRINTTIME(T) \
fprintf (stderr, #T "= %ld s %ld ns\n", (long) T.tv_sec, (long) T.tv_nsec)

#define PRINTNS(N) fprintf (stderr, #N "= %ld ns\n", (long) N)

#define TRACE(S) fprintf (stderr, S "\n")


void * start_routine(void *arg)
{
    struct timespec now, wakeup_time;
    long lateness;
    int i;

    pthread_mutex_lock(&M);
    while (bisecting) {
        while (bisecting && !running)
           CHECK(pthread_cond_wait (&C,&M));
        if (!bisecting) break;
        CHECK (pthread_mutex_unlock(&M));
        wakeup_time = start_time;
        running = 1;
        CHECK (pthread_mutex_lock(&M));
        CHECK (clock_gettime(CLOCK_REALTIME, &now));
        while (running) {
            pthread_cond_timedwait (&C,&M,&wakeup_time);
            pthread_mutex_unlock(&M);
            add_tl (&wakeup_time, PERIOD_AS_NS);
            if (less_tt (&stop_time, &wakeup_time)) break;
            for (i =1; i < load_parm; i++)
	      CHECK (clock_gettime(CLOCK_REALTIME, &now));
            lateness = subtract_tt(&now, &wakeup_time);
            if (lateness > 0) {
               missed_deadline = 1; break;
            }               
            CHECK (pthread_mutex_lock(&M));
        }
        if (missed_deadline)
            fprintf(stderr, "missed deadline by %ld ns\n", lateness);
        else fprintf(stderr, "no missed deadlines\n");
        running = 0;
        CHECK (pthread_cond_signal (&C));
    }
    running = 0;
    CHECK (pthread_cond_signal (&C));
    CHECK (pthread_mutex_unlock(&M));
    return 0;
}

int
schedulable () {
    int i;

    missed_deadline = 0;
    CHECK (clock_gettime (CLOCK_REALTIME, &start_time));
    add_tl (&start_time, START_OFFSET_AS_NS);
    stop_time = start_time;
    for (i = 0; i < TEST_ITERATIONS; i++) {
        add_tl (&stop_time, PERIOD_AS_NS);
    }
    CHECK (pthread_mutex_lock (&M));
    running = 1;
    CHECK (pthread_cond_signal (&C));
    while (running) CHECK (pthread_cond_wait (&C, &M));
    CHECK (pthread_mutex_unlock (&M));
    return ! missed_deadline;
}

#define MAXVAL 1000
#define NTICKS 100

void 
check_clock () {
  struct timespec t0, t1, t2;
  int val[MAXVAL];
  long d, d2;
  int i;
  long ticks = 0;

  memset (val, 0, sizeof (val));
  CHECK (clock_gettime (CLOCK_REALTIME, &t0));
  t1 = t0;
  do {
    for (i = 0; i<NTICKS; i++) {
      CHECK (clock_gettime (CLOCK_REALTIME, &t2));
      d = subtract_tt(&t2, &t1);
      if (d > MAXVAL) { d = MAXVAL-1; ticks++; }
      val[(int) d]++;
      t1 = t2;
    }
    d2 = subtract_tt (&t2, &t0);
  } while (d2 < SECOND_AS_NS);

  fprintf (stderr, "clock tick = %10ld nsec\n", SECOND_AS_NS / ticks);
  if (PERIOD_AS_NS < (SECOND_AS_NS / ticks)*MIN_TICKS_TO_PERIOD) {
     fprintf (stderr, "quitting: clock not accurate enough\n");
     /*
     fprintf (stderr, "elapsed time = %ld nsec\n", d2);
     fprintf (stderr, "clock ticks per second = %10ld\n", ticks);
     */
     fprintf(stderr, "ticks frequency\n----- --------\n");
     for (i = 0; i < MAXVAL; i++) {
       if (val[i]) fprintf (stderr, "%5d %5d\n", i, val[i]);
     }
     exit (-1);
  }
}

int 
main () {
    pthread_attr_t attr;
    int lower, upper;
    struct sched_param param;

    fprintf (stderr, "finding breakdown point for one periodic task\n");
    fprintf (stderr, "period = %d nsec\n", PERIOD_AS_NS);

    check_clock();

    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
    CHECK (pthread_attr_init (&attr));
    CHECK (pthread_attr_setschedpolicy (&attr, SCHED_FIFO));
    CHECK (pthread_attr_setschedparam (&attr, &param));
    CHECK (pthread_mutex_init (&M, NULL));  
    CHECK (pthread_cond_init (&C, NULL));  
    CHECK (pthread_create (&thread, NULL, start_routine, 0));

    bisecting = 1;
    lower = 1;
    fprintf(stderr, "lower = %d\n", upper);
    upper = lower;
    do {
        upper *= 16;
        fprintf(stderr, "upper = %d\n", upper);
        load_parm = upper;
    } while (schedulable() && (load_parm < INT_MAX));
    fprintf(stderr, "upper = %d\n", upper);
    do {
      load_parm = (upper+lower)/2;
      if (schedulable()) {
         lower = load_parm;
         fprintf(stderr, "lower = %d\n", lower);
      } else {
         upper = load_parm;
         fprintf(stderr, "upper = %d\n", upper);
      }
    } while (upper+lower < (upper-lower)*100);
    bisecting = 0;
    CHECK (pthread_cond_signal (&C));
    CHECK (pthread_join (thread, NULL));
    return 0;
}