/******************************************************************************
*                                                                             *
*   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 "skb.h"

/* Configuration parameters ******************************************/

// now in config.h

#include "probe.h"


/* Constants **********************************************************/


#define KERNEL_VA   (0xC0000000UL) 
#define MFN "/dev/mem"
#define PROC_FILE "/proc/nprobe"

struct timeval tsleep    = { 0, 10000 };       // 10ms XXXXXXXXXXXXX!!!!
struct timeval timeout1s = { 1, 0 };           // 1s

/* Statics that are effectively constant after fork *******************/

unsigned long np_off = 0;
np_t * np            = NULL;
int chan             = -1;

/* Statics that are thread specific ***********************************/

char * km = NULL;

/* Prototypes *********************************************************/

void worker_thread( void );


/**********************************************************************/


#ifdef ATM

#include <atm.h>

int atm_open_vc( char *vc )
{
  struct sockaddr_atmpvc addr;
  struct atm_qos qos;
  int s;
  int on = 1;   /*  1 = non blocking  */

  if ((s = socket(PF_ATMPVC,SOCK_DGRAM,0)) < 0) {
    perror("socket");
    return -1;
  }

  memset(&addr,0,sizeof(addr));
  if (text2atm(vc,(struct sockaddr *) &addr,sizeof(addr),
	       T2A_PVC | T2A_UNSPEC | T2A_WILDCARD) < 0) 
    fprintf(stderr,"Incorrect use of open_vc: %s\n",vc);

  memset(&qos,0,sizeof(qos));
  qos.aal = ATM_AAL5;
  qos.rxtp.traffic_class = ATM_UBR;
  qos.rxtp.max_sdu = 1024;

  if (setsockopt(s,SOL_ATM,SO_ATMQOS,&qos,sizeof(qos)) < 0) {
    perror("setsockopt SO_ATMQOS");
    return -1;
  }

  if (bind(s,(struct sockaddr *) &addr,sizeof(addr)) < 0) {
    perror("bind");
    return -1;
  }

  if (ioctl(s, FIONBIO, (char*)&on) < 0) {
    perror("FIONBIO");
    return -1;
  }

  return s;
}

#endif

typedef enum { off, on, poll } nmode_t;

static int fd[4];
static unsigned long pks[4], bytes[4];

int network( nmode_t mode, int num_ifaces )  
{
  
#ifdef ETHERNET
  {
    int rc,i;
    char buf[256];
    char eth[][6]={"eth1","eth2","eth3","eth4"};

    for(i=0;i<num_ifaces;i++)
      {
	switch( mode )
	  {
	  case off:
	    sprintf(buf,"/sbin/ifconfig %s down\n",eth[i]);
	    rc = system(buf);
	    break;

	  case on:
	    sprintf(buf,"/sbin/ifconfig %s up promisc\n",eth[i]);
	    rc = system(buf);
	    break;	    

	  case poll:
	    return 0;
	  }
	fprintf(stderr,"network %d: -%s- returned %d\n",on,buf,rc);
      }
  return 0;
  }

#endif

#ifdef ATM
  {
    int rc,i;
    unsigned char buf[65535];
    char atm[16];

    //num_ifaces=1; // XXXXXXXXXXXXX

    for(i=0;i<num_ifaces;i++)
      {
	//i=1;
	switch( mode )
	  {
	  case off:
	    if(fd[i]) close(fd[i]);
	    fd[i] = 0;
	    break;

	  case on:
	    sprintf(atm,"%i.%i.%i", i, DEFAULT_VPI, DEFAULT_VCI);
	    //	    sprintf(atm,"%i.%i.%i", i, 0,41);
	    fd[i] = atm_open_vc(atm);
	    fprintf(stderr,"open_vc %s: returned %d.\n",atm,fd[i]);
	    break;	    

	  case poll:
	    if( fd[i] )
	      {
	      while( (rc = read( fd[i], buf, 65535 )) > 0 )
		{
		  pks[i]++;
		  bytes[i] += rc;
		  fprintf(stderr,
			  "iface %d: got oversize pkt %d. %d.%d.%d.%d %d.%d.%d.%d (%ld pkts, %ld bytes)\n",
			  i,rc, 
			  buf[20],buf[21],buf[22],buf[23],
			  buf[24],buf[25],buf[26],buf[27],
			  pks[i],bytes[i]);
		}
	      }
	    break;
	  }
      }

  return 0;
  }

#endif


}


/*************************************************************************/

static unsigned long *
acenic_ioctl( char * devname )
{
  unsigned long *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 long *) *((unsigned long**)ifr.ifr_data);


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

  return p;
}

typedef volatile unsigned long vul;

vul * ace_tstamp[4];

struct timeval ace_tv[NUM_IFACES];
unsigned long long ace_tick[NUM_IFACES];

static inline long acenic_getcurtime( int iface )
{
  return * ace_tstamp[iface];
}

static inline struct timeval 
acenic_maptotimeofday( long ace_tstamp, int ace_dev )
{
}
	

static int acenic_init_tstamp()
{
  int rc,i,j;
  char buf[256];
  char eth[][6]={"eth1","eth2","eth3","eth4"};

  for(i=0;i<NUM_IFACES;i++)
    {	
      ace_tstamp[i] = acenic_ioctl(eth[i]);
      if( ace_tstamp[i] == NULL ) return -1;

      fprintf(stderr, "Acenic %d tstamp addr %p\n",i,ace_tstamp[i]);
      *ace_tstamp[i];
	fprintf(stderr, "MAdeit!!\n");
    }

  for(i=0;i<NUM_IFACES;i++)
    {	
#define N 20
      long best = 1000000, best_index;
      struct timeval tv[N];
      unsigned long t1[N],t2[N];

      for(j=0;j<N;j++)
	{ // have a few goes at this

	    t1[j] = acenic_getcurtime(i);
	    gettimeofday(&tv[j], (struct timezone *)0);
	    t2[j] = acenic_getcurtime(i);
	  
	}

      for(j=0;j<N;j++)
	{
	  long d;
	  d = t2[j]-t1[j];	   	      

	  if( d<0 )
	    {
	      fprintf(stderr,"Acenic %i: d is %d !!!\n",i,d);
	      exit(-1);
	    }

	  if( d <= best )
	    {
	      best_index = j;
	      best = d;
	    }
	}
      
      if ( best > 10 )
	{
	  fprintf(stderr,"Acenic %i: Failed to get accurate sync (%d) !!!\n",
		  i,best);
	  exit(-1);
	}

      ace_tick[i] = t1[best_index];
      ace_tv[i]   = tv[best_index];

      fprintf(stderr,"Acenic %i: tick is %llu, systime %d.%d, delta %d\n",
		  i,ace_tick[i],ace_tv[i].tv_sec,ace_tv[i].tv_usec, best );
    }


  return 0;
}


					   

/*************************************************************************/


void sigcatch()
{
  // shut the network down

  fprintf(stderr, "[%d] Caught signal.\n",chan); 
  network(off, NUM_IFACES);

  exit(-1);
}


int check_magic()
{
  int pfd2;

  printf("Magic1 read as %lx %lx\n", np->magic, np->end_magic );

  if( np->magic != NP_MAGIC ||  np->end_magic != NP_MAGIC ) 
    {
      fprintf(stderr,"MAGICFAIL %x %x\n",
	      np->magic, np->end_magic);        
    }


  // cause nprobe to write stage2 magic by frobbing /proc/nprobe

  np->magic = np->end_magic = NP_MAGIC2;

  printf("Magic2 read as %lx %lx\n", np->magic, np->end_magic );

  if( (pfd2 = open(PROC_FILE, O_RDONLY)) < 0) {
    perror("failed to open proc file");
    exit(errno);
  } else fprintf(stderr, "Opened %s (fd = %d)\n", PROC_FILE, pfd2);

  close(pfd2);

  // stage 3 magic should now be present...

  printf("Magic3 read as %lx %lx\n", np->magic, np->end_magic );

  if( np->magic != NP_MAGIC3 ||  np->end_magic != NP_MAGIC3 )
    {
      printf("Magic3 FAIL!\n");
      return 0;
    }

  return 1;
}

inline void * kern2user( void * kern )
{
  return (void *) (km+( ((unsigned long)kern) & 0x3fffffff));
}

inline void * user2kern ( void * user )
{
  return (void *) ( ((unsigned long)((char*)user-km)) | 0xc0000000 );
}
  
inline struct sk_buff * get_skb( int chan )
{
  struct sk_buff *skb;
  
  if (np->x[chan].tous_in > np->x[chan].tous_out )
    {
      skb = np->tous_skb[chan][ np->x[chan].tous_out % FIFO_SIZE ];
      
#if 0
      np->tous_skb[chan][ np->x[chan].tous_out % FIFO_SIZE ] = 
	(struct sk_buff *) 0x05a5a5a5; // if paranoid
#endif	  

      np->x[chan].tous_out++;   // XXX should ensure ordering


#if 0
      if ( ((unsigned long)skb) & 0xf0000000 != 0xc0000000 )
	{
	  fprintf(stderr,"READ %p!!!!\n",skb); 
	}
#endif

      return kern2user( skb );
    }
  else
    return NULL;
}

inline void put_skb( int chan, struct sk_buff * skb )
{

#ifdef MARK_BUFFERS
  {
  // mark this buffer with a magic to indicate that it is returned 
  char * head = kern2user( skb->head );
  *((unsigned long*)(head+4)) = 0x55000001;
  }
#endif


  if( np->x[chan].frus_in - np->x[chan].frus_out < FIFO_SIZE )
    {
      np->frus_skb[chan][ np->x[chan].frus_in % FIFO_SIZE ] = 
	user2kern( skb );

      np->x[chan].frus_in++;   // XXX should really ensure ordering!!!!
      
    }
  else
    {
      // this _really_ shouldn't happen. 	  
      fprintf(stderr,
	      "return: FIFO full! - tous %d,%d : frus %d,%d \n",
	      np->x[chan].tous_in, np->x[chan].tous_out, 
	      np->x[chan].frus_in, np->x[chan].frus_out );      
    }
}

int main(int argc,char **argv)
{
    int pfd, kfd; 
    char buf[256], buf1[256];
    int maxdev, fifosize;
    int rc, i;


    network(off, NUM_IFACES); /* ensure network tap is down before 
				 messing with proc file */


    if( (pfd = open(PROC_FILE, O_RDONLY)) < 0) {
        perror("failed to open proc file");
        exit(errno);
    } else fprintf(stderr, "Opened %s (fd = %d)\n", PROC_FILE, pfd); 

    if( (rc = read( pfd, buf, 255 )) < 0 ) {
	perror("read error on proc file");
	exit(errno);
    } 
    
    buf[(rc>255)?255:rc] = 0; // just in case...

    //    printf("Read %d bytes --- %s\n", rc, buf );

    buf1[0]=0;

    rc = sscanf(buf,"%s %p %d %d", buf1, &np_off, &maxdev, &fifosize);

    printf("rc = %d, %s ptr = %lx maxdev = %d fifosize = %d\n",
	   rc, buf1, np_off, maxdev, fifosize );
	

    /* final read to collect the EOF */

    if( (rc = read( pfd, buf, 255 )) < 0 ) {
	perror("read error on proc file");
	exit(errno);
    } 

    if (rc>0) 
	printf("Expected to read 0 bytes got %d --- %s\n", rc, buf );

    
    np_off &=0x3fffffff ;

    printf("Truncate np_off to %p\n", np_off);


    /* 
       this parent thread will become the status/control thread.
       we fork off NUM_CPUS threads to do the work.
       The parent thread is eventually identified as chan = -1 
       */
    

    while( ++chan < NUM_CPUS )
      {
	int pid;

	fprintf(stderr,"[%d] Ready to fork.\n",chan);

	pid = fork();

	if (pid == -1)
	  {
	    perror("unable to fork");
	    exit(-1);
	  }

	if (pid == 0)  
	  { // child process (chan = 0,1,2,3...)
	    worker_thread();
	    fprintf(stderr,"[%d] Shouldn't get here!!!!\n",chan);
	    exit(-1);
	  }
	
	/* else must be parent */
	

      }

    chan = -1;  /* set chan back to parent status */

    signal(SIGINT, sigcatch);

    sleep(3);

    if( argc > 1 )
      {	
	fprintf(stderr,"Opening just VC %s\n", argv[1]);
#ifdef ATM
	fd[0] = atm_open_vc(argv[1]);
#endif
      }
    else
      {
	fprintf(stderr,"[%d] network On.\n",chan);
	network( on, NUM_IFACES ); //XXXX
      }


    acenic_init_tstamp();

    while(1)
      {
	struct timeval temp = timeout1s;
	int rc = waitpid(-1, NULL, WNOHANG);
	int i,j;

	if( rc )
	  {	    
	    fprintf(stderr,"[%d] Whoa! We've lost one! (%d). Bail.\n",chan,rc);
	    exit(-1);
	  }
       
	network( poll, NUM_IFACES );

#if 0
	for(i=0;i<NUM_IFACES;i++)
	i=0;
	for(j=0;j<25;j++)
	{
	  struct timeval tv;
	  unsigned long at;

	  gettimeofday(&tv, (struct timezone *)0);
	  at = acenic_getcurtime(i);
	  
	  fprintf(stderr,"Acenic %i: Time is %d.%d, systime %d.%d\n",
		  i,at/1000000,at%1000000,tv.tv_sec,tv.tv_usec );
	}
#endif
	
	{ 
	  int i=0;
	  struct timeval tv;
	  unsigned long t1,t2;
	  long d;
	  t1 = acenic_getcurtime(i);
	  gettimeofday(&tv, (struct timezone *)0);
	  t2 = acenic_getcurtime(i);
	  
	  if( t2<t1 )
	    ace_tick[i]+= 0x100000000;

	  d = t2-t1;
	  fprintf(stderr,"Acenic %i: d is %lu: t2 %08x      0x%012llx\n",
		  i,d,t2,ace_tick[i] );
	}


	select(0, NULL, NULL, NULL, &temp);

      }
    /* on exit an oops is generated ;-( */

}

static void
tvsub(struct timeval *tdiff, struct timeval *t1, struct timeval *t0)
{
    tdiff->tv_sec = t1->tv_sec - t0->tv_sec;
    tdiff->tv_usec = t1->tv_usec - t0->tv_usec;
    if (tdiff->tv_usec < 0)
	tdiff->tv_sec--, tdiff->tv_usec += 1000000;
}

static long
utvsub(struct timeval *t1, struct timeval *t0)
{
  struct timeval tdiff;
  tdiff.tv_sec = t1->tv_sec - t0->tv_sec;
  tdiff.tv_usec = t1->tv_usec - t0->tv_usec;
  if (tdiff.tv_usec < 0)
    tdiff.tv_sec--, tdiff.tv_usec += 1000000;

  return ((long)tdiff.tv_sec*1000000) + tdiff.tv_usec;
}


void worker_thread( void )
{
    
    int kfd, i; 
    int total=0, totalpkts=0;
    struct timeval lastpkt, laststats, now ;

    fprintf(stderr,"[%d] Worker ready for action.\n",chan);


    if( (kfd = open(MFN, O_RDWR)) < 0) {
        perror("failed to open /dev/[k]mem");
        exit(errno);
    } else fprintf(stderr, "[%d] Opened /dev/[k]mem (fd = %d)\n",
		   chan, kfd); 

    // must MAP_SHARED otherwise copy_on_write happens!!!

    km = mmap(NULL, MEM_SIZE, PROT_READ | PROT_WRITE, MAP_SHARED, kfd, 0 );

    close(kfd);

    if(km == MAP_FAILED) {
        perror("failed to map /dev/[k]mem"); 
        exit(errno); 
    }

    np = (np_t*) (km + np_off);

    printf("mmap at %p. Control area at %p.\n",
	   km,np);

#if 0
    if ( !check_magic() )  // just for the parnoid
      {
	fprintf(stderr,"[%d] Magic failed\n",chan);
	exit(-1);
      }
#endif 

    // XXXX Use the 3seconds the parent gives us to recycle stale packets

    for(i=0;i<FIFO_SIZE;i++)
      {
	struct sk_buff *skb;
	skb = get_skb(chan);
	if(!skb) break;
	put_skb(chan, skb);	  
      }

    fprintf(stderr,"Read %d Stale packets\n",i);


    gettimeofday(&lastpkt, (struct timezone *)0);  // initialise
    gettimeofday(&laststats, (struct timezone *)0);  // initialise

    while(1)
    {
      int i,iplen,pkts=0;
      struct sk_buff *skb;
      unsigned char  *ipdata;
      struct timeval temp;
      long udiff, dev;
      unsigned long nictstamp;

#if 0
      if ( np->x[chan].tous_in % 64 == 0 )
	  for(i=0;i<FIFO_SIZE;i++)
	  {
	      printf("[%d] %d = %08x %08x\n",
		     chan,i,np->x[chan].tous_skb[i],np->x[chan].frus_skb[i] );
	  }  
#endif

      while( skb = get_skb(chan) )
	{
	  // got packet
	  ipdata = kern2user( skb->data );
	  iplen  = skb->len;

	  dev    = skb->dev;   // device that we arrived on

	  pkts++; // stats

	  if( iplen <= 0 )
	    {
	      /* probably just not IP... */
	      if( iplen < 0 )
		fprintf(stderr,
			"[%d] recvd %d len pkt. s=%p h=%p d=+%d\n",
			chan, iplen, skb, kern2user( skb->head ), 
			ipdata - ((unsigned char*) kern2user( skb->head ))
			);

	      put_skb(chan, skb);  // remember to do this !!!!
	      continue;
	    }

	  /* a few stats */
	  total += iplen;
	  totalpkts ++;

#if 0
	  if ( np->x[chan].tous_out % (1024*1024) == 0 )
	    {
	      fprintf(stderr,
			"[%d] FINISHED.\n",chan);
	      return;
	    }		
#endif

	  nictstamp = (ipdata[-14]<<24) | (ipdata[-13]<<16) | 
	    (ipdata[-12]<<8) | ipdata[-11] ;

	  /* dump every few packets */
	  if ( 0 && np->x[chan].tous_out % (1024*1) == 0 )
	    {

	      fprintf(stderr,"[%d] s=%p h=%p d=+%d %d - tous %d,%d : frus %d,%d : (%d %d)\n",
		      chan, skb, kern2user( skb->head ), 
		      ipdata - ((unsigned char*) kern2user( skb->head )), iplen, 
		      np->x[chan].tous_in, np->x[chan].tous_out, 
		      np->x[chan].frus_in, np->x[chan].frus_out,
		      np->x[chan].tous_in - np->x[chan].tous_out, 
		      np->x[chan].frus_in - np->x[chan].frus_out
		      );
	      
	      fprintf(stderr,"[%d] ",chan);

	      
	      fprintf(stderr," :%08x: ", nictstamp);

	      /*	      fprintf(stderr," :%02x%02x%02x%02x: ",
			ipdata[-14],ipdata[-13],ipdata[-12],ipdata[-11]); */

	      fprintf(stderr,"% 4d %02x%02x%02x%02x#", iplen,
			ipdata[-4],ipdata[-3],ipdata[-2],ipdata[-1]);

	      for(i=0;i<32;i+=4)
		fprintf(stderr,"%02x%02x%02x%02x:",
			ipdata[i],ipdata[i+1],ipdata[i+2],ipdata[i+3]);


	      /*	      fprintf(stderr," :%02x%02x%02x%02x: ",
		      ipdata[iplen],ipdata[iplen+1],
		      ipdata[iplen+2],ipdata[iplen+3]); */

	      fprintf(stderr,"\n");

	    }
	  put_skb(chan, skb);

	} // end got packets

      //fprintf(stderr,"%d : thru skb=%p %d %d\n",chan,skb,np->x[chan].tous_in,np->x[chan].tous_out);


      gettimeofday(&now, (struct timezone *)0);

      if( pkts )
	{
	  lastpkt = now;
	}
      else
	{ // we didn't get any packets this time round
	  gettimeofday(&now, (struct timezone *)0);
	  udiff = utvsub(&now, &lastpkt);

	  if ( udiff > 4000000L )
	    {
	      fprintf(stderr,
		      "[%d] No packets seen! tous %d,%d : frus %d,%d : (%d %d)\n",
		      chan,
		      np->x[chan].tous_in, np->x[chan].tous_out, 
		      np->x[chan].frus_in, np->x[chan].frus_out,
		      np->x[chan].tous_in - np->x[chan].tous_out, 
		      np->x[chan].frus_in - np->x[chan].frus_out
		      );

	      // pretend we've sen a packet to stop message repeating */
	      lastpkt = now;
	    }
	}

      
      if ( (udiff = utvsub(&now, &laststats)) > 2000000L )
	{
	  double mbs = ( 8.0 * total ) / udiff;

	  fprintf(stderr,"[%d] recvd: tous %d,%d : frus %d,%d : (%d %d) %.2lf Mbs, %d pkts\n",
		      chan, 
		      np->x[chan].tous_in, np->x[chan].tous_out, 
		      np->x[chan].frus_in, np->x[chan].frus_out,
		      np->x[chan].tous_in - np->x[chan].tous_out, 
		      np->x[chan].frus_in - np->x[chan].frus_out,
		      mbs, totalpkts
		      );  

	  total = 0;
	  totalpkts = 0;
	  laststats = now;	  


#if 1 
	  if(chan == 1) { // gross hack for debugging
	  int tous0 = np->x[0].tous_in - np->x[0].tous_out;
	  int frus0 = np->x[0].frus_in - np->x[0].frus_out;
	  int tous1 = np->x[1].tous_in - np->x[1].tous_out;
	  int frus1 = np->x[1].frus_in - np->x[1].frus_out;

	  fprintf(stderr,
		  "[X] delta tous[ %d %d ], delta frus [ %d %d ], tot = %d\n",
		  tous0, tous1, frus0, frus1,
		  tous0 + tous1 + frus0 + frus1
		  );
	}
#endif


	}


      temp = tsleep;
      select(0, NULL, NULL, NULL, &temp);
      
    }


}
