// 2008 Kasper Pedersen


//Atom 230 1.6GHz, Realtek 100Mbit:  18900 /s
//Atom N270 1.6GHz,  Intel Pro1000:   19700 /s
//AVR32 65MHz, onchip MAC: 9300 /s  (50% the performance at 4% clock speed of the atom, and at 2% the power)
//Core2Q 2.4GHz, Marvell, chronyd: 21600 /s 
//Core2Q 2.4GHz, Marvell, ntpd: 39200 /s (ntpd 64% cpu)





LOOK HERE! CHANGE THIS!
#define TARGET "10.5.5.8"









#define _GNU_SOURCE 1

#include <stdlib.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <unistd.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 <sched.h>
#include <fcntl.h>
#include <string.h>
#include <stdio.h>
#include <errno.h>

#include <syslog.h>
#include <sys/poll.h>
#include <linux/ppdev.h>
#include <linux/parport.h>

#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>

#include <getopt.h>
#include <math.h>


#pragma pack(push)
#pragma pack(1)
typedef struct { unsigned h,l; } fix64;
typedef struct {
  unsigned char li_vn_mode;	/* peer leap indicator */
  unsigned char stratum;	/* peer stratum */
  unsigned char	ppoll;		/* peer poll interval */
  unsigned char	precision;	/* peer clock precision */
  unsigned rootdelay, rootdisp, refid;
  fix64 reftime,org,rec,xmt;
} Tsntp;
  Tsntp txpk;
  Tsntp rxpk;
#pragma pack(pop)

unsigned long long rd64(fix64 *fix)
{
  unsigned long long t;
  t=ntohl(fix->h);
  t<<=32;
  t|=ntohl(fix->l);
  return t;
}

unsigned long long wr64(fix64 *fix, unsigned long long t)
{
  fix->l=htonl(t);
  fix->h=htonl(t>>32);
}


double frd64(fix64 *fix)
{
  double t;
  unsigned long h;
  t=ntohl(fix->l);
  t*=(1.0/(65536.0*65536.0));
  t+=h=ntohl(fix->h);
  if (h<3463661540UL) t+=4294967296.0;
  return t;
}




int waitsok(int sok, int seks)
{
  struct timeval stTimeOut;
  fd_set stReadFDS;
  FD_ZERO(&stReadFDS);

  // Timeout 3 sec
  stTimeOut.tv_sec = seks;
  stTimeOut.tv_usec = 0;
  FD_SET(sok, &stReadFDS);
  return select(sok+1, &stReadFDS, 0, 0, &stTimeOut);
}





  int sockfd,n;
  struct sockaddr_in servaddr,cliaddr;
  unsigned sendbuf[10];
  unsigned recvbuf[10];
  int dt;
          


struct timeval ttv,tv,rtv;
struct timezone tz;





extern int errno;

int main(argc, argv)
     int argc;
     char *argv[];
{
  int i;
  int sp_fd = 0, pp_fd = 0, n_fd=0, zoo_poll = 0, hitolow = 0, flap = 0;
  int deviceis_serial = 0,  deviceis_parallel = 0, deviceis_nic=0, bogon = 0;
  int unit = 0;
  unsigned sec_last,hz;

  int locked=0;
  unsigned int pp_nfds = 1;

  struct pollfd pp_pollfd[2];
  struct ppdev_frob_struct frobit;
  struct sched_param sp;
  struct rlimit rl;

  char *prog;
  char *pps_dev[256];
  char *pps_sig[256];		/* name of sig line: CD RTS DSR */
  int sig = TIOCM_CD;

  double T1,T2,T3,T4,offset,delay,delaymaxpeak=0,myt,remt,f1,f2,maxpeakoffset=0;
  unsigned bumpdown_power=0;
  unsigned bumpdown_rmt=0;

// printf("%i %i %i %i\n",sizeof(unsigned),sizeof(unsigned long),sizeof(txpk),sizeof(fix64));
  prog = argv[0];
  
  
  sockfd=socket(AF_INET,SOCK_DGRAM,0);
  bzero(&servaddr,sizeof(servaddr));
  servaddr.sin_family = AF_INET;

  gettimeofday(&ttv,&tz);
  sec_last=ttv.tv_sec;

  hz=0;
  for(;;) {
    gettimeofday(&ttv,&tz);
    if (sec_last!=ttv.tv_sec) {
      sec_last=ttv.tv_sec;
      printf("%u %u %f %f \n",sec_last,hz,delaymaxpeak, maxpeakoffset);
      hz=0;
      delaymaxpeak=0;
      maxpeakoffset=0;
    }
    ++hz;
  

abortntp:
    //////////////////////// ntp tracker /////////////////////////////////////////////
    txpk.li_vn_mode=0x23;
    txpk.stratum=0;
    txpk.ppoll=6;
    txpk.precision=0;
    
    txpk.rootdelay=0;
    txpk.rootdisp=0;
    txpk.refid=0;

    gettimeofday(&ttv,&tz);

    wr64(&txpk.reftime,0);
    wr64(&txpk.org,0);
    wr64(&txpk.rec,0);
    wr64(&txpk.xmt,ttv.tv_sec);
    
    servaddr.sin_port=htons(123);
    servaddr.sin_addr.s_addr=inet_addr(TARGET);
    gettimeofday(&ttv,&tz);
    sendto(sockfd,(void*)&txpk,sizeof(txpk),0,(struct sockaddr *)&servaddr,sizeof(servaddr));
rerx3:
    if (waitsok(sockfd,1)) {
      n=recvfrom(sockfd,(void*)&rxpk,sizeof(rxpk),0,NULL,NULL);
      gettimeofday(&rtv,&tz);
      if (n!=sizeof(rxpk)) goto rerx3;
      if (rd64(&rxpk.org)!=rd64(&txpk.xmt)) goto rerx3;
      if (rxpk.stratum==0) {
        printf("KoD\n");
        bumpdown_rmt=86400;
        goto abortntp;
      }
      T1=frd64(&rxpk.org); //copied from xmt timestamp
      T2=frd64(&rxpk.rec);
      T3=frd64(&rxpk.xmt);
      T4=frd64(&rxpk.reftime); //setto 0, or the last sync event time. 0=bad
       
      T1=ttv.tv_sec+0.000001*ttv.tv_usec + 2208988800.0;
      T4=rtv.tv_sec+0.000001*rtv.tv_usec + 2208988800.0;
      
      delay=(T4-T3)+(T2-T1);
      offset=((T2+T3)-(T1+T4))*0.5;
      if (delay>delaymaxpeak) delaymaxpeak=delay;
      
      myt=(T1+T4)*0.5 - 2208988800.0;
      remt=(T2+T3)*0.5 - 2208988800.0;
      
      myt-=remt;
      if (myt<0) myt=-myt;
      if (myt>maxpeakoffset) maxpeakoffset=myt;
      

      
      /*
      bumpdown_rmt=13;*/
    }
  }
  
  
  
  
  
}