#include <sys/types.h>

#ifdef __FreeBSD__
#include <netinet/in_systm.h>
#endif

#include <net/ethernet.h>
#include <netinet/in.h>
#include <netinet/ip.h>

#include <pcap.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>

static char filter_code[] = "";

void got_packet(u_char *args, const struct pcap_pkthdr *header,
                const u_char *packet) {
  //fwrite(packet, 1, header->len, stdout);
  //fflush(stdout);
  printf("Packet\n"); //, len %d (avail: %d)\n", header->len, header->caplen);
}

int main(int argc, char **argv) {
  struct bpf_program filter;
  struct bpf_program prefilter;
  pcap_t *handle;
  char *dev = NULL;
  char errbuf[PCAP_ERRBUF_SIZE];

  /* Generated with:
   * tcpdump -n -dd '(ip and (host 192.168.0.20 or broadcast or multicast))'
   */
  struct bpf_insn prefilter_insns[] = {
    { 0x28, 0, 0, 0x0000000c },
    { 0x15, 0, 11, 0x00000800 },
    { 0x20, 0, 0, 0x0000001a },
    { 0x15, 8, 0, 0xc0a80014 }, /* My IP: 192.168.0.20 */
    { 0x20, 0, 0, 0x0000001e },
    { 0x15, 6, 0, 0xc0a80014 }, /* My IP: 192.168.0.20 */
    { 0x20, 0, 0, 0x00000002 },
    { 0x15, 0, 2, 0xffffffff }, /* mac FF:FF:FF:FF ... */
    { 0x28, 0, 0, 0x00000000 }, 
    { 0x15, 2, 0, 0x0000ffff }, /* mac ... FF:FF */
    { 0x30, 0, 0, 0x00000000 },
    { 0x45, 0, 1, 0x00000001 },
    BPF_STMT(BPF_JMP+BPF_JA, 1), /* success, jump to the specified filter now */
    { 0x6, 0, 0, 0x00000000 },
  };

  dev = pcap_lookupdev(errbuf);
  handle = pcap_open_live(dev, BUFSIZ, 1, 1000, errbuf);
  if (pcap_compile(handle, &filter, argv[1], 0, 0) == -1) {
    printf("Error: %s\n", pcap_geterr(handle));
    return 1;
  }

  prefilter.bf_len = 14;
  prefilter.bf_insns = prefilter_insns;
  printf("Len: %d\n", prefilter.bf_len);

  /* Inject our prefilter before the provided filter. */
  filter.bf_len += prefilter.bf_len;
  filter.bf_insns = (struct bpf_insn *) 
                    realloc(filter.bf_insns,  
                            filter.bf_len * sizeof(struct bpf_insn));

  memmove(filter.bf_insns + prefilter.bf_len,
          filter.bf_insns,
          (filter.bf_len - prefilter.bf_len) * sizeof(struct bpf_insn));
  memcpy(filter.bf_insns, prefilter_insns,
         prefilter.bf_len * sizeof(struct bpf_insn));

  if (0) { 
    printf("AFTER\n");
    for (int i = 0; i < filter.bf_len; i++) {
      printf("{ 0x%04x, %01d, %01d, 0x%08x }\n",
        filter.bf_insns[i].code,
        filter.bf_insns[i].jt,
        filter.bf_insns[i].jf,
        filter.bf_insns[i].k);
    }
  }

  printf("OK\n");
  pcap_setfilter(handle, &filter);

  pcap_loop(handle, -1, got_packet, NULL);
  return 0;
}