/*
modprobe xeno_native
modprobe  rt_e1000
modprobe rtpacket
 */

#include <unistd.h>
#include <string.h>
#include <stdlib.h>
#include <sys/mman.h>
#include <linux/kernel.h>
#include <linux/net.h>
#include <linux/if_packet.h>
#include <netinet/ether.h>
#include <arpa/inet.h>
#include <native/task.h>
#include <native/timer.h>
#include <rtnet.h>
#include <net/if.h>
#include <nucleus/trace.h>

//#define PROTOCOL    0x88a4
#define PROTOCOL    0x1234


typedef struct {
//  unsigned char head[16];
  uint64_t seq;
  RTIME sent1;
  RTIME sent2;
  RTIME period;
  uint64_t missed;
  char pad [1500];
} message_t;

typedef struct {
  int fd;
  int index;
} raw_socket_t;

#define BINS 10000
#define START_DELAY 0
#define NETWORK_DELAY 1
#define NETWORK_JITTER 2
#define REMOTE_PERIOD 3

static uint64_t total, spurious_timeout, spurious_sleep,
  local_missed, remote_missed, seq;
static long long histogram[4][BINS];

static void update_histogram(RTIME nominal_start, 
			     RTIME true_start, message_t *buffer)
{
  if (buffer->seq) {
    RTIME t;
    unsigned int bin;
  
    t = rt_timer_read();

    bin = abs(true_start - nominal_start) / 10000;
    if (bin >= BINS) {
      bin = BINS - 1;
    }
    histogram[START_DELAY][bin]++;

    bin = (t - buffer->sent2) / 10000;
    if (bin >= BINS) {
      bin = BINS - 1;
    }
    histogram[NETWORK_DELAY][bin]++;
    
    bin = (t - nominal_start) / 10000;
    if (bin >= BINS) {
      bin = BINS - 1;
    }
    histogram[NETWORK_JITTER][bin]++;
    
    bin = buffer->period / 10000;
    if (bin >= BINS) {
      bin = BINS - 1;
    }
    histogram[REMOTE_PERIOD][bin]++;
    
    remote_missed = buffer->missed;
  }
}

static void print_histogram(uint64_t total, 
			    uint64_t spurious_timeout,
			    uint64_t spurious_sleep,
			    uint64_t local_missed,
			    uint64_t remote_missed)
{
  int i;
  
  printf("delay = [\n");
  for (i = 0 ; i < BINS ; i++) {
    if (histogram[START_DELAY][i] != 0 ||
	histogram[NETWORK_DELAY][i] != 0 ||
	histogram[NETWORK_JITTER][i] != 0 ||
	histogram[REMOTE_PERIOD][i] != 0) {
      printf("  %6d %10Ld %10Ld %10Ld %10Ld;\n",
	     i * 10, 
	     histogram[START_DELAY][i],
	     histogram[NETWORK_DELAY][i],
	     histogram[NETWORK_JITTER][i],
	     histogram[REMOTE_PERIOD][i]);
    }
  }
  printf("];\n");
  printf("total = %lld\n", total);
  printf("spurious_timeout = %lld\n", spurious_timeout);
  printf("spurious_sleep = %lld\n", spurious_sleep);
  printf("local_missed = %lld\n", local_missed);
  printf("remote_missed = %lld\n", remote_missed);
}

static void ftrace_freeze()
{
  int fd;

  fd = open("/sys/kernel/debug/tracing/tracing_on", O_RDWR);
  write(fd, "0", 1);
  close(fd);
  print_histogram(total, spurious_timeout, spurious_sleep, 
		  local_missed, remote_missed);
  exit(1);
}

static int raw_open(raw_socket_t *sock, char *name)
{
  int err;
  struct sockaddr_ll local_addr;
  struct ifreq ifr;
  

  /* create rt-socket */
  err = rt_dev_socket(AF_PACKET, SOCK_DGRAM, htons(PROTOCOL));
  if (err < 0) {
    fprintf(stderr, "rt_dev_socket() = %d!\n", err);
    goto socket_failed;
  }
  sock->fd = err;

  strncpy(ifr.ifr_name, name, IFNAMSIZ);
  err = rt_dev_ioctl(sock->fd, SIOCGIFINDEX, &ifr);
  if (err < 0) {
    fprintf(stderr, "cannot get interface index %d\n", err);
    goto index_failed;
  }
  fprintf(stderr, "local interface: %d\n", ifr.ifr_ifindex);
  sock->index = ifr.ifr_ifindex;

  /* bind the rt-socket to a port */
  memset(&local_addr, 0, sizeof(struct sockaddr_ll));
  local_addr.sll_family   = AF_PACKET;
  local_addr.sll_protocol = htons(PROTOCOL);
  local_addr.sll_ifindex  = ifr.ifr_ifindex;
  err = rt_dev_bind(sock->fd, (struct sockaddr *)&local_addr,
		    sizeof(struct sockaddr_ll));
  if (err < 0) {
    fprintf(stderr, "rt_dev_bind() = %d!\n", err);
    goto bind_failed;
  }
  strncpy(ifr.ifr_name, name, IFNAMSIZ);
  err = rt_dev_ioctl(sock->fd, SIOCGIFHWADDR, &ifr);
  if (err < 0) {
    fprintf(stderr, "failed to get ethernet address: %d\n", err);
    goto get_ethernet_failed;
  }
  fprintf(stderr,
	  "Ethernet address: %02x:%02x:%02x:%02x:%02x:%02x\n", 
	  (int) ((unsigned char *) &ifr.ifr_hwaddr.sa_data)[0],
	  (int) ((unsigned char *) &ifr.ifr_hwaddr.sa_data)[1],
	  (int) ((unsigned char *) &ifr.ifr_hwaddr.sa_data)[2],
	  (int) ((unsigned char *) &ifr.ifr_hwaddr.sa_data)[3],
	  (int) ((unsigned char *) &ifr.ifr_hwaddr.sa_data)[4],
	  (int) ((unsigned char *) &ifr.ifr_hwaddr.sa_data)[5]);
  err = 0;
  goto out;

get_ethernet_failed:
bind_failed:
index_failed:
  rt_dev_close(sock->fd);
socket_failed:
  
out:
  return err;
}
     
static int raw_receive(raw_socket_t *sock, 
		       struct sockaddr_ll *from,
		       void *buffer, int size)
{
  int err;
  struct msghdr msg;
  struct iovec iov;
      
  iov.iov_base = buffer;
  iov.iov_len  = size;
  memset(&msg, 0, sizeof(msg));
  msg.msg_name    = from;
  msg.msg_namelen = sizeof(*from);
  msg.msg_iov     = &iov;
  msg.msg_iovlen  = 1;
  err = rt_dev_recvmsg(sock->fd, &msg, 0);
  if (0 && err > 0) {
    unsigned char *p = buffer;
    int i;

    for (i = 0 ; i < err ; i++) {
      printf("%02x ", p[i]);
    }
    printf("\n%d\n", err);
  }
  return err;
}

static int raw_transmit(raw_socket_t *sock, 
			struct sockaddr_ll *to,
			void *buffer, int size)
{
  int err;
  struct msghdr msg;
  struct iovec iov;
      
  iov.iov_base = buffer;
  iov.iov_len  = size;
  memset(&msg, 0, sizeof(msg));
  msg.msg_name    = to;
  msg.msg_namelen = sizeof(*to);
  msg.msg_iov     = &iov;
  msg.msg_iovlen  = 1;
  ether_aton_r("ff:ff:ff:ff:ff:ff", (void*)&to->sll_addr);
  
  err = rt_dev_sendmsg(sock->fd, &msg, 0);
  
  return err;
}

void server(char *name)
{
  raw_socket_t sock;

  if (raw_open(&sock, name) == 0) {
    RTIME t1, t2;
    t1 = 0;
    t2 = 0;
    uint64_t seq, missed;
    
    seq = 0;
    missed = 0;
    while (1) {
      message_t buffer;
      int err;
      struct sockaddr_ll addr;

      err = raw_receive(&sock, &addr, &buffer, sizeof(buffer));

      t1 = rt_timer_read();
      if (err < 0) {
	fprintf(stderr, "recvmsg failed: %d\n", err);
	break;
      }
      buffer.sent2 = buffer.sent1;
      buffer.sent1 = rt_timer_read();
      buffer.period = t1 - t2;
      t2 = t1;
      if (buffer.seq == 0) {
	seq = 0;
	missed = 0;
      }
      if (buffer.seq != seq) {
	missed += buffer.seq - seq;
	seq = buffer.seq;
      }
      seq++;
      buffer.missed = missed;
      err = raw_transmit(&sock, &addr, &buffer, err);

      if (err < 0) {
	fprintf(stderr, "sendmsg failed: %d\n", err);
	break;
      }
    }
  }
}
 
static void client(char *name, char *server_mac)
{
  raw_socket_t sock;

  if (raw_open(&sock, name) == 0) {
    struct sockaddr_ll dest_addr, addr;
    int err, savederr;
    message_t buffer;
    RTIME start, true_start, t1;
  
    /* set destination address */
    memset(&dest_addr, 0, sizeof(struct sockaddr_ll));
    dest_addr.sll_family   = AF_PACKET;
    dest_addr.sll_protocol = htons(PROTOCOL);
    dest_addr.sll_ifindex  = sock.index;
    dest_addr.sll_halen    = 6;
    ether_aton_r(server_mac, (void*)&dest_addr.sll_addr);

    fprintf(stderr, 
	    "destination mac address: %02X:%02X:%02X:%02X:%02X:%02X\n",
	    dest_addr.sll_addr[0], dest_addr.sll_addr[1],
	    dest_addr.sll_addr[2], dest_addr.sll_addr[3],
	    dest_addr.sll_addr[4], dest_addr.sll_addr[5]);
    
    memset(histogram, 0, sizeof(histogram));
      
    seq = 0;
    total = 0;
    spurious_timeout = 0;
    spurious_sleep = 0;
    local_missed = 0;
    start = rt_timer_read();
    while (1) {
      int64_t timeout;

      start += 1000000;
      // start += 150000;
      rt_task_sleep_until(start);
      true_start = rt_timer_read();

      timeout = -1;
      rt_dev_ioctl(sock.fd, RTNET_RTIOC_TIMEOUT, &timeout);
      while (1) {
	// Read packets received during sleep (should be 0) 
	err = raw_receive(&sock, &addr, &buffer, sizeof(buffer));
	if (err < 0) {
	  break;
	}
	update_histogram(start, true_start, &buffer);
	spurious_sleep++;
	xntrace_user_freeze(savederr, 1);
	ftrace_freeze();
      }
      
      memset(&buffer, 0, sizeof(buffer));
      t1 = rt_timer_read();
      buffer.sent1 = t1;
      buffer.seq = seq++;  
      err = raw_transmit(&sock, &dest_addr, &buffer, 60);
      if (err < 0) {
	fprintf(stderr, "sendmsg failed: %d\n", err);
	break;
      }

      timeout = 500000L;
      rt_dev_ioctl(sock.fd, RTNET_RTIOC_TIMEOUT, &timeout);
      memset(&buffer, 0, sizeof(buffer));
      err = raw_receive(&sock, &addr, &buffer, sizeof(buffer));
      if (err > 0) { 
	total++;
	update_histogram(start, true_start, &buffer);
      } else {
	savederr = err;
	timeout = -1;
	rt_dev_ioctl(sock.fd, RTNET_RTIOC_TIMEOUT, &timeout);
	rt_task_sleep(10000);
	err = raw_receive(&sock, &addr, &buffer, sizeof(buffer));
	fprintf(stderr, "%d\n", err);
	if (err > 0) {
	  update_histogram(start, true_start, &buffer);
	  spurious_timeout++;
	  xntrace_user_freeze(savederr, 1);
	  ftrace_freeze();
	} else{
	  local_missed++;
	}
      }
      //fprintf(stderr, "Recv = %d\n", err);
      if (err < 0 && err != -ETIMEDOUT && err != -EAGAIN) {
	fprintf(stderr, "recvmsg failed: %d\n", err);
	break;
      }
    }
  }
}

static void reporter(void *cookie)
{
  while (1) {
    sleep(1);
    print_histogram(total, spurious_timeout, spurious_sleep, 
		    local_missed, remote_missed);
    
  }
}


int main(int argc, char *argv[])
{
  RT_TASK task_self, task_reporter;

  mlockall(MCL_CURRENT|MCL_FUTURE);
  rt_task_shadow(&task_self, "raw_test", 50, T_FPU);

  if (argc == 2) {
    server(argv[1]);
  } else if (argc == 3) {
    rt_task_spawn(&task_reporter, "reporter", 50 * 1024, 0, T_FPU, 
		  reporter, NULL);
    client(argv[1], argv[2]);
  }
  return 0;
}