/******************************************************************************
*                                                                             *
*   Copyright 2005 University of Cambridge Computer Laboratory.               *
*                                                                             *
*   This file is part of Nprobe.                                              *
*                                                                             *
*   Nprobe is free software; you can redistribute it and/or modify            *
*   it under the terms of the GNU General Public License as published by      *
*   the Free Software Foundation; either version 2 of the License, or         *
*   (at your option) any later version.                                       *
*                                                                             *
*   Nprobe is distributed in the hope that it will be useful,                 *
*   but WITHOUT ANY WARRANTY; without even the implied warranty of            *
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
*   GNU General Public License for more details.                              *
*                                                                             *
*   You should have received a copy of the GNU General Public License         *
*   along with Nprobe; if not, write to the Free Software                     *
*   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA *
*                                                                             *
******************************************************************************/


#include <stdio.h>
#include <stdlib.h>
#include <stdarg.h>

#include <sys/types.h>
#include <sys/wait.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/errno.h>
#include <sys/socket.h>
#include <fcntl.h>
#include <string.h>
#include <sys/ioctl.h>
#include <net/if.h>
#include <netinet/in.h>
#include <unistd.h>
#include <signal.h>
#include <sys/time.h>

#include "ace_clk.h"


static acenic_clk_t ace_clk;
static unsigned int ace_cal_period = 0;


static int acenic_init_tstamp(int chan);
static int acenic_tstamp_synch();
static int acenic_calibrate(unsigned long long us);
static unsigned int *acenic_ioctl( char * devname );

//#define ACE_TRACE


static unsigned int *
acenic_ioctl( char * devname )
{
  unsigned int *p;
  int sock;
  struct ifreq ifr;

  /* Open a socket for our ioctls */
  sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);

  /* Set up basic ioctl structure */
  ifr.ifr_data = (void*) &p;
  strcpy(ifr.ifr_name, devname);

  if (ioctl(sock, SIOCDEVPRIVATE+0x2, &ifr))
    {
      printf("Failed ioctl on %s\n", devname);
      perror("ioctl failed:");
      return NULL;
    }

  p = (unsigned int *) *((unsigned long**)ifr.ifr_data);


  printf("Did ioctl on %s, got %p\n", devname, p);

  return p;
}


inline long acenic_getcurtime()
{
  return * ace_clk.tstampp;
}


static int acenic_calibrate( unsigned long long us)
{
  unsigned long long tot_nictm, tot_sysclocktm;
  unsigned long long ts_base_hold, clk_base_hold;
  int drift;
  
  double tott, totc;
  double freq;
  long adj;
  
  ts_base_hold = ace_clk.ts_base;
  clk_base_hold = ace_clk.sysclock_base;
  
  acenic_tstamp_synch();
  adj = (long)(ace_clk.ts_base - us);

  /* Calculate frequency and drift this period */
  tot_nictm = ace_clk.ts_base - ts_base_hold;
  tot_sysclocktm = ace_clk.sysclock_base - clk_base_hold;
  tott = (double)tot_nictm;
  totc = (double)tot_sysclocktm;
  freq = (double)tott/totc;
  tot_nictm = (unsigned int long)(tot_nictm/ace_clk.freq);
  drift = (int)(tot_nictm - tot_sysclocktm);

  
  
  fprintf(stderr, "Freq this period %fMHz drift %dus in %llu.%.6llus\n", freq, 
	  drift, tot_nictm/1000000, tot_nictm%1000000);
  
  /* Now the ace clock frequency o/a */
  tot_nictm = ace_clk.ts_base - ace_clk.ts_start;
  tot_sysclocktm = ace_clk.sysclock_base - ace_clk.sysclock_start;
  tott = (double)tot_nictm;
  totc = (double)tot_sysclocktm;
  freq = (double)tott/totc;
  fprintf(stderr, "O/A freq = %fMHz\n", freq);
  tot_nictm = (unsigned int long)(tot_nictm/freq);
  
#ifdef ACE_TRACE
  
  fprintf(stderr, "Tot. run time %llu.%.6u\n", 
	  tot_sysclocktm/1000000, tot_sysclocktm%1000000);
  fprintf(stderr, "Tot. ace time %llu.%.6u\n", 
	  tot_nictm/1000000, tot_nictm%1000000);
  
#endif  
  
  ace_cal_period = ACE_CAL_PER;

  ace_clk.ts_base = us;
  ace_clk.sysclock_base -= adj;
  ace_clk.freq = freq;

  return 0;
  
}
  

inline unsigned long long 
acenic_maptotimeofday( unsigned int ace_tstamp)
{

  
  unsigned long long us, off, tm;
  

  if (ace_tstamp < ace_clk.last_tstamp)
    {
      ace_clk.wrap += 0x100000000;
      //fprintf (stderr, "Clock wrap\n");
    }
  

  ace_clk.last_tstamp = ace_tstamp;

  us = ace_clk.wrap + ace_tstamp;

  if ((int)(us - ace_clk.ts_base) > ace_cal_period)
    {
      acenic_calibrate(us);
    }
  

  off = (unsigned int long)(((int)(us-ace_clk.ts_base))/ace_clk.freq);
  tm = ace_clk.sysclock_base + off;

  if (tm < ace_clk.last_us)
    fprintf(stderr, "XXX Clock backwards %dus\n", (int)(ace_clk.last_us - tm));
  ace_clk.last_us = tm;

  return  tm;

}

static int acenic_tstamp_synch()
{
#define N 20
  unsigned int t, uset;
  long d;
  int j;
  int mind = 1000000;
  struct timeval tv, usetv;

  for(j=0;j<N;j++)
    { // have a few goes at this
      //fprintf(stderr, "rep=%d\n", reps);
      t = acenic_getcurtime();
      gettimeofday(&tv, (struct timezone *)0);
      d = acenic_getcurtime() - t;
      if (d == 0L)
	{
	  //fprintf(stderr, "0 delta rep=%d\n", j);
	  mind = d;
	  uset = t;
	  usetv = tv;
	  j += 1;
	  break;
	}
      else if (d < mind)
	{
	  mind = d;
	  uset = t;
	  usetv = tv;
	}
      
    }
  
  if ( mind > 10 )
    {
      fprintf(stderr,"Acenic %d: Failed to get accurate sync (%d) !!!\n",
	      ace_clk.chan, mind);
      exit(-1);
    }	   	      
      
  else if( mind < 0 )
    {
      fprintf(stderr,"Acenic %d: d is %d !!!\n",ace_clk.chan,mind);
      exit(-1);
    }
  
  ace_clk.ts_base = uset + ace_clk.wrap;
  ace_clk.sysclock_base   = (((unsigned int long)usetv.tv_sec)*1000000)
                       + usetv.tv_usec;
#ifdef ACE_TRACE 
  fprintf(stderr,"Acenic %d synch: base is %llu, systime %llu.%.6llu, delta %ld reps %d\n",
	  ace_clk.chan, ace_clk.ts_base,
	  ace_clk.sysclock_base/1000000,
	  ace_clk.sysclock_base%1000000, mind, j);
#endif
  
  return 0;
  
}

	

static int acenic_init_tstamp(int chan)
{
  char eth[][6]={"eth1","eth2","eth3","eth4"};

  ace_clk.chan = chan;
  ace_clk.tstampp = acenic_ioctl(eth[chan]);
  if( ace_clk.tstampp == NULL )
    {
      fprintf(stderr,"Channel %d: Failed to get tstamp addr for acenic %s!!!\n", chan, eth[chan]);
      exit(-1);
    }
  
  //fprintf(stderr, "Acenic %d tstamp addr %p\n",chan,ace_clk.tstampp);
  *ace_clk.tstampp;
  //fprintf(stderr, "MAdeit!!\n");


  
  return 0;
}

int acenic_start_clock(int chan)
{
  acenic_init_tstamp(chan);
  ace_clk.wrap = ace_clk.last_us = 0ULL;
  acenic_tstamp_synch();
  ace_clk.last_tstamp = ace_clk.ts_start = ace_clk.ts_base;
  ace_clk.sysclock_start = ace_clk.sysclock_base;
  fprintf(stderr, "Acenic %d clock started\n", chan);
  return 0;
  
}


