#include <sys/types.h>
#include <sys/ipc.h>
#include <sys/shm.h>
#include <sys/time.h>
#include <sys/resource.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <asm/termios.h>
#include <asm/ioctls.h>
#include <sched.h>
#include <fcntl.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>

/* SHM driver to allow PPS time sources to work without a PPS kernel */
/* The PPS pin must be connected to the DCD pin on a serial port     */
/* This is Linux-specific code                                       */

/* By: David J. Schwartz  */
/* <davids@webmaster.com> */
/* Version 1.03           */

/* Put into the public domain. Please keep the acknowledgement above  */
/* intact. Portions taken from the NTP source (the time_shm structure */
/* and attach_shm code) are covered by the NTP copyright/license      */

/* Compile with '-O2 -Os -static -march=[your_arch]' to minimize      */
/* memory footprint and maximize accuracy.                            */

/* Notes:                                                               */
/* 1) You should have a line like the following in your /etc/ntpd.conf  */
/*    server 127.127.28.0                                               */
/* 2) Make sure no 'restrict' statement in your conf file prevents      */
/*    you from trusting the clock. If in doubt, add:                    */
/*    restrict 127.127.28.0                                             */
/* 3) You must configure NTPD with the SHM clock driver                 */

/* TODO:                                                                */
/* 1) Average multiple PPS readings together, filtering out those that  */
/*    appear to be 'late'.                                              */
/* 2) Have two states, synchronized and unsynchronized. Start in the    */
/*    unsynchronized state. If synchronized, go unsynchronized if most  */
/*    readings exceed the threshold. If unsynchronized, go synchronized */
/*    if 'sysinfo' indicates a reasonable stratum and root distance.    */

#define DEBUG

#define DAEMON

#define SERIAL_PORT "/dev/ttyS0"

/* Accept a timestamp if it's within this many microseconds of the       */
/* system's second boundary -- do not exceed 250000                      */
/* Setting this low helps avoid false lock, but requires a more accurate */
/* starting time synchronization. Setting this high makes capture easier */
/* but risks false lock if the clock starts too far off                  */
#define TOLERANCE 40000

/* Assume it takes this long for us to get scheduled and get the time */
/* (in microseconds) */
#define ASSUME_DELAY 0

/* Set this if a high-to-low transition marks the second boundary */
/* Normally, a low-to-high transition marks the second boundary   */
/* #define HIGH_TO_LOW */

/* Accuracy is assumed to be 2^PRECISION seconds -11 is approximately 490uS */
/* This is just an initial estimate, it will be adjusted                    */
#define PRECISION (-11)

/* Define this if you want the baud rate set to anything in particular */
#define SET_BAUD B4800

struct time_shm
{
 int shm_mode;
 int stamp_count;
 time_t clock_sec;
 int clock_usec;
 time_t receive_sec;
 int receive_usec;
 int leap_indicator;
 int precision;
 int number_of_samples;
 int is_valid;
 int place_holder[10];
};

struct time_shm *sh;

struct time_shm *attach_shm(int unit)
{
 int shm_id=0;
 shm_id=shmget(0x4e545030+unit, sizeof(struct time_shm),
  IPC_CREAT|0700);
 if(shm_id==-1) exit(0);
 sh=(struct time_shm *) shmat(shm_id, 0, 0);
 if(sh==(void *) -1 || sh==0) exit(0);
#ifdef DEBUG
 fprintf(stderr, "shmat ok\n");
#endif
 return sh;
}

/* This is our current estimate of precision. Our microseconds estimated */
/* error is 1000000*2^(precision_est/32)                                 */
int precision_est=(PRECISION*32);

int update_offset(int offset)
{
 static int last_reading=0;
 static int average_offset=0;
 int target_precision;

 /* How far is this reading from the last one? */
 int diff=(last_reading>offset) ? (last_reading-offset)
                                : (offset-last_reading);

 /* On average, how much are these readings varying?  */
 /* In units of 16 microseconds, exponential average  */
 average_offset=(average_offset*15/16)+diff;

 last_reading=offset;
#ifdef DEBUG
 fprintf(stderr,
  "diff=%d\toff=%d\tavgdiff=%0.1f\n", diff, offset, average_offset/16.0);
#endif

 /* Adjust our precision estimate based upon how well our readings are */
 /* agreeing with each other. This will give low values when our       */
 /* system clock's rate is off or due to system jitter, but that's     */
 /* actually reasonable.                                               */
 /* The magic constants are based on:                                  */
 /* average_offset/16/1000000=2^(target_precision/32)                  */
 /* Or, to put it another way, our estimated precision is equal to our */
 /* average offset of sequential readings after units are converted    */
 /* average_offset is in units of 16ths of a microsecond               */
 /* target_precision is in units of 32nds of a power of 2              */

 if(average_offset<61) target_precision=-18*32;
 else if(average_offset<122) target_precision=-17*32;
 else if(average_offset<244) target_precision=-16*32;
 else if(average_offset<488) target_precision=-15*32;
 else if(average_offset<976) target_precision=-14*32;
 else if(average_offset<1953) target_precision=-13*32;
 else if(average_offset<3906) target_precision=-12*32;
 else if(average_offset<7812) target_precision=-11*32;
 else if(average_offset<15625) target_precision=-10*32;
 else if(average_offset<31250) target_precision=-9*32;
 else target_precision=-8*32;
 
 if(precision_est<target_precision)
  precision_est+=4; /* lose precision fast */
 if(precision_est>target_precision)
  precision_est--; /* gain precision slow */
#ifdef DEBUG
 fprintf(stderr, "targ_prec=%.1f precision=%.1f\n",
  target_precision/32.0, precision_est/32.0);
#endif

 return average_offset;
}

void PutStamp(time_t cloc_sec, int cloc_usec, time_t sys_sec, int sys_usec)
{
 sh->shm_mode=1;
 sh->leap_indicator=0;
 sh->is_valid=0; /* this is perhaps overly paranoid */
 sh->stamp_count++;

 __asm__ __volatile__("": : :"memory");

 sh->precision=precision_est/32;
 sh->clock_sec=cloc_sec;
 sh->clock_usec=cloc_usec;
 sh->receive_sec=sys_sec;
 sh->receive_usec=sys_usec;

 __asm__ __volatile__("": : :"memory");

 sh->stamp_count++;
 sh->is_valid=1;
#ifdef DEBUG
 fprintf(stderr, "sys:%d/%d\tref:%d/%d\n", (int) sys_sec, (int) sys_usec,
  (int) cloc_sec, (int) cloc_usec);
#endif
}

int main(void)
{
 struct timeval tv;
 struct timezone tz;
#ifdef SET_BAUD
 struct termios t;
#endif
 int f, fd;
 struct sched_param sp;
 
#ifdef DAEMON
 if(fork()!=0) return;
 setsid();
 setpgrp();
 setsid();
#endif
 
 mlockall(MCL_FUTURE);
 setpriority(PRIO_PROCESS, getpid(), -20);
 memset(&sp, 0, sizeof(sp));
 sp.sched_priority=sched_get_priority_max(SCHED_FIFO);
 if(sched_setscheduler(0, SCHED_FIFO, &sp)!=0)
 {
#ifdef DEBUG
  fprintf(stderr, "Unable to set RR scheduling\n");
#endif
 }
 
 attach_shm(0);
 
 fd=open(SERIAL_PORT, O_RDWR);
 if(fd<0) return -1;

#ifdef SET_BAUD
 tcgetattr(fd, &t);
 cfsetospeed(&t, SET_BAUD);
 cfsetispeed(&t, SET_BAUD); 
 tcsetattr(fd, 0, &t);
#endif

 while(1)
 {
  while(ioctl(fd, TIOCMIWAIT, TIOCM_CAR)!=0); /* wait for transition */
  gettimeofday(&tv, &tz); /* grab timestamp */
  ioctl(fd, TIOCMGET, &f); /* check polarity */
#ifndef HIGH_TO_LOW
  if((f&TIOCM_CAR)!=0)
#else
  if((f&TIOCM_CAR)==0)
#endif
  { /* correct transition */
   if(tv.tv_usec>(1000000-TOLERANCE))
   {
    update_offset(tv.tv_usec-1000000);
    PutStamp(tv.tv_sec+1, ASSUME_DELAY, tv.tv_sec, tv.tv_usec);
   }
   else if(tv.tv_usec<TOLERANCE)
   {
    update_offset(tv.tv_usec);
    PutStamp(tv.tv_sec, ASSUME_DELAY, tv.tv_sec, tv.tv_usec);
   }
   else
   {
#ifdef DEBUG
    fprintf(stderr, "out of range\n");
#endif
   }
  }
 }
}