#include <fcntl.h>
#include <stdio.h>
#include <unistd.h>

#include <arpa/inet.h>
#include <libnetfilter_queue/libnetfilter_queue.h>
#include <linux/icmp.h>
#include <linux/ip.h>
#include <linux/netfilter.h>
#include <sys/stat.h>
#include <sys/types.h>

#define BUFSIZE 1024

static int
nfq_cb(struct nfq_q_handle *nfqqh,
       struct nfgenmsg *nfgmsg,
       struct nfq_data *nfqd,
       void *data)
{
  int id, plen, useable, num, ret = 0;
  char *pkt, *icmp_data;
  struct nfqnl_msg_packet_hdr *h;
  struct iphdr *iph;
  struct icmphdr *icmph;

  h = nfq_get_msg_packet_hdr(nfqd);
  if (h) {
    id = ntohl(h->packet_id);
    plen = nfq_get_payload(nfqd, &pkt);
    printf("Processing packet %d ... ", id);

    iph = (struct iphdr *)pkt;
    icmph = (struct icmphdr *)(pkt + sizeof(struct iphdr));
    icmp_data = (char *)(pkt + sizeof(struct iphdr) + sizeof(struct icmphdr));
    if (icmph->type != 8 && icmph->code != 0)
      ret = nfq_set_verdict(nfqqh, id, NF_ACCEPT, 0, NULL);

    useable = plen - sizeof(struct iphdr) - sizeof(struct icmphdr);
    num = read((int)data, icmp_data, useable);
    printf("embedded %d bytes\n", num);
    ret = nfq_set_verdict(nfqqh, id, NF_ACCEPT, plen, (void *)pkt);
  }

  return ret;
}

int
main(int argc, char **argv)
{
  int fd, nfqfd, num;
  char buf[BUFSIZE];
  struct nfq_handle *nfqh;
  struct nfq_q_handle *nfqqh;

  if (argc != 2) {
    fprintf(stderr, "Usage: %s FILENAME\n",
            argv[0]);
    return 1;
  }

  if ((fd = open(argv[1], O_RDONLY)) < 0) {
    fprintf(stderr, "*** open()\n");
    return 1;
  }

  nfqh = nfq_open();
  if (!nfqh) {
    fprintf(stderr, "*** nfq_open()\n");
    return 1;
  }

  nfq_unbind_pf(nfqh, AF_INET);
  if (nfq_bind_pf(nfqh, AF_INET) < 0) {
    fprintf(stderr, "*** nfq_unbind_pf()\n");
    return 1;
  }

  nfqqh = nfq_create_queue(nfqh, 0, nfq_cb, (void *)fd);
  if (!nfqqh) {
    fprintf(stderr, "*** nfq_create_queue()\n");
    return 1;
  }

  if (nfq_set_mode(nfqqh, NFQNL_COPY_PACKET, BUFSIZE) < 0) {
    fprintf(stderr, "*** nfq_set_mode()\n");
    return 1;
  }

  nfqfd = nfq_fd(nfqh);
  while((num = recv(nfqfd, buf, BUFSIZE, 0)) >= 0) {
    nfq_handle_packet(nfqh, buf, num);
  }

  nfq_destroy_queue(nfqqh);
  nfq_close(nfqh);
  close(fd);
  return 0;
}