raw_wifi_probe.c

#include <errno.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <net/if.h>
#include <linux/if_packet.h>
#include <linux/if_ether.h>
#include <linux/nl80211.h>
#include <linux/genetlink.h>
#include <netlink/genl/genl.h>
#include <netlink/genl/family.h>
#include <netlink/genl/ctrl.h>
#include <netlink/msg.h>
#include <netlink/attr.h>

#define RATES           \
    "\x01\x04\x02\x04\x0B\x16\x32\x08\x0C\x12\x18\x24\x30\x48\x60\x6C"

#define PROBE_REQ       \
    "\x40\x00\x00\x00\xFF\xFF\xFF\xFF\xFF\xFF\xCC\xCC\xCC\xCC\xCC\xCC"  \
    "\xFF\xFF\xFF\xFF\xFF\xFF\x00\x00"

int get_interface_idx(int fd, char* iface) {
  struct ifreq ifr;
  memset( &ifr, 0, sizeof( ifr ) );
  strncpy( ifr.ifr_name, iface, sizeof( ifr.ifr_name ) - 1 );
  if( ioctl( fd, SIOCGIFINDEX, &ifr ) < 0 ) {
    fprintf(stderr, "ioctl(SIOCGIFINDEX) failed\n");
    return -1;
  }
  return ifr.ifr_ifindex;
}

int main(int argc, char* argv[]) {
  struct nl_cache *nl_cache;
  struct genl_family *nl80211;
  struct nl_sock *nl_sock = nl_socket_alloc();
  unsigned char h80211[4096];
  int err = 0;

  if (!nl_sock) {
    fprintf(stderr, "Failed to allocate netlink socket.\n");
    return -ENOMEM;
  }

  if (genl_connect(nl_sock)) {
    fprintf(stderr, "Failed to connect to generic netlink.\n");
    err = -ENOLINK;
    goto out_handle_destroy;
  }

  if (genl_ctrl_alloc_cache(nl_sock, &nl_cache)) {
    fprintf(stderr, "Failed to allocate generic netlink cache.\n");
    err = -ENOMEM;
    goto out_handle_destroy;
  }

  nl80211 = genl_ctrl_search_by_name(nl_cache, "nl80211");
  if (!nl80211) {
    fprintf(stderr, "nl80211 not found.\n");
    err = -ENOENT;
    goto out_cache_free;
  }

  int fd_out = socket( PF_PACKET, SOCK_RAW, htons( ETH_P_ALL ));
  if (fd_out < 0) {
    fprintf(stderr, "socket(PF_PACKET) failed.\n");
    goto close_fd;
  }

  int ifidx = get_interface_idx(fd_out, "wlan0");
  if (ifidx < 0)
    goto close_fd;

  struct sockaddr_ll sll;
  memset( &sll, 0, sizeof( sll ) );
  sll.sll_family   = AF_PACKET;
  sll.sll_ifindex  = ifidx;
  sll.sll_protocol = htons( ETH_P_ALL );

  if( bind( fd_out, (struct sockaddr *) &sll, sizeof( sll ) ) < 0 ) {
    fprintf(stderr, "bind(ETH_P_ALL) failed\n");
    goto close_fd;
  }

  unsigned char outbuf[4096];
  unsigned char u8aRadiotap[] = {
    0x00, 0x00, // <-- radiotap version
    0x0c, 0x00, // <- radiotap header length
    0x04, 0x80, 0x00, 0x00, // <-- bitmap
    0x00, // <-- rate
    0x00, // <-- padding for natural alignment
    0x18, 0x00, // <-- TX flags
  };
  unsigned long nb_pkt_sent = 0;

  int len = 24;
  memcpy(h80211, PROBE_REQ, 24);
  h80211[24] = 0x00;      //ESSID Tag Number
  h80211[25] = 0x00;      //ESSID Tag Length
  len += 2;
  memcpy(h80211+len, RATES, 16);
  len += 16;

  if( (len > 24) && (h80211[1] & 0x04) == 0 && (h80211[22] & 0x0F) == 0) {
    h80211[22] = (nb_pkt_sent & 0x0000000F) << 4;
    h80211[23] = (nb_pkt_sent & 0x00000FF0) >> 4;
  }

  memcpy(outbuf, u8aRadiotap, sizeof (u8aRadiotap) );
  memcpy(outbuf + sizeof (u8aRadiotap), h80211, len);
  len += sizeof (u8aRadiotap);

  int ret = write(fd_out, outbuf, len);
  if (ret < 0) {
    fprintf(stderr, "write failed: %s.\n", strerror(errno));
  }

  close_fd:
    close(fd_out);
  out_cache_free:
    nl_cache_free(nl_cache);
  out_handle_destroy:
    nl_socket_free(nl_sock);
    return err;
}