#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <net/if.h>
#include <linux/net_tstamp.h>
#include <linux/sockios.h>
#include <sys/ioctl.h>
#include <errno.h>
#include <sys/select.h>

#define PACKET_SIZE  300

unsigned char delayreq[] = {
  0x01, 0x02, 0x00, 0x2c, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
  0x00, 0x00, 0x00, 0x00, 0x00, 0x0d, 0x3f, 0xff, 0xfe, 0x01, 0x28, 0xd5, 0x00, 0x01, 0x00, 0x01,
  0x01, 0x7f, 0x00, 0x00, 0x00, 0x00, 0x00, 0x14, 0x32, 0x56, 0xdf, 0x10
};

int initsock(bool event)
{
  int sockfd;
  struct ifreq ifreq;
  struct sockaddr_in addr;
  struct in_addr interfaceAddr, netAddr;
  struct ip_mreq imr;
  const char* ifaceName = "eth0";
  int temp;

  sockfd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  if (sockfd < 0) {
    perror("socket");
    return -1;
  }

  if( setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &temp, sizeof(int)) < 0) {
    perror("reuseaddr");
    return -2;
  }

  addr.sin_family = AF_INET;
  addr.sin_addr.s_addr = htonl(INADDR_ANY);
  addr.sin_port = htons(event ? 319 : 320);
  if(bind(sockfd, (struct sockaddr*)&addr, sizeof(struct sockaddr_in)) < 0) {
    perror("bind");
    return -3;
  }

  ifreq.ifr_addr.sa_family = AF_INET;
  strncpy(ifreq.ifr_name, "eth0", IFNAMSIZ-1);
  if (ioctl(sockfd, SIOCGIFADDR, &ifreq) < 0) {
    perror("get eth0 address");
    return -4;
  }
  interfaceAddr.s_addr = ((struct sockaddr_in *)&ifreq.ifr_addr)->sin_addr.s_addr;

  if(!inet_aton("224.0.1.129", &netAddr))
  {
    perror("inet_aton");
    return -5;
  }

  imr.imr_multiaddr.s_addr = netAddr.s_addr;
  imr.imr_interface.s_addr = interfaceAddr.s_addr;
  if( setsockopt(sockfd, IPPROTO_IP, IP_MULTICAST_IF, &imr.imr_interface.s_addr, sizeof(struct in_addr)) < 0) {
    perror("multicast on");
    return -6;
  }

  if (event) {
    struct ifreq ifreq;
    struct hwtstamp_config cfg, req;
    memset(&ifreq, 0, sizeof ifreq);
    memset(&cfg, 0, sizeof cfg);
  
    strncpy(ifreq.ifr_name, ifaceName, sizeof ifreq.ifr_name);
    ifreq.ifr_data = (void *) &cfg;
    cfg.tx_type    = HWTSTAMP_TX_ON;
    cfg.rx_filter  = HWTSTAMP_FILTER_PTP_V2_EVENT;
    req = cfg;
    if (ioctl(sockfd, SIOCSHWTSTAMP, &ifreq)) {
      perror("SIOCSHWTSTAMP");
      return -10;
    }
    if (memcmp(&cfg, &req, sizeof cfg)) {
      printf("driver changed our HWTSTAMP options\n");
      printf("tx_type   got %d wanted %d\n", cfg.tx_type, req.tx_type);
      printf("rx_filter got %d wanted %d\n", cfg.rx_filter, req.rx_filter);
      fflush(stdout);
      if (cfg.tx_type != req.tx_type ||
          (cfg.rx_filter != HWTSTAMP_FILTER_ALL &&
           cfg.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)) {
        printf("%s: something funky\n", __func__);
        fflush(stdout);
      }
    }
  }

  int flags;
  flags = (SOF_TIMESTAMPING_TX_HARDWARE |
      SOF_TIMESTAMPING_RX_HARDWARE |
      SOF_TIMESTAMPING_RAW_HARDWARE);
  if (setsockopt(sockfd, SOL_SOCKET, SO_TIMESTAMPING, &flags, sizeof flags) < 0) {
    perror("SO_TIMESTAMPING");
    return -11;
  }

  return sockfd;
}

int send_packet(int sockfd, int port, char *buf, int length)
{
  ssize_t ret;
  struct sockaddr_in addr;
  
  addr.sin_family = AF_INET;
  addr.sin_port = htons(port);
  inet_aton("224.0.1.129", &addr.sin_addr);
  
  ret = sendto(sockfd, buf, length, 0, (struct sockaddr *) &addr, sizeof (struct sockaddr_in));
  printf("sendto %d: %d; %m\n", port, ret);
  return ret;
}

int send_delayreq(int sockfd)
{
  struct timeval tv;
  gettimeofday(&tv, NULL);
  void *p = &delayreq[sizeof(delayreq) - 8];
  uint32_t *u = p;
  u[0] = htonl(tv.tv_sec);
  u[1] = htonl(tv.tv_usec * 1000);
  return send_packet(sockfd, 319, delayreq, sizeof(delayreq));
}

int main(int argc, char **argv)
{
  int event = initsock(1);
  if (event < 0) {
    return event;
  }

  int limit = 1;
  if (argc > 1) {
    limit = atoi(argv[1]);
  }

  int i;
  for (i = 0; i < limit; i++) {
    printf("sending %d\n", i);
    send_delayreq(event);
    sleep(1);
  }

  close(event);
  return 0;
}