8 files changed, 86 insertions, 56 deletions

@@ -32,8 +32,7 @@
  *  - QoS Data Frames
  *  - Control Frames
  */
-int libwifi_get_wifi_frame(struct libwifi_frame *fi, const unsigned char *frame, size_t frame_len,
-                           int radiotap) {
+int libwifi_get_wifi_frame(struct libwifi_frame *fi, const unsigned char *frame, size_t frame_len, int radiotap) {
     union libwifi_frame_header fh = {0};
     size_t header_len = 0;
     size_t frame_data_len = frame_len;
@@ -55,6 +54,10 @@ int libwifi_get_wifi_frame(struct libwifi_frame *fi, const unsigned char *frame,
             fi->flags |= LIBWIFI_FLAGS_FCS_PRESENT;
             frame_data_len -= sizeof(uint32_t); // FCS is 4 bytes wide
         }
+
+        fi->flags |= LIBWIFI_FLAGS_RADIOTAP_PRESENT;
+        fi->radiotap_info = malloc(sizeof(struct libwifi_radiotap_info));
+        memcpy(fi->radiotap_info, &rtap_info, sizeof(struct libwifi_radiotap_info));
     }
 
     struct libwifi_frame_ctrl *frame_control = (struct libwifi_frame_ctrl *) frame_data;
@@ -134,5 +137,6 @@ int libwifi_get_wifi_frame(struct libwifi_frame *fi, const unsigned char *frame,
 }
 
 void libwifi_free_wifi_frame(struct libwifi_frame *fi) {
+    free(fi->radiotap_info);
     free(fi->body);
 }
@@ -17,6 +17,7 @@
 #define LIBWIFI_CORE_FRAME_H
 
 #include "../../core/misc/byteswap.h"
+#include "../../core/misc/radiotap.h"
 
 #include <stdint.h>
 #include <sys/types.h>
@@ -25,6 +26,7 @@
 #define LIBWIFI_FLAGS_FCS_PRESENT (1 << 0)
 #define LIBWIFI_FLAGS_IS_QOS (1 << 1)
 #define LIBWIFI_FLAGS_IS_ORDERED (1 << 2)
+#define LIBWIFI_FLAGS_RADIOTAP_PRESENT (1 << 3)
 
 /* Defined frame types and sub-types */
 enum libwifi_frame_type {
@@ -306,6 +308,7 @@ union libwifi_mgmt_frame_header {
  * frame in libwifi.
  */
 struct libwifi_frame {
+    struct libwifi_radiotap_info *radiotap_info;
     uint16_t flags;
     struct libwifi_frame_ctrl frame_control;
     size_t len;
@@ -244,6 +244,13 @@ struct libwifi_tag_vendor_header {
     int8_t type;
 } __attribute__((packed));
 
+/*
+ * Element extension tagged parameters have a tag number
+ */
+struct libwifi_tag_extension_header {
+    uint8_t tag_num;
+} __attribute__((packed));
+
 /**
  * Add a tagged parameter to a list of frame tagged parameters.
  *
@@ -21,13 +21,22 @@
 #define LIBWIFI_MAX_RADIOTAP_LEN 128
 #define LIBWIFI_MAX_RADIOTAP_ANTENNAS 16
 
+#define LIBWIFI_RADIOTAP_BAND_2GHZ (1 << 0)
+#define LIBWIFI_RADIOTAP_BAND_5GHZ (1 << 1)
+#define LIBWIFI_RADIOTAP_BAND_6GHZ (1 << 2)
+#define LIBWIFI_RADIOTAP_BAND_900MHZ (1 << 3)
+
 /**
  * A channel field in radiotap consists of a 2-byte wide flags
- * sub-field and a 2-byte wide frequency field
+ * sub-field and a 2-byte wide frequency field.
+ *
+ * libwifi will also calculate the band and center channel.
  */
 struct libwifi_radiotap_channel {
     uint16_t flags;
     uint16_t freq;
+    uint8_t center;
+    uint8_t band;
 } __attribute__((packed));
 
 /**
@@ -61,6 +61,7 @@ int libwifi_parse_beacon(struct libwifi_bss *bss, struct libwifi_frame *frame) {
     }
 
     // At least one Tagged Parameter must be present
+    // + 2 represents two additional bytes: Tag number and a single byte value
     if (frame->len < (frame->header_len + sizeof(struct libwifi_beacon_fixed_parameters) + 2)) {
         return -EINVAL;
     }
@@ -142,6 +142,7 @@ int libwifi_bss_handle_msft_tag(struct libwifi_bss *bss, const unsigned char *ms
  */
 int libwifi_bss_tag_parser(struct libwifi_bss *bss, struct libwifi_tag_iterator *it) {
     struct libwifi_tag_vendor_header *vendor_header = NULL;
+    struct libwifi_tag_extension_header *extension_header = NULL;
 
     do {
         switch (it->tag_header->tag_num) {
@@ -167,6 +168,16 @@ int libwifi_bss_tag_parser(struct libwifi_bss *bss, struct libwifi_tag_iterator
                     }
                 }
                 break;
+            case TAG_ELEMENT_EXTENSION:
+                extension_header = (struct libwifi_tag_extension_header *) it->tag_data;
+
+                switch (extension_header->tag_num) {
+                    default:
+                        /* Not Implemented */
+                        break;
+                }
+
+                break;
         }
     } while (libwifi_tag_iterator_next(it) != -1);
 
@@ -47,6 +47,23 @@ int libwifi_parse_radiotap_info(struct libwifi_radiotap_info *info, const unsign
             case IEEE80211_RADIOTAP_CHANNEL:
                 info->channel.freq = le16toh(*(uint16_t *) it.this_arg);
                 info->channel.flags = le16toh(*(uint16_t *) (it.this_arg + 2));
+
+                // Handle band and channel
+                if (info->channel.freq >= 2412 && info->channel.freq <= 2484) {
+                    info->channel.band |= LIBWIFI_RADIOTAP_BAND_2GHZ;
+                    if (info->channel.freq == 2484) {
+                        info->channel.center = 14;
+                    } else {
+                        info->channel.center = (info->channel.freq - 2407) / 5;
+                    }
+                } else if (info->channel.freq >= 5160 && info->channel.freq <= 5885) {
+                    info->channel.band |= LIBWIFI_RADIOTAP_BAND_5GHZ;
+                    info->channel.center = (info->channel.freq - 5000) / 5;
+                } else if (info->channel.freq >= 5955 && info->channel.freq <= 7115) {
+                    info->channel.band |= LIBWIFI_RADIOTAP_BAND_6GHZ;
+                    info->channel.center = (info->channel.freq - 5950) / 5;
+                }
+
                 break;
             case IEEE80211_RADIOTAP_RATE:
                 info->rate_raw = *it.this_arg;
@@ -132,26 +132,36 @@ void print_tag_info(unsigned char *data, size_t data_len) {
     } while (libwifi_tag_iterator_next(&it) != -1);
 }
 
-void parse_radiotap(const unsigned char *packet, size_t packet_len) {
-    struct libwifi_radiotap_info rtap_info;
-    libwifi_parse_radiotap_info(&rtap_info, packet, packet_len);
+void parse_radiotap(const struct libwifi_frame *frame) {
+    const struct libwifi_radiotap_info *rtap_info = frame->radiotap_info;
 
     printf("=== Radiotap Parsing ===\n");
-    printf("Radiotap Channel: %d\n", rtap_info.channel.freq);
-    printf("Radiotap Channel Flags: 0x%04x\n", rtap_info.channel.flags);
-    printf("Radiotap Rate: %.2f Mb/s\n", rtap_info.rate);
-    printf("Radiotap Rate Raw: 0x%02x\n", rtap_info.rate_raw);
-    printf("Radiotap Signal: %d dBm\n", rtap_info.signal);
-    for (int i = 0; i < rtap_info.antenna_count; i++) {
-        printf("Radiotap Antenna %d: %d dBm\n", rtap_info.antennas[i].antenna_number, rtap_info.antennas[i].signal);
+    printf("Radiotap Channel Freq: %d MHz\n", rtap_info->channel.freq);
+    printf("Radiotap Freq Band: ");
+    if (rtap_info->channel.band & LIBWIFI_RADIOTAP_BAND_2GHZ) {
+        printf("2.4 GHz\n");
+    } else if (rtap_info->channel.band & LIBWIFI_RADIOTAP_BAND_5GHZ) {
+        printf("5 GHz\n");
+    } else if (rtap_info->channel.band & LIBWIFI_RADIOTAP_BAND_6GHZ) {
+        printf("6 GHz\n");
+    } else {
+        printf("Unknown Band\n");
+    }
+    printf("Radiotap Channel: %d\n", rtap_info->channel.center);
+    printf("Radiotap Channel Flags: 0x%04x\n", rtap_info->channel.flags);
+    printf("Radiotap Rate: %.2f Mb/s\n", rtap_info->rate);
+    printf("Radiotap Rate Raw: 0x%02x\n", rtap_info->rate_raw);
+    printf("Radiotap Signal: %d dBm\n", rtap_info->signal);
+    for (int i = 0; i < rtap_info->antenna_count; i++) {
+        printf("Radiotap Antenna %d: %d dBm\n", rtap_info->antennas[i].antenna_number, rtap_info->antennas[i].signal);
     }
-    printf("Radiotap Flags: 0x%04x\n", rtap_info.flags);
-    printf("Radiotap Extended Flags: 0x%08x\n", rtap_info.extended_flags);
-    printf("Radiotap RX Flags: 0x%04x\n", rtap_info.rx_flags);
-    printf("Radiotap TX Flags: 0x%04x\n", rtap_info.tx_flags);
-    printf("Radiotap TX Power: %d\n", rtap_info.tx_power);
-    printf("Radiotap RTS Retries: %d\n", rtap_info.rts_retries);
-    printf("Radiotap Data Retries: %d\n", rtap_info.data_retries);
+    printf("Radiotap Flags: 0x%04x\n", rtap_info->flags);
+    printf("Radiotap Extended Flags: 0x%08x\n", rtap_info->extended_flags);
+    printf("Radiotap RX Flags: 0x%04x\n", rtap_info->rx_flags);
+    printf("Radiotap TX Flags: 0x%04x\n", rtap_info->tx_flags);
+    printf("Radiotap TX Power: %d\n", rtap_info->tx_power);
+    printf("Radiotap RTS Retries: %d\n", rtap_info->rts_retries);
+    printf("Radiotap Data Retries: %d\n", rtap_info->data_retries);
     printf("=== Radiotap End ===\n");
 }
 
@@ -165,10 +175,6 @@ void parse_beacon(struct libwifi_frame frame, unsigned char *args, const struct
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         print_bss_info(&bss);
     }
 }
@@ -183,10 +189,6 @@ void parse_probe_request(struct libwifi_frame frame, unsigned char *args, const
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         print_sta_info(&sta);
     }
 }
@@ -200,10 +202,6 @@ void parse_probe_response(struct libwifi_frame frame, unsigned char *args, const
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         print_bss_info(&bss);
     }
 }
@@ -218,10 +216,6 @@ void parse_deauth(struct libwifi_frame frame, unsigned char *args, const struct
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         printf("=== Deauthentication Frame ===\n");
         if (deauth.ordered) {
             printf("Address 1: " MACSTR "\n", MAC2STR(deauth.frame_header.ordered.addr1));
@@ -257,10 +251,6 @@ void parse_disassoc(struct libwifi_frame frame, unsigned char *args, const struc
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         printf("=== Disassociation Frame ===\n");
         if (disassoc.ordered) {
             printf("Address 1: " MACSTR "\n", MAC2STR(disassoc.frame_header.ordered.addr1));
@@ -295,10 +285,6 @@ void parse_assoc_request(struct libwifi_frame frame, unsigned char *args, const
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         print_sta_info(&sta);
     }
 }
@@ -312,10 +298,6 @@ void parse_assoc_response(struct libwifi_frame frame, unsigned char *args, const
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         print_bss_info(&bss);
     }
 }
@@ -329,10 +311,6 @@ void parse_reassoc_request(struct libwifi_frame frame, unsigned char *args, cons
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         print_sta_info(&sta);
     }
 }
@@ -346,10 +324,6 @@ void parse_reassoc_response(struct libwifi_frame frame, unsigned char *args, con
             return;
         }
 
-        if (got_radiotap && parse_radiotap_header) {
-            parse_radiotap(packet, header->caplen);
-        }
-
         print_bss_info(&bss);
     }
 }
@@ -427,6 +401,10 @@ void parse_packet(unsigned char *args, const struct pcap_pkthdr *header, const u
         return;
     }
 
+    if (got_radiotap && parse_radiotap_header && frame.flags & LIBWIFI_FLAGS_RADIOTAP_PRESENT) {
+        parse_radiotap(&frame);
+    }
+
     memset(&bss, 0, sizeof(struct libwifi_bss));
     memset(&sta, 0, sizeof(struct libwifi_sta));