Initial Monitor Mode support

CMakeLists.txt

@@ -46,6 +46,7 @@ if (PICO_CYW43_SUPPORTED)
         # specific projects in subdirectories
         add_subdirectory(dump_rom)
         add_subdirectory(dump_console)
+        add_subdirectory(monitor_mode)
         if(NOT DEFINED NO_NEXMON)
             add_subdirectory(ioctl_test)
         else()

README.md

@@ -7,6 +7,7 @@ App|Description
 ---|---
 [dump_rom<sup>1</sup>](dump_rom) | Read ROM content from WLAN SoC and hex dump it to Pico's console.
 [dump_console<sup>1</sup>](dump_console) | Read WLAN SoC's ARM core internal console and dump it to Pico's console.
+[monitor_mode<sup>1</sup>](monitor_mode) | Enable monitor mode and provide a callback for sniffing packets
 [ioctl_test<sup>2</sup>](ioctl_test) | Write string via IOCTL to SoC's internal console, read back internal console, and dump it to Pico's console.
 
 <sup>1</sup> Works with unmodified firmware provided by [cyw43-driver](https://github.com/georgerobotics/cyw43-driver).  

monitor_mode/CMakeLists.txt

@@ -0,0 +1,21 @@
+add_executable(picow_monitor_mode
+        picow_monitor_mode.c
+        )
+target_include_directories(picow_monitor_mode PRIVATE
+        ${CMAKE_CURRENT_LIST_DIR}
+        ${CMAKE_CURRENT_LIST_DIR}/.. # for our common lwipopts
+        )
+target_link_libraries(picow_monitor_mode
+        pico_cyw43_arch_lwip_threadsafe_background
+        pico_stdlib
+        )
+
+# Ucomment when using NEXMON driver
+# target_compile_definitions(picow_monitor_mode PRIVATE
+#         CYW43_CHIPSET_FIRMWARE_INCLUDE_FILE="${NEXMON_ROOT}/patches/bcm43439a0/7_95_49_2271bb6/nexmon/w43439A0_7_95_49_00_combined.h"
+#         CYW43_WIFI_NVRAM_INCLUDE_FILE="${PICO_NEXMON_PATH}/cyw43-driver/firmware/wifi_nvram_43439.h"
+#         CYW43_ENABLE_BLUETOOTH=0
+#         )
+
+pico_add_extra_outputs(picow_monitor_mode)
+

monitor_mode/lwipopts.h

@@ -0,0 +1,10 @@
+#ifndef _LWIPOPTS_H
+#define _LWIPOPTS_H
+
+// Generally you would define your own explicit list of lwIP options
+// (see https://www.nongnu.org/lwip/2_1_x/group__lwip__opts.html)
+//
+// This example uses a common include to avoid repetition
+#include "lwipopts_examples_common.h"
+
+#endif

monitor_mode/picow_monitor_mode.c

@@ -0,0 +1,77 @@
+#include <stdio.h>
+
+#include "pico/stdlib.h"
+#include "pico/cyw43_arch.h"
+
+#define MONITOR_DISABLED        0
+#define MONITOR_IEEE80211       1
+/* RADIOTAP MODE REQUIRES A NEXMON FW! */
+#define MONITOR_RADIOTAP        2
+#define MONITOR_LOG_ONLY        16
+
+const char *frame_type_names[3] = {
+            "Management",
+            "Control",
+            "Data"
+        };
+const char *frame_subtype_names[4][16] = {
+        {
+            "Association Request", "Association Response", "Reassociation Request", "Reassociation Response",
+            "Probe Request", "Probe Response", "Timing Advertisement", "Reserved",
+            "Beacon", "ATIM", "Disassociation", "Authentication", "Deauthentication", "Action", "Action No Ack (NACK)", "Reserved"
+        },
+        {
+            "Reserved", "Reserved", "Trigger[3]", "TACK",
+            "Beamforming Report Poll", "VHT/HE NDP Announcement", "Control Frame Extension", "Control Wrapper",
+            "Block Ack Request (BAR)", "Block Ack (BA)", "PS-Poll", "RTS", "CTS", "ACK", "CF-End", "CF-End + CF-ACK"
+        },
+        {
+            "Data", "Reserved", "Reserved", "Reserved",
+            "Null (no data)", "Reserved", "QoS Data", "QoS Data + CF-ACK",
+            "QoS Data + CF-Poll", "QoS Data + CF-ACK + CF-Poll", "QoS Null (no data)", "Reserved", "QoS CF-Poll (no data)", "QoS CF-ACK + CF-Poll (no data)", "Reserved", "Reserved"
+        },
+        {
+            "DMG Beacon", "S1G Beacon", "Reserved", "Reserved",
+            "Reserved", "Reserved", "Reserved", "Reserved",
+            "Reserved", "Reserved", "Reserved", "Reserved", "Reserved", "Reserved", "Reserved", "Reserved"
+        }
+    };
+
+void monitor_mode_cb(void *data, int itf, size_t len, const uint8_t *buf) {
+    uint16_t offset_80211 = 0;
+    if (cyw43_state.is_monitor_mode == MONITOR_RADIOTAP)
+        offset_80211 = *(uint16_t*)(buf+2);
+    uint8_t frame_type = buf[offset_80211] >> 2 & 3;
+    uint8_t frame_subtype = buf[offset_80211] >> 4;
+    printf("Frame type=%d (%s) subtype=%d (%s) len=%d data=", frame_type, frame_type_names[frame_type], frame_subtype, frame_subtype_names[frame_type][frame_subtype], len);
+    for (size_t i = 0; i < len; ++i) {
+        printf("%02x ", buf[i]);
+    }
+    printf("\n");
+    return;
+}
+
+int main() {
+    stdio_init_all();
+
+    if (cyw43_arch_init()) {
+        printf("failed to initialise\n");
+        return 1;
+    }
+
+    const char *ap_name = "picow_test";
+    const char *password = "password";
+    uint32_t channels[] = {1, 6, 11};
+    uint8_t chan_idx = 0;
+    cyw43_arch_enable_ap_mode(ap_name, password, CYW43_AUTH_WPA2_AES_PSK);
+    cyw43_set_monitor_mode(&cyw43_state, MONITOR_IEEE80211, monitor_mode_cb);
+
+    while(true) {
+        cyw43_wifi_ap_set_channel(&cyw43_state, channels[chan_idx]);
+        chan_idx = (chan_idx + chan_idx) % (sizeof(channels)/sizeof(channels[0]));
+        sleep_ms(200);
+    }
+
+    cyw43_arch_deinit();
+    return 0;
+}

patches/002_cyw43-add_monitor_mode_support.patch

@@ -0,0 +1,129 @@
+diff --git a/src/cyw43.h b/src/cyw43.h
+index 0900440..97fcd4e 100644
+--- a/src/cyw43.h
++++ b/src/cyw43.h
+@@ -149,6 +149,9 @@ typedef struct _cyw43_t {
+     #if CYW43_ENABLE_BLUETOOTH
+     bool bt_loaded;
+     #endif
++
++    uint8_t is_monitor_mode;
++    void (*monitor_mode_cb)(void *, int, size_t, const uint8_t *);
+ } cyw43_t;
+
+ extern cyw43_t cyw43_state;
+@@ -678,6 +681,29 @@ int cyw43_bluetooth_hci_write(uint8_t *buf, size_t len);
+ void cyw43_bluetooth_hci_process(void);
+ #endif
+
++/**
++ * @brief Callback function to handle monitor mode data.
++ *
++ * @param cb_data   The driver state object.
++ * @param itf       The interface identifier.
++ * @param len       The length of the received data.
++ * @param buf       A pointer to the buffer containing the received data.
++ */
++void cyw43_cb_monitor_mode(void *cb_data, int itf, size_t len, const uint8_t *buf);
++
++/**
++ * @brief Set the monitor mode of the CYW43 device.
++ *
++ * @param self  the driver state object. This should always be \c &cyw43_state
++ * @param value The value to set monitor mode (1 for enabled, 0 for disabled).
++ * @param cb    A callback function to handle monitor mode data.
++ *             The callback should have the signature:
++ *             `void (*cb)(void *, int, size_t, const uint8_t *)`
++ *
++ * @return 0 on success, an error code on failure.
++ */
++int cyw43_set_monitor_mode(cyw43_t *self, int value, void (*cb)(void *, int, size_t, const uint8_t *));
++
+ //!\} // cyw43_driver doxygen group
+
+ #endif // CYW43_INCLUDED_CYW43_H
+diff --git a/src/cyw43_ctrl.c b/src/cyw43_ctrl.c
+index dde5ca7..fb28519 100644
+--- a/src/cyw43_ctrl.c
++++ b/src/cyw43_ctrl.c
+@@ -794,3 +794,23 @@ int cyw43_bluetooth_hci_write(uint8_t *buf, size_t len) {
+     return 0;
+ }
+ #endif
++
++void cyw43_cb_monitor_mode(void *cb_data, int itf, size_t len, const uint8_t *buf) {
++    cyw43_t *self = cb_data;
++    if(self->is_monitor_mode && self->monitor_mode_cb)
++        self->monitor_mode_cb(cb_data, itf, len, buf);
++}
++
++int cyw43_set_monitor_mode(cyw43_t *self, int value, void (*cb)(void *, int, size_t, const uint8_t *)) {
++    CYW43_THREAD_ENTER;
++    int ret = cyw43_ensure_up(self);
++    if (ret) {
++        CYW43_THREAD_EXIT;
++        return ret;
++    }
++    cyw43_ll_set_monitor_mode(&self->cyw43_ll, value);
++    self->is_monitor_mode = value;
++    self->monitor_mode_cb = cb;
++    CYW43_THREAD_EXIT;
++    return 0;
++}
+\ No newline at end of file
+diff --git a/src/cyw43_ll.c b/src/cyw43_ll.c
+index 11fb696..7e10b8d 100644
+--- a/src/cyw43_ll.c
++++ b/src/cyw43_ll.c
+@@ -1167,6 +1167,7 @@ void cyw43_ll_process_packets(cyw43_ll_t *self_in) {
+             cyw43_cb_process_async_event(self, cyw43_ll_parse_async_event(len, buf));
+         } else if (ret == DATA_HEADER) {
+             cyw43_cb_process_ethernet(self->cb_data, len >> 31, len & 0x7fffffff, buf);
++            cyw43_cb_monitor_mode(self->cb_data, len >> 31, len & 0x7fffffff, buf);
+         } else if (CYW43_USE_SPI && ret == CYW43_ERROR_WRONG_PAYLOAD_TYPE) {
+             // Ignore this error when using the SPI interface.  It can occur when there
+             // is a lot of traffic over the SPI (eg sending UDP packets continuously)
+@@ -1838,26 +1839,12 @@ static uint32_t cyw43_read_iovar_u32(cyw43_int_t *self, const char *var, uint32_
+     return cyw43_get_le32(buf);
+ }
+
+-#if 0
+ #define WLC_SET_MONITOR (108)
+-int cyw43_set_monitor_mode(cyw43_ll_t *self, int value) {
+-    CYW_THREAD_ENTER;
+-    int ret = cyw43_ensure_up(self);
+-    if (ret) {
+-        CYW_THREAD_EXIT;
+-        return ret;
+-    }
+-
+-    CYW_ENTER;
+-    self->is_monitor_mode = value;
+-    cyw43_write_iovar_u32(self, "allmulti", value, WWD_STA_INTERFACE);
+-    cyw43_set_ioctl_u32(self, WLC_SET_MONITOR, value, WWD_STA_INTERFACE);
+-    CYW_EXIT;
+-    CYW_THREAD_EXIT;
+-
+-    return 0;
++void cyw43_ll_set_monitor_mode(cyw43_ll_t *self, int value) {
++    //self->is_monitor_mode = value;
++    cyw43_write_iovar_u32(CYW_INT_FROM_LL(self), "allmulti", value, WWD_STA_INTERFACE);
++    cyw43_set_ioctl_u32(CYW_INT_FROM_LL(self), WLC_SET_MONITOR, value, WWD_STA_INTERFACE);
+ }
+-#endif
+
+ // Requires cyw43_ll_bus_init to have been called first
+ int cyw43_ll_wifi_on(cyw43_ll_t *self_in, uint32_t country) {
+diff --git a/src/cyw43_ll.h b/src/cyw43_ll.h
+index 2750238..c98fc08 100644
+--- a/src/cyw43_ll.h
++++ b/src/cyw43_ll.h
+@@ -277,6 +277,8 @@ void cyw43_ll_process_packets(cyw43_ll_t *self);
+ int cyw43_ll_ioctl(cyw43_ll_t *self, uint32_t cmd, size_t len, uint8_t *buf, uint32_t iface);
+ int cyw43_ll_send_ethernet(cyw43_ll_t *self, int itf, size_t len, const void *buf, bool is_pbuf);
+
++void cyw43_ll_set_monitor_mode(cyw43_ll_t *self, int value);
++
+ int cyw43_ll_wifi_on(cyw43_ll_t *self, uint32_t country);
+ int cyw43_ll_wifi_pm(cyw43_ll_t *self, uint32_t pm, uint32_t pm_sleep_ret, uint32_t li_bcn, uint32_t li_dtim, uint32_t li_assoc);
+ int cyw43_ll_wifi_get_pm(cyw43_ll_t *self, uint32_t *pm, uint32_t *pm_sleep_ret, uint32_t *li_bcn, uint32_t *li_dtim, uint32_t *li_assoc);

script/patch

@@ -3,6 +3,7 @@
 CYW43_DRIVER_DIR="cyw43-driver"
 PATCH_DIR="patches"
 PATCH_001="$PATCH_DIR/001_cyw43-driver_expose_backplane_read.patch"
+PATCH_002="$PATCH_DIR/002_cyw43-add_monitor_mode_support.patch"
 
 # exit on error
 set -e
@@ -25,3 +26,13 @@ fi
 # apply patch 001
 printf "applying patch %s\n" $PATCH_001
 cd $CYW43_DRIVER_DIR && git apply ../$PATCH_001 && cd ..
+
+# check if patch 002 exists
+if ! [ -f $PATCH_002 ]; then
+    printf "Patchfile 2 not found, expected: %s\n" $PATCH_002
+    exit 2
+fi
+
+# apply patch 002
+printf "applying patch %s\n" $PATCH_002
+cd $CYW43_DRIVER_DIR && git apply ../$PATCH_002 && cd ..