probecap.c

/* SPDX-License-Identifier: 0BSD OR OR MIT-0 OR Unlicense OR CC0-1.0+
Copyright ©2023 Ryan Castellucci, no rights reserved.
gcc -O2 probecap.c -lpcap -o probecap #*/

#include <string.h>
#include <stdint.h>
#include <errno.h>
#include <stdio.h>

#include <sys/socket.h>

#include <pcap/pcap.h>

#define SNAPLEN 1432

// identical to "subtype probe-req"
static const char probes[] = "((wlan[0] & 0xfc) = 0x40)";

/* 40 bytes */
struct header_s {
  uint32_t  magic_number;
  uint16_t  version_major;
  uint16_t  version_minor;
  int32_t   thiszone;
  uint32_t  sigfigs;
  uint32_t  snaplen;
  uint32_t  network;
  uint32_t  ts_sec;
  uint32_t  ts_usec;
  uint32_t  incl_len;
  uint32_t  orig_len;
};

struct userdata_s {
  struct addrinfo *ai;
  int fd;
  int linktype;
  uint32_t snaplen;
};

void handler(uint8_t *user, const struct pcap_pkthdr *h, const uint8_t *bytes) {
  uint8_t buf[65536];

  struct userdata_s *u = (struct userdata_s *)user;
  struct header_s *hdr = (struct header_s *)buf;
  uint8_t *pkt = buf + sizeof(struct header_s);

  uint32_t caplen = h->caplen <= u->snaplen ? h->caplen : u->snaplen;

  hdr->magic_number = 0xa1b2c3d4;
  hdr->version_major = 2;
  hdr->version_minor = 4;
  hdr->thiszone = 0;
  hdr->sigfigs = 0;
  hdr->snaplen = u->snaplen;
  hdr->network = u->linktype;
  hdr->ts_sec = h->ts.tv_sec;
  hdr->ts_usec = h->ts.tv_usec;
  hdr->incl_len = caplen;
  hdr->orig_len = h->len;

  memcpy(pkt, bytes, caplen);
  ssize_t len = sizeof(struct header_s) + caplen;
  ssize_t n = sendto(u->fd, buf, len, 0, u->ai->ai_addr, u->ai->ai_addrlen);
  if (n != len) { perror("sendto"); }
}

int main(int argc, char *argv[]) {
  const char *filter = probes;
  char errbuf[PCAP_ERRBUF_SIZE];
  ssize_t rv;

  struct addrinfo hints, *res, *ai;
  struct userdata_s user;
  struct bpf_program fp;

  if (argc < 4 || argc > 5) {
    fprintf(stderr, "Usage: %s IFACE HOST PORT [FILTER]\n", argv[0]);
    return -1;
  } else if (argc == 4) {
    filter = argv[4];
  }

  char *iface = argv[1];
  char *host = argv[2];
  char *port = argv[3];

  memset(&hints, 0, sizeof hints);
  hints.ai_family = AF_UNSPEC;
  hints.ai_socktype = SOCK_DGRAM;
  hints.ai_flags = AI_PASSIVE;

  getaddrinfo(host, port, &hints, &res);

  for (ai = res; ai != NULL; ai = ai->ai_next) {
    user.fd = socket(ai->ai_family, ai->ai_socktype, ai->ai_protocol);
    if (user.fd == -1) {
      continue;
    }

    break;
  }

  if (ai == NULL) {
    fprintf(stderr, "Could not open socket.");
    return -1;
  }

  user.ai = ai;
  user.snaplen = SNAPLEN;

  pcap_t *pcap = pcap_create(iface, errbuf);

  if ((rv = pcap_set_snaplen(pcap, user.snaplen)) != 0) {
    pcap_perror(pcap, "pcap_set_snaplen");
    return -1;
  }
  if ((rv = pcap_set_promisc(pcap, 1)) != 0) {
    pcap_perror(pcap, "pcap_set_promisc");
    return -1;
  }
  if ((rv = pcap_set_timeout(pcap, 100)) != 0) {
    pcap_perror(pcap, "pcap_set_timeout");
    return -1;
  }
  if ((rv = pcap_set_immediate_mode(pcap, 1)) != 0) {
    pcap_perror(pcap, "pcap_set_immediate_mode");
    return -1;
  }
  if ((rv = pcap_activate(pcap)) != 0) {
    pcap_perror(pcap, "pcap_activate");
    return -1;
  }

  if ((rv = pcap_compile(pcap, &fp, filter, 1, PCAP_NETMASK_UNKNOWN)) != 0) {
    pcap_perror(pcap, "pcap_compile");
    return -1;
  }
  if ((rv = pcap_setfilter(pcap, &fp)) != 0) {
    pcap_perror(pcap, "pcap_setfilter");
    return -1;
  }
  if ((rv = pcap_setdirection(pcap, PCAP_D_IN)) != 0) {
    pcap_perror(pcap, "pcap_setdirection");
    return -1;
  }

  user.linktype = pcap_datalink(pcap);

  return pcap_loop(pcap, -1, &handler, (uint8_t *)&user);
}