381 lines
12 KiB
Arduino
381 lines
12 KiB
Arduino
#include <WiFi.h>
|
|
#include <WiFiUdp.h>
|
|
#include "esp_efuse.h"
|
|
#include "esp_mac.h"
|
|
#include "config.h"
|
|
|
|
#if PROBE_SNIFF
|
|
#include "esp_wifi.h"
|
|
#include "freertos/queue.h"
|
|
#endif
|
|
|
|
WiFiUDP udp;
|
|
String nodeId;
|
|
|
|
unsigned long lastScan = 0;
|
|
unsigned long lastHeartbeat = 0;
|
|
|
|
// ─── Probe sniffing globals ───────────────────────────────────────────────────
|
|
|
|
#if PROBE_SNIFF
|
|
|
|
struct ProbeEvent {
|
|
uint8_t src_mac[6];
|
|
char ssid[33];
|
|
int8_t rssi;
|
|
};
|
|
|
|
struct DeauthEvent {
|
|
uint8_t src[6];
|
|
uint8_t dst[6];
|
|
uint8_t bssid[6];
|
|
uint8_t subtype; // 0x0C = deauth, 0x0A = disassoc
|
|
uint16_t reason;
|
|
int8_t rssi;
|
|
};
|
|
|
|
static QueueHandle_t probeQueue;
|
|
static QueueHandle_t deauthQueue;
|
|
|
|
// Dedup cache — suppress repeated MAC+SSID pairs within PROBE_DEDUP_SECS
|
|
struct DedupEntry { uint8_t mac[6]; char ssid[33]; uint32_t last_ms; };
|
|
static DedupEntry dedupCache[32];
|
|
static int dedupNext = 0;
|
|
|
|
static bool isDuplicate(const uint8_t* mac, const char* ssid) {
|
|
uint32_t now = millis();
|
|
for (int i = 0; i < 32; i++) {
|
|
if (dedupCache[i].last_ms == 0) continue;
|
|
if (memcmp(dedupCache[i].mac, mac, 6) == 0 &&
|
|
strcmp(dedupCache[i].ssid, ssid) == 0) {
|
|
if ((now - dedupCache[i].last_ms) < (uint32_t)(PROBE_DEDUP_SECS * 1000))
|
|
return true;
|
|
dedupCache[i].last_ms = now;
|
|
return false;
|
|
}
|
|
}
|
|
// Not found — write to next slot (ring)
|
|
memcpy(dedupCache[dedupNext].mac, mac, 6);
|
|
strncpy(dedupCache[dedupNext].ssid, ssid, 32);
|
|
dedupCache[dedupNext].ssid[32] = '\0';
|
|
dedupCache[dedupNext].last_ms = now;
|
|
dedupNext = (dedupNext + 1) % 32;
|
|
return false;
|
|
}
|
|
|
|
// Promiscuous callback — runs in WiFi task context, not safe to call UDP here.
|
|
// Parse probe request, deauth, and disassoc frames and push to queues.
|
|
static void promiscuous_rx_cb(void* buf, wifi_promiscuous_pkt_type_t type) {
|
|
if (type != WIFI_PKT_MGMT) return;
|
|
|
|
const wifi_promiscuous_pkt_t* pkt = (const wifi_promiscuous_pkt_t*)buf;
|
|
const uint8_t* d = pkt->payload;
|
|
uint16_t len = pkt->rx_ctrl.sig_len;
|
|
|
|
if (len < 24) return;
|
|
uint8_t subtype = d[0];
|
|
|
|
// ── Probe request (0x40) ─────────────────────────────────────────────────
|
|
if (subtype == 0x40) {
|
|
if (len < 28) return;
|
|
const uint8_t* src = d + 10;
|
|
|
|
char ssid[33] = "";
|
|
if (d[24] == 0x00) {
|
|
uint8_t slen = d[25];
|
|
if (slen > 0 && slen <= 32 && (26 + slen) <= len) {
|
|
bool printable = true;
|
|
for (uint8_t i = 0; i < slen; i++) {
|
|
if (d[26 + i] < 32 || d[26 + i] > 126) { printable = false; break; }
|
|
}
|
|
if (printable) { memcpy(ssid, d + 26, slen); ssid[slen] = '\0'; }
|
|
}
|
|
}
|
|
|
|
if (isDuplicate(src, ssid)) return;
|
|
|
|
ProbeEvent ev;
|
|
memcpy(ev.src_mac, src, 6);
|
|
memcpy(ev.ssid, ssid, 33);
|
|
ev.rssi = (int8_t)pkt->rx_ctrl.rssi;
|
|
xQueueSend(probeQueue, &ev, 0);
|
|
return;
|
|
}
|
|
|
|
// ── Deauth (0xC0) and Disassoc (0xA0) ───────────────────────────────────
|
|
if (subtype == 0xC0 || subtype == 0xA0) {
|
|
// 802.11 frame header: addr1 (dst) @ 4, addr2 (src) @ 10, addr3 (bssid) @ 16
|
|
// Frame body starts at byte 24: 2-byte reason code
|
|
if (len < 26) return;
|
|
|
|
DeauthEvent ev;
|
|
memcpy(ev.dst, d + 4, 6);
|
|
memcpy(ev.src, d + 10, 6);
|
|
memcpy(ev.bssid, d + 16, 6);
|
|
ev.subtype = subtype;
|
|
ev.reason = (uint16_t)d[24] | ((uint16_t)d[25] << 8);
|
|
ev.rssi = (int8_t)pkt->rx_ctrl.rssi;
|
|
xQueueSend(deauthQueue, &ev, 0);
|
|
return;
|
|
}
|
|
}
|
|
|
|
#endif // PROBE_SNIFF
|
|
|
|
// ─── Setup ───────────────────────────────────────────────────────────────────
|
|
|
|
void setup() {
|
|
Serial.begin(115200);
|
|
delay(500);
|
|
|
|
// Derive node ID from MAC (last 4 bytes, no colons)
|
|
uint8_t mac[6];
|
|
esp_efuse_mac_get_default(mac);
|
|
char macBuf[9];
|
|
snprintf(macBuf, sizeof(macBuf), "%02X%02X%02X%02X", mac[2], mac[3], mac[4], mac[5]);
|
|
nodeId = String(macBuf);
|
|
|
|
Serial.printf("\n[NODE] ID: %s\n", nodeId.c_str());
|
|
connectWiFi();
|
|
|
|
#if PROBE_SNIFF
|
|
memset(dedupCache, 0, sizeof(dedupCache));
|
|
probeQueue = xQueueCreate(PROBE_QUEUE_SIZE, sizeof(ProbeEvent));
|
|
deauthQueue = xQueueCreate(DEAUTH_QUEUE_SIZE, sizeof(DeauthEvent));
|
|
esp_wifi_set_promiscuous_rx_cb(promiscuous_rx_cb);
|
|
esp_wifi_set_promiscuous(true);
|
|
Serial.println("[PROBE] Promiscuous mode enabled");
|
|
#endif
|
|
}
|
|
|
|
// ─── Loop ────────────────────────────────────────────────────────────────────
|
|
|
|
void loop() {
|
|
if (WiFi.status() != WL_CONNECTED) {
|
|
Serial.println("[WIFI] Lost connection, reconnecting...");
|
|
connectWiFi();
|
|
return;
|
|
}
|
|
|
|
unsigned long now = millis();
|
|
|
|
if (now - lastHeartbeat >= HEARTBEAT_INTERVAL_MS) {
|
|
lastHeartbeat = now;
|
|
sendHeartbeat();
|
|
}
|
|
|
|
if (now - lastScan >= SCAN_INTERVAL_MS) {
|
|
lastScan = now;
|
|
scanAndSend();
|
|
}
|
|
|
|
delay(100);
|
|
}
|
|
|
|
// ─── WiFi ────────────────────────────────────────────────────────────────────
|
|
|
|
void connectWiFi() {
|
|
Serial.printf("[WIFI] Connecting to %s\n", WIFI_SSID);
|
|
WiFi.mode(WIFI_STA);
|
|
WiFi.begin(WIFI_SSID, WIFI_PASSWORD);
|
|
|
|
int attempts = 0;
|
|
while (WiFi.status() != WL_CONNECTED && attempts < 20) {
|
|
delay(500);
|
|
Serial.print(".");
|
|
attempts++;
|
|
}
|
|
|
|
if (WiFi.status() == WL_CONNECTED) {
|
|
Serial.printf("\n[WIFI] Connected — IP: %s\n", WiFi.localIP().toString().c_str());
|
|
} else {
|
|
Serial.println("\n[WIFI] Failed to connect, will retry in loop");
|
|
}
|
|
}
|
|
|
|
// ─── Scan ────────────────────────────────────────────────────────────────────
|
|
|
|
void scanAndSend() {
|
|
Serial.println("[SCAN] Starting...");
|
|
int n = WiFi.scanNetworks(false, true); // async=false, show_hidden=true
|
|
|
|
if (n == WIFI_SCAN_FAILED) {
|
|
Serial.println("[SCAN] Failed");
|
|
return;
|
|
}
|
|
|
|
Serial.printf("[SCAN] Found %d networks\n", n);
|
|
|
|
for (int i = 0; i < n; i++) {
|
|
sendBeaconEvent(i);
|
|
}
|
|
|
|
WiFi.scanDelete();
|
|
|
|
#if PROBE_SNIFF
|
|
// Scan may have disabled promiscuous mode internally — re-enable it.
|
|
esp_wifi_set_promiscuous(true);
|
|
flushProbeQueue();
|
|
flushDeauthQueue();
|
|
#endif
|
|
}
|
|
|
|
// ─── Event helpers ───────────────────────────────────────────────────────────
|
|
|
|
const char* encStr(wifi_auth_mode_t enc) {
|
|
switch (enc) {
|
|
case WIFI_AUTH_OPEN: return "OPEN";
|
|
case WIFI_AUTH_WEP: return "WEP";
|
|
case WIFI_AUTH_WPA_PSK: return "WPA";
|
|
case WIFI_AUTH_WPA2_PSK: return "WPA2";
|
|
case WIFI_AUTH_WPA_WPA2_PSK: return "WPA/WPA2";
|
|
case WIFI_AUTH_WPA3_PSK: return "WPA3";
|
|
case WIFI_AUTH_WPA2_WPA3_PSK: return "WPA2/WPA3";
|
|
default: return "UNKNOWN";
|
|
}
|
|
}
|
|
|
|
const char* importance(int rssi) {
|
|
if (rssi >= -50) return "high";
|
|
if (rssi <= -80) return "low";
|
|
return "normal";
|
|
}
|
|
|
|
// ─── Send ────────────────────────────────────────────────────────────────────
|
|
|
|
void sendBeaconEvent(int idx) {
|
|
String ssid = WiFi.SSID(idx);
|
|
ssid.replace("\"", "\\\""); // escape quotes for JSON
|
|
|
|
String bssid = WiFi.BSSIDstr(idx);
|
|
int rssi = WiFi.RSSI(idx);
|
|
int ch = WiFi.channel(idx);
|
|
const char* enc = encStr(WiFi.encryptionType(idx));
|
|
const char* imp = importance(rssi);
|
|
unsigned long ts = millis();
|
|
|
|
char buf[512];
|
|
snprintf(buf, sizeof(buf),
|
|
"{\"node_id\":\"%s\",\"ts\":%lu,\"type\":\"beacon\","
|
|
"\"conf\":\"high\",\"imp\":\"%s\","
|
|
"\"ssid\":\"%s\",\"bssid\":\"%s\","
|
|
"\"rssi\":%d,\"ch\":%d,\"enc\":\"%s\"}",
|
|
nodeId.c_str(), ts, imp,
|
|
ssid.c_str(), bssid.c_str(),
|
|
rssi, ch, enc
|
|
);
|
|
|
|
udp.beginPacket(COORDINATOR_IP, COORDINATOR_PORT);
|
|
udp.print(buf);
|
|
udp.endPacket();
|
|
|
|
Serial.printf(" → %-32s %s ch%-3d %4ddBm %s\n",
|
|
ssid.c_str(), bssid.c_str(), ch, rssi, enc);
|
|
}
|
|
|
|
// ─── Heartbeat ───────────────────────────────────────────────────────────────
|
|
|
|
void sendHeartbeat() {
|
|
char buf[256];
|
|
snprintf(buf, sizeof(buf),
|
|
"{\"node_id\":\"%s\",\"ts\":%lu,\"type\":\"heartbeat\","
|
|
"\"uptime_ms\":%lu,\"free_heap\":%lu,\"wifi_rssi\":%d}",
|
|
nodeId.c_str(), millis(),
|
|
millis(),
|
|
(unsigned long)ESP.getFreeHeap(),
|
|
WiFi.RSSI()
|
|
);
|
|
udp.beginPacket(COORDINATOR_IP, COORDINATOR_PORT);
|
|
udp.print(buf);
|
|
udp.endPacket();
|
|
Serial.printf("[HB] uptime=%lus heap=%luK ap=%ddBm\n",
|
|
millis() / 1000,
|
|
(unsigned long)ESP.getFreeHeap() / 1024,
|
|
WiFi.RSSI()
|
|
);
|
|
}
|
|
|
|
// ─── Probe send / flush ───────────────────────────────────────────────────────
|
|
|
|
#if PROBE_SNIFF
|
|
|
|
void sendProbeEvent(const ProbeEvent& ev) {
|
|
char mac_str[18];
|
|
snprintf(mac_str, sizeof(mac_str), "%02X:%02X:%02X:%02X:%02X:%02X",
|
|
ev.src_mac[0], ev.src_mac[1], ev.src_mac[2],
|
|
ev.src_mac[3], ev.src_mac[4], ev.src_mac[5]);
|
|
|
|
// Escape any quotes in SSID
|
|
char ssidEsc[66];
|
|
int j = 0;
|
|
for (int i = 0; ev.ssid[i] && j < 64; i++) {
|
|
if (ev.ssid[i] == '"' || ev.ssid[i] == '\\') ssidEsc[j++] = '\\';
|
|
ssidEsc[j++] = ev.ssid[i];
|
|
}
|
|
ssidEsc[j] = '\0';
|
|
|
|
char buf[256];
|
|
snprintf(buf, sizeof(buf),
|
|
"{\"node_id\":\"%s\",\"ts\":%lu,\"type\":\"probe\","
|
|
"\"conf\":\"high\",\"imp\":\"%s\","
|
|
"\"src_mac\":\"%s\",\"ssid\":\"%s\",\"rssi\":%d}",
|
|
nodeId.c_str(), millis(), importance(ev.rssi),
|
|
mac_str, ssidEsc, (int)ev.rssi
|
|
);
|
|
|
|
udp.beginPacket(COORDINATOR_IP, COORDINATOR_PORT);
|
|
udp.print(buf);
|
|
udp.endPacket();
|
|
|
|
Serial.printf(" [PROBE] %s → \"%s\" %ddBm\n",
|
|
mac_str, ev.ssid[0] ? ev.ssid : "<wildcard>", (int)ev.rssi);
|
|
}
|
|
|
|
void flushProbeQueue() {
|
|
if (WiFi.status() != WL_CONNECTED) return;
|
|
ProbeEvent ev;
|
|
while (xQueueReceive(probeQueue, &ev, 0) == pdTRUE) {
|
|
sendProbeEvent(ev);
|
|
}
|
|
}
|
|
|
|
void sendDeauthEvent(const DeauthEvent& ev) {
|
|
auto macStr = [](const uint8_t* m, char* out) {
|
|
snprintf(out, 18, "%02X:%02X:%02X:%02X:%02X:%02X",
|
|
m[0], m[1], m[2], m[3], m[4], m[5]);
|
|
};
|
|
char src[18], dst[18], bssid[18];
|
|
macStr(ev.src, src);
|
|
macStr(ev.dst, dst);
|
|
macStr(ev.bssid, bssid);
|
|
|
|
const char* stype = (ev.subtype == 0xC0) ? "deauth" : "disassoc";
|
|
|
|
char buf[384];
|
|
snprintf(buf, sizeof(buf),
|
|
"{\"node_id\":\"%s\",\"ts\":%lu,\"type\":\"%s\","
|
|
"\"src\":\"%s\",\"dst\":\"%s\",\"bssid\":\"%s\","
|
|
"\"reason\":%u,\"rssi\":%d}",
|
|
nodeId.c_str(), millis(), stype,
|
|
src, dst, bssid,
|
|
(unsigned)ev.reason, (int)ev.rssi
|
|
);
|
|
|
|
udp.beginPacket(COORDINATOR_IP, COORDINATOR_PORT);
|
|
udp.print(buf);
|
|
udp.endPacket();
|
|
|
|
Serial.printf(" [%s] %s → %s bssid=%s reason=%u %ddBm\n",
|
|
stype, src, dst, bssid, (unsigned)ev.reason, (int)ev.rssi);
|
|
}
|
|
|
|
void flushDeauthQueue() {
|
|
if (WiFi.status() != WL_CONNECTED) return;
|
|
DeauthEvent ev;
|
|
while (xQueueReceive(deauthQueue, &ev, 0) == pdTRUE) {
|
|
sendDeauthEvent(ev);
|
|
}
|
|
}
|
|
|
|
#endif // PROBE_SNIFF
|