/******************************************************************************
*                                                                             *
*   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 <unistd.h>
#include <math.h>
#include <time.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 <assert.h>
#include <err.h>

#include "skb.h"

#include <sk98_timers.h>

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

// now in config.h

#include "probe.h"

typedef struct {

    // interface id
    unsigned long dev;
    unsigned int dev_num;

    // general stats - never zeroed 
    unsigned long long total_bytes;
    unsigned int total_pkts;

    // output stats - zeroed each output period
    unsigned int output_bytes;
    unsigned int output_pkts;

    // used for mean/std stats
    unsigned long long sum_bytes;
    unsigned long long sum2_bytes;
    unsigned long long sum_pkts;
    unsigned long long sum2_pkts;
    unsigned int sum_n;
    unsigned int period_bytes;
    unsigned int period_pkts;
    unsigned long nextperiod;

    // clock drift calculation
    clock_retimer_t *timer;

    // packet length distribution
    unsigned int pktlendist[1600];

} stats_t;    

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


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

#define MEAN(samples,total)  ((total) / (samples))
#define STD(samples,total,squared_total) (sqrt(((squared_total) / (samples)) - pow((total)/(samples), 2.0)))

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

char *eth[MAX_ETH_IFACES];
int eth_ifaces;

#define MAX_ATM_IFACES 4
static int fd[MAX_ATM_IFACES];
static char *atm[MAX_ATM_IFACES];
static int atm_ifaces = 0;
static unsigned long pks[MAX_ATM_IFACES], bytes[MAX_ATM_IFACES];

static unsigned int output_time = 1;

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

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

static int verbosity = 1;

static int pids[NUM_CPUS];

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

static int worker_quit = 0;

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

static void worker_thread( void );


/* clock calibration **************************************************/

static void doMeasure(stats_t *st, unsigned long nictstamp, int iplen);

static unsigned int output_pktlendist = 0;
static unsigned long measure_period = 0;

/* tcpdump output defines *********************************************/

/* pcap* types copied from libpcap-0.7.1
 * bpf_ typedefs included to make it work */
typedef unsigned int bpf_u_int32;
//typedef unsigned short u_short;
typedef int bpf_int32;

struct pcap_file_header {
    bpf_u_int32 magic;
    u_short version_major;
    u_short version_minor;
    bpf_int32 thiszone;     /* gmt to local correction */
    bpf_u_int32 sigfigs;    /* accuracy of timestamps */
    bpf_u_int32 snaplen;    /* max length saved portion of each pkt */
    bpf_u_int32 linktype;   /* data link type (LINKTYPE_*) */
};

struct pcap_timeval {
    bpf_int32 tv_sec;           /* seconds */
    bpf_int32 tv_usec;          /* microseconds */
};

struct pcap_sf_pkthdr {
    struct pcap_timeval ts;     /* time stamp */
    bpf_u_int32 caplen;         /* length of portion present */
    bpf_u_int32 len;            /* length this packet (off wire) */
};

/* included from libpcap-0.7.1 */
#define TCPDUMP_MAGIC 0xa1b2c3d4
#define PCAP_VERSION_MAJOR 2
#define PCAP_VERSION_MINOR 4
#define LINKTYPE_RAW 101             /* raw IP */

static FILE *tcpdump_output = NULL;
static char *tcpdump_prefix = NULL;
static unsigned int tcpdump_num = 0;
static unsigned int tcpdump_size = 0;
static unsigned short tcpdump_snaplen = USHRT_MAX;
#define MAX_TCPDUMP_FILE_SIZE (600*1024*1024)

static void init_tcpdump_file();
static void tcpdump_packet(unsigned char *ipdata, struct timeval *tv, unsigned plen);


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


#ifdef ATM

#include <atm.h>

static 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 network( nmode_t mode )  
{

#ifdef ETHERNET
    {
	int rc=0,i;
	char buf[256];
	//char eth[][6]={"eth2","eth3","eth4","eth5"};

	for(i=0;i<eth_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];

	for(i=0;i<atm_ifaces;i++)
	{
	    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 inline void * kern2user( void * kern )
{
    return (void *) kern;
}

static inline void * user2kern ( void * user )
{
    return (void *) user;
}
  
static inline struct sk_buff * get_skb( int chan )
{
    struct sk_buff *skb;
  
    /* Indicates that one side is compiled with spinlock debug and the
       other not. */
    assert(np->x[chan].tous_in != 0xdead4ead);

    if (np->x[chan].tous_out - np->x[chan].tous_in > 0 )
    {
	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	  
	mb();
	np->x[chan].tous_out++;   // XXX should ensure ordering
	asm volatile("sfence");

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

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

static 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 );
	mb();
	np->x[chan].frus_in++;   // XXX should really ensure ordering!!!!
	asm volatile("sfence");
      
    }
    else
    {
	// this _really_ shouldn't happen. 	  
	fprintf(stderr,
		"return: FIFO full! - tous %ld,%ld : frus %ld,%ld \n",
		np->x[chan].tous_in, np->x[chan].tous_out, 
		np->x[chan].frus_in, np->x[chan].frus_out );      
    }
}

static void worker_sigcatch()
{
    fprintf(stderr, "[%d] Worker exiting.\n", chan);
    worker_quit = 1;
}

static void sigcatch()
{
    int chan = 0, n = 0;
    struct sk_buff *skb;
    const union sigval value;

    // shut the network down
    fprintf(stderr, "[%d] Caught signal.\n",chan);
    network(off);

    for(chan = 0; chan < NUM_CPUS; chan++)
    {
	if(sigqueue(pids[chan], SIGINT, value) == -1)
	    perror("Signalling to worker thread");
	else
	    waitpid(pids[chan], NULL, 0);
    }

    np = (np_t *) np_off;

    while((skb = get_skb(chan)))
    {
	put_skb(chan, skb);
	n++;
    }
    fprintf(stderr, "Emptied %d packets from buffers\n", n);

    exit(-1);
}
	   
	                                                        

int main(int argc,char **argv)
{
    int pfd; 
    char buf[256], buf1[256];
    int maxdev, fifosize;
    int rc, i;
    char *drift_fname;
    unsigned long nictstamp_freq = 0;
    unsigned do_clock_retime = 1;

    //    assert(sizeof(struct sk_buff) == 172);

    for(i=0; i<MAX_ETH_IFACES; i++)
	eth[i] = NULL;
    for(i=0; i<MAX_ATM_IFACES; i++)
    {
	atm[i] = NULL;
	fd[i] = pks[i] = bytes[i] = 0;
    }

    while ((i = getopt(argc, argv, "he:a:t:pm:v:o:d:f:s:n:x")) != -1)
    {
	switch(i) {

	case 'h':
	    fprintf(stderr, "%s\n"
		    "\t-h        :this help message\n"
#ifdef ETHERNET
		    "\t-e <id>   :monitor ethernet id, ie eth2\n"
#else
		    "\t-e <id>   :not supported\n"
#endif
#ifdef ATM
		    "\t-a <vc>   :monitor atm VC\n"
#else
		    "\t-a <vc>   :not supported\n"
#endif
		    "\t-n <tics> :nictstamp frequency, tics per second\n"
		    "\t-m <t>    :generate stats over t nic tstamp tics period\n"
		    "\t-t <n>    :output stats every n seconds (0 no output)\n"
		    "\t-p        :output packet length distribution information\n"
		    "\t-v        :sets verbosity output\n"
		    "\t-d <mul>  :drift frequency multiplier, def: min=0.16s; mean=0.8s, max=1.44s)\n"
		    "\t-o <file> :dump nic drift stats into <file>\n"
		    "\t-x        :no timer recalibration\n"
		    "\t-f <pref> :dump packets into tcpdump format <pref>_%%04u\n"
		    "\t-s <snap> :snaplen for tcpdump file, def: USHRT_MAX\n"
		    "\n", argv[0]);
	    return 0;
	    break;
	
#ifdef ETHERNET
	case 'e':
	    if(eth_ifaces == MAX_ETH_IFACES)
	    {
		fprintf(stderr, "Limit reached on number of eth interfaces.\n");
		return 0;
	    }
	    eth[eth_ifaces++] = strdup(optarg);
	    break;
#else
	case 'e':
	    fprintf(stderr, "Ethernet not supported.\n");
	    break;
#endif
	    
#ifdef ATM
	case 'a':
	    if(atm_ifaces == MAX_ATM_IFACES)
	    {
		fprintf(stderr, "Limit reached on number of atm interfaces.\n");
		return 0;
	    }
	    atm[atm_ifaces++] = strdup(optarg);
	    break;
#else
	case 'a':
	    fprintf(stderr, "ATM not supported.\n");
	    break;
#endif

	case 'n':
	    {
		unsigned long new_freq;
		if(sscanf(optarg, "%lu", &new_freq) != 1)
		{
		    fprintf(stderr, "Couldn't get nictstamp frequency from '%s'\n", optarg);
		    return 0;
		}
		if(new_freq < nictstamp_freq * 0.8 || new_freq > nictstamp_freq * 1.2)
		    fprintf(stderr, "Warning: nictstamp frequency changed by more than 20%%\n");

		nictstamp_freq = new_freq;
		break;
	    }

	case 'm':
	    if(sscanf(optarg, "%lu", &measure_period) != 1)
	    {
		fprintf(stderr, "Couldn't get measurement period from '%s'\n", optarg);
		return 0;
	    }
	    break;

	case 'p':
	    output_pktlendist = 1;
	    break;

	case 't':
	    if(sscanf(optarg, "%u", &output_time) != 1)
	    {
		fprintf(stderr, "Couldn't understand time to pause from '%s'\n", optarg);
		return 0;
	    }
	    break;

	case 'v':
	    if(sscanf(optarg, "%u", &verbosity) != 1)
	    {
		fprintf(stderr, "Couldn't understand time to pause from '%s'\n", optarg);
		return 0;
	    }
	    break;

	case 'd':
	    {
		double d;
		if(sscanf(optarg, "%lg", &d) != 1)
		{
		    fprintf(stderr, "Couldn't understand drift frequency multipler from '%s'\n", optarg);
		    return 0;
		}
		if(d < 0.25)
		    fprintf(stderr, "Warning: minimum drift check frequency close to 10ms\n");

		drift_period = (unsigned long) ((double) drift_period * d);
		break;	    
	    }

	case 'o':
	    drift_fname = optarg;
	    break;

	case 'x':
	    do_clock_retime = 0;
	    break;

	case 'f':
	    tcpdump_prefix = strdup(optarg);
	    break;

	case 's':
	    if(sscanf(optarg, "%hu", &tcpdump_snaplen) != 1)
	    {
		fprintf(stderr, "Couldn't get snaplen from '%s'\n", optarg);
		return 0;
	    }
	    if(tcpdump_snaplen < 20)
		fprintf(stderr, "Warning: will not get all of IP packet headers with snaplen of %hu\n", tcpdump_snaplen);

	    break;

	default:
	    fprintf(stderr, "Option %c not supported\n", i);
	    break;

	}
    }

    initialise_timestamps(do_clock_retime, nictstamp_freq, drift_fname);

    if(eth_ifaces == 0 && atm_ifaces == 0)
    {
	fprintf(stderr, "Must give either -e or -a option to configure interface(s) to monitor\n");
	return 0;
    }

    if(tcpdump_prefix != NULL)
	init_tcpdump_file();

    /* ensure network tap is down before messing with proc file.
     * Fairly obvious problems if using ATM and not shut down cleanly,
     * since fd array is not set, and if using different sets of
     * ethernet names from last run that was not shut down cleanly.
     */
    network(off);

    if( (pfd = open(PROC_FILE, O_RDWR)) < 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 0x%lx %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 );

    
    printf("np_off is %p\n", (np_t *) np_off);


    // write random crap back to engage Memhack

    if( write( pfd, buf, 1 ) <0 ) {
	perror("write on proc file");
	exit(errno);
    }


    /* 
       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...)
	    signal(SIGINT, worker_sigcatch);
	    worker_thread();
	    exit(-1);
	}
	else
	{
	    pids[chan] = pid;
	}
	
	/* else must be parent */
	

    }

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

    signal(SIGINT, sigcatch);

    sleep(3);


#ifdef ATM
    for(i=0; i<atm_ifaces; i++)
    {
	fprintf(stderr,"Opening just VC %s\n", atm[i]);
	fd[i] = atm_open_vc(atm[i]);
    }
#endif

    fprintf(stderr,"[%d] network On.\n",chan);
    network( on );
    fprintf(stderr,"[%d] network On now.\n",chan);

    fprintf(stderr,"[%d] timestamps go.\n",chan);

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

	if(output_time > 0)
	    temp.tv_sec *= output_time;

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

	if(output_time && verbosity >= 3)
	{
	    for(i=0;i<eth_ifaces;i++)
	    {
		for(j=0;j<1;j++)
		{
		    struct timeval tv;
		    unsigned long at;

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

		    at = card_tstamp_getcurtime(eth[i]);
	  
#if defined(SK98)
		    fprintf(stderr,"SK98 %d: Time is %lu %08lx, systime %d.%06d\n",
			    i,at,at,(int) tv.tv_sec,(int) tv.tv_usec );
#elif defined(ACENIC)
		    fprintf(stderr,"Acenic %d: Time is %d.%d, systime %d.%06d\n",
			    i,at/1000000,at%1000000,tv.tv_sec,tv.tv_usec );
#else
		    fprintf(stderr, "%d: No nic timestamp, systime %d.%06d\n", 
			    i,tv_sec,tv.tv_usec);
#endif
		}
	    }
	}
	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;
}

static stats_t *stats;

static stats_t *find_stats(unsigned long dev)
{
    int i;

    // match the dev code with the stats structure
    for(i=0; i<eth_ifaces + atm_ifaces; i++)
	if(stats[i].dev == dev)
	    return &stats[i];

    // maybe the first packet with this dev code
    for(i=0; i<eth_ifaces + atm_ifaces; i++)
	if(stats[i].dev == 0)
	{
	    stats[i].dev = dev;
	    stats[i].dev_num = i;
	    stats[i].timer = new_clock_retimer(eth[i], i);
	    fprintf(stderr, "Got packet from dev code %lu, num %d\n", dev, i);
	    return &stats[i];
	}
    return NULL;
}

static void worker_thread( void )
{
    int i, valid = 0;
    struct timeval lastpkt, laststats, now ;
    unsigned calibrated_interfaces;

    stats = (stats_t *) malloc(sizeof(stats_t) * (eth_ifaces + atm_ifaces));
    memset(stats, 0, sizeof(stats_t) * (eth_ifaces + atm_ifaces));

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

    np = (np_t*) np_off;

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

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


	while( (skb = get_skb(chan)) )
	{
	    struct timeval pkttime;

	    ipdata = kern2user( skb->data );
	    iplen  = skb->len;
	    dev    = (unsigned long)skb->dev;   // device that we arrived on

	    st = find_stats(dev);
	    if(st == NULL)
	    {
		fprintf(stderr, "More dev: %lu.\n", dev);
		continue;
	    }

	    pkts++; // zeroed every loop inside while(1)
	    st->total_pkts++; // never zeroed

	    if( iplen <= 0)
	    {
		fprintf(stderr,
			"IPLEN < 0 [%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;
	    }

	    nictstamp = skb_to_nictstamp(skb);
	    if(st->output_pkts == 1)
		srand48(nictstamp);

	    doMeasure(st, nictstamp, iplen);  // get mean/std statistics
	    if(doTimer(st->timer, nictstamp, st->total_pkts))
	    {
		calibrated_interfaces++;
		if(calibrated_interfaces == eth_ifaces + atm_ifaces)
		{
		    /* All interfaces calibrated, start doing some
		     * real work. */
		    valid = 1;
		    drift_period *= 10;
		}
	    }

	    getTime(st->timer, nictstamp, &pkttime, NULL);

	    if(valid && tcpdump_output)
		tcpdump_packet(skb->data, &pkttime, skb->len);
	    put_skb(chan, skb);
	}

	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 %lu,%lu : frus %lu,%lu : (%lu %lu)\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 (output_time && (udiff = utvsub(&now, &laststats)) > output_time * 1000000L)
	{
	    struct timeval tv;

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

	    if(verbosity >= 2)
		fprintf(stderr,"%d [%d] recvd: tous %lu,%lu : frus %lu,%lu : (%lu %lu)\n",
			(int) tv.tv_sec,
			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);

	    for(i=0; i<eth_ifaces + atm_ifaces; i++)
	    {
		stats_t *st = stats+i;
		double mbs = ( 8.0 * st->output_bytes ) / udiff;
		struct timeval tv;

		if(st->dev == 0)
		    continue;

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

                if(verbosity == 1)
                    fprintf(stderr, "%12d.%06d ", (int) tv.tv_sec, (int) tv.tv_usec);

		if(verbosity >= 1)
		    fprintf(stderr,"%d: %.2lf Mbs, %d p/s  ",
			    st->dev_num, mbs, st->output_pkts / output_time);

		if(measure_period > 0 && st->sum_n > 3)
		    fprintf(stderr, "[P %.6g | %.6g  B %.6g | %.6g ]\n",
			    MEAN((double) st->sum_n, (double) st->sum_pkts),
			    STD((double) st->sum_n, (double) st->sum_pkts, (double) st->sum2_pkts),
			    MEAN((double) st->sum_n, (double) st->sum_bytes),
			    STD((double) st->sum_n, (double) st->sum_bytes, (double) st->sum2_bytes));

		if(verbosity >= 1 || (measure_period > 0 && st->sum_n > 3))
		    fprintf(stderr, "\n");

		if(output_pktlendist)
		{
		    if(verbosity >= 1 && !measure_period)
			fprintf(stderr, "\n");
		    for(i=0; i<1600; i++)
			if(st->pktlendist[i])
			{
			    //tbc += (i * pktlendist[i]);
			    fprintf(stderr, "PktLen %d : %d\n", i, st->pktlendist[i]);
			    st->pktlendist[i] = 0;
			}
		}
		st->output_bytes = st->output_pkts = 0;
	    }
	    laststats.tv_sec += output_time; 
	}

	temp = tsleep;

	if(valid)
	    select(0, NULL, NULL, NULL, &temp);
    }

    if(tcpdump_output)
	fclose(tcpdump_output);

    tcpdump_output = NULL;
    printf("Worker thread dieing.\n");
}

static void doMeasure(stats_t *st, unsigned long nictstamp, int iplen)
{
    // a few more stats
    st->total_bytes += iplen;

    st->output_bytes += iplen;
    st->output_pkts++;

    // packet length distribution
    if(iplen >= 0 && iplen <= 1600) {
	st->pktlendist[iplen]++;
    } else {
	fprintf(stderr, "pktlen: %d, not saving length for distribution\n", iplen);
    }

    // period based measurement stats (mean/std)
    if(measure_period > 0)
    {
	if(st->nextperiod == 0)
	{
	    st->nextperiod = nictstamp + measure_period;
	}
	else
	{
	    int done = 0;

	    do
	    {
		if(nictstamp < st->nextperiod)
		{
		    // if the nicstamp has wrapped, finish old periods
		    if(st->nextperiod - nictstamp > measure_period)
		    {
			st->sum_bytes += st->period_bytes;
			st->sum2_bytes += (long long int) st->period_bytes * (long long int) st->period_bytes;
			st->sum_pkts += st->period_pkts;
			st->sum2_pkts += (long long int) st->period_pkts * (long long int) st->period_pkts;
			st->sum_n ++;
				
			//fprintf(stderr, "Nic<NPd %d: %lu  Pkts:%d/%lld/%lld   Bytes:%d/%lld/%lld\n", sum_n, nextperiod, period_pkts, sum_pkts, sum2_pkts, period_bytes, sum_bytes, sum2_bytes);
				
			st->period_bytes = st->period_pkts = 0;
			st->nextperiod += measure_period;
		    }
		    else
			done = 1;
		}
		else
		{
		    // skip over this finish period
		    if(nictstamp + measure_period > st->nextperiod)
		    {
			st->sum_bytes += st->period_bytes;
			st->sum2_bytes += (long long int) st->period_bytes * (long long int) st->period_bytes;
			st->sum_pkts += st->period_pkts;
			st->sum2_pkts += (long long int) st->period_pkts * (long long int) st->period_pkts;
			st->sum_n ++;

			//fprintf(stderr, "Nic>NPd %d: %lu  Pkts:%d/%lld/%lld   Bytes:%d/%lld/%lld\n", sum_n, nextperiod, period_pkts, sum_pkts, sum2_pkts, period_bytes, sum_bytes, sum2_bytes);
				
			st->period_bytes = st->period_pkts = 0;
			st->nextperiod += measure_period;
		    }
		    else
			done = 1;
		}
	    } while(done == 0);
	}
	st->period_pkts++;
	st->period_bytes += iplen;
    }
}

static void init_tcpdump_file()
{
    struct pcap_file_header hdr;
    char filename[1024];

    if(tcpdump_output != NULL)
	fclose(tcpdump_output);

    sprintf(filename, "%s_%04u", tcpdump_prefix, tcpdump_num++);
    if(!(tcpdump_output = fopen(filename, "w")))
    {
	fprintf(stderr, "Failed to open tcpdump file '%s'\n", filename);
	free(tcpdump_prefix);
	tcpdump_prefix = NULL;
    }

    hdr.magic = TCPDUMP_MAGIC;
    hdr.version_major = PCAP_VERSION_MAJOR;
    hdr.version_minor = PCAP_VERSION_MINOR;
    
    hdr.thiszone = 0;
    hdr.snaplen = tcpdump_snaplen;
    hdr.sigfigs = 0;
    hdr.linktype = LINKTYPE_RAW;

    if(fwrite((char *)&hdr, sizeof(hdr), 1, tcpdump_output) != 1)
    {
        fprintf(stderr, "Failed to write header to tcpdump dumpfile.");
        if(fclose(tcpdump_output))
            fprintf(stderr, "...and then failed to close tcpdump dumpfile.");

	tcpdump_output = NULL;
	tcpdump_prefix = NULL;
    }

    fflush(tcpdump_output);

    tcpdump_size = sizeof(hdr);
}

static void tcpdump_packet(unsigned char *ipdata, struct timeval *tv,
			   unsigned plen)
{
    struct pcap_sf_pkthdr sf_hdr;
    char *local_buf;
    unsigned short bytes = plen < tcpdump_snaplen ? plen : tcpdump_snaplen;

    if(bytes == 0)
	return;

    sf_hdr.ts.tv_sec  = tv->tv_sec;
    sf_hdr.ts.tv_usec = tv->tv_usec;
    sf_hdr.caplen     = bytes;
    sf_hdr.len        = plen;

    /* If the buffer is in kernel space, we need to copy it to user
       memory before fwriteing it. */
    local_buf = malloc(bytes);
    memcpy(local_buf, ipdata, bytes);

    /* write packet header and data out */
    if(fwrite(&sf_hdr, sizeof(sf_hdr), 1, tcpdump_output) != 1)
    {
        warn("Failed to write packet header to tcpdump dumpfile.");
	if(fclose(tcpdump_output))
	    warn("...and then failed to close tcpdump dumpfile.");
	tcpdump_output = NULL;
    }
    else if(fwrite(local_buf, bytes, 1, tcpdump_output) != 1)
    {
        warn("Failed to write packet contents to tcpdump dumpfile.");
	if(fclose(tcpdump_output))
	    warn("...and then failed to close tcpdump dumpfile.");
	tcpdump_output = NULL;
	tcpdump_prefix = NULL;
    }
    free(local_buf);

    fflush(tcpdump_output);

    if(worker_quit)
	fprintf(stderr, "%d bytes\n", bytes);

    if((tcpdump_size += sizeof(sf_hdr) + bytes) > MAX_TCPDUMP_FILE_SIZE)
	init_tcpdump_file();
}
