1 files changed, 94 insertions, 125 deletions

@@ -14,10 +14,12 @@
 
 #include "addr_list.h"
 #include "beacon_data.h"
+#include "retransmit.h"
+#include "transmit.h"
 
-static unsigned char kBroadcastAddress[] = "\xFF\xFF\xFF\xFF\xFF\xFF";
+#define CHANNEL_NUM 13
 
-static pthread_mutex_t socket_mutex = PTHREAD_MUTEX_INITIALIZER;
+static unsigned char kBroadcastAddress[] = "\xFF\xFF\xFF\xFF\xFF\xFF";
 
 static pthread_mutex_t seq_mutex = PTHREAD_MUTEX_INITIALIZER;
 static int seq_number = 0;
@@ -50,7 +52,8 @@ int get_interface_index(int sock, const char *interface, int *index)
   return 0;
 }
 
-int get_next_seq_number() {
+int get_next_seq_number()
+{
   pthread_mutex_lock(&seq_mutex);
   int next = seq_number++;
   pthread_mutex_unlock(&seq_mutex);
@@ -62,7 +65,7 @@ int make_beacon_frame(int index, const unsigned char tx_addr[6], int seq_number,
 {
   struct libwifi_beacon beacon = {0};
 
-  if (libwifi_create_beacon(&beacon, kBroadcastAddress, tx_addr, tx_addr, NULL, 13))
+  if (libwifi_create_beacon(&beacon, kBroadcastAddress, tx_addr, tx_addr, NULL, CHANNEL_NUM))
   {
     printf("Could not create beacon frame.\n");
     return -3;
@@ -80,7 +83,8 @@ int make_beacon_frame(int index, const unsigned char tx_addr[6], int seq_number,
   }
 
   static const unsigned char tim_bitmap[] = {0x01, 0x02, 0x00, 0x00};
-  if (libwifi_quick_add_tag(&beacon.tags, TAG_TIM, tim_bitmap, 4)) {
+  if (libwifi_quick_add_tag(&beacon.tags, TAG_TIM, tim_bitmap, 4))
+  {
     printf("Could not add TIM tag.\n");
     return -7;
   }
@@ -134,88 +138,17 @@ int make_beacon_frame(int index, const unsigned char tx_addr[6], int seq_number,
   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)) {
+  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));
+  int ret = send_packet_with_radiotap(sock, device_index, dst_addr, (unsigned char *)&ack_frame, sizeof(struct libwifi_cts));
 
   return ret;
 }
@@ -232,10 +165,22 @@ struct thread_info
   int sock;
   unsigned char tx_addr[6];
   int device_index;
+  struct retransmitter *retransmitter;
 
   struct addr_list authenticated;
 };
 
+enum auth_state
+{
+  kAuthStateAuthenticated,
+  kAuthStateAssociated,
+};
+
+struct auth_info
+{
+  enum auth_state auth_state;
+};
+
 void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const u_char *bytes)
 {
   struct thread_info *tinfo = (struct thread_info *)userdata;
@@ -249,7 +194,8 @@ void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const
 
   if (frame.frame_control.type == TYPE_MANAGEMENT && frame.frame_control.subtype == SUBTYPE_AUTH)
   {
-    if (strncmp(frame.header.data.addr1, tinfo->tx_addr, 6)) {
+    if (strncmp(frame.header.data.addr1, tinfo->tx_addr, 6))
+    {
       // This is not for me. Misdelivery.
       return;
     }
@@ -271,11 +217,14 @@ void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const
       return;
     }
 
-    //usleep(frame.header.ctrl.duration / 2 * 100);
+    // 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 auth_info *ainfo = (struct auth_info *)malloc(sizeof(struct auth_info));
+    ainfo->auth_state = kAuthStateAuthenticated;
+
+    addr_list_add(&tinfo->authenticated, frame.header.data.addr2, ainfo);
 
     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))
@@ -287,8 +236,8 @@ void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const
     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);
+    // 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);
@@ -306,23 +255,73 @@ void handle_incoming_packet(u_char *userdata, const struct pcap_pkthdr *h, const
 
     libwifi_free_auth(&auth_resp);
 
-    //usleep(frame.header.ctrl.duration * 100);
+    // usleep(frame.header.ctrl.duration * 100);
+
+    send_until_acked(tinfo->retransmitter, frame.header.data.addr2, auth_resp_output, auth_resp_size);
 
-    if (send_packet_with_radiotap(tinfo->sock, tinfo->device_index, frame.header.data.addr2, auth_resp_output, auth_resp_size))
+    printf("Successfully authenticated %s!\n", in_addr);
+  }
+  else if (frame.frame_control.type == TYPE_CONTROL && frame.frame_control.subtype == SUBTYPE_ACK)
+  {
+    if (strncmp(frame.body, tinfo->tx_addr, 6))
     {
-      printf("Could not send auth response.\n");
+      // This is not for me. Misdelivery.
       return;
     }
 
-    usleep(5000);
+    handle_ack(tinfo->retransmitter);
+  }
+  else if (frame.frame_control.type == TYPE_MANAGEMENT && frame.frame_control.subtype == SUBTYPE_ASSOC_REQ)
+  {
+    if (strncmp(frame.header.data.addr1, tinfo->tx_addr, 6))
+    {
+      // This is not for me. Misdelivery.
+      return;
+    }
 
-    if (send_packet_with_radiotap(tinfo->sock, tinfo->device_index, frame.header.data.addr2, auth_resp_output, auth_resp_size))
+    unsigned char in_addr[18];
+    format_mac_address(frame.header.data.addr2, in_addr);
+    printf("Association request from %s\n", in_addr);
+
+    if (!addr_list_contains(&tinfo->authenticated, frame.header.data.addr2))
     {
-      printf("Could not send auth response.\n");
+      // Not authenticated.
       return;
     }
 
-    printf("Successfully authenticated %s!\n", in_addr);
+    struct auth_info *ainfo = addr_list_get(&tinfo->authenticated, frame.header.data.addr2);
+    if (ainfo->auth_state != kAuthStateAuthenticated)
+    {
+      // Already associated.
+      return;
+    }
+
+    send_ack(tinfo->sock, tinfo->device_index, frame.header.data.addr2);
+
+    ainfo->auth_state = kAuthStateAssociated;
+
+    struct libwifi_assoc_resp assoc_resp = {0};
+    if (libwifi_create_assoc_resp(&assoc_resp, frame.header.data.addr2, tinfo->tx_addr, tinfo->tx_addr, CHANNEL_NUM))
+    {
+      printf("Could not create assoc response.\n");
+      return;
+    }
+
+    assoc_resp.frame_header.seq_control.sequence_number = get_next_seq_number();
+
+    size_t assoc_resp_length = libwifi_get_assoc_resp_length(&assoc_resp);
+    unsigned char *assoc_resp_buffer = malloc(assoc_resp_length);
+    if (libwifi_dump_assoc_resp(&assoc_resp, assoc_resp_buffer, assoc_resp_length) < 0)
+    {
+      printf("Could not dump assoc response.\n");
+      return;
+    }
+
+    libwifi_free_assoc_resp(&assoc_resp);
+
+    send_until_acked(tinfo->retransmitter, frame.header.data.addr2, assoc_resp_buffer, assoc_resp_length);
+
+    printf("Successfully associated with %s!\n", in_addr);
   }
 }
 
@@ -330,31 +329,6 @@ 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)
@@ -396,21 +370,16 @@ int main(int argc, char **argv)
     return 2;
   }
 
+  struct retransmitter *retransmitter = start_retransmit_thread(sock, device_index);
+
   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;
+  tinfo->retransmitter = retransmitter;
   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);
+  pthread_create(&tinfo->thread_id, NULL, &polling_thread, tinfo);
 
   int beacon_index = 0;
   for (;;)