1 files changed, 446 insertions, 0 deletions

@@ -0,0 +1,446 @@
+#include <errno.h>
+#include <fcntl.h>
+#include <libwifi.h>
+#include <linux/if_packet.h>
+#include <net/if.h>
+#include <netinet/ether.h>
+#include <netinet/ip.h>
+#include <pcap/pcap.h>
+#include <poll.h>
+#include <pthread.h>
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+
+#include "addr_list.h"
+#include "beacon_data.h"
+
+static unsigned char kBroadcastAddress[] = "\xFF\xFF\xFF\xFF\xFF\xFF";
+
+static pthread_mutex_t socket_mutex = PTHREAD_MUTEX_INITIALIZER;
+
+static pthread_mutex_t seq_mutex = PTHREAD_MUTEX_INITIALIZER;
+static int seq_number = 0;
+
+int get_interface_mac_address(int sock, const char *interface, unsigned char *output)
+{
+  struct ifreq ifr = {0};
+  strncpy(ifr.ifr_name, interface, IFNAMSIZ - 1);
+
+  if (ioctl(sock, SIOCGIFHWADDR, &ifr) < 0)
+  {
+    return -1;
+  }
+
+  memcpy(output, ifr.ifr_hwaddr.sa_data, 6);
+  return 0;
+}
+
+int get_interface_index(int sock, const char *interface, int *index)
+{
+  struct ifreq ifr = {0};
+  strncpy(ifr.ifr_name, interface, IFNAMSIZ - 1);
+
+  if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0)
+  {
+    return -1;
+  }
+
+  *index = ifr.ifr_ifindex;
+  return 0;
+}
+
+int get_next_seq_number() {
+  pthread_mutex_lock(&seq_mutex);
+  int next = seq_number++;
+  pthread_mutex_unlock(&seq_mutex);
+
+  return next;
+}
+
+int make_beacon_frame(int index, const unsigned char tx_addr[6], int seq_number, unsigned char **buf)
+{
+  struct libwifi_beacon beacon = {0};
+
+  if (libwifi_create_beacon(&beacon, kBroadcastAddress, tx_addr, tx_addr, NULL, 13))
+  {
+    printf("Could not create beacon frame.\n");
+    return -3;
+  }
+
+  beacon.frame_header.seq_control.sequence_number = seq_number;
+  beacon.fixed_parameters.capabilities_information = (1 << CAPABILITIES_ESS) | (1 << CAPABILITIES_SHORT_PREAMBLE) | (1 << CAPABILITIES_SHORT_SLOT);
+  beacon.fixed_parameters.beacon_interval = 200;
+
+  static const unsigned char supported_rates[] = {0x82, 0x84, 0x0b, 0x16, 0x24, 0x30, 0x48, 0x6c};
+  if (libwifi_quick_add_tag(&beacon.tags, TAG_SUPP_RATES, supported_rates, 8))
+  {
+    printf("Could not add supported rates tag.\n");
+    return -7;
+  }
+
+  static const unsigned char tim_bitmap[] = {0x01, 0x02, 0x00, 0x00};
+  if (libwifi_quick_add_tag(&beacon.tags, TAG_TIM, tim_bitmap, 4)) {
+    printf("Could not add TIM tag.\n");
+    return -7;
+  }
+
+  static const unsigned char nothing = 0;
+  libwifi_quick_add_tag(&beacon.tags, TAG_ERP, &nothing, 1);
+  libwifi_quick_add_tag(&beacon.tags, 47, &nothing, 1);
+
+  static const unsigned char extended_supported_rates[] = {0x0c, 0x12, 0x18, 0x60};
+  if (libwifi_quick_add_tag(&beacon.tags, TAG_EXTENDED_SUPPORTED_RATES, extended_supported_rates, 4))
+  {
+    printf("Could not add extended supported rates tag.\n");
+    return -7;
+  }
+
+  unsigned char payload_data[BEACON_PAYLOAD_LENGTH + 8];
+  payload_data[0] = 0x00;
+  payload_data[1] = 0x09;
+  payload_data[2] = 0xBF;
+  payload_data[3] = 0x00;
+  payload_data[4] = 0xFF;
+  payload_data[5] = 0xFF;
+  payload_data[6] = 0x00;
+  payload_data[7] = 0x00;
+  memcpy(payload_data + 8, kBeaconPayloads[index], BEACON_PAYLOAD_LENGTH);
+
+  if (libwifi_quick_add_tag(&beacon.tags, TAG_VENDOR_SPECIFIC, payload_data, BEACON_PAYLOAD_LENGTH + 8))
+  {
+    printf("Could not add beacon data tag.\n");
+    return -6;
+  }
+
+  size_t beacon_size = libwifi_get_beacon_length(&beacon);
+  unsigned char *beacon_output = (unsigned char *)malloc(beacon_size);
+  if (beacon_output == NULL)
+  {
+    printf("Could not allocate beacon output.\n");
+    return -4;
+  }
+
+  if (libwifi_dump_beacon(&beacon, beacon_output, beacon_size) < 0)
+  {
+    printf("Could not dump beacon.\n");
+    return -5;
+  }
+
+  libwifi_free_beacon(&beacon);
+
+  *buf = beacon_output;
+
+  return beacon_size;
+}
+
+int prepend_radiotap(const unsigned char *input, int input_size, unsigned char **output)
+{
+  static const unsigned char radiotap[] = "\x00\x00\x08\x00\x00\x00\x00\x00";
+  static const int radiotap_size = 8;
+
+  int output_size = input_size + radiotap_size;
+
+  unsigned char *buf = (unsigned char *)malloc(output_size);
+  if (buf == NULL)
+  {
+    return -1;
+  }
+
+  memcpy(buf, radiotap, radiotap_size);
+  memcpy(buf + radiotap_size, input, input_size);
+
+  *output = buf;
+
+  return output_size;
+}
+
+int send_packet(int sock, int device_index, const unsigned char dst_addr[6], const unsigned char *packet, int packet_size)
+{
+  struct sockaddr_ll socket_address;
+  socket_address.sll_ifindex = device_index;
+  socket_address.sll_halen = ETH_ALEN;
+  memcpy(socket_address.sll_addr, dst_addr, 6);
+
+  pthread_mutex_lock(&socket_mutex);
+
+  int ret = 0;
+  if (sendto(sock, packet, packet_size, 0, (struct sockaddr *)&socket_address, sizeof(struct sockaddr_ll)) < 0)
+  {
+    printf("Could not send packet: %d\n", errno);
+    ret = errno;
+  }
+
+  pthread_mutex_unlock(&socket_mutex);
+
+  return ret;
+}
+
+int send_packet_with_radiotap(int sock, int device_index, const unsigned char dst_addr[6], const unsigned char *packet, int packet_size)
+{
+  struct libwifi_radiotap_info radiotap_info = {0};
+  radiotap_info.present = (1 << IEEE80211_RADIOTAP_FLAGS) | (1 << IEEE80211_RADIOTAP_RATE);
+  radiotap_info.flags = IEEE80211_RADIOTAP_F_FCS | IEEE80211_RADIOTAP_F_SHORTPRE;
+  radiotap_info.rate_raw = 4;
+
+  unsigned char radiotap_buffer[256];
+  int radiotap_size = libwifi_create_radiotap(&radiotap_info, radiotap_buffer);
+  
+  uint32_t fcs = libwifi_calculate_fcs(packet, packet_size);
+
+  int buffer_size = radiotap_size + packet_size + sizeof(uint32_t);
+  unsigned char *buffer = malloc(buffer_size);
+  if (buffer == NULL)
+  {
+    printf("Could not prepend radiotap.\n");
+    return -2;
+  }
+
+  memcpy(buffer, radiotap_buffer, radiotap_size);
+  memcpy(buffer + radiotap_size, packet, packet_size);
+  memcpy(buffer + radiotap_size + packet_size, (unsigned char*)&fcs, sizeof(uint32_t));
+
+  int ret = send_packet(sock, device_index, dst_addr, buffer, buffer_size);
+  free(buffer);
+
+  return ret;
+}
+
+int send_ack(int sock, int device_index, const unsigned char dst_addr[6])
+{
+  struct libwifi_cts ack_frame = {0};
+  if (libwifi_create_cts(&ack_frame, dst_addr, 0)) {
+    printf("Could not create ack packet.\n");
+  }
+
+  ack_frame.frame_header.frame_control.subtype = SUBTYPE_ACK;
+
+  int ret = send_packet_with_radiotap(sock, device_index, dst_addr, (unsigned char*)&ack_frame, sizeof(struct libwifi_cts));
+
+  return ret;
+}
+
+// output buffer size must be at least 18 bytes
+void format_mac_address(const unsigned char addr[6], char *output)
+{
+  sprintf(output, "%02X:%02X:%02X:%02X:%02X:%02X", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
+}
+
+struct thread_info
+{
+  pthread_t thread_id;
+  int sock;
+  unsigned char tx_addr[6];
+  int device_index;
+
+  struct addr_list authenticated;
+};
+
+void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const u_char *bytes)
+{
+  struct thread_info *tinfo = (struct thread_info *)userdata;
+
+  struct libwifi_frame frame = {0};
+  if (libwifi_get_wifi_frame(&frame, bytes, h->caplen, true))
+  {
+    printf("Could not parse frame.\n");
+    return;
+  }
+
+  if (frame.frame_control.type == TYPE_MANAGEMENT && frame.frame_control.subtype == SUBTYPE_AUTH)
+  {
+    if (strncmp(frame.header.data.addr1, tinfo->tx_addr, 6)) {
+      // This is not for me. Misdelivery.
+      return;
+    }
+
+    unsigned char in_addr[18];
+    format_mac_address(frame.header.data.addr2, in_addr);
+    printf("Authentication request from %s\n", in_addr);
+
+    if (addr_list_contains(&tinfo->authenticated, frame.header.data.addr2))
+    {
+      // Already authenticated.
+      return;
+    }
+
+    struct libwifi_auth_fixed_parameters *afp = (struct libwifi_auth_fixed_parameters *)frame.body;
+    if (afp->algorithm_number != AUTH_OPEN || afp->status_code != STATUS_SUCCESS || afp->transaction_sequence != 1)
+    {
+      // Incorrect auth details.
+      return;
+    }
+
+    //usleep(frame.header.ctrl.duration / 2 * 100);
+
+    send_ack(tinfo->sock, tinfo->device_index, frame.header.data.addr2);
+
+    addr_list_add(&tinfo->authenticated, frame.header.data.addr2);
+
+    struct libwifi_auth auth_resp = {0};
+    if (libwifi_create_auth(&auth_resp, frame.header.data.addr2, frame.header.data.addr1, frame.header.data.addr1, AUTH_OPEN, 2, STATUS_SUCCESS))
+    {
+      printf("Could not create auth response.\n");
+      return;
+    }
+
+    auth_resp.frame_header.duration = frame.header.ctrl.duration;
+    auth_resp.frame_header.seq_control.sequence_number = get_next_seq_number();
+
+    //static const unsigned char broadcom_tag_value[] = {0x00, 0x10, 0x18, 0x02, 0x00, 0xf0, 0x00, 0x00, 0x00};
+    //libwifi_quick_add_tag(&auth_resp.tags, TAG_VENDOR_SPECIFIC, broadcom_tag_value, 9);
+
+    size_t auth_resp_size = libwifi_get_auth_length(&auth_resp);
+    unsigned char *auth_resp_output = (unsigned char *)malloc(auth_resp_size);
+    if (auth_resp_output == NULL)
+    {
+      printf("Could not allocate auth response output.\n");
+      return;
+    }
+
+    if (libwifi_dump_auth(&auth_resp, auth_resp_output, auth_resp_size) < 0)
+    {
+      printf("Could not dump auth response.\n");
+      return;
+    }
+
+    libwifi_free_auth(&auth_resp);
+
+    //usleep(frame.header.ctrl.duration * 100);
+
+    if (send_packet_with_radiotap(tinfo->sock, tinfo->device_index, frame.header.data.addr2, auth_resp_output, auth_resp_size))
+    {
+      printf("Could not send auth response.\n");
+      return;
+    }
+
+    usleep(5000);
+
+    if (send_packet_with_radiotap(tinfo->sock, tinfo->device_index, frame.header.data.addr2, auth_resp_output, auth_resp_size))
+    {
+      printf("Could not send auth response.\n");
+      return;
+    }
+
+    printf("Successfully authenticated %s!\n", in_addr);
+  }
+}
+
+void *polling_thread(void *arg)
+{
+  struct thread_info *tinfo = (struct thread_info *)arg;
+
+  /*for (;;) {
+    struct pollfd pfd;
+    pfd.fd = tinfo->sock;
+    pfd.events = POLLIN;
+
+    if (poll(&pfd, 1, -1) < 0) {
+      printf("Error polling.\n");
+    } else {
+      if (pfd.revents & POLLIN) {
+        unsigned char message[1024];
+        int msglen = read(tinfo->sock, message, 1024);
+        if (msglen < 0) {
+          printf("Could not read from socket.\n");
+        } else {
+          struct libwifi_frame frame = {0};
+          if (libwifi_get_wifi_frame(&frame, message, msglen, true)) {
+            printf("Could not parse frame.\n");
+          } else {
+            printf("Frame type: %d:%d\n", frame.frame_control.type, frame.frame_control.subtype);
+          }
+        }
+      }
+    }
+  }*/
+
+  char errbuf[PCAP_ERRBUF_SIZE];
+  pcap_t *sniffer = pcap_create("mon0", errbuf);
+  if (sniffer == NULL)
+  {
+    printf("Could not open sniffer: %s\n", errbuf);
+    return NULL;
+  }
+
+  pcap_set_promisc(sniffer, 1);
+  pcap_set_immediate_mode(sniffer, 1);
+  pcap_activate(sniffer);
+  pcap_loop(sniffer, -1, handle_incoming_packet, (u_char *)tinfo);
+
+  return NULL;
+}
+
+int main(int argc, char **argv)
+{
+  static const char *interface_name = "mon0";
+
+  int sock = socket(AF_PACKET, SOCK_RAW, 0);
+  if (sock == -1)
+  {
+    printf("Could not open socket.\n");
+    return 1;
+  }
+
+  unsigned char tx_addr[6];
+  if (get_interface_mac_address(sock, interface_name, tx_addr))
+  {
+    printf("Could not get hardware address.\n");
+    return 2;
+  }
+
+  int device_index = 0;
+  if (get_interface_index(sock, interface_name, &device_index))
+  {
+    printf("Could not get device index.\n");
+    return 2;
+  }
+
+  struct thread_info *tinfo = (struct thread_info *)malloc(sizeof(struct thread_info *));
+  tinfo->sock = sock;
+  memcpy(tinfo->tx_addr, tx_addr, 6);
+  tinfo->device_index = device_index;
+  addr_list_init(&tinfo->authenticated);
+
+  pthread_attr_t thread_attr;
+  if (pthread_attr_init(&thread_attr))
+  {
+    printf("Could not initialize thread attr.\n");
+    return 6;
+  }
+
+  pthread_create(&tinfo->thread_id, &thread_attr, &polling_thread, tinfo);
+  pthread_attr_destroy(&thread_attr);
+
+  int beacon_index = 0;
+  for (;;)
+  {
+    unsigned char *beacon_output;
+    int beacon_size = make_beacon_frame(beacon_index, tx_addr, get_next_seq_number(), &beacon_output);
+    if (beacon_size < 0)
+    {
+      return 3;
+    }
+
+    if (send_packet_with_radiotap(sock, device_index, kBroadcastAddress, beacon_output, beacon_size))
+    {
+      return 4;
+    }
+
+    free(beacon_output);
+
+    beacon_index = (beacon_index + 1) % 10;
+
+    usleep(1024 * 200);
+  }
+
+  if (pthread_join(tinfo->thread_id, NULL))
+  {
+    printf("Could not join thread I guess.\n");
+    return 6;
+  }
+
+  free(tinfo);
+
+  return 0;
+}