/*
 * periodic.c
 */

#include <signal.h>
#include <pthread.h>
#include <unistd.h>
#include <errno.h>
#include <limits.h>
#include <sys/time.h>
#include <asm/bitops.h>
#include <stdio.h>

#define APERIODIC		0
#define PERIODIC                1
#define TIMERARMED              2
#define STOPPED			3
#define BUSY			4
#define TASK_INIT		((0 << sizeof (unsigned char) * 8 ) | 1) 
#define THREADS_MAX		1024

// Maps the real_thread id into my thread id
// WARNING!! This is highly dependent on the particular
// thread implementation, and it should be changed. 
#define THREAD_ID(x)		(x / THREADS_MAX) - 1
#define USECS_PER_SEC 		1000000
#define NSECS_PER_USEC		1000

struct tt
{
  long long period;	/* task period */
  long long resume; 	/* task's resume time */	
};

typedef struct tt th_time;

static struct pp
{
  th_time timer;
  /* The task field indicates the type of task, which can be 
     APERIODIC / TIMERARMED  / PERIODIC / STOPPED / BUSY */
  unsigned char task;      
  pthread_cond_t cond;
  pthread_mutex_t mutex;
} periodic [THREADS_MAX] = 
  {[0 ... THREADS_MAX-1] = {{0, 0}, TASK_INIT, PTHREAD_COND_INITIALIZER, 
			    PTHREAD_MUTEX_INITIALIZER}};

void null_handler (int arg) { }


int start_rt(void)
{
  struct sigaction sa;

  //sigaddset(&wait_set, SIGALRM); 
  //sigaddset(&wait_set, SIGCONT);
  sa.sa_handler = null_handler;
  sigfillset(&sa.sa_mask);
  sa.sa_flags = 0;
  sigaction(SIGALRM, &sa, NULL);
  sigaction(SIGCONT, &sa, NULL);
  return 0;
}

struct timeval timeval_from_ns (long long t)
{
  struct timeval ts;
  __asm__("idivl %%ecx\n\t"
	  :"=a" (ts.tv_sec), "=d" (ts.tv_usec)
	  : "A" (t), "c" (USECS_PER_SEC)
	  );
  if (ts.tv_usec < 0) {
    ts.tv_usec += USECS_PER_SEC;
    ts.tv_sec --;
  }
  return ts;
}

long long timeval_to_ns (const struct timeval *ts)
{
  long long t;

  __asm__("imull %%edx\n"
	  "add %%ebx, %%eax\n\t"
	  "adc $0, %%edx\n\t"
	  :"=A" (t)
	  : "a" (ts->tv_sec), "d" (USECS_PER_SEC), "b" (ts->tv_usec)
	  );
  return t;
}

int pthread_make_periodic_np (pthread_t p, long long start_time, 
			      long long period)
{
  sigset_t wait_set;
  int sig;
  pthread_t tid = THREAD_ID(p);
  pthread_t my_tid = THREAD_ID (pthread_self());
  th_time *t = &periodic[tid].timer;
  unsigned char *task = &periodic[tid].task;
  unsigned char *my_task = &periodic[my_tid].task;
  pthread_cond_t *cond = &periodic[tid].cond;
  pthread_mutex_t *mutex = &periodic[tid].mutex;
	
  if (period < 0)
    return EINVAL;

  /* task busy in critical section can't be stopped */
  set_bit(BUSY, my_task);
  pthread_mutex_lock(mutex);
  t->period = period;		/* relative value */
  t->resume = start_time;	/* absolute value */
  pthread_mutex_unlock(mutex);
  clear_bit(BUSY, my_task);	/* end of critical section */
  set_bit(TIMERARMED, task);
  clear_bit(PERIODIC, task);
  pthread_cond_signal(cond);
  if (test_bit(STOPPED, my_task)) {  /* stop pending from critical region */
    sigaddset(&wait_set, SIGCONT);	/* task  must sleep til wakeup_np */ 
    sigwait(&wait_set, &sig);                      /* waiting for SIGCONT */
  }
  return 0;
}

/* 
 * SIGCONT and SIGALRM must be blocked on thread start due to the use
 * of sigwait .
 */ 
int pthread_wait_np (void)
{
  sigset_t wait_set;
  int sig;
  struct itimerval timer;
  struct timeval now;
  long long hrnow, period, resume;
  pthread_t tid = THREAD_ID(pthread_self());
  th_time *t = &periodic[tid].timer;
  unsigned char *task = &periodic[tid].task;
  pthread_cond_t *cond = &periodic[tid].cond;
  pthread_mutex_t	*mutex = &periodic[tid].mutex;
	
  if (!test_bit(PERIODIC, task)) {
    set_bit(BUSY, task); /* task busy in critical section cant'be stopped */
    pthread_mutex_lock(mutex);
    pthread_cond_wait(cond, mutex);
    pthread_mutex_unlock(mutex);
    set_bit(BUSY, task);	/* end of critical section */
  }
  if (test_and_clear_bit(TIMERARMED, task)) {                               
    set_bit(BUSY, task); /* task busy in critical section can't be stopped */
    pthread_mutex_lock(mutex);
    resume = t->resume;
    period = t->period;
    pthread_mutex_unlock(mutex);
    clear_bit(BUSY, task);	/* end of critical section */
    gettimeofday(&now, NULL);
    hrnow = timeval_to_ns(&now);
    if (period) {		/* periodic task */
      while (resume <= hrnow)	/* start time jitter */
	resume += period;	/* adjust resume time by period multiple */
      
      /* period set in timer */
      timer.it_interval = timeval_from_ns(period);
    }
    /* resume set in timer */
    if (resume <= hrnow)	/* aperiodic task must be awaked as 
				   soon as possible */
      timer.it_value = timeval_from_ns(1);   
    else
      timer.it_value = timeval_from_ns(resume - hrnow);	

    setitimer(ITIMER_REAL, &timer, NULL);
    if (period) {
      set_bit(PERIODIC, task);	/* periodic task flag */
    }
  }	
  sigaddset(&wait_set, SIGALRM);
  sigaddset(&wait_set, SIGCONT);
  if (test_bit(STOPPED, task))      /* stop pending from critical region */	
    sigdelset(&wait_set, SIGALRM);     /* task  must sleep til wakeup_np */ 
  
  sigwait(&wait_set, &sig);            /* waiting for SIGCONT or SIGALRM */
  return 0;
}

int pthread_suspend_np (pthread_t thread)
{
  pthread_t tid = THREAD_ID(thread);
  unsigned char *task = &periodic[tid].task;
	
  set_bit(STOPPED, task);
  if (!test_bit(BUSY, task))
    pthread_kill(thread, SIGSTOP);
  pthread_testcancel();
  return 0;
}

int pthread_wakeup_np (pthread_t thread)
{
  pthread_t tid = THREAD_ID(thread);
  pthread_cond_t *cond = &periodic[tid].cond;
  unsigned char *task = &periodic[tid].task;

  clear_bit(STOPPED, task);
  pthread_kill(thread, SIGCONT);
  pthread_cond_signal(cond);
  return 0;
}