diff --git a/.gitignore b/.gitignore index f617d4b..7e2942e 100644 --- a/.gitignore +++ b/.gitignore @@ -13,3 +13,4 @@ tests/chipid_probe # Local lab notes / Claude Code state (never commit) INVENTORY.md .claude/ +build-upstream/ diff --git a/CMakeLists.txt b/CMakeLists.txt index 851859b..e7377bb 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -207,6 +207,50 @@ target_include_directories(devourer PUBLIC hal/phydm/rtl8822b) target_include_directories(devourer PUBLIC hal/phydm/rtl8821c) target_include_directories(devourer PUBLIC src) +# --------------------------------------------------------------------------- +# APFPV additions -- station-mode WPA2/CCMP + the APFPV IP/FPV/session layer, +# ported onto upstream's rebuilt tree. Only WPA2Supplicant/Dot11Frames/ +# Wpa2Crypto/crypto/*/ApfpvDhcp/LqFeedback/Wpa2Authenticator so far (zero +# driver-layer dependency, confirmed by direct #include inspection); RxDeframe +# and ApfpvStation come later once their driver-layer API updates land. +# --------------------------------------------------------------------------- +add_library(apfpv + src/apfpv/Wpa2Supplicant.cpp src/apfpv/Wpa2Supplicant.h + src/apfpv/Dot11Frames.cpp src/apfpv/Dot11Frames.h + src/apfpv/Wpa2Crypto.cpp src/apfpv/Wpa2Crypto.h + src/apfpv/crypto/AesCcm.cpp src/apfpv/crypto/AesCcm.h + src/apfpv/crypto/AesCmac.cpp src/apfpv/crypto/AesCmac.h + src/apfpv/crypto/Sha1Hmac.cpp src/apfpv/crypto/Sha1Hmac.h + src/apfpv/ScanProbe.cpp src/apfpv/ScanProbe.h + src/ApfpvDhcp.cpp src/ApfpvDhcp.h + src/LqFeedback.cpp src/LqFeedback.h + src/Wpa2Authenticator.cpp src/Wpa2Authenticator.h) +target_compile_features(apfpv PUBLIC cxx_std_20) +target_include_directories(apfpv PUBLIC src src/apfpv src/apfpv/crypto) +if(WIN32) + target_link_libraries(apfpv PUBLIC ws2_32 bcrypt) +endif() + +# ApfpvStation/RxDeframe DO need the driver layer (RtlAdapter/RtlJaguarDevice/ +# RadioManagementModule) -- separate target so `apfpv` itself stays +# driver-independent (it's also used as-is on hosts with no libusb at all). +add_library(apfpv_station + src/ApfpvStation.cpp src/ApfpvStation.h + src/RxDeframe.cpp src/RxDeframe.h + src/apfpv/StationMode.cpp src/apfpv/StationMode.h + src/apfpv/StationTxDesc.cpp src/apfpv/StationTxDesc.h) +target_link_libraries(apfpv_station PUBLIC apfpv devourer) +target_compile_features(apfpv_station PUBLIC cxx_std_20) +if(NOT ANDROID) + target_include_directories(apfpv_station PUBLIC src/apfpv/compat) +endif() +# Host (non-Android) builds: Wpa2Supplicant.cpp etc. call __android_log_print +# unconditionally (ported as-is from WiFiDriver) -- same compat shim already +# proven there, reused verbatim rather than reinvented. +if(NOT ANDROID) + target_include_directories(apfpv PUBLIC src/apfpv/compat) +endif() + # --- per-chip source groups (see options above) --- if(DEVOURER_JAGUAR1) target_sources(devourer PRIVATE diff --git a/src/ApfpvDhcp.cpp b/src/ApfpvDhcp.cpp new file mode 100644 index 0000000..e2f9c46 --- /dev/null +++ b/src/ApfpvDhcp.cpp @@ -0,0 +1,94 @@ +// ============================================================================ +// ApfpvDhcp.cpp — minimal DHCP client, route/DNS-suppressed (impl) +// Mirrors the real VRX udhcpc.apfpv.script ("not add routes and dns for apfpv +// interfaces"): take the IP, ignore router(opt 3)/DNS(opt 6). Declarations in +// ApfpvDhcp.h. APFPV reliably hands 192.168.0.10. +// ============================================================================ +#include "ApfpvDhcp.h" +#include + +namespace apfpv { + +static void put32(std::vector& v, uint32_t x) { + v.push_back(x>>24); v.push_back(x>>16); v.push_back(x>>8); v.push_back(x); +} + +ApfpvDhcp::ApfpvDhcp(const Mac& self, SendUdpFn send) : _self(self), _send(std::move(send)) {} + +// Build a BOOTP/DHCP message (op=1 BOOTREQUEST). Minimal but valid. +static std::vector bootp(const Mac& self, uint32_t xid, uint8_t msgType, + uint32_t reqIp, uint32_t serverId) { + std::vector p; + p.push_back(1); p.push_back(1); p.push_back(6); p.push_back(0); // op,htype,hlen,hops + put32(p, xid); + p.push_back(0); p.push_back(0); // secs + p.push_back(0x80); p.push_back(0x00); // flags = broadcast + put32(p, 0); put32(p, 0); put32(p, 0); put32(p, 0); // ci,yi,si,gi addr + p.insert(p.end(), self.begin(), self.end()); // chaddr + p.resize(p.size() + 10, 0); // chaddr pad + p.resize(236, 0); // sname+file + put32(p, 0x63825363); // magic cookie + // option 53: DHCP message type + p.push_back(53); p.push_back(1); p.push_back(msgType); + if (reqIp) { p.push_back(50); p.push_back(4); put32(p, reqIp); } // requested IP + if (serverId) { p.push_back(54); p.push_back(4); put32(p, serverId); } // server id + // option 55: param request list — deliberately request ONLY subnet mask, + // NOT router(3)/DNS(6), matching udhcpc.apfpv.script intent. + p.push_back(55); p.push_back(1); p.push_back(1); + p.push_back(255); // end + return p; +} + +std::vector ApfpvDhcp::buildDiscover() { return bootp(_self, _xid, 1, 0, 0); } +std::vector ApfpvDhcp::buildRequest(uint32_t ip, uint32_t sid) { return bootp(_self, _xid, 3, ip, sid); } + +bool ApfpvDhcp::start() { + _xid = 0x41504650; // "APFP" + _state = St::Discover; + return _send(buildDiscover()); +} + +void ApfpvDhcp::onBootpReply(const uint8_t* p, size_t len) { + if (len < 240) return; + if ((p[236]<<24|p[237]<<16|p[238]<<8|p[239]) != 0x63825363) return; // cookie + uint32_t yi = (p[16]<<24)|(p[17]<<16)|(p[18]<<8)|p[19]; + // parse options for msg type (53), netmask (1), server id (54). IGNORE 3/6. + uint8_t msgType = 0; uint32_t mask = 0, sid = 0; + size_t i = 240; + while (i + 2 <= len) { + uint8_t opt = p[i], olen = p[i+1]; + if (opt == 255) break; + if (opt == 0) { i++; continue; } + if (i + 2 + olen > len) break; + const uint8_t* d = p + i + 2; + if (opt == 53 && olen == 1) msgType = d[0]; + else if (opt == 1 && olen == 4) mask = (d[0]<<24)|(d[1]<<16)|(d[2]<<8)|d[3]; + else if (opt == 54 && olen == 4) sid = (d[0]<<24)|(d[1]<<16)|(d[2]<<8)|d[3]; + // opt 3 (router) and opt 6 (DNS) intentionally skipped. + i += 2 + olen; + } + if (_state == St::Discover && msgType == 2) { // OFFER -> REQUEST + _state = St::Request; + _offerIp = yi; _offerSid = sid; // remember for REQUEST retransmits + _send(buildRequest(yi, sid)); + } else if (_state == St::Request && msgType == 5) { // ACK -> bound + _lease.ip = yi; _lease.netmask = mask; _lease.server = sid; _lease.valid = true; + _state = St::Bound; + } +} + +void ApfpvDhcp::retransmit() { + // Resend whichever message we're waiting on a reply for, WITHOUT resetting the state + // machine — so a missed ACK retries the REQUEST (not a fresh DISCOVER that restarts DORA). + if (_state == St::Discover || _state == St::Init) _send(buildDiscover()); + else if (_state == St::Request) _send(buildRequest(_offerIp, _offerSid)); +} + +void ApfpvDhcp::claimStatic(uint32_t ip, uint32_t netmask, uint32_t gateway) { + if (netmask == 0) netmask = 0xFFFFFF00; // default /24 + if (gateway == 0) gateway = (ip & netmask) | 1; // default = subnet's .1 + _lease.ip = ip; _lease.netmask = netmask; _lease.server = gateway; + _lease.valid = true; _state = St::Bound; +} + +} // namespace apfpv diff --git a/src/ApfpvDhcp.h b/src/ApfpvDhcp.h new file mode 100644 index 0000000..d70a3d5 --- /dev/null +++ b/src/ApfpvDhcp.h @@ -0,0 +1,27 @@ +#pragma once +#include +#include +#include +#include +namespace apfpv { +using Mac = std::array; +class ApfpvDhcp { +public: + using SendUdpFn = std::function&)>; + struct Lease { uint32_t ip=0, netmask=0, server=0; bool valid=false; }; + ApfpvDhcp(const Mac& self, SendUdpFn send); + bool start(); + void retransmit(); // resend the CURRENT pending message (DISCOVER or REQUEST), no reset + void onBootpReply(const uint8_t* bootp, size_t len); + const Lease& lease() const { return _lease; } + // Skip DHCP and bind a fixed lease (static-IP option). netmask 0 -> /24, gateway 0 -> subnet .1. + void claimStatic(uint32_t ip, uint32_t netmask = 0, uint32_t gateway = 0); + void claimStatic_192_168_0_10() { claimStatic(0xC0A8000A, 0xFFFFFF00, 0xC0A80001); } +private: + enum class St { Init, Discover, Request, Bound }; + std::vector buildDiscover(); + std::vector buildRequest(uint32_t offeredIp, uint32_t serverId); + Mac _self; SendUdpFn _send; St _state = St::Init; Lease _lease; uint32_t _xid = 0; + uint32_t _offerIp = 0, _offerSid = 0; // remembered from the OFFER for REQUEST retransmits +}; +} diff --git a/src/ApfpvStation.cpp b/src/ApfpvStation.cpp new file mode 100644 index 0000000..0815197 --- /dev/null +++ b/src/ApfpvStation.cpp @@ -0,0 +1,1780 @@ +// ============================================================================ +// ApfpvStation.cpp — orchestrator + DRIVER-LEVEL persistent reconnect +// Mirrors the VRX's wpa_supplicant -B daemon behavior at the driver level: +// a supervisor thread re-establishes the link on loss (deauth or RX timeout), +// with no app/UI involvement. Faster than the VRX on recovery because we +// re-arm on the known channel (no full rescan). +// ============================================================================ +#include "ApfpvStation.h" +#include "StationMode.h" +#include "Wpa2Supplicant.h" +#include "Wpa2Authenticator.h" +#include "hal_com_reg.h" // RCR / RCR_* / MSR -- station-mode RX-filter diagnostics + tuning +#include "Wpa2Crypto.h" +#include "ApfpvDhcp.h" +#include "RxDeframe.h" +#include "StationTxDesc.h" +#include "Dot11Frames.h" +#include "jaguar1/FrameParser.h" +#include "ScanProbe.h" +#include "LqFeedback.h" +#if defined(__ANDROID__) +#include +#endif +#include "jaguar1/PhydmWatchdog.h" +#include "RtlUsbAdapter.h" +#include "jaguar1/RtlJaguarDevice.h" +#include "SelectedChannel.h" +#include "jaguar1/RadioManagementModule.h" +#include +#include +#if defined(__ANDROID__) +#include +#endif + +namespace { +// Android has no settable process env, so DEVOURER_* getenv knobs never fire in the app. Mirror the +// key streaming knobs to `debug.pixelpilot.*` props (adb setprop, no root). Returns true if the prop +// exists and is not "0". Host builds (no __ANDROID__) always return false → env-only there. +static bool apfpvProp(const char* name) { +#if defined(__ANDROID__) + char v[PROP_VALUE_MAX] = {0}; + if (__system_property_get(name, v) > 0 && v[0] != '0') return true; +#else + (void)name; +#endif + return false; +} +// ACCEPT the AP's BlockAck (arm RX-BA regs + ADDBA-init + StatusCode 0) instead of declining. +// Env DEVOURER_ENABLE_BA (host) or `debug.pixelpilot.ba=1` (adb setprop, Android). THE fundamental-fix +// test: now that FW_RA makes the AP actively aggregate A-MPDU to us, does accepting BA make the chip's +// MAC auto-emit the SIFS compressed BlockAck so the AP retransmits lost subframes → losses recovered +// (no more reference-corruption freeze)? Prior sessions tested this only when the AP WASN'T aggregating. +static bool enableBaOn() { + return std::getenv("DEVOURER_ENABLE_BA") != nullptr || apfpvProp("debug.pixelpilot.ba"); +} +// Buffer size (in MPDUs) to negotiate in OUR ADDBA Response, overriding the AP's request (see +// handleAddbaRequest). 0/unset = echo the AP's requested value unchanged. DEVOURER_BA_BUFSZ (host) +// / debug.pixelpilot.babufsz (Android). +static int baRespBufSizeOverride() { + if (const char* e = std::getenv("DEVOURER_BA_BUFSZ")) { int n = std::atoi(e); if (n > 0) return n; } +#if defined(__ANDROID__) + char v[PROP_VALUE_MAX] = {0}; + if (__system_property_get("debug.pixelpilot.babufsz", v) > 0) { + int n = std::atoi(v); + if (n > 0) return n; + } +#endif + return 0; +} +// Upstream's RtlJaguarDevice::StartRxLoop (~= the old RtlJaguarDevice::Init) is a BLOCKING +// read loop -- it never returns until StopRxLoop() flips should_stop. Every RX-owning +// call site below therefore runs it on its own std::thread (_rxThread) rather than inline, +// same as the pre-rebuild StartMonitorAsyncRx/StopAsyncRx contract this replaces. Join-safe: +// tolerates being called from the thread being joined (detach instead -- avoids the EINVAL/ +// SIGABRT a self-join throws, seen on replug when JNI teardown races the app's disconnect) +// and an already-reaped thread. +static void safeJoinThread(std::thread& t) { + if (!t.joinable()) return; + if (t.get_id() == std::this_thread::get_id()) { t.detach(); return; } + try { t.join(); } catch (...) { if (t.joinable()) { try { t.detach(); } catch (...) {} } } +} +} // namespace +// Platform-agnostic: is the NDK header on Android and the +// compat stderr shim on native host builds (see WiFiDriver/compat), so the same +// diagnostics surface on Windows/Linux too — essential for chasing parity. +#include +#define SCANLOG(...) __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", __VA_ARGS__) + +namespace apfpv { + +// Map a primary channel + desired bandwidth (MHz) to a SelectedChannel kept +// INSIDE the legal DE APFPV band 5170-5250 MHz (5.2 GHz UNII-1; 200 mW @ 20 MHz +// under the 10 mW/MHz PSD cap). For 40 MHz we pick the HT40 secondary side that +// never crosses 5170/5250 — primary 36/44 extend ABOVE (offset LOWER = primary +// is the lower 20 MHz), primary 40/48 extend BELOW (offset UPPER); both land on +// centres 38 (5170-5210) or 46 (5210-5250). 20 MHz is pass-through; 80 MHz +// centres on 42 and fills the band exactly. NB: WFB's 5.8 GHz / 25 mW band is a +// separate code path and is unaffected by this helper. +static SelectedChannel legalApfpvChannel(int ch, int bwMHz) { + uint8_t c = (uint8_t)(ch > 0 ? ch : 40); + if (bwMHz >= 80) + return SelectedChannel{ c, (uint8_t)HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_80 }; + if (bwMHz == 40) { + // WFB proven 40MHz rule: odd (channel/4) -> LOWER, even -> UPPER. + // ch36(9 odd)->LOWER ch40(10 even)->UPPER ch44(11 odd)->LOWER ch48(12 even)->UPPER. + uint8_t off = (c > 14) ? (((c / 4) & 1) ? (uint8_t)HAL_PRIME_CHNL_OFFSET_LOWER + : (uint8_t)HAL_PRIME_CHNL_OFFSET_UPPER) + : ((c <= 7) ? (uint8_t)HAL_PRIME_CHNL_OFFSET_LOWER + : (uint8_t)HAL_PRIME_CHNL_OFFSET_UPPER); + return SelectedChannel{ c, off, CHANNEL_WIDTH_40 }; + } + return SelectedChannel{ c, (uint8_t)HAL_PRIME_CHNL_OFFSET_DONT_CARE, CHANNEL_WIDTH_20 }; +} + +void ApfpvStation::set(State s) { + _state.store(s); + // 0Idle 1Scan 2Arm 3Auth 4Assoc 5Handshake 6Dhcp 7Stream 8FailNoAp 9FailTx + // 10FailNoAck 11FailAuth 12FailDhcp 13LinkLost 14Reconnecting + SCANLOG("apfpv state -> %d", (int)s); + if (_onState) _onState(s); +} + +ApfpvStation::ApfpvStation(void* dev, void* rm, OnRtpFn onRtp, OnStateFn onState) + : _dev(dev), _rm(rm), _onRtp(std::move(onRtp)), _onState(std::move(onState)) {} + +ApfpvStation::~ApfpvStation() { stopBeaconCal(); disconnect(); } + +// VRX EIRP-calibration beacon: inject an open beacon periodically so a second +// phone can scan + measure the dongle's EIRP. Not a station while beaconing. +void ApfpvStation::startBeaconCal(const std::string& ssid, int channel, int txIndex) { + stopBeaconCal(); + disconnect(); // can't be a station and beacon at once + if (!_rtl || !_dev) return; + auto* rtl = reinterpret_cast(_rtl); + auto* dev = reinterpret_cast(_dev); + try { + rtl->StopRxLoop(); safeJoinThread(_rxThread); // no RX needed for a TX-only beacon + rtl->InitWrite(legalApfpvChannel(channel, 20)); // tune + bring up (no RX loop) + rtl->SetTxPowerIndexOverride(std::max(0, std::min(63, txIndex))); + } catch (...) { return; } + // **TX-RADIATION FIX.** The init/channel-set IQK (Iqk8812a::ConfigureMac) pauses + // all 6 TX queues (REG_TXPAUSE 0x522 = 0x3f) for calibration. The beacon/monitor + // path had NO clear (only the station arm() path got one), so the MAC drained the + // FIFO ("injected OK") but the scheduler never keyed the PHY/PA -> zero RF on air. + // Release the queues unconditionally (mirrors StationMode.cpp arm step 4c). + dev->rtw_write8(0x0522, 0x00); + SCANLOG("beacon: REG_TXPAUSE -> 0x%02x", dev->rtw_read8(0x0522)); + // our MAC (REG_MACID); synthesize a locally-administered one if unprogrammed + Mac self{}; + for (int i = 0; i < 6; ++i) self[i] = dev->rtw_read8(0x0610 + i); + bool bad = true; for (int i = 0; i < 6; ++i) if (self[i] != 0 && self[i] != 0xFF) bad = false; + if (bad) { const uint8_t m[6] = {0x02,0x11,0x22,0x33,0x44,0x55}; + for (int i = 0; i < 6; ++i) self[i] = m[i]; } + auto mpdu = BuildBeacon(self, ssid, (uint8_t)channel); + _beaconRun.store(true); + _beaconThread = std::thread([this, dev, mpdu]() { + std::vector frame(40 + mpdu.size(), 0); // 40 = 8812 TX desc + std::memcpy(frame.data() + 40, mpdu.data(), mpdu.size()); + FillStationTxDesc(frame.data(), (uint16_t)mpdu.size(), 40, + 0, StationFrameKind::BroadcastMgmt, 0, 0x04); // BMC=1, no ACK + int n = 0; + while (_beaconRun.load()) { + dev->rtw_write8(0x0522, 0x00); // keep all TX queues released every beacon + bool ok = false; + try { ok = dev->send_packet(frame.data(), frame.size()); } catch (...) {} + if ((n++ % 20) == 0) SCANLOG("beacon: tx #%d ok=%d len=%zu", n, (int)ok, frame.size()); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // ~10 beacons/s + } + }); +} + +void ApfpvStation::stopBeaconCal() { + _beaconRun.store(false); + if (_beaconThread.joinable()) _beaconThread.join(); +} + +// ---- AP MODE (SoftAP) ------------------------------------------------------- +void ApfpvStation::apSend(const std::vector& mpdu, bool eapol) { + if (mpdu.empty()) return; + std::vector frame(40 + mpdu.size(), 0); + std::memcpy(frame.data()+40, mpdu.data(), mpdu.size()); + FillStationTxDesc(frame.data(), (uint16_t)mpdu.size(), 40, 0, + eapol ? StationFrameKind::EapolData : StationFrameKind::Mgmt, 0, 0x04); + // Queue rather than send_packet inline: apSend is called from apOnRx, which runs on + // _rxThread (inside IRtlDevice::StartRxLoop's callback) -- send_packet from that thread + // returns BUSY (docs/ap-mode.md: "send_packet must run off the RX event thread"). The + // AP TX-pump thread (started in startAp) drains this queue from its own thread instead. + std::lock_guard lk(_apTxMtx); + if (_apTxQ.size() < 128) _apTxQ.push_back(std::move(frame)); +} + +void ApfpvStation::apTrack(const Mac& sta, uint8_t rssiRaw, int state) { + int dbm = (int)(rssiRaw & 0x7f) - 110; if (dbm < -100) dbm = -100; if (dbm > -10) dbm = -10; + std::lock_guard lk(_apMtx); + for (auto& s : _apStaList) if (s.mac == sta) { s.rssiDbm = dbm; if (state >= 0) s.state = state; return; } + _apStaList.push_back(ApStation{sta, dbm, state >= 0 ? state : 4}); +} + +std::vector ApfpvStation::apStations() { + std::lock_guard lk(_apMtx); + return _apStaList; +} + +void ApfpvStation::apOnRx(const uint8_t* f, size_t len, uint8_t rssiRaw) { + if (len < 24) return; + uint16_t fc = f[0] | (f[1] << 8); + uint8_t type = (fc >> 2) & 0x3, sub = (fc >> 4) & 0xf; + static int g_apRxN = 0; + if ((++g_apRxN % 100) == 1) fprintf(stderr, "[ap-rx-any] #%d type=%u sub=%u len=%zu\n", g_apRxN, type, sub, len); + Mac a1, a2; std::memcpy(a1.data(), f+4, 6); std::memcpy(a2.data(), f+10, 6); + bool toUs = std::memcmp(a1.data(), _apSelf.data(), 6) == 0; + bool grp = (f[4] & 0x01); + if (type == 0) { // management + if ((sub==4||sub==11||sub==0) && (toUs||grp)) + fprintf(stderr, "[ap-rx] mgmt sub=%u from %02x:%02x:%02x:%02x:%02x:%02x toUs=%d\n", + sub, a2[0],a2[1],a2[2],a2[3],a2[4],a2[5], (int)toUs); + if (sub == 4 && (toUs || grp)) { // Probe Request -> Probe Response + apTrack(a2, rssiRaw, -1); + apSend(BuildProbeResp(_apSelf, a2, _apSsid, (uint8_t)_apChannel, _apWpa2), false); + } else if (sub == 11 && toUs) { // Auth (open seq1) -> Auth Resp (seq2) + apTrack(a2, rssiRaw, -1); + apSend(BuildAuthResp(_apSelf, a2), false); + } else if (sub == 0 && toUs) { // Assoc Request -> Assoc Resp [+ 4-way] + apTrack(a2, rssiRaw, 4); + apSend(BuildAssocResp(_apSelf, a2, 1), false); + if (_apWpa2) { + _apAuth = std::make_unique(MakeWpa2Crypto(), + [this](const std::vector& m){ apSend(m, true); return true; }); + _apAuth->begin(_apPmk, _apSelf, a2, _apGtk, 1); + _apAuth->startHandshake(); + } else apTrack(a2, rssiRaw, 7); // open: associated == connected + } + return; + } + if (type == 2 && toUs) { // data from the station (to-DS) + size_t hdr = 24; if (fc & 0x0080) hdr += 2; // QoS-data + if (len < hdr + 8) return; + const uint8_t* llc = f + hdr; + static const uint8_t snap[6] = {0xaa,0xaa,0x03,0x00,0x00,0x00}; + if (std::memcmp(llc, snap, 6) == 0 && ((llc[6]<<8)|llc[7]) == 0x888E && _apAuth) { + apTrack(a2, rssiRaw, -1); + if (_apAuth->onEapolKey(llc + 8, (len - hdr) - 8)) apTrack(a2, rssiRaw, 7); // 4-way done + } + } +} + +// ⚠️ WORK IN PROGRESS — status UNTESTED against real hardware post-rebuild. The dongle beacons +// (open/WPA2), tracks clients + RSSI, and the full WPA2 Authenticator (Wpa2Authenticator) is +// implemented + crypto self-tested. Previously blocked on "the 8812 AP/master-mode HW bring-up" +// (HW beacon queue + TSF + per-station ACK), which the old software-beacon-loop + manual register +// pokes never actually reached correctly. Upstream's rebuild adds exactly that HW bring-up as a +// library primitive (IRtlDevice::StartBeacon/StopBeacon) -- bench-proven against a real Linux +// station in docs/ap-mode.md (probe/auth/assoc at retry=0, the ACK engine armed by StartBeacon +// itself programming REG_MACID/net_type=AP; tests/ap_wpa2.cpp shows the same WPA2 4-way + ARP/ +// ICMP/DHCP responder shape this class already has). Wired onto that here: no more manual +// MSR/RCR/beacon-timing register writes, no more a software poll-loop re-sending the beacon MPDU +// every 100ms -- StartBeacon's hardware TBTT engine airs it. Real-hardware association is +// untested (this session ported the wiring; it did not have hardware to validate the retry=0 +// claim end-to-end for OUR beacon/IE shape). Still not exposed via JNI/the app. +void ApfpvStation::startAp(const std::string& ssid, int channel, const std::string& password) { + stopAp(); stopBeaconCal(); disconnect(); + if (!_rtl || !_dev) return; + auto* rtl = reinterpret_cast(_rtl); + auto* dev = reinterpret_cast(_dev); + _apWpa2 = !password.empty(); _apChannel = channel; _apSsid = ssid; + { std::lock_guard lk(_apMtx); _apStaList.clear(); } + { std::lock_guard lk(_apTxMtx); _apTxQ.clear(); } + _apAuth.reset(); + + Mac self{}; // self MAC = BSSID (synthesize if unprogrammed) + for (int i = 0; i < 6; ++i) self[i] = dev->rtw_read8(0x0610 + i); + bool bad = true; for (int i=0;i<6;i++) if (self[i]!=0 && self[i]!=0xFF) bad=false; + // docs/ap-mode.md: the BSSID MUST be unicast (I/G bit clear) -- a multicast SA makes a real + // station silently drop its own auth before it hits the air. 0x02 (locally-administered, + // unicast) is the bench-proven prefix. + if (bad) { const uint8_t m[6]={0x02,0x11,0x22,0x33,0x44,0x55}; for(int i=0;i<6;i++) self[i]=m[i]; } + _apSelf = self; + + if (_apWpa2) { + Crypto c = MakeWpa2Crypto(); + _apPmk = c.pbkdf2_pmk(password, (const uint8_t*)ssid.data(), ssid.size()); + FILE* u = fopen("/dev/urandom","rb"); // random GTK (broadcast key) + if (u) { size_t r=fread(_apGtk.data(),1,16,u); (void)r; fclose(u); } + else { for (int i=0;i<16;i++) _apGtk[i] = (uint8_t)((i*97)^0x3c); } + } + + try { + SelectedChannel sc = legalApfpvChannel(channel, 20); + rtl->InitWrite(sc); // device bring-up (firmware/PHY/channel), no RX yet + rtl->SetTxPowerIndexOverride(40); + rtl->StopRxLoop(); safeJoinThread(_rxThread); // clean slate before arming the AP RX thread + _rxThread = std::thread([this, rtl]() { // full-duplex RX (docs/ap-mode.md) so apOnRx + try { // actually fires (client mgmt/data RX) + rtl->StartRxLoop([this](const Packet& p){ + apOnRx(p.Data.data(), p.Data.size(), p.RxAtrib.rssi[0]); + }); + } catch (...) {} + }); + } catch (...) { return; } + dev->rtw_write8(0x0522, 0x00); // release TX-queue pause left by InitWrite's IQK/cal + + // StartBeacon (IRtlDevice) does the FULL vendor AP bring-up in the correct order (port + // identity from the beacon's own addr2/addr3, MSR->AP, beacon-DMA/ATIM/TSF timing, the + // DUAL_TSF_RST arm pulse, BCN_CTRL) and downloads the beacon to the hardware TBTT engine's + // reserved page -- replacing BOTH the old manual register-poke block AND the old software + // 100ms send_packet poll loop. beacon interval must match BuildBeacon's own hardcoded + // fixed-header value (100 TU) or the advertised vs. actual timing would mismatch. + auto beacon = BuildBeacon(self, ssid, (uint8_t)channel, _apWpa2); + bool bok = false; + try { bok = rtl->StartBeacon(beacon.data(), beacon.size(), 100); } catch (...) {} + SCANLOG("AP StartBeacon: %s", bok ? "OK" : "FAILED"); + + // AP TX-pump: apSend() (probe/auth/assoc responses, EAPOL, encrypted data) queues into + // _apTxQ instead of calling send_packet inline from apOnRx, which runs on _rxThread inside + // StartRxLoop's callback -- send_packet from there returns BUSY (docs/ap-mode.md). Drain + // the queue from this separate thread instead, same division of labour as tests/ap_wpa2.cpp. + _apRun.store(true); _beaconRun.store(true); + _beaconThread = std::thread([this, dev]() { + while (_beaconRun.load()) { + std::vector> batch; + { std::lock_guard lk(_apTxMtx); batch.swap(_apTxQ); } + for (auto& frame : batch) { + dev->rtw_write8(0x0522, 0x00); + try { dev->send_packet(frame.data(), frame.size()); } catch (...) {} + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } + }); + SCANLOG("AP up: ssid=\"%s\" ch=%d %s bssid=%02x:%02x:%02x:%02x:%02x:%02x", ssid.c_str(), channel, + _apWpa2?"WPA2-PSK":"OPEN", self[0],self[1],self[2],self[3],self[4],self[5]); +} + +void ApfpvStation::stopAp() { + _apRun.store(false); + // STOP the beacon TX thread FIRST and join it. Previously stopAp cleared _apRun but NOT + // _beaconRun, so the beacon thread kept calling send_packet into a device being torn down → + // the bulk-OUT endpoint jammed (USBDEVFS_BULK OUT failed: -1) and stayed wedged until a + // physical replug. Joining here guarantees no TX is in flight before we tear the HW down. + _beaconRun.store(false); + if (_beaconThread.joinable()) { try { _beaconThread.join(); } catch (...) {} } + { std::lock_guard lk(_apTxMtx); _apTxQ.clear(); } + // StopBeacon (IRtlDevice) tears down exactly what StartBeacon armed: EN_BCN_FUNCTION off, + // StopTxBeacon, net_type -> No Link. Best-effort (device may already be gone). + if (_rtl) { try { reinterpret_cast(_rtl)->StopBeacon(); } catch (...) {} } + stopBeaconCal(); + if (_rtl) { try { reinterpret_cast(_rtl)->StopRxLoop(); } catch (...) {} } // else hangs on exit + safeJoinThread(_rxThread); + _apAuth.reset(); +} + +static uint16_t ipChecksum(const uint8_t* p, size_t n); +static std::vector buildTcp(uint32_t srcIp, uint32_t dstIp, uint16_t sport, uint16_t dport, + uint32_t seq, uint32_t ack, uint8_t flags, + const uint8_t* payload, size_t plen); + +// Compute the WPA2 PMK (PBKDF2, slow) ONCE and cache it. The PMK depends only on +// passphrase+SSID, so it survives across (re)connects. Called before the scan so +// becomeStation can use beginCached() and switch the RX to the EAPOL phase within the +// AP's ~4s M1-retry window (inline PBKDF2 in becomeStation missed every M1). +void ApfpvStation::ensurePmk() { + if (_pmkValid.load()) return; + auto c = MakeWpa2Crypto(); + _pmkCache = c.pbkdf2_pmk(_params.passphrase, + reinterpret_cast(_params.ssid.data()), + _params.ssid.size()); + _pmkValid.store(true); +} + +// DOWNLINK sink setter. Stores the sink AND applies it to the live RxDeframe if the RX loop +// is already up — so the VpnService can arm/disarm the bridge at runtime (after connect), not +// only before. (connect() also re-applies _ipSink once _rx exists.) +void ApfpvStation::setIpSink(OnRtpFn fn) { + _ipSink = std::move(fn); + if (_rx) _rx->setIpSink(_ipSink); +} + +// UPLINK general-IP: CCMP-encrypt + TX an arbitrary IPv4 datagram (SSH, any TCP/UDP) the +// VpnService TUN read from the OS. Same encrypt+TX-desc path as DHCP/RTP uplink. +bool ApfpvStation::sendIpPacket(const uint8_t* ip, size_t len) { + if (!_wpa || !_wpa->ready() || !ip || len < 20) return false; + auto mpdu = _wpa->buildEncryptedData(ip, len); + if (mpdu.empty()) return false; + auto* dev = reinterpret_cast(_dev); + std::vector frame(40 + mpdu.size(), 0); // 40 = 8812 TX desc + std::memcpy(frame.data() + 40, mpdu.data(), mpdu.size()); + apfpv::FillStationTxDesc(frame.data(), (uint16_t)mpdu.size(), 40, + 1, apfpv::StationFrameKind::CcmpData, 7, 0x04); + return dev->sendStationFrameSync(frame.data(), frame.size()); +} + +uint32_t ApfpvStation::leaseIp() const { return _dhcp ? _dhcp->lease().ip : 0; } +uint32_t ApfpvStation::leaseServerIp() const { return _dhcp ? _dhcp->lease().server : 0; } + +// Minimal TCP/HTTP GET over the link — proves the dongle carries general TCP (so SSH/REST work). +std::string ApfpvStation::httpGet(uint32_t dstIp, uint16_t port, const std::string& path, int timeoutMs) { + if (!_rx || !_dhcp || !_dhcp->lease().valid) return ""; + using namespace std::chrono; + uint32_t srcIp = _dhcp->lease().ip; + const uint16_t sport = 0xC350; // ephemeral source port 50000 + { std::lock_guard lk(_ipQMtx); _ipQ.clear(); } + _ipCapture.store(true); + _rx->setIpSink([this](const uint8_t* p, size_t n){ + if (!_ipCapture.load()) return; + std::lock_guard lk(_ipQMtx); _ipQ.emplace_back(p, p + n); + }); + auto poll = [&](uint8_t wantFlags, uint32_t& srvSeqOut, std::string& data)->bool { + auto t0 = steady_clock::now(); + while (steady_clock::now() - t0 < milliseconds(timeoutMs)) { + std::vector> batch; + { std::lock_guard lk(_ipQMtx); batch.swap(_ipQ); } + for (auto& pk : batch) { + if (pk.size() < 40 || pk[9] != 6) continue; // IPv4 TCP + uint32_t s = ((uint32_t)pk[12]<<24)|((uint32_t)pk[13]<<16)|((uint32_t)pk[14]<<8)|pk[15]; + if (s != dstIp) continue; + size_t ihl = (size_t)(pk[0]&0xf)*4; if (pk.size() < ihl + 20) continue; + const uint8_t* t = pk.data() + ihl; + if ((uint16_t)((t[0]<<8)|t[1]) != port) continue; // src port = server + srvSeqOut = ((uint32_t)t[4]<<24)|((uint32_t)t[5]<<16)|((uint32_t)t[6]<<8)|t[7]; + size_t toff = (size_t)(t[12]>>4)*4; + if (pk.size() > ihl + toff) data.append((const char*)(t + toff), pk.size() - ihl - toff); + if (t[13] & wantFlags) return true; + } + std::this_thread::sleep_for(milliseconds(10)); + } + return false; + }; + uint32_t srvSeq = 0, seq = 1000; std::string resp, ignore; + auto syn = buildTcp(srcIp, dstIp, sport, port, seq, 0, 0x02, nullptr, 0); // SYN + sendIpPacket(syn.data(), syn.size()); + if (!poll(0x12, srvSeq, ignore)) { _ipCapture.store(false); return ""; } // wait SYN|ACK + seq = 1001; + std::string req = "GET " + path + " HTTP/1.0\r\nHost: vtx\r\n\r\n"; + auto get = buildTcp(srcIp, dstIp, sport, port, seq, srvSeq + 1, 0x18, // ACK + GET (PSH|ACK) + (const uint8_t*)req.data(), req.size()); + sendIpPacket(get.data(), get.size()); + poll(0x01, srvSeq, resp); // gather until FIN/timeout + auto fin = buildTcp(srcIp, dstIp, sport, port, seq + (uint32_t)req.size(), srvSeq + 1, 0x11, nullptr, 0); + sendIpPacket(fin.data(), fin.size()); // FIN|ACK (polite close) + _ipCapture.store(false); + return resp; +} + +// ---- raw bidirectional TCP (relay transport for SSH over the dongle) -------------------- +// Parse an inbound IPv4 packet and, if it belongs to the active connection (from _tcDst:_tcDport +// to us:_tcSport, TCP), extract its seq/flags/payload. +bool ApfpvStation::matchTcp(const std::vector& pk, uint32_t& seq, uint8_t& flags, + const uint8_t*& payload, size_t& plen) { + if (pk.size() < 40 || pk[9] != 6) return false; // IPv4 TCP + uint32_t s = ((uint32_t)pk[12]<<24)|((uint32_t)pk[13]<<16)|((uint32_t)pk[14]<<8)|pk[15]; + if (s != _tcDst) return false; + size_t ihl = (size_t)(pk[0]&0xf)*4; if (pk.size() < ihl + 20) return false; + const uint8_t* t = pk.data() + ihl; + uint16_t sp = (uint16_t)((t[0]<<8)|t[1]), dp = (uint16_t)((t[2]<<8)|t[3]); + if (sp != _tcDport || dp != _tcSport) return false; + seq = ((uint32_t)t[4]<<24)|((uint32_t)t[5]<<16)|((uint32_t)t[6]<<8)|t[7]; + flags = t[13]; + size_t toff = (size_t)(t[12]>>4)*4; + payload = t + toff; + plen = (pk.size() > ihl + toff) ? (pk.size() - ihl - toff) : 0; + return true; +} + +bool ApfpvStation::tcpConnect(uint32_t dstIp, uint16_t dport, int timeoutMs) { + if (!_rx || !_dhcp || !_dhcp->lease().valid) return false; + _tcSrc = _dhcp->lease().ip; _tcDst = dstIp; _tcDport = dport; + _tcSport = 0xC351; // ephemeral src port 50001 + _tcSeq = 2000; _tcAck = 0; _tcOpen = false; + { std::lock_guard lk(_ipQMtx); _ipQ.clear(); } + _ipCapture.store(true); + _rx->setIpSink([this](const uint8_t* p, size_t n){ + if (!_ipCapture.load()) return; + std::lock_guard lk(_ipQMtx); _ipQ.emplace_back(p, p + n); + }); + auto syn = buildTcp(_tcSrc,_tcDst,_tcSport,_tcDport,_tcSeq,0,0x02,nullptr,0); + sendIpPacket(syn.data(), syn.size()); + using namespace std::chrono; auto t0 = steady_clock::now(); auto tRetx = t0; + while (steady_clock::now() - t0 < milliseconds(timeoutMs)) { + if (steady_clock::now() - tRetx > milliseconds(400)) { // SYN retransmit (userspace TX drops) + sendIpPacket(syn.data(), syn.size()); tRetx = steady_clock::now(); + } + std::vector> batch; + { std::lock_guard lk(_ipQMtx); batch.swap(_ipQ); } + for (auto& pk : batch) { + uint32_t seq; uint8_t flags; const uint8_t* pl; size_t pn; + if (!matchTcp(pk, seq, flags, pl, pn)) continue; + if ((flags & 0x12) == 0x12) { // SYN|ACK + _tcAck = seq + 1; // ack their SYN + _tcSeq += 1; // our SYN consumed a seq + auto ack = buildTcp(_tcSrc,_tcDst,_tcSport,_tcDport,_tcSeq,_tcAck,0x10,nullptr,0); + sendIpPacket(ack.data(), ack.size()); + _tcOpen = true; return true; + } + if (flags & 0x04) { fprintf(stderr, "[tcp] RST from %u.%u.%u.%u:%u — port closed\n", + (_tcDst>>24)&255,(_tcDst>>16)&255,(_tcDst>>8)&255,_tcDst&255,_tcDport); + _ipCapture.store(false); return false; } // RST + } + std::this_thread::sleep_for(milliseconds(3)); + } + fprintf(stderr, "[tcp] no SYN|ACK from %u.%u.%u.%u:%u within timeout\n", + (_tcDst>>24)&255,(_tcDst>>16)&255,(_tcDst>>8)&255,_tcDst&255,_tcDport); + _ipCapture.store(false); return false; +} + +int ApfpvStation::tcpPoll(std::string& out, int timeoutMs) { + if (!_tcOpen) return -1; + using namespace std::chrono; auto t0 = steady_clock::now(); int appended = 0; + do { + std::vector> batch; + { std::lock_guard lk(_ipQMtx); batch.swap(_ipQ); } + for (auto& pk : batch) { + uint32_t seq; uint8_t flags; const uint8_t* pl; size_t pn; + if (!matchTcp(pk, seq, flags, pl, pn)) continue; + if (flags & 0x04) { _tcOpen = false; return appended > 0 ? appended : -1; } // RST + if (pn) { + if (seq == _tcAck) { // in-order data + out.append((const char*)pl, pn); + _tcAck += (uint32_t)pn; appended += (int)pn; + } + // in-order or duplicate: (re-)ACK so the peer advances / retransmits + auto ack = buildTcp(_tcSrc,_tcDst,_tcSport,_tcDport,_tcSeq,_tcAck,0x10,nullptr,0); + sendIpPacket(ack.data(), ack.size()); + } + if ((flags & 0x01) && seq + (uint32_t)pn == _tcAck) { // FIN (in order) + _tcAck += 1; + auto fa = buildTcp(_tcSrc,_tcDst,_tcSport,_tcDport,_tcSeq,_tcAck,0x11,nullptr,0); + sendIpPacket(fa.data(), fa.size()); + _tcOpen = false; return appended > 0 ? appended : -1; + } + } + if (appended > 0) return appended; + std::this_thread::sleep_for(milliseconds(2)); + } while (steady_clock::now() - t0 < milliseconds(timeoutMs)); + return 0; +} + +bool ApfpvStation::tcpSend(const uint8_t* data, size_t n) { + if (!_tcOpen || !n) return false; + size_t off = 0; + while (off < n) { + size_t chunk = (n - off > 1024) ? 1024 : (n - off); + auto seg = buildTcp(_tcSrc,_tcDst,_tcSport,_tcDport,_tcSeq,_tcAck,0x18,data+off,chunk); + sendIpPacket(seg.data(), seg.size()); + _tcSeq += (uint32_t)chunk; off += chunk; + } + return true; +} + +void ApfpvStation::tcpClose() { + if (_tcOpen) { + auto fin = buildTcp(_tcSrc,_tcDst,_tcSport,_tcDport,_tcSeq,_tcAck,0x11,nullptr,0); + sendIpPacket(fin.data(), fin.size()); + _tcSeq += 1; _tcOpen = false; + } + _ipCapture.store(false); +} + +static uint16_t ipChecksum(const uint8_t* p, size_t n) { + uint32_t s = 0; + for (size_t i = 0; i + 1 < n; i += 2) s += (uint32_t)((p[i] << 8) | p[i+1]); + if (n & 1) s += (uint32_t)(p[n-1] << 8); + while (s >> 16) s = (s & 0xffff) + (s >> 16); + return (uint16_t)~s; +} +// Wrap a BOOTP/DHCP payload in IPv4+UDP (0.0.0.0:68 -> 255.255.255.255:67). The ApfpvDhcp +// emits only the BOOTP message (RX strips IP/UDP too), but buildEncryptedData expects a +// COMPLETE IPv4 packet — without these headers the AP's IP stack sees "IP version 0" and +// drops the DISCOVER before dnsmasq, so no lease is ever offered. +static std::vector wrapDhcpUdpIp(const uint8_t* bootp, size_t blen) { + size_t udpLen = 8 + blen, ipLen = 20 + udpLen; + std::vector p(ipLen, 0); + p[0]=0x45; p[2]=(uint8_t)(ipLen>>8); p[3]=(uint8_t)(ipLen&0xff); // ver4/ihl5, total len + p[8]=64; p[9]=17; // ttl=64, proto=UDP (src stays 0.0.0.0) + p[16]=p[17]=p[18]=p[19]=0xff; // dst = 255.255.255.255 + uint16_t ick = ipChecksum(p.data(), 20); + p[10]=(uint8_t)(ick>>8); p[11]=(uint8_t)(ick&0xff); + p[21]=68; p[23]=67; // UDP sport 68, dport 67 + p[24]=(uint8_t)(udpLen>>8); p[25]=(uint8_t)(udpLen&0xff); // UDP len; checksum 0 (optional v4) + std::memcpy(p.data()+28, bootp, blen); + return p; +} + +// Build an IPv4 + TCP segment (flags: SYN=0x02 ACK=0x10 PSH=0x08 FIN=0x01 RST=0x04). +static std::vector buildTcp(uint32_t srcIp, uint32_t dstIp, uint16_t sport, uint16_t dport, + uint32_t seq, uint32_t ack, uint8_t flags, + const uint8_t* payload, size_t plen) { + size_t tcpLen = 20 + plen, ipLen = 20 + tcpLen; + std::vector p(ipLen, 0); + p[0]=0x45; p[2]=(uint8_t)(ipLen>>8); p[3]=(uint8_t)(ipLen&0xff); p[8]=64; p[9]=6; // proto TCP + p[12]=(uint8_t)(srcIp>>24);p[13]=(uint8_t)(srcIp>>16);p[14]=(uint8_t)(srcIp>>8);p[15]=(uint8_t)srcIp; + p[16]=(uint8_t)(dstIp>>24);p[17]=(uint8_t)(dstIp>>16);p[18]=(uint8_t)(dstIp>>8);p[19]=(uint8_t)dstIp; + uint16_t ick = ipChecksum(p.data(), 20); p[10]=(uint8_t)(ick>>8); p[11]=(uint8_t)(ick&0xff); + uint8_t* t = p.data()+20; + t[0]=(uint8_t)(sport>>8);t[1]=(uint8_t)sport;t[2]=(uint8_t)(dport>>8);t[3]=(uint8_t)dport; + t[4]=(uint8_t)(seq>>24);t[5]=(uint8_t)(seq>>16);t[6]=(uint8_t)(seq>>8);t[7]=(uint8_t)seq; + t[8]=(uint8_t)(ack>>24);t[9]=(uint8_t)(ack>>16);t[10]=(uint8_t)(ack>>8);t[11]=(uint8_t)ack; + t[12]=0x50; t[13]=flags; t[14]=0xff; t[15]=0xff; // data-offset 5, window 0xffff + if (plen) std::memcpy(t+20, payload, plen); + uint32_t s = ((srcIp>>16)&0xffff)+(srcIp&0xffff)+((dstIp>>16)&0xffff)+(dstIp&0xffff)+6u+(uint32_t)tcpLen; + for (size_t i=0;i+1>16) s=(s&0xffff)+(s>>16); + uint16_t ck=(uint16_t)~s; t[16]=(uint8_t)(ck>>8); t[17]=(uint8_t)(ck&0xff); + return p; +} + +// Build an IPv4 + UDP datagram (UDP checksum 0 = optional in v4). Used to carry the LQ-feedback +// percentage to the VTX aalink over the dongle's own IP stack (see the LqFeedback sink) — the host +// socket path can't reach the VTX on the libusb (Win/WSL) dongle path. +static std::vector buildUdp(uint32_t srcIp, uint32_t dstIp, uint16_t sport, uint16_t dport, + const uint8_t* payload, size_t plen) { + size_t udpLen = 8 + plen, ipLen = 20 + udpLen; + std::vector p(ipLen, 0); + p[0]=0x45; p[2]=(uint8_t)(ipLen>>8); p[3]=(uint8_t)(ipLen&0xff); p[8]=64; p[9]=17; // proto UDP + p[12]=(uint8_t)(srcIp>>24);p[13]=(uint8_t)(srcIp>>16);p[14]=(uint8_t)(srcIp>>8);p[15]=(uint8_t)srcIp; + p[16]=(uint8_t)(dstIp>>24);p[17]=(uint8_t)(dstIp>>16);p[18]=(uint8_t)(dstIp>>8);p[19]=(uint8_t)dstIp; + uint16_t ick = ipChecksum(p.data(), 20); p[10]=(uint8_t)(ick>>8); p[11]=(uint8_t)(ick&0xff); + uint8_t* u = p.data()+20; + u[0]=(uint8_t)(sport>>8);u[1]=(uint8_t)sport;u[2]=(uint8_t)(dport>>8);u[3]=(uint8_t)dport; + u[4]=(uint8_t)(udpLen>>8);u[5]=(uint8_t)(udpLen&0xff); // UDP length; checksum left 0 (optional) + if (plen) std::memcpy(u+8, payload, plen); + return p; +} + +// The gated establishment chain (arm -> auth -> assoc -> WPA2 -> DHCP -> stream). +// Returns true only on a held, streaming link. Used for both initial connect +// AND each supervisor reconnect attempt. +bool ApfpvStation::runConnectChain() { + _pendingAddbaTids = 0; // fresh ADDBA debounce per connection + _raCfgSent.store(false); // re-send MACID_CFG once on this (re)connection + auto& dev = *reinterpret_cast(_dev); + auto& rm = *reinterpret_cast(_rm); + auto sendFrame = [&dev](const std::vector& f) { + // Station TX coexisting with the kernel-tasklet async RX. libusb userspace + // cannot push a bulk-OUT while bulk-IN URBs are pending (WinUSB + USB/IP + // both serialise OUT behind INs, unlike the Linux kernel USB core), so a + // bare async send_packet times out (status 2, 0 bytes). sendStationFrameSync + // drains the RX pool, does the proven sync bulk-OUT, then re-arms RX fast + // enough to catch the AP's reply. DEVOURER_BARE_ASYNC_TX forces the old + // (broken) path for A/B; DEVOURER_SYNC_IO is the legacy all-sync mode. + if (std::getenv("DEVOURER_BARE_ASYNC_TX")) + return dev.send_packet(const_cast(f.data()), f.size()); + if (std::getenv("DEVOURER_SYNC_IO")) + return dev.bulk_send_sync(const_cast(f.data()), f.size(), 200) == 0; + return dev.sendStationFrameSync(const_cast(f.data()), f.size()); + }; + StationMode sta(dev, rm, sendFrame); + // Use user-selected bandwidth: 40 or 80 MHz. If the selected bandwidth fails + // to connect, set DEVOURER_FORCE_20MHZ=1 to force 20 MHz (proven stable). + ChannelWidth_t userBw = _params.bandwidth >= 80 ? CHANNEL_WIDTH_80 + : _params.bandwidth >= 40 ? CHANNEL_WIDTH_40 + : CHANNEL_WIDTH_20; + if (std::getenv("DEVOURER_FORCE_20MHZ")) userBw = CHANNEL_WIDTH_20; + // Arm at the AP's ACTUAL width/center. This is why 80 MHz worked but 40/20 didn't: at 80 MHz + // the station tuned to center 42 and MATCHED the AP, so it RXed the AP's HT/VHT data — incl. + // the EAPOL M1 — and the 4-way completed. Arming narrower (center 36/38) than the AP's real + // operating width mis-captures its aggregated data frames -> M1 missed -> handshake times out. + // Wide-center arming only ever failed because the IQK wedged the synth; IQK is now disabled, + // so we can (and must) match the AP's width here. DEVOURER_FORCE_20MHZ still forces 20 for A/B. + sta.setConnectWidth(userBw); + MacAddr self{}, bssid{}; + // NOTE: our MAC is read from REG_MACID AFTER the device is brought up (below). + // Reading it before Init returned EFUSE-unloaded garbage (e.g. ea:ea:ea:...). + + // Bring the PHY/RF up BEFORE any channel change (scanForSsid + arm call + // set_channel_bwmode -> phy_SwChnl8812, which null-derefs if never Init'd), + // AND wire the RX so discovery (beacons -> onScanFrame) and the arm handshake + // (auth/assoc/deauth -> onMgmtFrame) actually receive. Previously this used a + // no-op processor, so _scanResult and _gotAuthResp were never set -> the link + // always failed at discovery. The real RxDeframe RX is wired after handshake. + { std::lock_guard lk(_scanMtx); _scanSeen.clear(); } // diag: SSIDs heard + // Run the device RX loop on its OWN thread. RtlJaguarDevice::Init is a BLOCKING + // infinite read loop ("Listening air..."); calling it inline froze the connect + // on the hint channel so scanForSsid never hopped (it only ever heard ch40). + // The connect thread now drives channel hops + arm while this thread feeds RX. + // One dispatch routes by phase: discovery/arm -> StationMode; streaming -> RxDeframe. + if (_rtl) { + auto* rtl = reinterpret_cast(_rtl); + // shared_ptr survives sta destruction; dispatch checks before calling + auto alive = std::make_shared>(true); + sta._alivePtr = alive; + StationMode* sp = &sta; + auto dispatch = [sp, this, alive](const Packet& pkt){ + _rxReady.store(true); + if (_rxPhase.load() == 1) { + // STREAMING/handshake path: routes to RxDeframe via _rx (owned by ApfpvStation, + // outlives the per-attempt StationMode). MUST NOT be gated by `alive`: when + // runConnectChain returns at the Streaming transition, the local `sta` is + // destroyed (alive=false) — gating here dropped EVERY data frame -> RX silence + // -> watchdog -> reconnect loop. RxDeframe doesn't touch `sp`, so it's safe. + // NOTE: NO per-packet fprintf here — at 65Mbps that's 5600 blocking writes/s on + // the RX dispatch thread, which stalls RX and shreds frame assembly. + RxDeframe* rx = _rx.get(); if (rx) rx->onPacket(pkt); return; + } + // DISCOVERY/ARM path below calls into `sp` (the StationMode). Guard it: if `sta` + // was destroyed, sp is dangling — TOCTOU-safe via the shared `alive` flag. + if (!alive->load()) return; + size_t n = pkt.Data.size(); + if (n < 24 || n > 4096) return; + // Copy the frame off the RX span before parsing: pkt.Data points into the + // USB transfer buffer, which the async RX path can recycle while the beacon + // parser is still walking it -> use-after-free -> SIGSEGV in ScanProbe on + // busy RF. A stable local copy removes that window. + std::vector framebuf(pkt.Data.begin(), pkt.Data.end()); + const uint8_t* f = framebuf.data(); + uint16_t fc = (uint16_t)(f[0] | (f[1] << 8)); + if (((fc>>2)&3)==0 && n >= 30) { + uint8_t sub = (fc>>4)&0xF; + if (sub==0x1 && n >= 24 + 6) { // assoc response: parse HT/VHT Op IEs for BW + // rtw_ies_get_chbw: extract bandwidth from AP's HT/VHT Operation IEs. + // Without this, we never know what bandwidth the AP actually assigned us + // and stay at 20 MHz even when the AP supports 40/80 MHz. + uint8_t assocBw = 0, assocOff = 0; // 0=20, 1=40, 2=80 MHz + const uint8_t* ie = f + 24 + 6; // skip mgmt hdr + fixed assoc resp (6B) + size_t ieLen = n - 24 - 6; + for (size_t pos = 0; pos + 2 <= ieLen; ) { + uint8_t id = ie[pos], len = ie[pos+1]; + if (pos + 2 + len > ieLen) break; + if (id == 0x3D && len >= 22) { // HT Capabilities IE + if (ie[pos+2+1] & 0x04) assocBw = 1; // Supported Chan Width Set + } else if (id == 0x3E && len >= 3) { // HT Operation IE + if (assocBw == 1) { + uint8_t staChWidth = ie[pos+2+2] & 0x01; // STA Chan Width + if (!staChWidth) assocBw = 0; // AP says 20 MHz only + uint8_t secOff = (ie[pos+2+1] & 0x03); // Secondary Chan Offset + if (secOff == 1) assocOff = 2; // SCA -> UPPER + else if (secOff == 3) assocOff = 1; // SCB -> LOWER + } + } else if (id == 0xC0 && len >= 3) { // VHT Operation IE + uint8_t chWidth = ie[pos+2]; // 0=20/40, 1=80, 2=160 + if (chWidth >= 1 && assocBw >= 1) assocBw = 2; // 80 MHz + } + pos += 2 + len; + } + if (assocBw > 0) { + // ADOPT the AP's advertised bandwidth automatically — the band + channel + // already come from the scan, and the operating width should match what the + // AP actually provides (a fixed user pref of 20 would waste an 80MHz AP). The + // firmware RA adapts the MCS within the width, so wider is strictly better when + // the AP offers it. DEVOURER_MAX_BW=20|40|80 caps it only if you deliberately + // want to limit the width; otherwise it follows the AP. + int apBw = assocBw==2?80 : assocBw==1?40 : 20; + int cap = 160; if (const char* m = std::getenv("DEVOURER_MAX_BW")) cap = atoi(m); + _params.bandwidth = apBw < cap ? apBw : cap; + SCANLOG("assoc-response: AP bw=%dMHz -> using %dMHz (auto; off=%d cap=%d)", + apBw, (int)_params.bandwidth, (int)assocOff, cap); + } + } + } + sp->onScanFrame(f, n); // beacons -> _scanResult + std::string ss; ApInfo bi; + if (ScanProbe::parseAnyBeacon(f, n, ss, bi) && !ss.empty()) { + std::lock_guard lk(_scanMtx); + if (_scanSeen.insert(ss).second) + SCANLOG("discovery heard \"%s\" ch%d", ss.c_str(), bi.channel); + } + MacAddr a1{}, a2{}; + std::memcpy(a1.b.data(), f + 4, 6); + std::memcpy(a2.b.data(), f + 10, 6); + sp->onMgmtFrame(fc, a1, a2); // auth/assoc/deauth -> flags + }; + _rxPhase.store(0); _rxReady.store(false); + // WFB-style bandwidth from the start: use user-selected bw, not hardcoded 20. + // The kernel's init_hw_mlme_ext does the same — sets cur_bwmode before auth. + uint8_t initCh = (uint8_t)(_params.channel > 0 ? _params.channel : 40); + // ALWAYS bring up at the 20 MHz primary. The Init runs an IQK on the init channel; + // at a 40/80 MHz center (e.g. ch36@40 -> center 38) that IQK wedges the RF synth + // (RF_CH=0xea) and — critically — the wedge PERSISTS across app restarts AND survives + // USBDEVFS_RESET (only a physical replug clears it). After the first assoc adopts the + // AP's 40 MHz, _params.bandwidth=40, so a bandwidth-driven initBw would re-wedge on + // every reconnect. Keep every IQK at a 20 MHz center; the real width is applied + // post-handshake (same channel, no band change -> no fresh IQK -> no wedge). + ChannelWidth_t initBw = CHANNEL_WIDTH_20; + uint8_t initOff = 0; + SelectedChannel initSel{ .Channel=initCh, .ChannelOffset=initOff, .ChannelWidth=initBw }; + // RtlJaguarDevice::Init (bring-up + StartRxLoop) is a BLOCKING read loop on this + // driver base -- the pre-rebuild split between a "legacy sync" thread and a + // "kernel-style async URB pool" pumped by the caller no longer applies (there is + // no non-blocking RX submission left to pump); both collapsed to the same shape: + // run Init on its own thread, TX (sendStationFrameSync) proceeds concurrently on + // this one exactly as the old async path required. + rtl->StopRxLoop(); safeJoinThread(_rxThread); + _rxThread = std::thread([rtl, dispatch, initSel]() { + try { rtl->Init(dispatch, initSel); } catch (...) {} + }); + // wait until the RX loop is live (device brought up) or a short timeout + using namespace std::chrono; + auto t0 = steady_clock::now(); + while (!_rxReady.load() && steady_clock::now() - t0 < seconds(4)) + std::this_thread::sleep_for(milliseconds(50)); + + // ---- RF-WEDGE AUTO-RECOVERY ---------------------------------------- + // A full cold software re-init (power-on + hw_reset + firmware + PHY/RF + // tables) runs every bring-up but does NOT clear an RF-synth wedge (RF + // 0x18 reads 0xea -> can't tune ANY channel -> scan finds nothing / auth + // TX fails). Proven on-device. The only lever left short of a physical + // replug is a USB port reset (USBDEVFS_RESET), which cycles the chip's + // reset the way VBUS removal does. On detecting the wedge, reset + re-init + // and re-check, up to a few times. Gate off with DEVOURER_NO_USB_RESET. + if (!std::getenv("DEVOURER_NO_USB_RESET")) { + // USBDEVFS_RESET (the only USB reset reachable via the Java-wrapped fd) is a + // protocol reset, NOT a VBUS power-cycle — proven on-device to NOT clear an + // existing RF wedge (rc=0 but RF_CH stays 0xea). So this is best-effort only; + // the real fix is prevention (all IQK at 20 MHz centers). 1 attempt by default. + int maxTries = 1; + if (const char* e = std::getenv("DEVOURER_USB_RESET_TRIES")) maxTries = atoi(e); + for (int attempt = 1; attempt <= maxTries && rm.rf_wedged(); ++attempt) { + SCANLOG("RF WEDGED (RF_CH=0xea) after bring-up -> USB port reset attempt %d/%d", + attempt, maxTries); + rtl->StopRxLoop(); safeJoinThread(_rxThread); + std::this_thread::sleep_for(milliseconds(50)); + int rc = dev.reset_device(); // libusb_reset_device / USBDEVFS_RESET + SCANLOG("USB reset_device() rc=%d (0=ok; <0 = libusb err, fd likely re-enumerated)", rc); + std::this_thread::sleep_for(milliseconds(300)); // let the chip re-appear + _rxPhase.store(0); _rxReady.store(false); + // Init runs on _rxThread (blocking loop); a spawn failure (bad_alloc, etc) is the + // only thing that can throw here -- the RX loop's own errors surface via _rxReady + // timing out below, same as the first bring-up above. + try { + _rxThread = std::thread([rtl, dispatch, initSel]() { + try { rtl->Init(dispatch, initSel); } catch (...) {} + }); + } catch (...) { SCANLOG("re-init after USB reset threw -- bailing"); break; } + auto tr = steady_clock::now(); + while (!_rxReady.load() && steady_clock::now() - tr < seconds(4)) + std::this_thread::sleep_for(milliseconds(50)); + SCANLOG("post-reset bring-up: rf_wedged=%d", rm.rf_wedged() ? 1 : 0); + } + if (rm.rf_wedged()) + SCANLOG("RF STILL WEDGED after %d USB resets — likely needs a physical replug", maxTries); + } + // -------------------------------------------------------------------- + + // TX POWER: the station auth/assoc TX uses the same chip/PA the beacon-cal + // (startBeaconCal -> SetTxPower) and the verified-working monitor injection + // (txdemo SetTxPower(40)) drive — but the connect path NEVER set it, so the + // auth-req keyed the PA at unset/zero power = on-air silence. Set a solid + // index now so the frame actually radiates. (Java applies the legal-EIRP + // clamp via nativeStaSetTxPower; 40 here is the txdemo working value.) + try { rtl->SetTxPowerIndexOverride(40); SCANLOG("station TX power set to idx 40"); } + catch (...) { SCANLOG("station SetTxPower threw"); } + } + + // Our MAC, read NOW that the device is up (REG_MACID populated from EFUSE). + // Used as addr2 in auth/assoc + for WPA2 PTK; a garbage MAC makes the AP + // reply to a bogus address. 0x0610 = REG_MACID. + for (int i = 0; i < 6; ++i) self.b[i] = dev.rtw_read8(0x0610 + i); + bool bad = true; for (int i=0;i<6;i++){ if(self.b[i]!=0 && self.b[i]!=0xFF) bad=false; } + if (bad) { self.b[0]=0x02; self.b[1]=0x11; self.b[2]=0x22; + self.b[3]=0x33; self.b[4]=0x44; self.b[5]=0x55; } + ensurePmk(); // precompute the PBKDF2 PMK now (cached) so becomeStation is instant + dev.setTxFastPath(false); // connect handshake wants the per-TX drain diag; Streaming flips it true + + // DISCOVERY: scan beacons for the SSID -> BSSID + channel + negotiated RSN. + // Removes the hardcoded-channel / empty-BSSID / fixed-cipher assumptions + // (security + discovery parity with the kernel wpa_supplicant the VRX uses). + ApInfo ap; + if (_params.haveBssid) { + // PHONE-ASSISTED: the phone already resolved the AP's BSSID + channel, so + // skip the dongle sweep entirely and arm straight to it (CCMP/PSK default). + for (int i = 0; i < 6; ++i) bssid.b[i] = _params.bssid[i]; + sta.setNegotiatedCipher(0x000FAC04, 0x000FAC04); + SCANLOG("phone-assisted: BSSID %02x:%02x:%02x:%02x:%02x:%02x ch%d — skipping sweep", + bssid.b[0],bssid.b[1],bssid.b[2],bssid.b[3],bssid.b[4],bssid.b[5], _params.channel); + } else if (_params.scan) { + set(State::Scanning); + // Sweep beacons for the SSID -> BSSID + channel + negotiated RSN (now works: + // the RX thread feeds frames while this thread hops channels). + rm.setScanMode(true); // suppress IQK during the hop (band-cross IQK wedges the RF -> found=0) + ap = sta.scanForSsid(_params.ssid.c_str(), _params.channel, /*ms*/300); + rm.setScanMode(false); // arm channel gets a clean, settled IQK below + SCANLOG("scan: \"%s\" found=%d ch=%d", _params.ssid.c_str(), ap.found?1:0, ap.channel); + if (!ap.found) { set(State::FailNoAp); return false; } + bssid.b = ap.bssid; + // Persist the discovered BSSID into _params so downstream users get the REAL AP + // MAC — notably setSecCamKey (HW-decrypt CAM entries) and the ADDBA frames, which + // read _params.bssid. Before this it stayed zero-initialized on the scan path, so + // the HW-decrypt CAM entry was written with MAC 00:00:.. → the security engine + // found no key for A2= → SWDEC=1 → HW decrypt silently never engaged. (This does + // NOT set _params.haveBssid, so the supervisor still re-scans on reconnect.) + _params.bssid = ap.bssid; + _params.channel = ap.channel ? ap.channel : _params.channel; + // NOTE: do NOT set _params.haveBssid here. Persisting it made every + // supervisor RECONNECT skip the scan and jump straight to auth — and the + // auth TX then FAILS (arm result 3 = TXFAIL_NoAuth), looping forever. The + // scan both re-confirms the channel AND warms the TX path; the parseBeacon + // crash it was meant to avoid is now covered by the _alive guard + try/catch. + // haveBssid stays reserved for explicit phone-assisted connect only. + sta.setNegotiatedCipher(ScanProbe::chooseCipher(ap.pairwise), + ap.rsnPresent ? ap.groupCipher : 0x000FAC04); + SCANLOG("CIPHERS: pairwise=0x%08x group=0x%08x (CCMP=0x000FAC04 TKIP=0x000FAC02)", + ScanProbe::chooseCipher(ap.pairwise), ap.rsnPresent ? ap.groupCipher : 0x000FAC04); + } + + // RETUNE to the AP's actual channel before the arm. The scanForSsid hop leaves + // the radio on the last scanned channel (e.g. ch46), not the AP's channel (ch44). + // arm() uses _rm.current_channel() which must equal the AP's actual channel or + SCANLOG("pre-arm tune: want=%d current=%d", (int)_params.channel, (int)rm.current_channel()); + // the 40 MHz offset selects the wrong secondary -> AP sends 20 MHz frames. + rm.set_channel_bwmode((uint8_t)_params.channel, 0, CHANNEL_WIDTH_20); + SCANLOG("pre-arm tuned: now=%d", (int)rm.current_channel()); + sta.setPmf(_params.pmf); // 802.11w RSN caps in the assoc-req (0 = off, no change) + set(State::Arming); + sta.setPhaseCb([this](int p){ set(p == 1 ? State::Authenticating : State::Associating); }); + // hold=0: return the instant assoc succeeds. The AP starts the 4-way (M1) immediately + // after assoc and only retries for ~4s; any hold here keeps the RX in the arm phase + // (data frames dropped) so M1 is missed. becomeStation must switch RX to the EAPOL + // phase ASAP. Link confirmation/deauth handling is the supervisor's job afterward. + auto r = sta.runProbe(self, bssid, _params.ssid.c_str(), /*hold*/0); + SCANLOG("arm result: %d (0=GO 1=Deauth 2=NoAssocResp 3=TXFAIL_NoAuth 4=Error)", (int)r); + switch (r) { + case StationMode::Result::TXFAIL_NoAuthResp: set(State::FailTx); return false; + case StationMode::Result::NOGO_Deauthed: set(State::FailNoAck); return false; + case StationMode::Result::NOGO_NoAssocResp: set(State::FailAuth); return false; + case StationMode::Result::Error: set(State::FailNoAp); return false; + case StationMode::Result::GO_LinkHeld: break; + } + // BANDWIDTH UPGRADE is DEFERRED until AFTER the 4-way handshake completes (see below, + // right after _wpa->ready()). Auth, assoc AND the 4-way all run at the 20 MHz primary: + // retuning to 40/80 here — right before the AP fires M1 — disrupts RX during the + // ~4 s EAPOL window so M1/M3 are missed and the handshake times out (state 5 -> 11). + set(State::Handshaking); + // ONE supplicant (member), used for handshake + RX decrypt + encrypted TX. + Mac selfMac{}, bss{}; + std::copy(self.b.begin(), self.b.end(), selfMac.begin()); + std::copy(bssid.b.begin(), bssid.b.end(), bss.begin()); + // EAPOL M2/M4 are DATA frames that need a TX descriptor. Auth/assoc add theirs in + // StationMode and DHCP adds its in dhcpSend, but BuildEapolKey returns the bare 802.11 + // frame and sendFrame adds nothing — so M2's first 40B (the 802.11 header) were misread + // as the descriptor => garbled on air => the AP never advanced past PTKSTART. Wrap the + // supplicant's send to prepend the data TX descriptor (mirrors dhcpSend). + auto wpaSend = [&dev](const std::vector& mpdu) -> bool { + std::vector frame(40 + mpdu.size(), 0); // 40 = 8812 TX desc + std::memcpy(frame.data() + 40, mpdu.data(), mpdu.size()); + // Use the MGMT TX descriptor (macid=1, raid=7) like auth/assoc — PROVEN to radiate + // on this chip+USB path. CcmpData with macid=0/raid=0 drains the FIFO but never + // goes on-air (verified via VTX hostapd: assoc seen, M2 never received). + apfpv::FillStationTxDesc(frame.data(), (uint16_t)mpdu.size(), 40, + /*macid*/1, apfpv::StationFrameKind::Mgmt, /*raid*/7, /*rate*/0x04); + return dev.sendStationFrameSync(frame.data(), frame.size()); + }; + _wpa = std::make_unique(MakeWpa2Crypto(), wpaSend); + _wpa->setPmf(_params.pmf); // 802.11w: M2 RSN caps must match the assoc-req + ensurePmk(); // no-op if already cached + _wpa->beginCached(_pmkCache, selfMac, bss); // INSTANT (no PBKDF2) -> RX switches to + // the EAPOL phase in time to catch M1 + + // Real (encrypted) DHCP send path — usable once keys are installed. + auto dhcpSend = [&](const std::vector& bootp) -> bool { + if (!_wpa || !_wpa->ready()) return false; + auto full = wrapDhcpUdpIp(bootp.data(), bootp.size()); // BOOTP -> full IPv4/UDP packet + auto mpdu = _wpa->buildEncryptedData(full.data(), full.size()); + if (mpdu.empty()) return false; + std::vector frame(40 + mpdu.size(), 0); // 40 = 8812 TX desc + std::memcpy(frame.data()+40, mpdu.data(), mpdu.size()); + // macid=1, raid=7 like auth/assoc — the only TX path proven to radiate on + // this chip. macid=0 is special/reserved on Jaguar and drains the FIFO silently. + apfpv::FillStationTxDesc(frame.data(), (uint16_t)mpdu.size(), 40, + 1, apfpv::StationFrameKind::CcmpData, 7, 0x04); + return sendFrame(frame); + }; + _dhcp = std::make_unique(self.b, dhcpSend); + + // Register the RX path NOW so EAPOL (handshake) and DHCP replies actually + // reach the supplicant / DHCP machine before we wait on them. + if (_params.lqFeedback) { LqFeedback::Config lqcfg{}; + if (const char* e = std::getenv("DEVOURER_LQ_MS")) { int ms = std::atoi(e); + if (ms > 0) { lqcfg.send_interval_ms = ms; lqcfg.min_interval_ms = ms; } } + _lq = std::make_unique(lqcfg); + // Route LQ over the dongle's IP stack — OPT-IN (DEVOURER_LQ_DONGLE). Default OFF: + // the periodic CCMP TX from the LQ thread collapses the dongle RX during streaming + // (bitrate -> ~0.3 Mbps on Win + Android; OUT serialises behind the IN pipeline), + // and greg's aalink (air_man) is disabled anyway so the feedback does nothing. When + // off, LqFeedback falls back to the (harmless, dead-ending) host socket. Enable only + // when air_man/adaptive-bitrate is running AND the TX/RX serialisation is solved. + bool lqDongle; +#if defined(__ANDROID__) + { char v[8]={0}; __system_property_get("persist.pixelpilot.lqdongle", v); + lqDongle = (v[0] != '0'); } // default ON; `setprop persist.pixelpilot.lqdongle 0` disables +#else + lqDongle = (std::getenv("DEVOURER_LQ_DONGLE") != nullptr); +#endif + if (lqDongle) + _lq->setSink([this](const char* b, int n){ + uint32_t src = (_dhcp && _dhcp->lease().valid) ? _dhcp->lease().ip + : 0xC0A8000Au; // 192.168.0.10 + auto pkt = buildUdp(src, 0xC0A80001u /*192.168.0.1*/, 0xC351, 12345, + reinterpret_cast(b), (size_t)n); + bool ok = sendIpPacket(pkt.data(), pkt.size()); + static int lqDbg = 0; + if ((lqDbg++ % 30) == 0) + std::fprintf(stderr, "[lq-tx] -> 192.168.0.1:12345 payload='%.*s' srcIp=%08x ok=%d\n", + n, b, src, (int)ok); + }); + _lq->start("192.168.0.1", 12345); } + _rx = std::make_unique(selfMac, bss, _wpa.get(), _lq.get(), _onRtp); + _rx->setStation(this); + { ApfpvDhcp* dh = _dhcp.get(); + _rx->setDhcpSink([dh](const uint8_t* b, size_t n){ dh->onBootpReply(b, n); }); } + if (_ipSink) _rx->setIpSink(_ipSink); // general-IP downlink -> VpnService TUN (SSH etc.) + // Switch the ALREADY-RUNNING RX thread to the streaming path — do NOT re-Init + // (that blocks). EAPOL (handshake), DHCP replies, and RTP now route through + // RxDeframe via the dispatch. The device is already tuned to the AP's channel. + _rxPhase.store(1); + { + uint32_t r = dev.rtw_read32(0x0608); + fprintf(stderr, "[hs] RCR=0x%08x ADF=%d ACF=%d FLT0mgmt=0x%04x FLT1ctl=0x%04x FLT2data=0x%04x MSR=0x%02x\n", + r, (int)((r>>11)&1), (int)((r>>12)&1), + dev.rtw_read16(0x06A0), dev.rtw_read16(0x06A2), dev.rtw_read16(0x06A4), + dev.rtw_read8(0x0102)); + } + + // Wait (bounded) for the 4-way handshake to install keys. + { + using namespace std::chrono; + auto t0 = steady_clock::now(); + while (!_wpa->ready()) { + if (steady_clock::now() - t0 > seconds(5)) { set(State::FailAuth); return false; } + std::this_thread::sleep_for(milliseconds(20)); + } + } + + // Bandwidth already set correctly by the arm (sta.setConnectWidth matches the + // AP's negotiated width). Running pauseAsyncRx/resumeAsyncRx here — even when + // set_channel_bwmode is a no-op — disrupts RX exactly when data starts flowing, + // causing the "black screen + inactivity timeout" pattern. The pause/resume was + // historically needed for IQK control-I/O isolation; IQK is now disabled by + // default, so there is no reason to touch the RX pipeline here. + + // Enable fire-and-forget TX NOW (before LQ/DHCP start). The per-TX TXPKT_EMPTY + // drain in sendStationFrameSync blocks the USB bus for ~100ms each call, and LQ + // fires ~1/s + DHCP retransmits — starving RX with 100ms gaps (rx-gap log storm). + // send_packet already blocks on the bulk write; the drain is a connect diagnostic. + dev.setTxFastPath(true); + + // ⭐ Lever C.2 (kernel-parity HW CCMP decrypt) — DEFAULT ON (2026-07-14). The chip decrypts RX + // in hardware so the single RX worker just de-aggregates + forwards plaintext (the kernel's + // lean tasklet), removing per-packet SW AES-CCM from the hot path. At VHT-2SS/80MHz (~50Mbps+, + // 5000+ frames/s) SW-CCMP on one worker falls behind and the link oscillates; HW decrypt keeps + // it clean and is what the firmware's full-station RA path expects. PTK (unicast video) → CAM + // entry 4 keyed to the AP BSSID/keyid0; GTK (bcast) → entry 5. RxDeframe skips SW decrypt for + // bdecrypted frames (same default). Fixed 2026-07-14: SECCFG 0x010c + _params.bssid populated + // → bdecrypted=4000/4000 verified. DEVOURER_NO_HW_DECRYPT reverts to the SW path. + if (std::getenv("DEVOURER_HW_DECRYPT") != nullptr || apfpvProp("debug.pixelpilot.hwdec")) { // HW-decrypt arms the HW reorder + compressed-BA (rig: loss 163→21). Needs RCR_APP for correct framing (debug.pixelpilot.rcrapp). `debug.pixelpilot.hwdec=1` on Android. + const auto& tk = _wpa->tk(); + dev.setSecCamKey(4, _params.bssid.data(), 0, tk.data()); // PTK pairwise, keyid 0 + if (_wpa->gtkKeyId() != 0xff) { + const auto& gtk = _wpa->gtk(); + dev.setSecCamKey(5, _params.bssid.data(), _wpa->gtkKeyId(), gtk.data()); // GTK group + } + dev.enableHwSec(); // REG_SECCFG = 0x0c01 + SCANLOG("HW-DECRYPT: PTK->CAM4 GTK->CAM5(kid=%u) SECCFG=0c01 (RX worker now forwards plaintext)", + (unsigned)_wpa->gtkKeyId()); + } + + // BlockAck is DISABLED by default. This dongle's MAC cannot emit the SIFS compressed + // BlockAck (silicon, [[apfpv-hw-blockack-fix]]), so initiating BA only makes the AP + // aggregate frames it can't ACK -> the AP retransmits -> PN-replay decFail -> ~3x + // throughput loss on an A-MPDU-on AP (measured Taiga: 8 -> 22 median / 35 peak Mbps when + // BA is declined instead). Opt back into the old BA-initiating + BA-register path with + // DEVOURER_ENABLE_BA (e.g. to re-test the HW auto-BA hypothesis on a kernel-matched AP). + if (enableBaOn()) { + // REG_AMPDU_MIN_SPACE: the kernel does NOT write this (leaves chip default). We forced + // 7 = 16µs min MPDU spacing, which over-restricts the AP's downlink A-MPDU packing (at + // high MCS a 1.5KB frame is <16µs, so 16µs spacing pads/caps the aggregate → the ~30Mbps + // ceiling). Set 0 = no extra spacing (chip handles dense A-MPDU; matches kernel intent). + dev.rtw_write8(0x045C, 0); // was 7 (16µs) — let the chip pack tight + dev.rtw_write8(0x1C, dev.rtw_read8(0x1C) | 0x60); // BIT5|BIT6: prevent MAC reset + + // ⭐ KERNEL-EXACT BA REGISTER SET (full usbmon init→connect diff vs the kernel @867Mbps + // A-MPDU). The streaming capture proved the BlockAck is HW-AUTOMATIC (no BACAM write, no + // BA H2C at runtime) — so it's armed purely by these init/response registers + the HW + // seeing the ADDBA. We were writing several of them WRONG; matching the kernel byte-for- + // byte is the only lever for the HW auto-BA. Each value is the kernel's final write. + if (!std::getenv("DEVOURER_SKIP_BAREGS")) { + // ★★ REG_RXFLTMAP1 (0x06A2) BIT8 = "accept BAR". The kernel sets this in its + // join-complete handler (HW_VAR_ENABLE_RX_BAR). The AP polls us with a BlockAckRequest + // after each A-MPDU burst; if BIT8 is clear the HW DROPS the BAR and never emits the + // compressed BlockAck → AP DELBAs reason 37. Diagnose+force here in the streaming path + // (proves whether our earlier writes survived to connect time). DEVOURER_NO_RXFLT skips. + if (!std::getenv("DEVOURER_NO_RXFLT")) { + uint16_t fm = dev.rtw_read16(0x06A2); + dev.rtw_write16(0x06A2, (uint16_t)(fm | 0x0700)); // BIT8 BAR | BIT9 BA | BIT10 PS-Poll + SCANLOG("RXFLTMAP1 0x06A2 before=%04x BAR_bit8=%d -> after=%04x (forced BAR+BA)", + fm, (fm>>8)&1, dev.rtw_read16(0x06A2)); + } + // ITERATION (2026-07-16 bench: userspace 6.57% incomplete-frames vs kernel 0.08%). Match + // the kernel's RUNTIME BA-register values devourer STILL differs on (from the 30s head-to- + // head mac_reg diff). Gated DEVOURER_BAREGS_K for clean A/B on the bench rig. + if (std::getenv("DEVOURER_BAREGS_K") || apfpvProp("debug.pixelpilot.baregsk")) { + dev.rtw_write16(0x06A2, 0x0520); // RXFLTMAP1: kernel=0x0520 (block above forces 0x0700) + dev.rtw_write32(0x0458, 0x8001ffff); // REG_AMPDU_MAX_LENGTH: kernel runtime (D was 0xffff0180) + dev.rtw_write8(0x0463, 0x03); // REG_RD_RESP_PKT_TH: kernel lowered 0x05->0x03 at connect + SCANLOG("BAREGS_K: RXFLTMAP1=%04x AMPDU_MAXLEN=%08x RD_RESP_TH=%02x", + dev.rtw_read16(0x06A2), (unsigned)dev.rtw_read32(0x0458), dev.rtw_read8(0x0463)); + } + // MATCH ALL kernel RUNTIME value-diffs (from the 30s mac_reg readback diff) that devourer + // differs on — these are VALUE diffs on registers BOTH write, so the address-level usbmon + // diff missed them. Hunting the compressed-BA arm. Gated DEVOURER_KVALS for A/B + bisect. + if (std::getenv("DEVOURER_KVALS") || apfpvProp("debug.pixelpilot.kvals")) { + dev.rtw_write32(0x0434, 0x07040302); // (kernel runtime; D=0x07050302) + dev.rtw_write32(0x045c, 0x1000f700); // REG_WMAC_LBK / RD region (D=0x1000f900) + dev.rtw_write32(0x0460, 0x03086666); // (D=0x05086666) + dev.rtw_write32(0x047c, 0x03030002); // (D=0x0000000c/d) + dev.rtw_write32(0x0480, 0xa40c0420); // REG_INIRTS_RATE_SEL (D=0xa40c0400) + dev.rtw_write32(0x04bc, 0x00040040); // REG_AMPDU_BURST_MODE (D=0x0004005f) + dev.rtw_write32(0x04f0, 0x0000c100); // REG_TX_RPT_TIME (D=0x04014100) + dev.rtw_write32(0x0524, 0x21ff4f0f); // REG_RD_CTRL region (D=0x21ff4fff) + dev.rtw_write32(0x0544, 0x00400180); // REG_RD_NAV_NXT (D=0x00400200) + SCANLOG("KVALS applied: 0x04bc=%08x 0x0480=%08x 0x0524=%08x 0x047c=%08x", + (unsigned)dev.rtw_read32(0x04bc), (unsigned)dev.rtw_read32(0x0480), + (unsigned)dev.rtw_read32(0x0524), (unsigned)dev.rtw_read32(0x047c)); + } + // ★★ TEST 3 (DEVOURER_T3_WAKE): MACID sleep/wake state — a HW state machine we never + // drove. The kernel calls rtw_hal_macid_wakeup(psta->mac_id) on join. If our AP-peer + // macid's bit is SET in REG_MACID_SLEEP (0x04D4 / _1 0x0488 / _2 0x04D0 / _3 0x0484), + // the HW thinks the peer is power-save asleep and PS-BUFFERS our TX to it — including + // the SIFS compressed BlockAck → the BA never radiates → AP DELBA reason 37. Read the + // state, then wake ALL macids (clear the sleep bitmaps). Default ON (it's correct for + // a station); DEVOURER_NO_T3WAKE reverts. + if (!std::getenv("DEVOURER_NO_T3WAKE")) { + uint32_t s0 = dev.rtw_read32(0x04D4), s1 = dev.rtw_read32(0x0488); + uint32_t s2 = dev.rtw_read32(0x04D0), s3 = dev.rtw_read32(0x0484); + dev.rtw_write32(0x04D4, 0); dev.rtw_write32(0x0488, 0); + dev.rtw_write32(0x04D0, 0); dev.rtw_write32(0x0484, 0); + SCANLOG("MACID_SLEEP before: 0x4D4=%08x 0x488=%08x 0x4D0=%08x 0x484=%08x (macid0_asleep=%d) -> woke all", + s0, s1, s2, s3, (int)(s0 & 1)); + } + // TEST 4 (DEVOURER_T4_JOIN): remaining join-state regs we never set, from the LIVE + // kernel dump — REG_BCN_INTERVAL/retry region 0x0540=0x80006404, 0x0544=0x00400180, + // and REG_RETRY_LIMIT (0x0541 byte within 0x0540) RL_VAL_STA=0x3030. The HW's beacon/ + // TSF window + TX retry shape the response/BA timing. DEVOURER_NO_T4 reverts. + if (std::getenv("DEVOURER_T4_JOIN")) { + dev.rtw_write32(0x0540, 0x80006404); // BCN_INTERVAL region (live kernel) + dev.rtw_write32(0x0544, 0x00400180); // (live kernel) + dev.rtw_write16(0x0541, 0x3030); // REG_RETRY_LIMIT short|long = 0x30 (RL_VAL_STA) + // TSF SYNC VERIFY: read REG_TSFTR (0x0560 low / 0x0564 high). If TSF is synced+running + // it is a large value counting up ~1e6/sec; if frozen/0 the HW never synced to the AP. + uint32_t tsfL = dev.rtw_read32(0x0560), tsfH = dev.rtw_read32(0x0564); + uint8_t bcn = dev.rtw_read8(0x0550); // REG_BCN_CTRL — bit4(0x10)=DIS_TSF_UDT + SCANLOG("T4 join-state set; TSF=0x%08x%08x BCN_CTRL=0x%02x (DIS_TSF_UDT=%d)", + tsfH, tsfL, bcn, (bcn>>4)&1); + } + // A-MPDU max length: we blasted 0xffffffff; kernel uses a SPECIFIC factor. The all-ones + // value mis-sizes the HW aggregate handling (BIT31 = RX_AMPDU enable is kept). + dev.rtw_write32(0x0458, 0xffff0180); // REG_AMPDU_MAX_LENGTH (kernel operational) + // ★ 0x0420 REG_FWHW_TXQ_CTRL — GROUND TRUTH from the LIVE kernel mac_reg_dump while + // streaming @867Mbps with RX-BA active = 0xff311f00. We were writing 0x001f71ff (a + // usbmon misread). This reg governs the HW TX/RESPONSE queues (where the chip auto- + // generates ACK/compressed-BA). If anything host-side gates the HW BA-response + // generation, this is it. DEVOURER_TXQ_OLD reverts to the old (wrong) value. + if (std::getenv("DEVOURER_TXQ_OLD")) dev.rtw_write32(0x0420, 0x001f71ff); + else dev.rtw_write32(0x0420, 0xff311f00); + dev.rtw_write32(0x0424, 0x3e30f7f7); // REG_HWSEQ_CTRL — LIVE kernel (was 0x1230f9f9) + dev.rtw_write32(0x04f0, 0x0000c100); // LIVE kernel (was 0x04014100) + { uint32_t rb = dev.rtw_read32(0x0420); + SCANLOG("LIVEREG 0x0420 wrote=ff311f00 readback=%08x (status bytes may differ)", rb); } + // REG_RRSR (0x0440, Response Rate Set): the rates our HW sends ACK/CTS/BlockAck at. + // We used 0x00000fff (kernel INIT value = all rates) → the HW may answer high-MCS data + // with a too-aggressive BA the AP misses on the weaker uplink (our measured 4-7% retry + // → slow per-A-MPDU cadence → ~30Mbps). Kernel OPERATIONAL (trace line 19277, streaming) + // = 0x0440=0x5001 + 0x0442=0x20 (robust response rates). Gated by DEVOURER_NO_RRSR. + if (!std::getenv("DEVOURER_NO_RRSR")) { + // ★ GROUND TRUTH from the LIVE kernel mac_reg_dump (0x0440=0x00200150), NOT the + // earlier usbmon misread. RRSR = the rate the HW emits ACK/CTS/compressed-BA at. + // Our old 0x5001 = 1M(CCK)+MCS0+MCS2 — CCK is UN-TRANSMITTABLE at 5GHz, so our + // compressed BA never radiated -> AP got no BA -> DELBA reason 37 -> no aggregation. + // 0x0150 = OFDM 6M/12M/24M (proper 5GHz basic response rates). DEVOURER_RRSR_OLD reverts. + if (std::getenv("DEVOURER_RRSR_OLD")) dev.rtw_write16(0x0440, 0x5001); + else dev.rtw_write16(0x0440, 0x0150); + dev.rtw_write8 (0x0442, 0x20); // RRSR byte2 (kernel live = 0x20) + } + // Response SIFS — LIVE kernel = 0x0e0a (we had 0x0e10 = 6µs too long; the compressed + // BA then radiates LATE and the AP misses it). 0x0e0a matches the live mac_reg_dump. + dev.rtw_write16(0x0514, 0x0e0a); // REG_SIFS_CTX (kernel LIVE 0x0e0a) + dev.rtw_write16(0x0516, 0x0e0a); // REG_SIFS_TRX (kernel LIVE 0x0e0a) + dev.rtw_write16(0x063A, 0x0e0a); // REG_SIFS (Trx) — kernel LIVE 0x0a/0x0e + dev.rtw_write8 (0x063C, 0x08); // SIFS_R2T_CCK (kernel 0x08) + dev.rtw_write8 (0x063D, 0x08); // SIFS_R2T_OFDM (kernel 0x08) + dev.rtw_write8 (0x063E, 0x0E); // SIFS_T2T_CCK (kernel 0x0E) + dev.rtw_write8 (0x063F, 0x0A); // SIFS_T2T_OFDM (kernel 0x0A) + dev.rtw_write8 (0x0429, 0x0E); // SPEC_SIFS OFDM (kernel operational 0x0E) + dev.rtw_write8 (0x051B, 0x09); // REG_SLOT = 9µs (kernel writes; we didn't) + // TCR (0x0607): the kernel's *operational* value (trace line 19733, during streaming) + // is 0x07 — it KEEPS BIT2 set. Our prior "fix" cleared BIT2 to 0x03 based on the + // early-init write (line 5193) and got the direction backwards: BIT2 governs the VHT + // A-MPDU response, so clearing it broke the exact thing we needed. Restore 0x07. + uint8_t tcr3 = dev.rtw_read8(0x0607); + dev.rtw_write8 (0x0607, (uint8_t)(tcr3 | 0x07)); // kernel operational 0x07 (set BIT0|1|2) + // EDCA BE: kernel operational = 0x2ba45e00 (line 20081). Our prior 0x2ba40000 came + // from a transient init write (line 19287) and zeroed the TXOP-limit field, hurting + // aggregation. Restore the operational value. + // ★ EDCA_BE — LIVE kernel = 0x005ea42b. Our 0x2ba45e00 had AIFS=0x00 (transmit with + // ZERO arbitration spacing) + TXOP=0x2ba4 — both wrong (byte-order mangled). The live + // value: AIFS=0x2b, CWmin/max=0xa4, TXOP=0x005e. DEVOURER_EDCA_OLD reverts. + if (std::getenv("DEVOURER_EDCA_OLD")) dev.rtw_write32(0x0508, 0x2ba45e00); + else dev.rtw_write32(0x0508, 0x005ea42b); + SCANLOG("BA-regs LIVE-kernel: 0x0420=ff311f00 0x0424=3e30f7f7 0x04f0=0000c100 0x0508=005ea42b RRSR=0150/20 SIFS=0e0a 0607|=07"); + } + // NOTE: a "KPROTO replay" experiment (set the 96 MAC registers the kernel writes that we + // don't — 0x0668 base, SECCFG, BFMER0/CSI_RPT VHT-sounding, SND_PTCL) was tested 2026-06-21 + // and made things MUCH WORSE (retryBit 30%→71%, throughput 17→4Mbps). Blindly injecting the + // kernel's registers breaks coherence: our overall chip state differs, so the kernel's values + // are wrong for OUR state. Conclusion: the BA gap is a STATE/SEQUENCE divergence, not copyable + // individual registers. Reverted. Next: diff OUR driver's full usbmon vs the kernel's. + } + + // ENCRYPT round-trip self-test: encrypt a dummy IPv4/UDP datagram, then decrypt it through + // the proven-standard decrypt path. roundtrip+match => our CCMP ENCRYPT is standard (so a + // dropped uplink is a framing/DHCP issue, not crypto); fail => the encrypt itself is wrong. + { + uint8_t tip[28] = {0x45,0,0,28, 0,0,0,0, 64,17,0,0, 192,168,250,9, 255,255,255,255, + 0,68,0,67,0,8,0,0}; + auto e = _wpa->buildEncryptedData(tip, sizeof(tip)); + std::vector dpl; + bool rt = !e.empty() && _wpa->decryptData(e.data(), e.size(), dpl); + bool match = rt && dpl.size() >= 8 + sizeof(tip) && std::memcmp(dpl.data()+8, tip, sizeof(tip))==0; + fprintf(stderr, "[enc-selftest] roundtrip=%d match=%d encLen=%zu decLen=%zu\n", + (int)rt, (int)match, e.size(), dpl.size()); + } + + // Initiate ADDBA Request for TID 0 (video) before DHCP. The kernel's station + // sends issue_addba_req to initiate the BA session; the AP responds and starts + // A-MPDU. Sending the request AFTER keys are installed ensures it's encrypted + // (QoS-data). The response handler in RxDeframe's dispatch catches the reply. + // + // ⚠️ GATED behind DEVOURER_ENABLE_BA (was unconditional — the throughput REGRESSION). + // This dongle's MAC cannot emit the SIFS compressed BlockAck ([[apfpv-hw-blockack-fix]], + // proven CLOSED). If we ASK the AP to start A-MPDU, the AP aggregates frames we can't ACK + // -> AP retransmits -> the VTX rate-controller collapses to the MCS floor (rate=12/20MHz ~= + // 2-3.9 Mbps). Declining/never-initiating BA keeps the stream non-aggregated -> the rate + // controller ramps to high MCS -> ~33 Mbps. So by default we must NOT initiate BA here; + // it belongs with the same env gate as the BA-register block above. + if (enableBaOn()) { + MacAddr bssid; for (int i=0;i<6;i++) bssid.b[i] = _params.bssid[i]; + std::vector addba; + // 802.11 QoS-Data header (subtype 8, ToDS, Protected) + addba.push_back(0x88); addba.push_back(0x41); // FC: QoS-Data, ToDS, Protected + addba.push_back(0x00); addba.push_back(0x00); // duration + addba.insert(addba.end(), bssid.b.data(), bssid.b.data() + 6); // A1=AP BSSID + addba.insert(addba.end(), selfMac.begin(), selfMac.end()); // A2=self + addba.insert(addba.end(), bssid.b.data(), bssid.b.data() + 6); // A3=AP BSSID + addba.push_back(0x00); addba.push_back(0x00); // seq ctl + addba.push_back(0x00); addba.push_back(0x00); // QoS control (TID=0, normal ACK) + // LLC/SNAP: AA AA 03 00 00 00 + EtherType 0x888E (EAPOL... no, action frames + // use a different encapsulation. Actually ADDBA is a mgmt action frame, not data. + // For mgmt action, use plain mgmt frame (FC=0x00D0), not QoS-Data. + // Build as mgmt action frame instead: + addba.clear(); + addba.push_back(0xD0); addba.push_back(0x00); // FC: mgmt, action + addba.push_back(0x00); addba.push_back(0x00); // duration + addba.insert(addba.end(), bssid.b.data(), bssid.b.data() + 6); // A1=AP + addba.insert(addba.end(), selfMac.begin(), selfMac.end()); // A2=self + addba.insert(addba.end(), bssid.b.data(), bssid.b.data() + 6); // A3=AP + static uint16_t mgmtSeq = 200; // offset from response seq + addba.push_back((u8)(mgmtSeq & 0xff)); + addba.push_back((u8)((mgmtSeq >> 4) & 0xff)); + mgmtSeq++; + // ADDBA Request body (matches kernel issue_addba_req) + addba.push_back(0x03); // Category: Block Ack + addba.push_back(0x00); // Action: ADDBA Request + addba.push_back(0x01); // Dialog token + // BA Parameter Set: TID=0, buf=64, immediate BA + addba.push_back(0x00); // low byte: TID=0, amsdu=0, buf[5:7]=0 + addba.push_back(0x08); // high byte: buf[3:0]=0b0000, policy=0(immediate) + addba.push_back(0x00); addba.push_back(0x00); // BA Timeout: 0 (default) + addba.push_back(0x00); addba.push_back(0x00); // BA Starting Seq: 0 (any) + // Send with TX descriptor (same as ADDBA Response) + std::vector txf(40 + addba.size(), 0); + std::memcpy(txf.data() + 40, addba.data(), addba.size()); + apfpv::FillStationTxDesc(txf.data(), (uint16_t)addba.size(), 40, + 1, apfpv::StationFrameKind::Mgmt, 0x0c, 0x04); + dev.sendStationFrameSync(txf.data(), txf.size()); + SCANLOG("ADDBA Request sent for TID 0 (initiating BA session)"); + } + + // Keys installed -> DHCP (or static). For dynamic, wait briefly for the lease. + set(State::Dhcp); + if (_params.staticIp) _dhcp->claimStatic(_params.staticIp, _params.staticNetmask, _params.staticGateway); + else { + using namespace std::chrono; + _dhcp->start(); // initial DISCOVER + auto t0 = steady_clock::now(); auto lastTx = steady_clock::now(); + while (!_dhcp->lease().valid) { + if (steady_clock::now() - t0 > seconds(8)) break; + // Retransmit the CURRENT pending message (DISCOVER, then REQUEST) every ~700ms: + // a single reply (OFFER or ACK) is easily missed in the RX re-arm gap after our + // sync TX, and we must retry the REQUEST (not restart DORA) to land the ACK. + if (steady_clock::now() - lastTx > milliseconds(700)) { + _dhcp->retransmit(); lastTx = steady_clock::now(); + } + std::this_thread::sleep_for(milliseconds(20)); + } + { uint32_t ip=_dhcp->lease().ip, sv=_dhcp->lease().server; + fprintf(stderr, "[dhcp] DORA done: valid=%d ip=%u.%u.%u.%u\n", (int)_dhcp->lease().valid, + (ip>>24)&255,(ip>>16)&255,(ip>>8)&255,ip&255); + // ALSO to logcat (stderr is invisible there): this is the IP to stream RTP to. + SCANLOG("DHCP lease valid=%d ip=%u.%u.%u.%u server=%u.%u.%u.%u <-- stream RTP here", + (int)_dhcp->lease().valid, (ip>>24)&255,(ip>>16)&255,(ip>>8)&255,ip&255, + (sv>>24)&255,(sv>>16)&255,(sv>>8)&255,sv&255); } + if (!_dhcp->lease().valid) _dhcp->claimStatic_192_168_0_10(); // fallback + } + + // Gratuitous ARP: announce our leased IP<->MAC (broadcast) so peers can resolve our MAC + // WITHOUT sending an ARP request (we don't answer those yet). Without this, the VTX that + // unicasts RTP to our IP — and anything doing SSH/TCP to us — can't address us at L2. + if (_dhcp->lease().valid) { + uint32_t ip = _dhcp->lease().ip; + // Build the gratuitous ARP once; the supervisor re-announces it every ~2s so the AP's + // ARP entry for us never goes STALE (else unicast RTP stalls). In the real setup the LQ + // feedback to the VTX also keeps it fresh, but this works on any subnet. + _gratArp = [this, &dev, selfMac, ip]() { + if (!_wpa || !_wpa->ready()) return; + uint8_t arp[28] = {0,1, 8,0, 6,4, 0,1}; + std::memcpy(arp+8, selfMac.data(), 6); + arp[14]=(ip>>24)&255; arp[15]=(ip>>16)&255; arp[16]=(ip>>8)&255; arp[17]=ip&255; + arp[24]=(ip>>24)&255; arp[25]=(ip>>16)&255; arp[26]=(ip>>8)&255; arp[27]=ip&255; + auto m = _wpa->buildEncryptedData(arp, 28, 0x0806); + if (m.empty()) return; + std::vector fr(40 + m.size(), 0); + std::memcpy(fr.data()+40, m.data(), m.size()); + apfpv::FillStationTxDesc(fr.data(), (uint16_t)m.size(), 40, + 1, apfpv::StationFrameKind::CcmpData, 7, 0x04); + dev.sendStationFrameSync(fr.data(), fr.size()); + }; + for (int k=0;k<3;++k) _gratArp(); // announce now + // Answer subsequent ARP requests for our IP (keeps the unicast video stream alive). + _rx->setArp(ip, [&dev](const std::vector& mpdu){ + std::vector fr(40 + mpdu.size(), 0); + std::memcpy(fr.data()+40, mpdu.data(), mpdu.size()); + apfpv::FillStationTxDesc(fr.data(), (uint16_t)mpdu.size(), 40, + 1, apfpv::StationFrameKind::CcmpData, 7, 0x04); + dev.sendStationFrameSync(fr.data(), fr.size()); + }); + } + + // mark link alive and go streaming. + // NOTE: A-MPDU RX Block-Ack is NOT yet implemented. Without it single-frame + // TXOPs cap at ~25Mbps regardless of MCS. 65+ Mbps requires ADDBA handshake + // + HW Block-Ack per-TID (H2C_BA_CTRL). USB agg now matches kernel values. + _deauth.store(false); + _lastRxMs.store(nowMs()); + set(State::Streaming); + // Streaming: make station TX fire-and-forget. The per-TX REG_TXPKT_EMPTY drain poll + // in sendStationFrameSync is a connect-time diagnostic (~6ms/call); during streaming + // the LQ-feedback loop hits it ~90-100x/s (~560ms/s of blocking) -> RX starves -> + // decode/render stutter on the phone. send_packet already blocked on the bulk write. + dev.setTxFastPath(true); + // ADDBA Requests are answered directly in handleAddbaRequest (raw-fd TX), + // which also covers requests arriving during streaming — no connect-time + // batch/pause needed. + + // NOTE: tried clearing RCR FORCEACK (BIT26) here post-handshake to mirror the + // Windows native driver (usbmon RCR 0x3f6800f4, FORCEACK=0). It did NOT durably + // help — DELBA churn returned (~17/20s) and throughput stayed at the single- + // frame ceiling. Windows runs FORCEACK=0 only because FWOffload has the firmware + // own ACK/BA; without that mode FORCEACK=0 just loses the normal ACK. Reverted. + // Opt back in for experiments via DEVOURER_STREAM_NOACK. + if (std::getenv("DEVOURER_STREAM_NOACK")) { + auto& dev = *reinterpret_cast(_dev); + uint32_t rcr = dev.rtw_read32(0x0608); + rcr &= ~0x04000000u; // clear FORCEACK (BIT26) + dev.rtw_write32(0x0608, rcr); + SCANLOG("STREAM_NOACK: RCR FORCEACK cleared -> 0x%08x", rcr); + } + return true; +} + +void ApfpvStation::handleAddbaRequest(const uint8_t* frame, size_t len) { + if (len < 24 + 9) return; + const uint8_t* body = frame + 24; + if (!(body[0] == 0x03 && body[1] == 0x00)) return; + u8 tid = ((body[3] | (body[4] << 8)) >> 2) & 0x0f; + uint16_t startSsn = (uint16_t)((body[7] | (body[8] << 8)) >> 4); // ADDBA-Req Starting Seq + + // ⭐ BA-CAM ARM (experimental, opt-in via DEVOURER_BACAM): the AP requested an RX-BA + // session for this TID. The HW MAC emits the SIFS compressed BlockAck ONLY when its + // BA-CAM (REG_BACAMCMD 0x0654 / REG_BACAMCONTENT 0x0658) holds a valid per-TID/per-peer + // entry. On the kernel the FIRMWARE auto-arms it from the ADDBA seen on the station RX + // path; our libusb RX path never triggers that auto-arm, so BACAMCMD reads 0 and the AP + // retransmits every unacked A-MPDU (the ~50% PN-replay / decFail at >20Mbps). Program it + // ourselves, modeled on rtw89_fw_h2c_ba_cam's w0 layout: + // valid:0 init_req:1 entry_idx:3-2 tid:7-4 macid:15-8 bmap_size:19-16 ssn:31-20 + // RESULT (2026-06-21): the writes take (CMD BIT31 polls clear) but do NOT arm the SIFS + // BlockAck — decFail stayed ~89% at 30Mbps and the CONTENT readback ≠ what we wrote, so + // this rtw89-derived w0 layout is WRONG for the 8812's BA-CAM (the vendor driver never + // writes it, so there's no reference format). Gated OFF; the 8812 BA-CAM arming is + // firmware-internal (FWOffload), not reproducible by a register poke. Opt-in to keep + // experimenting on the format via DEVOURER_BACAM. + if (std::getenv("DEVOURER_BACAM")) { + auto& d = *reinterpret_cast(_dev); + uint8_t macid = 1; // our AP peer macid (TX-desc / H2C use 1) + uint8_t entry = tid & 0x07; // one entry per TID for the single peer + uint8_t bmap = 0; // 0 = 64-frame bitmap (default BA window) + uint32_t content = (1u) // valid + | (1u << 1) // init_req + | ((entry & 0x3u) << 2) + | ((uint32_t)(tid & 0xf) << 4) + | ((uint32_t)(macid & 0xff) << 8) + | ((uint32_t)(bmap & 0xf) << 16) + | ((uint32_t)(startSsn & 0xfff) << 20); + d.rtw_write32(0x0658, content); // REG_BACAMCONTENT + d.rtw_write32(0x0654, 0x80000000u | 0x40000000u | entry); // poll | write | addr + for (int p = 0; p < 20; ++p) { // poll until HW clears BIT31 + if ((d.rtw_read32(0x0654) & 0x80000000u) == 0) break; + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + uint32_t cmdRb = d.rtw_read32(0x0654), contRb = d.rtw_read32(0x0658); + SCANLOG("BACAM arm tid=%u entry=%u ssn=%u content=0x%08x -> CMD=0x%08x CONTENT=0x%08x", + tid, entry, startSsn, content, cmdRb, contRb); + } + // Respond to EVERY ADDBA Request. A per-TID debounce here was actively + // harmful: after the connect-time burst set all 8 TID bits, the AP's later + // re-requests (session timeout / re-establish during streaming) were + // silently dropped -> the AP got no Response -> DELBA -> re-request loop. + // The AP only re-requests when it needs to; answering each one keeps the + // BA session alive. (raw-fd TX makes per-request sends cheap.) + + auto& dev = *reinterpret_cast(_dev); + MacAddr ba; for (int i=0;i<6;i++) ba.b[i] = _params.bssid[i]; + std::vector r; + r.push_back(0xD0); r.push_back(0x00); r.push_back(0x00); r.push_back(0x00); + r.insert(r.end(), ba.b.data(), ba.b.data()+6); + for (int i=0;i<6;i++) r.push_back(dev.rtw_read8(0x0610+i)); + r.insert(r.end(), ba.b.data(), ba.b.data()+6); + static uint16_t s=0; s++; r.push_back(s&0xff); r.push_back((s>>4)&0xff); + // ADDBA Response body (802.11 §9.6.5.3): Category + Action + DialogToken + + // StatusCode(2) + BAParameterSet(2) + BATimeout(2) = 9 bytes EXACTLY. Do NOT + // append the request's Starting Sequence Control — that is Request-only; the + // extra 2 bytes made the frame malformed and some APs silently reject it (so + // they never aggregate). Echo the AP's BA param set + timeout verbatim. + u8 dialog = body[2]; + // DECLINE the AP's ADDBA by default: this dongle can never emit the SIFS compressed + // BlockAck (MAC-silicon, not host-reachable), so on an A-MPDU-on AP every aggregate it + // can't ACK is retransmitted -> arrives as PN-replay -> ~50% decFail. Replying StatusCode + // 37 (REFUSED) forces the AP to stop aggregating this TID, trading aggregation gain we + // can't use for a clean non-aggregated stream (no PN-replay). DEVOURER_ENABLE_BA accepts + // (StatusCode 0) to restore the old behavior. + const bool declineBa = !enableBaOn(); + r.push_back(0x03); r.push_back(0x01); r.push_back(dialog); + if (declineBa) { r.push_back(37); r.push_back(0x00); } // StatusCode = 37 (REFUSED) + else { r.push_back(0x00); r.push_back(0x00); } // StatusCode = 0 (success) + // BA Parameter Set: bit0=AMSDU-supported, bit1=policy, bits2-5=TID, bits6-15=buffer size + // (10 bits). We normally echo the AP's requested value verbatim (see the note above about + // NOT appending extra bytes — that's still true and unrelated to this). But since we can't + // emit a real compressed BlockAck, a lost sub-frame inside a 64-deep aggregate has no correct + // way to be identified/retransmitted, and it shows up as a chunk of missing bitstream data + // (visually: motion-compensated "tails" on moving objects that persist until a later frame + // overwrites them). The responder is allowed by spec to negotiate a SMALLER buffer size than + // requested; a smaller window means the AP bursts fewer MPDUs per aggregate, so any one loss + // event corrupts a smaller slice of a frame instead of up to 64 frames' worth. Opt-in via + // DEVOURER_BA_BUFSZ (host) / debug.pixelpilot.babufsz (Android) so it's A/B-testable without + // a rebuild per value; 0 or unset = echo the AP's request unchanged (today's behavior). + uint16_t reqParam = (uint16_t)(body[3] | (body[4] << 8)); + uint16_t reqBufsz = (reqParam >> 6) & 0x3ff; + int bufszOverride = baRespBufSizeOverride(); + uint16_t sentBufsz = reqBufsz; + if (bufszOverride > 0) { + uint16_t newParam = (uint16_t)((reqParam & 0x3f) | ((uint16_t)bufszOverride << 6)); + r.push_back((uint8_t)(newParam & 0xff)); r.push_back((uint8_t)(newParam >> 8)); + sentBufsz = (uint16_t)bufszOverride; + } else { + r.push_back(body[3]); r.push_back(body[4]); // BA Parameter Set (echo) + } + r.push_back(body[5]); r.push_back(body[6]); // BA Timeout (echo) + SCANLOG("ADDBA Response BUILD dialog=%u tid=%u status=%u reqBufsz=%u sentBufsz=%u (override=%d)", + dialog, tid, declineBa ? 37 : 0, reqBufsz, sentBufsz, bufszOverride); + + // Send the ADDBA Response DIRECTLY via send_packet. With the raw-fd + // USBDEVFS_BULK TX path this is a synchronous kernel ioctl that the USB + // stack interleaves with the in-flight IN URBs — it does NOT block the + // libusb event loop or touch its async reap queue, so it is safe to call + // from the RX dispatch here. This answers the AP within ~ms (no pause hack) + // and also handles ADDBA Requests that arrive DURING streaming, not just + // the connect-time burst. + std::vector txf(40 + r.size(), 0); + memcpy(txf.data()+40, r.data(), r.size()); + FillStationTxDesc(txf.data(), (uint16_t)r.size(), 40, + 1, StationFrameKind::Mgmt, 0x0c, 0x04); + // RELIABLE ADDBA-Response delivery — the crux of the 28-vs-59 gap. The kernel uses + // issue_addba_rsp_wait_ack() (3 retries, wait for the AP's ACK). We sent ONCE, + // fire-and-forget; when that single copy is lost over-air the AP never learns we + // accepted -> DELBA reason 37 ("peer doesn't want BA") -> re-ADDBA loop -> the BA + // session never stays up -> intermittent A-MPDU -> ~half throughput. We can't easily + // detect the 802.11 ACK on the libusb path, so blind-retry: for the VIDEO TID (0) send + // 3x via the synchronous (TX-drain-confirmed) path so it's rock-solid; idle TIDs keep + // the cheap single async send (their teardown is harmless). DEVOURER_ADDBA_ASYNC reverts. + // Measured: making tid=0 reliable drove DELBA 56->0 (BA stable) but did NOT raise + // throughput (still ~25; the cap is the AP's per-STA delivery rate, not BA stability), + // and the 3x sync starves RX ~30ms/burst. So keep the cheap async send by DEFAULT; + // DEVOURER_ADDBA_RELIABLE opts into the kernel-style reliable (3x sync) tid=0 path. + if (tid == 0 && std::getenv("DEVOURER_ADDBA_RELIABLE")) { + for (int rep = 0; rep < 3; ++rep) dev.sendStationFrameSync(txf.data(), txf.size()); + SCANLOG("ADDBA Response tid=0 (3x reliable sync)"); + } else { + dev.send_packet(txf.data(), txf.size()); + SCANLOG("ADDBA Response tid=%u (async)", tid); + } +} + +bool ApfpvStation::connect(const Params& p) { + _params = p; + bool ok = runConnectChain(); + // Start the persistent reconnect supervisor (the daemon), even if the first + // attempt failed — it will keep trying, exactly like wpa_supplicant -B. + if (p.autoReconnect && !_run.exchange(true)) { + _supervisor = std::thread(&ApfpvStation::supervisorLoop, this); + } + return ok; +} + +// The supervisor: the driver-level equivalent of the wpa_supplicant daemon. +// Watches for link loss and re-establishes, with exponential backoff capped low +// (recovery is fast: same channel, no rescan). +void ApfpvStation::supervisorLoop() { + using namespace std::chrono; + int backoff = _params.reconnectBackoffMs; + int gratTick = 0; + while (_run.load()) { + std::this_thread::sleep_for(milliseconds(100)); + State s = _state.load(); + + if (s == State::Streaming) { + backoff = _params.reconnectBackoffMs; // reset backoff on healthy link + // Keepalive cadence: ~1s normally. DEVOURER_KEEPALIVE=1 fires every loop (~100ms) — + // a power-save probe: if the AP buffers our downlink because it thinks the injected + // station is asleep, a frequent uplink data frame (PWR_MGT=0) keeps us "awake" and the + // AP delivers continuously instead of dribbling. If throughput jumps, PS-buffering was it. + // TEST 1 (DEVOURER_T1_QUIET): make the supervisor fully silent during streaming — + // no uplink ARP TX, no H2C, no diagnostic control-reads — so NOTHING of ours contends + // with the RX/BA path on the USB bus. Tests the "flushing frames out of the HW" thesis. + // `debug.pixelpilot.quiet=1` (adb setprop) also enables it on Android, where env vars + // aren't settable — for testing whether our periodic supervisor TX (ARP + H2C RA/RSSI) + // is stalling the single RX worker and dropping bursts of video (the dongle-only freeze). + const bool t1quiet = std::getenv("DEVOURER_T1_QUIET") != nullptr || apfpvProp("debug.pixelpilot.quiet"); + // ARP re-announce cadence. Was 10 (1s) — but the gratuitous ARP is a SYNCHRONOUS + // bulk-OUT that briefly starves the single RX worker, and at 1Hz that periodic stall + // dropped a burst of video every ~1s (the "stutter every few seconds" on the dongle; + // phone-Wi-Fi has no such TX so it was smoother). 30 (3s) keeps the AP's ARP entry for + // us fresh (ARP timeout is tens of seconds) while cutting the periodic RX stall 3x. + static const int kaEvery = std::getenv("DEVOURER_KEEPALIVE") ? 1 : 30; + if (++gratTick >= kaEvery) { gratTick = 0; if (_gratArp && !t1quiet) _gratArp(); } // re-announce ARP (keep AP entry fresh) + // PERIODIC RA/RSSI H2C (~1Hz, matching the kernel usbmon cadence). The kernel re-sends + // H2C 0x40 (RA/MACID) + 0x42 (RSSI) every ~1s during streaming to keep the firmware's + // per-peer rate state fresh; we previously sent them ONCE at connect. Stale FW RA state + // may be why the AP rate-controls us conservatively (MCS5/30Mbps) vs the kernel (72Mbps). + // Gated by DEVOURER_NO_RA_TICK. Reuses the ~1s gratTick boundary. + // RX-FIFO occupancy probe: REG_RXPKT_NUM (0x0284) = # packets in the chip RXPKTBUF, + // REG_RXDMA_STATUS (0x0288). If RXPKT_NUM is consistently HIGH during streaming, the + // chip can't drain to USB fast enough → FIFO backs up → overflow drops → AP retransmit + // (the 6-7% retry). If ~0, the FIFO drains fine and the AP is simply sending ~30Mbps. + if (!t1quiet) { + auto& dd = *reinterpret_cast(_dev); + // One-shot full BB-register dump (0x800-0xffc) to diff against the kernel's + // bb_reg_dump and find the 2-stream MIMO RX config difference (we get 1SS, kernel 2SS). + static bool bbDumped = false; + if (!bbDumped && std::getenv("DEVOURER_BB_DUMP")) { + bbDumped = true; + try { + // MAC BA/protocol region (0x400-0x6fc) + BB (0x800-0xffc) for the + // kernel diff — hunting the MAC reg that enables RX-BA-CAM auto-fill. + for (uint32_t a = 0x400; a <= 0xffc; a += 0x10) { + if (a >= 0x700 && a < 0x800) continue; // skip FIFO/sensitive gap + uint32_t v0 = dd.rtw_read32(a), v1 = dd.rtw_read32(a+4); + uint32_t v2 = dd.rtw_read32(a+8), v3 = dd.rtw_read32(a+0xc); + SCANLOG("BBDUMP 0x%04x 0x%08x 0x%08x 0x%08x 0x%08x", a, v0, v1, v2, v3); + } + } catch (...) {} + } + // RF-path register dump (0x00-0x7f, path A+B) to diff vs the kernel's rf_reg_dump. + // Same dongle → the kernel reaches VHT-2SS, so any RX-quality gap is SOFTWARE: the + // RF-path calibration (LNA/gain/channel/IQK-LO) lives here, NOT in the BB page. + static bool rfDumped = false; + if (!rfDumped && std::getenv("DEVOURER_RF_DUMP")) { + rfDumped = true; + try { + auto& rm = *reinterpret_cast(_rm); + for (int p = 0; p < 2; ++p) { + RfPath pth = p == 0 ? RfPath::RF_PATH_A : RfPath::RF_PATH_B; + for (uint32_t a = 0x00; a <= 0x7f; ++a) { + uint32_t v = rm.phy_query_rf_reg(pth, a, 0xfffff); + SCANLOG("RFDUMP P%d 0x%02x 0x%05x", p, a, v); + } + } + } catch (...) {} + } + // ⚠️ THESE DIAGNOSTIC REGISTER READS ARE DEBUG-ONLY AND DEFAULT OFF. They ran EVERY + // 100ms loop = ~8 USB control reads x10/s = ~80 control transfers/s, all contending + // with the SINGLE libusb RX worker → continuous RX starvation (video quality + // degradation) on the dongle (phone-Wi-Fi, which has no such traffic, was smoother). + // Enable with DEVOURER_RXDIAG when debugging the RX/rate path. + if (std::getenv("DEVOURER_RXDIAG")) try { + uint16_t rxpktnum = dd.rtw_read16(0x0284); + // IGI (RX gain index) — PHYDM/DIG output. Path A = 0xc50[6:0], Path B = 0xe50[6:0]. + // Kernel adapts this to ~0x37 (55) at rssi -45. If OURS is parked far off (DIG not + // adapting), our RX is mis-tuned → 6-7% loss → AP caps us. Direct BB read (memory-mapped). + uint32_t igiA = dd.rtw_read32(0x0c50) & 0x7f; + uint32_t igiB = dd.rtw_read32(0x0e50) & 0x7f; + // TX-power index for the rates our compressed BlockAck radiates at. The BA goes + // out at a legacy (CCK/OFDM) basic rate per RRSR, so its airborne power comes from + // the CCK (0xc20) / OFDM-low (0xc24) / OFDM-high (0xc28) TX-AGC regs (path A) and + // 0xe20/0xe28 (path B). Each reg packs 4 per-rate bytes (index 0x00-0x3f, hi=more + // power). If these are LOW, our BAs reach the AP weakly → AP misses them → its BA + // window stalls → link sits ~95% idle (our 27 vs kernel 72 Mbps cap). Byte0 shown. + uint8_t pCckA = dd.rtw_read8(0x0c20); // path A CCK1 power idx + uint8_t pOfdmA = dd.rtw_read8(0x0c24); // path A OFDM6 power idx + uint8_t pO54A = dd.rtw_read8(0x0c28); // path A OFDM24-54 power idx + uint8_t pCckB = dd.rtw_read8(0x0e20); // path B CCK1 + uint8_t pOfdmB = dd.rtw_read8(0x0e24); // path B OFDM6 + static int probeN = 0, rxpktMax = 0; static long rxpktSum = 0; + probeN++; rxpktSum += rxpktnum; if (rxpktnum > rxpktMax) rxpktMax = rxpktnum; + if (probeN >= 10) { // ~1s + SCANLOG("rx-fifo: RXPKT_NUM avg=%ld max=%d | IGI_A=0x%02x IGI_B=0x%02x (kernel~0x37) " + "| TXPWR A[cck=0x%02x ofdm=0x%02x o54=0x%02x] B[cck=0x%02x ofdm=0x%02x]", + rxpktSum/probeN, rxpktMax, (unsigned)igiA, (unsigned)igiB, + pCckA, pOfdmA, pO54A, pCckB, pOfdmB); + probeN = 0; rxpktMax = 0; rxpktSum = 0; + } + } catch (...) {} + } + if (gratTick == 0 && !t1quiet && !std::getenv("DEVOURER_NO_RA_TICK")) { + auto& d = *reinterpret_cast(_dev); + // Kernel-verbatim MACID_CFG + RSSI (usbmon: [00,89,1a,00,80,ff,ff] / [00,00,2e]). + // DEVOURER_RA_OLD restores the prior (wrong) values for A/B. See StationMode.cpp. + if (std::getenv("DEVOURER_RA_OLD")) { + uint32_t ra_mask = std::getenv("DEVOURER_RA_CONSERVATIVE") ? 0x3FFFFFFFu : 0xfffff010u; + uint8_t ra[7] = { 0x00, (uint8_t)((9 & 0x1f) | (1 << 5) | (1 << 7)), (uint8_t)(2 | (1 << 4)), + (uint8_t)(ra_mask & 0xff), (uint8_t)((ra_mask >> 8) & 0xff), + (uint8_t)((ra_mask >> 16) & 0xff), (uint8_t)((ra_mask >> 24) & 0xff) }; + uint8_t rssi[4] = { 0x00, 0x00, 0x39, 0x06 }; + try { d.fillH2CCmd(0x40, 7, ra); d.fillH2CCmd(0x42, 4, rssi); } catch (...) {} + } else { + // ⭐ C2H feedback loop / kernel-cadence H2C for the firmware-RA uplink. + // MACID_CFG (0x40) sets up the RA — send ONCE per connection like the kernel. + // Re-sending it every tick re-inits the RA so it never converges → the uplink + // rate oscillates (the user-observed 0%↔100%). RSSI (0x42) is the real per-tick + // feedback the firmware RA uses for its rate ceiling — send the ACTUAL tracked + // RSSI (byte2 = rssi_dBm+100, clamped 1..100), not the old hardcoded 0x2e. + uint8_t ra[7] = { 0x00, 0x89, 0x1a, 0x00, 0x80, 0xff, 0xff }; // kernel-verbatim + if (!_raCfgSent.exchange(true)) { try { d.fillH2CCmd(0x40, 7, ra); } catch (...) {} } + int rdbm = PhydmWatchdog::RssiDbm(); + int rpct = rdbm + 100; if (rpct < 1) rpct = 1; if (rpct > 100) rpct = 100; + uint8_t rssi[3] = { 0x00, 0x00, (uint8_t)rpct }; + try { d.fillH2CCmd(0x42, 3, rssi); } catch (...) {} + static uint32_t raLogN = 0; + if ((++raLogN % 4) == 1) + SCANLOG("RA-loop: rssi=%ddBm(pct=%d) uplink_rate_idx=0x%02x", + rdbm, rpct, (unsigned)PhydmWatchdog::UplinkRate()); + } + } + bool lost = _deauth.load(); + // RX-silence watchdog: no data/beacon for rxTimeoutMs => link gone. + if (!lost && (nowMs() - _lastRxMs.load()) > _params.rxTimeoutMs) lost = true; + if (lost) { + set(State::LinkLost); + _deauth.store(false); + } + continue; + } + + if (s == State::LinkLost || s == State::FailNoAp || s == State::FailNoAck || + s == State::FailAuth || s == State::FailTx || s == State::FailDhcp) { + if (!_run.load()) break; + set(State::Reconnecting); + std::this_thread::sleep_for(milliseconds(backoff)); + if (!_run.load()) break; + // A USB register read/write can throw (rtw_read -> ios_base::failure) + // if the dongle hiccups or is accessed concurrently. Catch it here so + // the supervisor THREAD degrades to a failed attempt instead of + // terminating the whole app (std::terminate -> SIGABRT). + bool ok = false; + try { ok = runConnectChain(); } // re-arm on known channel + catch (const std::exception&) { ok = false; set(State::FailNoAp); } + catch (...) { ok = false; set(State::FailNoAp); } + if (!ok) { + backoff = std::min(backoff * 2, _params.maxBackoffMs); // exp backoff + } else { + backoff = _params.reconnectBackoffMs; + } + } + } +} + +void ApfpvStation::disconnect() { + _run.store(false); + PhydmWatchdog::SetUnlinked(); // restore monitor-mode DIG bounds (wfb-ng coexistence) + // safeJoinThread is join-safe: NEVER joins the current thread (-> EINVAL "Invalid + // argument" -> uncaught std::system_error -> SIGABRT, seen on replug when the JNI + // re-init teardown disconnect races/repeats the Java stopAdapters disconnect), and + // tolerates an already-reaped thread. + safeJoinThread(_supervisor); + if (_rtl) { + reinterpret_cast(_rtl)->StopRxLoop(); + } + safeJoinThread(_rxThread); + set(State::Idle); +} + +// All-SSID scan: tune the radio across the channel set, collect every beacon, +// and emit each NEW SSID. The RX collector runs on the device read thread; it +// captures only `this`, and _onScanAp is cleared (and the processor swapped to a +// no-op) before we return, so the still-running RX thread can't call a dead cb. +static inline bool isDfsChannel(int ch) { + return (ch >= 52 && ch <= 64) || (ch >= 100 && ch <= 144); +} + +void ApfpvStation::scanAll(int perChannelMs, bool includeDfs, const OnApFn& onAp) { + if (!_rtl || !_rm) return; + auto* rtl = reinterpret_cast(_rtl); + auto& rm = *reinterpret_cast(_rm); + + { std::lock_guard lk(_scanMtx); _scanSeen.clear(); _onScanAp = onAp; } + set(State::Scanning); + + // Diagnostics in MEMBERS so the stored collector (called from the device RX + // thread) captures only `this` — capturing scanAll locals by ref dangles. + _scanPkts.store(0); _scanBeacons.store(0); + auto collector = [this](const Packet& pkt) { + _scanPkts.fetch_add(1); + std::string ssid; ApInfo info; + if (!ScanProbe::parseAnyBeacon(pkt.Data.data(), pkt.Data.size(), ssid, info)) return; + _scanBeacons.fetch_add(1); + if (ssid.empty()) return; // skip hidden SSIDs + info.rssi = (int)pkt.RxAtrib.rssi[0] - 110; // approx dBm (gain byte) + std::lock_guard lk(_scanMtx); + if (_onScanAp && _scanSeen.insert(ssid).second) _onScanAp(ssid, info); + }; + SCANLOG("scanAll begin (perCh=%d ms)", perChannelMs); + + // hint first, then the full 5GHz set (UNII-1, UNII-2A/2C DFS, UNII-3) and all + // 2.4GHz channels — APs (incl. phone hotspots/home routers) can sit anywhere, + // e.g. DFS ch52-144. Passive RX-only listening on DFS is fine (no TX). + // Trimmed for the ~per-channel cost of a real retune: UNII-1, UNII-2A DFS, + // UNII-3 + the common 2.4 channels. (Dropped the big UNII-2C 100-144 block.) + const int channels[] = { + _params.channel, + 36, 40, 44, 48, // UNII-1 + 52, 56, 60, 64, // UNII-2A (DFS) + 149, 153, 157, 161, 165, // UNII-3 + 1, 6, 11 // 2.4GHz (common) + }; + int last = 36; bool inited = false; + for (int ch : channels) { + if (ch <= 0) continue; + if (!includeDfs && isDfsChannel(ch)) continue; // skip DFS when disabled + last = ch; + SelectedChannel sc{ .Channel=(uint8_t)ch, .ChannelOffset=0, + .ChannelWidth=CHANNEL_WIDTH_20 }; + // Tuning is a USB register op (rtw_read/write) that can throw on a dongle + // hiccup — catch per-channel so a single failure doesn't abort the app; + // skip the bad channel and keep sweeping. + try { + if (!inited) { + // Init (bring-up + StartRxLoop) blocks for as long as RX runs, so it owns + // _rxThread for the whole sweep; this (scanAll's) thread just retunes + + // sleeps per channel below, same division of labour as runConnectChain. + rtl->StopRxLoop(); safeJoinThread(_rxThread); + _rxThread = std::thread([rtl, collector, sc]() { + try { rtl->Init(collector, sc); } catch (...) {} + }); + inited = true; + } else { rm.set_channel_bwmode((uint8_t)ch, 0, CHANNEL_WIDTH_20); } // actually retunes + } catch (...) { continue; } + std::this_thread::sleep_for(std::chrono::milliseconds(perChannelMs)); + } + + // Detach: no further emits, and replace the RX processor so the device's + // read thread can't re-enter the collector after onAp's lifetime ends. + { std::lock_guard lk(_scanMtx); _onScanAp = nullptr; } + SCANLOG("scanAll done: %d RX pkts, %d beacons, %zu SSIDs", _scanPkts.load(), _scanBeacons.load(), _scanSeen.size()); + // Rest on `last` with no RX loop running (InitWrite = bring-up only, returns immediately) — + // the next caller (typically connect()) brings RX up fresh on whatever channel it needs. + rtl->StopRxLoop(); safeJoinThread(_rxThread); + try { + rtl->InitWrite(SelectedChannel{ .Channel=(uint8_t)last, + .ChannelOffset=0, .ChannelWidth=CHANNEL_WIDTH_20 }); + } catch (...) {} + set(State::Idle); +} + +} // namespace apfpv diff --git a/src/ApfpvStation.h b/src/ApfpvStation.h new file mode 100644 index 0000000..02c91e8 --- /dev/null +++ b/src/ApfpvStation.h @@ -0,0 +1,205 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "ScanProbe.h" +namespace apfpv { +using Mac = std::array; +// Top-level APFPV station orchestrator: assembles arming -> auth/assoc -> +// WPA2 -> DHCP -> RTP+LQ into one connect() the JNI layer can call. +// +// DRIVER-LEVEL PERSISTENT RECONNECT (matches/exceeds the VRX): the VRX gets +// auto-reconnect from running wpa_supplicant -B as a daemon (it re-associates +// on link loss automatically). We replicate that AT THE DRIVER LEVEL with a +// supervisor thread: it watches for link loss (deauth, or RTP/beacon RX +// timeout) and re-runs the gated connect chain — no app/UI involvement, exactly +// like the supplicant daemon. We can be faster: on loss we re-arm on the SAME +// known channel (no rescan), so recovery is a re-auth/assoc/4-way, not a full +// network search. +class ApfpvStation { +public: + enum class State { Idle, Scanning, Arming, Authenticating, Associating, Handshaking, + Dhcp, Streaming, FailNoAp, FailTx, FailNoAck, FailAuth, + FailDhcp, LinkLost, Reconnecting }; + using OnRtpFn = std::function; + using OnStateFn = std::function; + struct Params { int channel=40; int bandwidth=20; std::string ssid="OpenIPC"; + std::string passphrase="12345678"; bool lqFeedback=true; + // Static-IP option: staticIp != 0 -> SKIP DHCP and bind this IP (host byte + // order, e.g. 0xC0A8000A = 192.168.0.10). netmask 0 -> /24, gateway 0 -> subnet .1. + uint32_t staticIp=0, staticNetmask=0, staticGateway=0; + // 802.11w PMF: 0=off, 1=capable (MFPC, DEFAULT — modern + verified to still + // associate with non-PMF APs), 2=required (MFPC+MFPR). RSN caps in assoc-req+M2. + int pmf=1; + // discovery: if scan=true we find BSSID+channel+cipher via + // beacons (security+discovery parity with wpa_supplicant); + // channel above is the starting hint / fallback. + bool scan=true; + // PHONE-ASSISTED connect: the phone's own Wi-Fi already knows + // the AP's channel + BSSID, so we can SKIP the dongle's slow, + // flaky beacon sweep and arm straight to this BSSID on `channel`. + std::array bssid{}; + bool haveBssid=false; + // reconnect tuning — matched to wpa_supplicant defaults the + // VRX relies on: immediate re-associate, ~5s scan interval. + bool autoReconnect=true; + int rxTimeoutMs=8000; // no RTP/beacon => lost. Tolerant on purpose: + // a brief RX gap must NOT trigger the full + // re-scan/auth/4-way reconnect — on flaky USB + // hosts (MediaTek) that caused stream-killing + // flapping (audio blips, video keyframe never lands) + int reconnectBackoffMs=0; // immediate (wpa_supplicant-like) + int scanIntervalMs=5000; // wpa_supplicant default scan_interval + int maxBackoffMs=5000; }; + ApfpvStation(void* rtlUsbAdapter, void* radioMgmt, OnRtpFn onRtp, OnStateFn onState); + // Provide the RtlJaguarDevice so the station can register the RX callback + // (RxDeframe) as the device packet processor — the receive path. + void setDevice(void* rtlJaguarDevice) { _rtl = rtlJaguarDevice; } + ~ApfpvStation(); + bool connect(const Params& p); // runs the chain + starts the supervisor + void disconnect(); // stops supervisor + tears down + // GENERAL-IP BRIDGE (for an Android VpnService TUN). setIpSink: downlink — receives the + // full decrypted IPv4 packet (SSH / any TCP+UDP), making the dongle a full L3 interface + // (not RTP-only). sendIpPacket: uplink — CCMP-encrypts + TXes an arbitrary IPv4 datagram. + void setIpSink(OnRtpFn fn); // propagates to _rx immediately (so arming after connect works) + bool sendIpPacket(const uint8_t* ip, size_t len); + // Minimal TCP/HTTP client over the link — PROVES the dongle carries general TCP, so SSH + // and REST/HTTP (same transport) work over it. Real SYN/SYN-ACK/ACK + "GET " to + // dstIp:port; returns the response (empty on failure). Blocking; call after Streaming. + std::string httpGet(uint32_t dstIp, uint16_t port, const std::string& path, int timeoutMs = 4000); + // Raw bidirectional TCP over the link — enough to relay a whole SSH session (a local + // socket listener in the host harness bridges plink <-> here). Minimal in-order stack: + // accepts only in-order segments (the peer retransmits its own losses), ACKs everything, + // segments uplink at a conservative MSS. Call only after Streaming, one connection at a time. + bool tcpConnect(uint32_t dstIp, uint16_t dport, int timeoutMs = 4000); + int tcpPoll(std::string& out, int timeoutMs); // >=0 = bytes appended; -1 = peer closed (FIN/RST) + bool tcpSend(const uint8_t* data, size_t n); + void tcpClose(); + uint32_t leaseIp() const; // our DHCP IP (0 if none) + uint32_t leaseServerIp() const; // DHCP server / gateway (the VTX analog) + + // All-SSID RF scan for the UI picker. Channel-hops the APFPV/5GHz/2.4 set, + // parses every beacon, and calls onAp(ssid, info) once per NEW SSID as it is + // discovered (live updates). Runs synchronously — call it from a worker + // thread, and only while NOT connected (it drives the RX path). Requires + // setDevice() to have been called. + using OnApFn = std::function; + // includeDfs=false skips the DFS 5GHz channels (52-64, 100-144) — faster, and + // avoids tuning the radio onto radar-protected channels. + void scanAll(int perChannelMs, bool includeDfs, const OnApFn& onAp); + + // VRX EIRP-calibration beacon: inject an open beacon with `ssid` on `channel` + // at TX power index `txIndex`, so a second phone's Wi-Fi scan can read the + // dongle's RSSI and estimate its EIRP. Stops any station/supervisor first. + // Runs on its own thread; call stopBeaconCal() (or destruct) to end it. + void startBeaconCal(const std::string& ssid, int channel, int txIndex); + void stopBeaconCal(); + + // ---- AP MODE (SoftAP) — ⚠️ NOT READY / WORK IN PROGRESS ---------------- + // ⚠️ DO NOT EXPOSE via JNI or call from the app yet. Beacons (open/WPA2), tracks client + // RSSI, and runs the full WPA2 Authenticator; association completion is UNTESTED on real + // hardware post-rebuild. Now rides upstream's real HW AP primitives (IRtlDevice::StartBeacon/ + // StopBeacon, bench-proven against a real Linux station in docs/ap-mode.md) instead of the + // pre-rebuild manual register pokes + a software beacon-TX poll loop — see the banner on + // startAp() in the .cpp. Kept as in-progress scaffolding only. + void startAp(const std::string& ssid, int channel, const std::string& password = ""); + void stopAp(); + struct ApStation { Mac mac; int rssiDbm; int state; }; // state 4=assoc, 7=authenticated + std::vector apStations(); + + State state() const { return _state.load(); } + int rssiDbm() const { return _rssi.load(); } + int channel() const { return _params.channel; } // resolved AP channel (for UI) + // RX path calls this on every received data/beacon frame so the supervisor + // knows the link is alive (resets the loss timer). Wire from RxDeframe. + void notifyRxAlive() { _lastRxMs.store(nowMs()); } + // RX path calls this if a deauth/disassoc is seen (immediate loss). + void notifyDeauth() { _deauth.store(true); } + // Handle ADDBA Request from the AP: send ADDBA Response (accept TID 0, immediate BA). + // Called from RxDeframe when a Block-Ack action frame arrives. Enables A-MPDU RX. + void handleAddbaRequest(const uint8_t* frame, size_t len); +private: + void set(State s); + bool runConnectChain(); // the gated arm->...->stream sequence + void supervisorLoop(); // persistent reconnect watcher (the daemon) + static int64_t nowMs() { + using namespace std::chrono; + return duration_cast(steady_clock::now().time_since_epoch()).count(); + } + void* _dev; void* _rm; void* _rtl = nullptr; OnRtpFn _onRtp; OnStateFn _onState; + OnRtpFn _ipSink; // downlink general-IP (TUN) sink, optional + std::vector> _ipQ; // httpGet: captured inbound IP packets + std::mutex _ipQMtx; + std::atomic _ipCapture{false}; + // active raw-TCP connection state (tcpConnect/tcpSend/tcpPoll/tcpClose) + uint32_t _tcSrc=0,_tcDst=0; uint16_t _tcSport=0,_tcDport=0; + uint32_t _tcSeq=0,_tcAck=0; bool _tcOpen=false; + bool matchTcp(const std::vector& pk, uint32_t& seq, uint8_t& flags, + const uint8_t*& payload, size_t& plen); + std::function _gratArp; // re-announce our IP<->MAC (keeps unicast RTP alive) + // RX pipeline (lives across the connection so the device callback stays valid) + std::unique_ptr _rx; + std::unique_ptr _wpa; + std::unique_ptr _lq; + std::unique_ptr _dhcp; + // Cached WPA2 PMK (depends ONLY on passphrase+SSID). Precomputed in the background so + // the slow (~seconds) PBKDF2 never runs inline in becomeStation — otherwise the RX + // doesn't switch to the EAPOL phase until after the AP's ~4s M1 retry window expires. + std::array _pmkCache{}; + std::atomic _pmkValid{false}; + void ensurePmk(); // compute+cache the PBKDF2 PMK once (passphrase+SSID only) + Params _params; + std::atomic _state{State::Idle}; + std::atomic _rssi{-90}; + std::atomic _lastRxMs{0}; + uint8_t _pendingAddbaTids = 0; // TIDs queued by dispatch, sent from connect thread + std::atomic _deauth{false}; + std::atomic _raCfgSent{false}; // H2C MACID_CFG(0x40) sent this connection (once, like the kernel) + std::atomic _run{false}; // supervisor running + std::thread _supervisor; + // RX runs on its OWN thread (RtlJaguarDevice::Init is a blocking read loop). + // A single dispatch routes by phase: 0 = discovery/arm (StationMode), 1 = + // streaming (RxDeframe). _rxReady flips true once the loop is live. + std::thread _rxThread; + std::atomic _rxPhase{0}; + std::atomic _rxReady{false}; + // scanAll() state — the RX collector runs on the device thread, so access is + // mutex-guarded and _onScanAp is cleared before scanAll returns so the still- + // running RX thread can never call into a destroyed callback. + std::mutex _scanMtx; + std::set _scanSeen; + OnApFn _onScanAp; + // Diagnostics — MEMBERS (not scanAll locals): the RX collector is stored in + // the device and called from its read thread, so it must not capture + // stack-local counters by reference (that dangles -> SIGSEGV). + std::atomic _scanPkts{0}; + std::atomic _scanBeacons{0}; + // VRX EIRP-calibration beacon injector + std::atomic _beaconRun{false}; + std::thread _beaconThread; + // AP mode (SoftAP): _beaconThread is repurposed as the AP TX-pump (drains _apTxQ) since the + // actual beacon now airs from hardware (StartBeacon's TBTT engine, not a software poll loop). + // send_packet must run OFF the RX event thread (docs/ap-mode.md) -- apSend() queues here + // instead of calling send_packet inline from apOnRx (which runs on _rxThread). + std::atomic _apRun{false}; + std::mutex _apTxMtx; + std::vector> _apTxQ; + Mac _apSelf{}; + bool _apWpa2 = false; int _apChannel = 0; std::string _apSsid; + std::array _apPmk{}; std::array _apGtk{}; + std::unique_ptr _apAuth; + std::mutex _apMtx; + std::vector _apStaList; + void apOnRx(const uint8_t* f, size_t len, uint8_t rssiRaw); // AP RX: auth/assoc/eapol + void apSend(const std::vector& mpdu, bool eapol); // prepend TX desc + radiate + void apTrack(const Mac& sta, uint8_t rssiRaw, int state); // upsert station (state<0 keeps) +}; +} diff --git a/src/LqFeedback.cpp b/src/LqFeedback.cpp new file mode 100644 index 0000000..e707f51 --- /dev/null +++ b/src/LqFeedback.cpp @@ -0,0 +1,186 @@ +// ============================================================================ +// LqFeedback.cpp — downlink LQ sender to aalink UDP 192.168.0.1:12345 (impl) +// Declarations in LqFeedback.h. Modeled on aalink's documented input +// ("rssi : %d" / per-antenna form) and RSSI_SAMPLE_INTERVAL_MS=33. Two modes. +// ============================================================================ +#include "LqFeedback.h" +#include +#if defined(__ANDROID__) +#include +#endif +#include +#include +#include +#include +#include +#include +#include +#include +#if defined(_WIN32) +#include +#include +#define close closesocket +#else +#include +#include +#include +#include +#endif +#if defined(__ANDROID__) +#include +#include +#endif + +namespace apfpv { + +// LQ send interval override, live-tunable without a rebuild. Default (33ms/30Hz, matching +// aalink's own RSSI_SAMPLE_INTERVAL_MS) sends 2 packets/cycle through the SAME shared dongle +// TX path (sendIpPacket -> sendStationFrameSync -> a real synchronous USBDEVFS_BULK ioctl) that +// video RX competes with -- 60 TX calls/sec, continuously, for the whole streaming session, +// independent of any other feature (SSH route, etc). aalink's OWN decision cadence is on the +// order of SECONDS (UP_COOLDOWN_MS=2000 in aalink.conf), so 30Hz RSSI resolution is almost +// certainly far finer than its control loop needs -- worth testing whether a slower cadence +// reduces TX-path contention (and therefore RX hiccups at high throughput) without harming +// aalink's actual bitrate adaptation. DEVOURER_LQ_MS (host) / debug.pixelpilot.lqms (Android); +// 0/unset = unchanged default behavior. +static int lqSendIntervalOverrideMs() { + if (const char* e = std::getenv("DEVOURER_LQ_MS")) { int n = std::atoi(e); if (n > 0) return n; } +#if defined(__ANDROID__) + char v[PROP_VALUE_MAX] = {0}; + if (__system_property_get("debug.pixelpilot.lqms", v) > 0) { + int n = std::atoi(v); + if (n > 0) return n; + } +#endif + return 0; +} + +// internal state (PImpl-lite via file-static per instance would be cleaner; +// kept as members through a small struct mirrored from the header). +struct LqState { + LqFeedback::Config cfg; + int sock = -1; + sockaddr_in dst{}; + std::atomic a{-80}, b{-80}; + bool haveB = false; + std::atomic run{false}; + std::thread th; + std::mutex m; std::condition_variable cv; bool fresh = false; + std::function sink; // if set, route emit here (dongle IP TX) not the socket +}; + +// Single global map would be needed for many instances; APFPV uses one link, +// so we keep one state object behind the class via a static (documented). +static LqState g; + +int LqFeedback::rssiPct(int dbm) { + // EXACT OpenIPC ground-station mapping (from sbc-groundstations gsmenu.sh: + // sig_pct = int(2 * (signal_dBm + 100)), clamped 0..100). + // This is the calibration the air's aalink THRESH tables were tuned against, + // and it is STABLE across greg08/09/10/10.1/10.2 and current sbc-gs builds + // (the air-side THRESH/SCALE_RSSI values are essentially unchanged). Our + // earlier linear (-90..-20 => 0..100) ran ~30 points COLD vs this — feeding + // aalink low percentages and biasing it to over-conservative MCS. + int pct = 2 * (dbm + 100); + if (pct < 0) pct = 0; + if (pct > 100) pct = 100; + return pct; +} + +LqFeedback::LqFeedback() { g.cfg = Config{}; } +LqFeedback::LqFeedback(Config cfg) { g.cfg = cfg; } +void LqFeedback::setSink(std::function fn) { g.sink = std::move(fn); } + +static void emit(double& sA, double& sB, bool& init) { + int a = g.a.load(), b = g.b.load(); + double alpha = g.cfg.smoothing_alpha; + if (!init) { sA = a; sB = b; init = true; } + else { sA = alpha*a + (1-alpha)*sA; sB = alpha*b + (1-alpha)*sB; } + char buf[64]; int n; + // aalink parses: "%*[^=]=%*s rssi_a = %d(%%), rssi_b = %d(%%)" — i.e. it + // skips a leading "token=word " prefix, THEN reads the rssi fields. Without + // the prefix the sscanf fails and the ground RSSI is ignored. Match the + // firmware exactly (confirmed against the greg10.2 aalink binary). + int pctA = LqFeedback::rssiPct((int)sA), pctB = LqFeedback::rssiPct((int)sB); + if (g.haveB) + n = std::snprintf(buf, sizeof(buf), "gs_string=gs rssi_a = %d(%%), rssi_b = %d(%%)\n", pctA, pctB); + else + n = std::snprintf(buf, sizeof(buf), "gs_string=gs rssi_a = %d(%%)\n", pctA); + if (n > 0) { + if (g.sink) g.sink(buf, n); + else if (g.sock >= 0) ::sendto(g.sock, buf, (size_t)n, 0, (sockaddr*)&g.dst, sizeof(g.dst)); + } + // Also send the BARE percentage that the phone-Wi-Fi path uses (ApfpvWifiManager sends + // Integer.toString(pct)) — that variant is confirmed to move the VTX downlink %, whereas the + // verbose "gs_string=" form above may not parse on the current aalink. Belt-and-suspenders. + char bare[16]; int bn = std::snprintf(bare, sizeof(bare), "%d", pctA); + if (bn > 0) { + if (g.sink) g.sink(bare, bn); + else if (g.sock >= 0) ::sendto(g.sock, bare, (size_t)bn, 0, (sockaddr*)&g.dst, sizeof(g.dst)); + } +#if defined(__ANDROID__) + // DIAGNOSTIC: surface the RSSI→pct we are actually sending (rules out a 0-value RSSI conversion). + static int dbgN = 0; + if ((dbgN++ % 30) == 0) + __android_log_print(4, "apfpv-lq", "LQ send rssiA=%.0f pctA=%d (haveB=%d pctB=%d)", + sA, pctA, (int)g.haveB, pctB); +#endif +} + +static void loopFixed() { + using namespace std::chrono; double sA=0,sB=0; bool init=false; + while (g.run) { + emit(sA,sB,init); + int ms = lqSendIntervalOverrideMs(); + std::this_thread::sleep_for(milliseconds(ms > 0 ? ms : g.cfg.send_interval_ms)); + } +} +static void loopFrameDriven() { + using namespace std::chrono; double sA=0,sB=0; bool init=false; + while (g.run) { + std::unique_lock lk(g.m); + auto wait = g.cfg.keepalive_ms > 0 ? milliseconds(g.cfg.keepalive_ms) : milliseconds(1000); + g.cv.wait_for(lk, wait, []{ return g.fresh || !g.run; }); + bool wasFresh = g.fresh; g.fresh = false; lk.unlock(); + if (!g.run) break; + if (wasFresh || g.cfg.keepalive_ms > 0) { + emit(sA,sB,init); + std::this_thread::sleep_for(milliseconds(g.cfg.min_interval_ms)); + } + } +} + +bool LqFeedback::start(const char* airIp, uint16_t port) { + // A prior start() may have left a running thread (e.g. on replug/reconnect + // the connect chain re-runs without stop()). Move-assigning g.th below while + // it is still joinable calls std::terminate(). Tear the old one down first. + if (g.th.joinable()) stop(); + // When a sink is set (libusb dongle path), emit routes through it — no host socket needed. + // The host ::socket() also needs WSAStartup on Windows (only run lazily elsewhere), so a + // socket-open failure must NOT prevent the emit thread from starting. + if (!g.sink) { + g.sock = ::socket(AF_INET, SOCK_DGRAM, 0); + if (g.sock < 0) return false; + std::memset(&g.dst, 0, sizeof(g.dst)); + g.dst.sin_family = AF_INET; g.dst.sin_port = htons(port); + ::inet_pton(AF_INET, airIp, &g.dst.sin_addr); + } + g.run = true; + g.th = std::thread(g.cfg.mode == Mode::FixedTimer ? loopFixed : loopFrameDriven); + return true; +} +void LqFeedback::stop() { + g.run = false; g.cv.notify_all(); + if (g.th.joinable()) g.th.join(); + if (g.sock >= 0) { ::close(g.sock); g.sock = -1; } +} +void LqFeedback::update(int rssiA_dbm, int rssiB_dbm) { + g.a.store(rssiA_dbm); + if (rssiB_dbm != INT32_MIN) { g.b.store(rssiB_dbm); g.haveB = true; } + if (g.cfg.mode == Mode::FrameDriven) { + { std::lock_guard lk(g.m); g.fresh = true; } + g.cv.notify_one(); + } +} + +} // namespace apfpv diff --git a/src/LqFeedback.h b/src/LqFeedback.h new file mode 100644 index 0000000..115c65f --- /dev/null +++ b/src/LqFeedback.h @@ -0,0 +1,26 @@ +#pragma once +#include +#include +namespace apfpv { +class LqFeedback { +public: + enum class Mode { FixedTimer, FrameDriven }; + // send_interval_ms: RSSI link-feedback uplink rate to greg's aalink on UDP 192.168.0.1:12345. + // 33 ms (~30 Hz) = aalink's RSSI_SAMPLE_INTERVAL_MS, confirmed against the greg10.2 aalink binary + // (port 12345 + "rssi_a = %d(%%)" format). NOTE: this is greg's *aalink*, NOT the public + // OpenIPC/adaptive-link *alink* (port 9999, colon score 1000-2000, ~1-5 Hz) — different protocol, + // don't conflate the two. Keep 33 ms to match greg10.2 (the dongle did 130 fps at this rate). + struct Config { Mode mode = Mode::FixedTimer; int send_interval_ms = 33; + int min_interval_ms = 20; int keepalive_ms = 0; double smoothing_alpha = 0.3; }; + static int rssiPct(int dbm); + LqFeedback() ; explicit LqFeedback(Config cfg); + ~LqFeedback() { stop(); } // join thread + close socket on destroy (reconnect-safe) + bool start(const char* airIp = "192.168.0.1", uint16_t port = 12345); + // Route each LQ datagram through the caller instead of a host UDP socket. Needed on the + // libusb dongle path (Win/WSL): the host has NO route to the VTX, so ::sendto dead-ends; + // the sink lets ApfpvStation TX the payload over the dongle's own IP stack (sendIpPacket). + void setSink(std::function fn); + void stop(); + void update(int rssiA_dbm, int rssiB_dbm = INT32_MIN); +}; +} diff --git a/src/RtlAdapter.cpp b/src/RtlAdapter.cpp index 88b40fc..0d8de46 100644 --- a/src/RtlAdapter.cpp +++ b/src/RtlAdapter.cpp @@ -9,6 +9,8 @@ #include "RtlAdapter.h" #include +#include +#include #include #include @@ -270,3 +272,134 @@ void RtlAdapter::PHY_SetBBReg8812(uint16_t regAddr, uint32_t bitMask, /* RTW_INFO("BBW MASK=0x%x Addr[0x%x]=0x%x\n", BitMask, RegAddr, Data); */ } + +// --------------------------------------------------------------------------- +// APFPV additions, ported from the pre-rebuild RtlUsbAdapter (upstream's own +// RtlAdapter never needed these -- no station-mode client flow exists +// upstream). Logic unchanged; re-hosted onto this class's own +// rtw_read/rtw_write primitives (identical template signatures). +// --------------------------------------------------------------------------- +bool RtlAdapter::fillH2CCmd(uint8_t elementID, uint32_t cmdLen, + const uint8_t *cmdBuffer) { + if (cmdLen > 0 && !cmdBuffer) return false; + if (cmdLen > 7) return false; // 3 primary + 4 ext + uint8_t box = _lastH2CBox % 4; + // Wait until this box has been read by the FW (REG_HMETFR bit(box) == 0). + for (int i = 0; i < 100; ++i) { + uint8_t st = rtw_read8(REG_HMETFR); + if ((st & (1u << box)) == 0) break; + std::this_thread::sleep_for(1ms); + } + uint32_t h2c_cmd = 0, h2c_cmd_ex = 0; + uint8_t *pc = reinterpret_cast(&h2c_cmd); + pc[0] = elementID; + if (cmdLen <= 3) { + if (cmdLen) std::memcpy(pc + 1, cmdBuffer, cmdLen); + } else { + std::memcpy(pc + 1, cmdBuffer, 3); + std::memcpy(&h2c_cmd_ex, cmdBuffer + 3, cmdLen - 3); + } + bool ok = true; + ok &= rtw_write32((uint16_t)(REG_HMEBOX_EXT_0 + box * 4), h2c_cmd_ex); + ok &= rtw_write32((uint16_t)(REG_HMEBOX_0 + box * 4), h2c_cmd); + _lastH2CBox = (uint8_t)((box + 1) % 4); + return ok; +} + +bool RtlAdapter::sendH2CPacket(const uint8_t h2c[32]) { + static const uint16_t s_boxRegs[4] = { REG_HMEBOX_0, REG_HMEBOX_1, + REG_HMEBOX_2, REG_HMEBOX_3 }; + static const uint16_t s_boxExRegs[4] = { REG_HMEBOX_EXT_0, REG_HMEBOX_EXT_1, + REG_HMEBOX_EXT_2, REG_HMEBOX_EXT_3 }; + for (int box = 0; box < 4; box++) { + for (int retry = 0; retry < 100; retry++) { + uint8_t st = rtw_read8(REG_HMETFR); + if ((st & (1u << box)) == 0) break; + std::this_thread::sleep_for(1ms); + } + uint32_t w0 = (uint32_t)h2c[box*8+0] | ((uint32_t)h2c[box*8+1]<<8) | + ((uint32_t)h2c[box*8+2]<<16) | ((uint32_t)h2c[box*8+3]<<24); + uint32_t w1 = (uint32_t)h2c[box*8+4] | ((uint32_t)h2c[box*8+5]<<8) | + ((uint32_t)h2c[box*8+6]<<16) | ((uint32_t)h2c[box*8+7]<<24); + rtw_write32(s_boxExRegs[box], w1); + rtw_write32(s_boxRegs[box], w0); + } + return true; +} + +void RtlAdapter::setSecCamKey(uint8_t entry, const uint8_t mac[6], uint8_t keyid, + const uint8_t key[16]) { + const uint16_t kCamWriteReg = 0x0674, kCamCmdReg = 0x0670; + const uint32_t kCamPoll = 0x80000000u, kCamWr = 0x00010000u; + const uint8_t kAes = 4; + const uint8_t base = (uint8_t)(entry * 8); + uint32_t dw[8] = {0}; + dw[0] = (uint32_t)keyid | ((uint32_t)kAes << 2) | 0x8000u + | ((uint32_t)mac[0] << 16) | ((uint32_t)mac[1] << 24); + dw[1] = (uint32_t)mac[2] | ((uint32_t)mac[3] << 8) + | ((uint32_t)mac[4] << 16) | ((uint32_t)mac[5] << 24); + for (int i = 0; i < 4; i++) + dw[2 + i] = (uint32_t)key[i*4] | ((uint32_t)key[i*4+1] << 8) + | ((uint32_t)key[i*4+2] << 16) | ((uint32_t)key[i*4+3] << 24); + for (int a = 0; a < 8; a++) { + rtw_write32(kCamWriteReg, dw[a]); + rtw_write32(kCamCmdReg, kCamPoll | kCamWr | (uint32_t)(base + a)); + for (int p = 0; p < 50; p++) { if ((rtw_read32(kCamCmdReg) & kCamPoll) == 0) break; } + } +} + +void RtlAdapter::enableHwSec() { + // REG_SECCFG (0x0680): SCR_CHK_KEYID(BIT8)|SCR_RxDecEnable(BIT3)|SCR_TxEncEnable(BIT2). + // GROUND TRUTH from the LIVE kernel mac_reg_dump while connected = 0x010c. + // DEVOURER_SECCFG=0xNNNN overrides for A/B testing. + uint16_t v = 0x010c; + if (const char* e = std::getenv("DEVOURER_SECCFG")) v = (uint16_t)strtol(e, nullptr, 0); + rtw_write16(0x0680, v); +} + +bool RtlAdapter::sendStationFrameSync(uint8_t *data, size_t len) { + // Recover a possibly-halted/stalled TX EP before EACH station TX. OFF by + // default -- it was the root cause of on-air silence (PROVEN on native + // Windows against a real AP's own hostapd RX log: WITH clear_halt the + // auth never reached the AP; WITHOUT it the connect reached Handshaking). + // Opt-in via DEVOURER_TX_HALT_CLR for the WSL/vhci "FailTx wedge" case only. + if (std::getenv("DEVOURER_TX_HALT_CLR")) { + uint8_t ep = 0x02; + if (const char *e = std::getenv("DEVOURER_TX_EP")) ep = (uint8_t)std::strtoul(e, nullptr, 0); + else if (bulk_out_ep_count() > 0) ep = first_bulk_out_ep(); + bulk_clear_halt(ep); + } + // Concurrent IN/OUT exactly like the kernel USB core -- do NOT cancel the + // RX loop, so the AP's auth/assoc reply (arriving ~1ms after our TX) is + // caught on the still-live RX. The per-TX clear_halt above (opt-in) is + // what keeps the OUT from wedging behind pending INs on transports that + // need it. Validated: reaches Associating[PASS] on WSL-vhci, the + // worst-case transport. + bool ok = send_packet(data, len); + // ACK-DRIVEN (kernel-style) instead of a blind sleep: watch the chip + // confirm the frame left the TX FIFO via REG_TXPKT_EMPTY (0x041A). Logs + // the drain so a silent run is diagnosable. Fire-and-forget once + // streaming (_txFastPath): the drain poll is a connect-time diagnostic + // that costs ~6ms/call; during streaming it's called ~90-100x/s -> would + // starve RX -> decode/render stutters. send_packet already blocked on the + // real bulk write, so the frame is on the chip; just return. + if (_txFastPath && _txFastPath->load()) { + return ok; + } + if (!std::getenv("DEVOURER_TX_BLIND")) { + uint16_t e0 = 0, e = 0; + e0 = rtw_read16(0x041A); + auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(10); + int settled = 0; + e = e0; + do { + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + uint16_t cur = rtw_read16(0x041A); + if (cur == e) { if (++settled >= 3) break; } else { settled = 0; e = cur; } + } while (std::chrono::steady_clock::now() < deadline); + _logger->info("STA-TX ack: TXPKT_EMPTY 0x41a {:#06x}->settled {:#06x}", e0, e); + return ok; + } + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + return ok; +} diff --git a/src/RtlAdapter.h b/src/RtlAdapter.h index 8c422c1..78dccc3 100644 --- a/src/RtlAdapter.h +++ b/src/RtlAdapter.h @@ -16,6 +16,7 @@ * (Historically this class WAS the USB transport; the old `RtlUsbAdapter` * name remains as a deprecated alias in RtlUsbAdapter.h.) */ +#include #include #include #include @@ -130,6 +131,27 @@ class RtlAdapter { uint8_t rxagg_usb_timeout = 0; bool send_packet(uint8_t *packet, size_t length); + // APFPV addition: ApfpvStation flips this true on entering Streaming so + // sendStationFrameSync's per-TX ACK-drain diagnostic is skipped + // (send_packet already blocks on the actual bulk write; the drain poll + // during connect matters for diagnosing a silent chip, but during + // streaming it costs ~6ms/call at ~90-100 calls/s -> ~560ms/s of blocking + // that starves RX -> decode/render stutters). shared_ptr: survives + // RtlAdapter value-copies (it's a copyable value type). + std::shared_ptr> _txFastPath = + std::make_shared>(false); + void setTxFastPath(bool b) { if (_txFastPath) _txFastPath->store(b); } + // APFPV addition: station TX with the proven default behavior (concurrent + // IN/OUT, no RX pause -- matches the kernel USB core; validated to reach + // Associating[PASS] on WSL-vhci, the worst-case transport) plus an + // ACK-driven completion check via TXPKT_EMPTY (skipped once _txFastPath is + // set). The pre-rebuild version also had a legacy DEVOURER_TX_USE_PAUSE + // fallback that paused/resumed the async-RX worker around the TX; that + // worker doesn't exist in this form anymore (RX ownership moved to + // whoever calls bulk_read_async_loop -- see RtlJaguarDevice::StartRxLoop/ + // StopRxLoop) and that branch was already described as legacy/non-default, + // so it's dropped rather than force-fit onto a different architecture. + bool sendStationFrameSync(uint8_t *data, size_t len); /* Synchronous TX that blocks until completion or timeout. Returns bytes * submitted, or negative on error. */ int bulk_send_sync(uint8_t *packet, size_t length, int timeout_ms) { @@ -198,7 +220,32 @@ class RtlAdapter { void _8051Reset8812(); void ReadEFuseByte(uint16_t _offset, uint8_t *pbuf); + // --- APFPV additions: ported from the pre-rebuild RtlUsbAdapter, adapted + // to this class's rtw_read/rtw_write/WriteBytes primitives (same + // template signatures as before -- no logic changes, just re-hosted). --- + bool fillH2CCmd(uint8_t elementID, uint32_t cmdLen, const uint8_t *cmdBuffer); + bool sendH2CPacket(const uint8_t h2c[32]); + void setSecCamKey(uint8_t entry, const uint8_t mac[6], uint8_t keyid, + const uint8_t key[16]); + void enableHwSec(); + int reset_device() { return _transport->reset(); } + // Pre-rebuild RtlUsbAdapter paused/resumed the caller's own async-URB-submission pump + // (an `_rxQuiesce` flag the RX event-pump thread checked) around register-I/O-heavy + // calibration (StationMode::arm's IQK retry), so a synchronous control transfer wasn't + // stuck behind that thread's libusb_handle_events polling. Post-rebuild, RX ownership + // moved OUT of RtlAdapter entirely -- it's now IRtlDevice::StartRxLoop, a blocking loop + // run on the CALLER's own thread (see ApfpvStation::_rxThread), and libusb's threading + // model already lets a synchronous transfer on one thread proceed correctly while + // another thread is inside libusb_handle_events (it waits on its own transfer's + // completion rather than double-polling). So there is no in-adapter pump left to pause; + // these are genuine no-ops kept only so call sites written against the old contract + // (StationMode::arm's cal-guard) still compile unchanged. + void pauseAsyncRx() {} + void resumeAsyncRx() {} + private: + uint8_t _lastH2CBox = 0; // round-robins across the H2C mailboxes (REG_HMEBOX_0..3) + void init_from_transport(const devourer::DeviceConfig &cfg); void PHY_SetBBReg8812(uint16_t regAddr, uint32_t bitMask, uint32_t dataOriginal); diff --git a/src/RtlTransport.h b/src/RtlTransport.h index 19246b5..2503b57 100644 --- a/src/RtlTransport.h +++ b/src/RtlTransport.h @@ -45,6 +45,14 @@ class IRtlTransport { virtual bool is_usb() const = 0; + /* APFPV addition: a prior process (AP mode, or a station run killed + * mid-connect) can leave the chip's USB engine / TX path in a state a + * firmware re-download does NOT clear -- seen as bulk-OUT TX timeouts. + * Default no-op (e.g. PCIe has no equivalent concept); UsbTransport + * overrides with a real libusb_reset_device. Returns a libusb error code + * (or -1 if unsupported by this transport). */ + virtual int reset() { return -1; } + /* ---- register plane ---- */ virtual uint8_t read8(uint16_t reg) = 0; virtual uint16_t read16(uint16_t reg) = 0; diff --git a/src/RxDeframe.cpp b/src/RxDeframe.cpp new file mode 100644 index 0000000..637ad74 --- /dev/null +++ b/src/RxDeframe.cpp @@ -0,0 +1,672 @@ +// ============================================================================ +// RxDeframe.cpp — 802.11 data -> UDP/RTP -> port 5600 + RSSI tap (impl) +// Declarations in RxDeframe.h. Wire onPacket() as devourer's RX callback. +// ============================================================================ +#include "RxDeframe.h" +#include "ApfpvStation.h" +#include "jaguar1/PhydmWatchdog.h" +#include +#include +#include +#include +#include +#if defined(__ANDROID__) +#include +#endif + +// --- throughput bottleneck diag: logs timing breakdown every 500 packets --- +#define RX_DIAG_INTERVAL 500 +namespace apfpv { + +// HW-decrypt gate: env (host) or `debug.pixelpilot.hwdec=1` (Android prop). Must match the +// ApfpvStation gate — HW-decrypt arms the compressed-BA (rig: loss 163→21) but needs RCR_APP framing. +static bool rxHwDecryptOn() { + if (std::getenv("DEVOURER_HW_DECRYPT") != nullptr) return true; +#if defined(__ANDROID__) + char v[PROP_VALUE_MAX] = {0}; + if (__system_property_get("debug.pixelpilot.hwdec", v) > 0 && v[0] != '0') return true; +#endif + return false; +} + +static bool reorderEnabledInit() { + // DEFAULT ON: deliver RTP strictly in-order to :5600 like the kernel does for phone-wifi. + // The dongle's USB/A-MPDU delivery is bursty/out-of-order; without this the app's parser sees + // gaps -> drops reference NALUs -> long-GOP decoder freeze (the dongle-only stutter). Opt out + // with DEVOURER_REORDER=0 or debug.pixelpilot.reorder=0. + if (const char* e = std::getenv("DEVOURER_REORDER")) return e[0] != '0'; +#if defined(__ANDROID__) + char v[PROP_VALUE_MAX] = {0}; + if (__system_property_get("debug.pixelpilot.reorder", v) > 0) return v[0] != '0'; +#endif + return true; +} + +RxDeframe::RxDeframe(const Mac& self, const Mac& bssid, Wpa2Supplicant* wpa, + LqFeedback* lq, OnRtpFn onRtp) + : _self(self), _bssid(bssid), _wpa(wpa), _lq(lq), _onRtp(std::move(onRtp)) { + _reorderOn = reorderEnabledInit(); +} + +int RxDeframe::toDbm(uint8_t r) { + // RxAtrib.rssi[] is devourer's gain_trsw byte for the path: the RX + // descriptor's {TRSW(bit7), gain[6:0]} field (FrameParser.cpp fills it from + // driver_data.gain_trsw). It is a GAIN index, not dBm — so the placeholder + // r/2-95 was wrong. The RTL8812 (Jaguar) phystatus path in the Realtek HAL + // converts the OFDM RX gain to power as: rx_pwr_dBm = gain_index - 110 + // (the phydm rx_pwr_all base; LNA/VGA gain index maps ~1:1 to dB, offset by + // the front-end reference of -110 dBm at gain 0). We mask the TRSW bit and + // apply that. Clamp to a sane RF window. + int gain = r & 0x7F; // strip TRSW flag, keep gain[6:0] + int dbm = gain - 110; // Jaguar OFDM gain-index -> dBm + if (dbm < -100) dbm = -100; + if (dbm > -10) dbm = -10; + return dbm; +} + +void RxDeframe::onPacket(const Packet& pkt) { + // Layer diagnostic: count frames entering RxDeframe + forwarded to RTP + static uint64_t inFrames=0, fwdFrames=0, decryptOk=0, fwdBytes=0; + static auto layerClock = std::chrono::steady_clock::now(); + inFrames++; + // BURST-LOSS HUNT (2026-07-15): is the periodic ~150-pkt loss a dongle RX BLACKOUT (a >15ms gap + // with NO frames = our-side retune/stall/off-channel) or on-air/AP/chip loss (frames keep + // flowing, only a seq hole)? Log any inter-frame gap >12ms with the elapsed ms + how many frames + // we'd expect to have lost. If these gaps recur every ~8s = the smoking gun (RX went blind). + { + static auto lastPkt = std::chrono::steady_clock::now(); + static int gapN = 0; + auto nowp = std::chrono::steady_clock::now(); + long gapMs = (long)std::chrono::duration_cast(nowp - lastPkt).count(); + lastPkt = nowp; + if (gapMs > 12) + __android_log_print(ANDROID_LOG_WARN, "rx-gap", "#%d RX gap %ldms (>12ms = RX blind this long)", ++gapN, gapMs); + } + if (_station) _station->notifyRxAlive(); // any RX = link alive (supervisor) + if (_lq) _lq->update(toDbm(pkt.RxAtrib.rssi[0]), toDbm(pkt.RxAtrib.rssi[1])); + // Feed live RX RSSI to DIG so it uses phydm *connected-mode* boundaries + // (IGI floor tracks RSSI ~0x37 @ -45 dBm) instead of monitor coverage + // bounds (capped 0x2a → over-gained → FA storm → AP rate-caps us). + PhydmWatchdog::SetLinkRssi(toDbm(pkt.RxAtrib.rssi[0])); + + const uint8_t* f = pkt.Data.data(); + size_t len = pkt.Data.size(); + if (len < 24) return; + + uint16_t fc = f[0] | (f[1] << 8); + // mgmt deauth/disassoc from our AP -> immediate link-loss signal + if (((fc >> 2) & 0x3) == 0x0) { + uint8_t sub = (fc >> 4) & 0xF; + if (sub == 0xC || sub == 0xA) { + // ONLY honor a deauth/disassoc that is addressed to US (a1==self) AND sent by + // OUR AP (a2==bssid). Without this, in busy RF we overhear OTHER APs deauthing + // THEIR clients and falsely trip link-loss -> a Reconnecting churn every few + // seconds -> periodic video stutter. (Same a1/a2 filter the join path uses.) + bool toUs = (std::memcmp(f + 4, _self.data(), 6) == 0); + bool fromAp = (std::memcmp(f + 10, _bssid.data(), 6) == 0); + if (_station && toUs && fromAp) { // deauth/disassoc from AP -> immediate link-loss + if (!_wpa || !_wpa->pmfActive() || _wpa->verifyProtectedMgmt(f, len)) + _station->notifyDeauth(); + } + return; + } + // 802.11 Action frame (subtype 0xD): BlockAck frames + if (sub == 0xD && len >= 27) { + const uint8_t* b = f + 24; + // Full hex dump of ADDBA Request (cat=3 act=0) to compare buffer size/policy + if (b[0] == 0x03 && b[1] == 0x00 && len >= 24 + 9) { + u16 p = b[3] | (b[4] << 8); + __android_log_print(4, "apfpv-scan", + "ADDBA-REQ-IN dialog=%u param=0x%04x policy=%u tid=%u bufsz=%u timeout=%u startSeq=%u", + b[2], p, (p>>1)&1, (p>>2)&0x0f, (p>>6)&0x3ff, + b[5]|(b[6]<<8), (b[7]|(b[8]<<8))>>4); + } else if (b[0] == 0x03 && b[1] == 0x02 && len >= 24 + 6) { + // DELBA: param[2-3] = {initiator bit11, TID bits12-15}, reason[4-5]. + u16 dp = b[2] | (b[3] << 8); + __android_log_print(4, "apfpv-scan", "DELBA-IN tid=%u initiator=%u reason=%u", + (dp >> 12) & 0xf, (dp >> 11) & 1, (unsigned)(b[4] | (b[5] << 8))); + } else { + __android_log_print(4, "apfpv-scan", "ACTION-RX cat=%u act=%u", b[0], b[1]); + } + } + if (sub == 0xD && _station && len >= 24 + 3) { + const uint8_t* body = f + 24; // 3-addr mgmt header + if (body[0] == 0x03) { // BlockAck category + if (body[1] == 0x00) { // ADDBA Request from AP -> send Response + _station->handleAddbaRequest(f, len); + } else if (body[1] == 0x01) { // ADDBA Response from AP -> BA session established! + // AP accepted our ADDBA Request. Enable reorder for this TID. + u16 param = body[5] | (body[6] << 8); // BA Parameter Set + u8 tid = (param >> 2) & 0x0f; + u16 status = body[3] | (body[4] << 8); + if (status == 0 && tid < 16) { + _reorder[tid].enable = true; + _reorder[tid].wsize_b = 64; + _reorder[tid].indicate_seq = 0xffff; // accept any start seq + _reorder[tid].pending.clear(); + __android_log_print(4, "apfpv-scan", + "ADDBA Response ACCEPTED tid=%u — A-MPDU enabled!", tid); + } + } + } + } + return; + } + // CONTROL frames (type 1): BlockAckReq (subtype 8) — kernel rtw_process_bar_frame. The AP sends + // a BAR to move the receiver's reorder window past a gap; process it so the SW reorder flushes + // instead of stalling. Frame: FC|Dur|RA(4..9)|TA(10..15)|BAR_ctrl(16..17)|BAR_seq(18..19). + if (((fc >> 2) & 0x3) == 0x1) { + if (((fc >> 4) & 0xF) == 0x8 && len >= 20 && _reorderOn && + std::memcmp(f + 4, _self.data(), 6) == 0) { + uint16_t barctl = f[16] | (f[17] << 8); + uint16_t barseq = f[18] | (f[19] << 8); + processBar((uint8_t)((barctl >> 12) & 0xf), (uint16_t)(barseq >> 4)); + } + return; + } + if (((fc >> 2) & 0x3) != 0x2) return; // data frames only + if (!(fc & 0x0200)) return; // from-DS (AP->STA) + // fprintf removed: blocks RX thread at 65Mbps — use rxd-diag instead + // Accept unicast-to-us OR group-addressed (I/G bit in A1[0]): DHCP OFFER/ACK + ARP are + // broadcast and FPV video may be multicast. Group frames decrypt with the GTK below. + if (!(f[4] & 0x01) && std::memcmp(f + 4, _self.data(), 6) != 0) return; + + // Count data frames arriving, log every 500th + static thread_local uint32_t dataFrames = 0; + static thread_local bool firstData = true; + if (firstData) { + firstData = false; +#if defined(__ANDROID__) + __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", + "1st data frame: fc=0x%04x len=%zu prot=%d a1=%02x:%02x:%02x:%02x:%02x:%02x", + fc, len, (fc&0x4000)?1:0, f[4],f[5],f[6],f[7],f[8],f[9]); +#endif + } + if ((++dataFrames % 500) == 0) { +#if defined(__ANDROID__) + __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", + "data frames: %u total, decryptOk=%u decFail=%u", + dataFrames, decryptOk, _dbgDecFail); +#endif + } + + size_t hdrLen = 24; + if ((fc & 0x0300) == 0x0300) hdrLen += 6; // 4-addr + if (fc & 0x0080) hdrLen += 2; // QoS-data (subtype>=8): 2B QoS Control + // (was 0x8000 = Order flag — wrong; EAPOL + // M1 is QoS-data fc=0x0288, so this was the + // header-offset bug that hid the handshake) + if (len <= hdrLen) return; + // A-MSDU Present = bit 7 of the QoS Control field (the last 2B of the header, present only + // for QoS data). When set, the (decrypted) MSDU body is a chain of [DA|SA|len|sub-MSDU] + // subframes, NOT a bare LLC/SNAP. The greg VTX uses this WITH rtw_ampdu_enable=0 to + // aggregate several packets into one normally-ACKed MPDU — throughput gain without BlockAck. + const bool amsdu = (fc & 0x0080) && (f[hdrLen - 2] & 0x80); + // 802.11 sequence-control (seq is bits 4-15 of the 2B field at offset 22) + QoS TID — for the + // A-MPDU reorder (kernel recv_indicatepkt_reorder). Only meaningful for QoS data. + const uint16_t mpduSeq = (uint16_t)(((f[22] | (f[23] << 8)) >> 4) & 0xfff); + const uint8_t qtid = (fc & 0x0080) ? (uint8_t)(f[hdrLen - 2] & 0x0f) : 0; + + const uint8_t* body = f + hdrLen; size_t bodyLen = len - hdrLen; + // Pre-allocated buffer per thread — avoids heap alloc per packet (5600/s at 65Mbps) + static thread_local std::vector plainBuf(2048); + const uint8_t* llc; size_t llcLen; + if (fc & 0x4000) { // Protected + // Lever C.2: if HW CCMP decrypt is on AND the chip already decrypted this frame + // (descriptor SWDEC=0 -> bdecrypted), skip the SW AES entirely — the body is + // [CCMP hdr 8][plaintext LLC+payload][MIC 8]. This is the lean kernel-style path. + // GATED: env cached once; default OFF so the proven SW path stays active. + static const bool kHwDecrypt = rxHwDecryptOn(); // env or debug.pixelpilot.hwdec — arms HW reorder + compressed-BA; needs RCR_APP framing. + // DIAG: count HW-decrypted vs SW-fallback frames so we can SEE whether the chip is + // actually HW-decrypting (bdecrypted=1) after the SECCFG=0x010c fix. Logged every 4000. + static thread_local uint32_t hwDec = 0, swDec = 0, diagN = 0, grp = 0, uni = 0; + if (kHwDecrypt) { + if (pkt.RxAtrib.bdecrypted) hwDec++; else swDec++; + if (f[4] & 0x01) grp++; else uni++; // A1 group-addressed vs unicast-to-us + if ((++diagN % 4000) == 0) + fprintf(stderr, "[rxd-hwdec] bdecrypted(HW)=%u SW-fallback=%u | group=%u unicast=%u (last4000)\n", + hwDec, swDec, grp, uni), hwDec = swDec = grp = uni = 0; + } + if (kHwDecrypt && pkt.RxAtrib.bdecrypted) { + if (bodyLen <= 16) return; // need CCMP hdr(8) + MIC(8) + llc = body + 8; llcLen = bodyLen - 16; // strip CCMP header + trailing MIC + decryptOk++; + } else { + if (!_wpa || !_wpa->ready()) return; + if (!_wpa->decryptData(f, len, plainBuf)) { + _dbgDecFail++; +#if defined(__ANDROID__) + if ((_dbgDecFail % 200) == 1) + __android_log_print(ANDROID_LOG_WARN, "apfpv-scan", + "CCMP decFail count=%u a1=%02x:%02x fc=0x%04x len=%zu", + _dbgDecFail, f[4],f[5], fc, len); +#endif + return; + } + decryptOk++; + llc = plainBuf.data(); llcLen = plainBuf.size(); + } + } else { llc = body; llcLen = bodyLen; } + + // Deliver ONE MSDU (LLC/SNAP + payload) up the stack. Called once for a normal frame, or + // once per subframe for an A-MSDU. Early `return` = skip THIS MSDU (move to the next). + auto deliverMsdu = [&](const uint8_t* llc, size_t llcLen) { + if (llcLen < 8) return; + static const uint8_t snap[6] = {0xaa,0xaa,0x03,0x00,0x00,0x00}; + if (std::memcmp(llc, snap, 6) != 0) return; + uint16_t ethertype = (llc[6] << 8) | llc[7]; + // EAPOL (0x888E): the WPA2 4-way handshake. MUST route to the supplicant or + // the handshake never completes and the link stalls at Handshaking. + if (ethertype == 0x888E) { +#if defined(__ANDROID__) + __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", + "[rx] EAPOL frame -> supplicant (llcLen=%zu wpa=%d)", llcLen, _wpa?1:0); +#endif + if (_wpa) _wpa->onEapolKey(llc + 8, llcLen - 8); + return; + } + // A-MPDU reorder disabled — AP sends single frames (Block-Ack HW not working). + // The reorder buffer (processReorder) is ready; enable when HW BA works. + #if 0 + if ((fc & 0x0088) == 0x0088 && ethertype == 0x0800 && llcLen >= 28) { + const uint8_t* ip = llc + 8; size_t ipl = llcLen - 8; + if (ipl >= 20 && (ip[0] >> 4) == 4 && ip[9] == 17) { + size_t ihl = (ip[0] & 0x0f) * 4; + if (ipl >= ihl + 8) { + const uint8_t* u = ip + ihl; + if (((u[2] << 8) | u[3]) == 5600) { + processReorder((hdrLen>=26)?(f[24]&0xf):0, + (uint16_t)((f[23]<<4)|(f[22]>>4)), llc, llcLen); + return; + } + } + } + } + #endif + + // ARP responder: reply to "who has ?" so peers keep a fresh entry for us and the + // unicast RTP/SSH stream doesn't stall when their STALE entry needs re-validation. + if (ethertype == 0x0806 && _ourIp && _arpSend && _wpa && llcLen >= 8 + 28) { + const uint8_t* a = llc + 8; // ARP payload + uint32_t tip = (a[24]<<24)|(a[25]<<16)|(a[26]<<8)|a[27]; + if (a[6]==0x00 && a[7]==0x01 && tip == _ourIp) { // a request for OUR IP + uint8_t r[28]; std::memcpy(r, a, 28); + r[6]=0x00; r[7]=0x02; // oper = reply + std::memcpy(r+8, _self.data(), 6); // sender HW = us + std::memcpy(r+14, a+24, 4); // sender IP = our IP + std::memcpy(r+18, a+8, 6); // target HW = requester + std::memcpy(r+24, a+14, 4); // target IP = requester + auto m = _wpa->buildEncryptedData(r, 28, 0x0806); + if (!m.empty()) _arpSend(m); + static uint32_t s_arpN = 0; + if ((++s_arpN % 4) == 1) + __android_log_print(4, "rxd-arp", "answered ARP who-has %u.%u.%u.%u (n=%u)", + (_ourIp>>24)&255,(_ourIp>>16)&255,(_ourIp>>8)&255,_ourIp&255, s_arpN); + } + return; + } + if (ethertype != 0x0800) return; // IPv4 + const uint8_t* ip = llc + 8; size_t ipLen = llcLen - 8; + // DIAG (rxd-ip): log NON-RTP IPv4 arriving from the VTX — the SSH/mavlink RETURN path. If + // SSH/mavlink time out but these appear, the VTX IS replying (issue is app-side/routing); if + // they never appear, the VTX isn't unicasting back to our (static) IP (ARP/route gap). + if (ipLen >= 20 && (ip[0] >> 4) == 4) { + size_t dih = (ip[0] & 0x0f) * 4; + uint16_t ddp = (ipLen >= dih + 4) ? ((ip[dih+2] << 8) | ip[dih+3]) : 0; + if (!(ip[9] == 17 && ddp == 5600)) { // anything but the RTP video + static uint32_t s_nrN = 0; + uint32_t sip = (ip[12]<<24)|(ip[13]<<16)|(ip[14]<<8)|ip[15]; + uint16_t dsp = (ipLen >= dih + 4) ? ((ip[dih] << 8) | ip[dih+1]) : 0; + if ((++s_nrN % 4) == 1) + __android_log_print(4, "rxd-ip", "RX non-RTP IPv4 proto=%u src=%u.%u.%u.%u %u->%u len=%zu", + ip[9], (sip>>24)&255,(sip>>16)&255,(sip>>8)&255,sip&255, dsp, ddp, ipLen); + } + } + // De-dup 802.11 retransmissions of RTP BEFORE any sink: as a station we RX unicast RTP + // and the AP retransmits each frame until ACKed (~6x). This must run ahead of the _onIp + // (TUN) path below — otherwise the duplicates reach the decoder via the OS route. Retries + // reuse the RTP seq, so drop a UDP/5600 packet whose (payload-type, seq) repeats the last. + if (ipLen >= 20 && (ip[0] >> 4) == 4 && ip[9] == 17) { + size_t ihl0 = (ip[0] & 0x0f) * 4; + if (ipLen >= ihl0 + 8 + 4) { + const uint8_t* u = ip + ihl0; + if (((u[2] << 8) | u[3]) == 5600) { + const uint8_t* r = u + 8; + uint8_t pt = r[1] & 0x7f; + uint16_t sq = (uint16_t)((r[2] << 8) | r[3]); + _dbgRx++; + // NOTE: the RTP-seq de-dup loop (128-elem scan/pkt) was REMOVED — it was dead + // code (dropDup=0 every run). The CCMP PN-replay window already drops every + // 802.11 retransmit (same PN) at decrypt time, before this point. One less + // 716k-iter/s linear scan in the hot path. + // RX-seq-gap loss + OUT-OF-ORDER detection. Signed 16-bit delta vs the last seq: + // d > 0 : forward — d-1 packets missing (real loss so far, may fill in later) + // d < 0 : this packet is BEHIND the last = arrived OUT OF ORDER (A-MPDU reorder) + // d == 0 : duplicate (shouldn't reach here; PN-replay drops it earlier) + // Hypothesis under test: the dongle has NO A-MPDU RX reorder (RxDeframe:174 disabled), + // so aggregated frames reach the H265 depacketizer scrambled → NALU reassembly corrupts + // → decoder freeze while bitrate stays steady. Phone-Wi-Fi's HW MAC reorders → smooth. + // If _dbgOoo climbs, that's confirmed and the reorder buffer is the fix. + if (_lastSeqV[pt]) { + int16_t d = (int16_t)(sq - _lastSeq[pt]); + if (d > 1 && d < 2000) _dbgLoss += (d - 1); + else if (d < 0) _dbgOoo++; // arrived behind the last = reordered + // Only advance the high-water seq on forward progress so OOO packets don't + // rewind it (which would make the next in-order packet look like a huge gap). + if (d > 0) { _lastSeq[pt] = sq; } + } else { _lastSeq[pt] = sq; _lastSeqV[pt] = true; } + _lastSeqV[pt] = true; + // Health summary every 120 unique pkts: received / dup-dropped / decrypt-failed / lost / out-of-order. + if ((_dbgRx % 120) == 0) + __android_log_print(ANDROID_LOG_INFO, "rxd-health", + "rx=%d dropDup=%d decFail=%d lost=%d ooo=%d amsdu=%d sub=%d (pt=%u seq=%u) rxrate=%u bw=%u", + _dbgRx, _dbgDrop, _dbgDecFail, _dbgLoss, _dbgOoo, _dbgAmsdu, _dbgAmsduSub, pt, sq, + (unsigned)pkt.RxAtrib.data_rate, (unsigned)pkt.RxAtrib.bw); + // rxrate: 0-3=CCK 1/2/5.5/11, 4-11=OFDM 6..54, 12-27=HT MCS0-15, + // 28+=VHT. Low (<12) => rate-control collapsed (ACK problem); + // high MCS but sparse rx => A-MPDU/Block-Ack aggregation missing. + } + } + } + // GENERAL-IP BRIDGE: hand the IPv4 packet to the TUN sink so SSH / mavlink / any TCP+UDP + // traverses the dongle (a full L3 interface). ⚠️ EXCLUDE the RTP video (UDP/5600): it already + // reaches the decoder via _onRtp below. Bridging it to the TUN too makes the OS RE-DELIVER it + // to the local 5600 socket → EVERY video frame arrives TWICE. That was the "app OSD shows ~2x + // the VTX's actual bitrate (82 vs 43 Mbps)" bug, and the duplicate/out-of-order frames stutter + // the decoder. Phone-Wi-Fi has no TUN bridge, hence it was smooth. Skip 5600 here. + { + size_t ihlB = (ip[0] & 0x0f) * 4; + bool rtp5600 = (ip[9] == 17) && (ipLen >= ihlB + 4) && (((ip[ihlB + 2] << 8) | ip[ihlB + 3]) == 5600); + if (_onIp && !rtp5600) _onIp(ip, ipLen); + } + if (ipLen < 20 || (ip[0] >> 4) != 4 || ip[9] != 17) return; // IPv4/UDP + size_t ihl = (ip[0] & 0x0f) * 4; + if (ipLen < ihl + 8) return; + const uint8_t* udp = ip + ihl; + uint16_t dport = (udp[2] << 8) | udp[3]; + uint16_t udlen = (udp[4] << 8) | udp[5]; + // DHCP: replies (OFFER/ACK) arrive on UDP port 68 (BOOTP client). Forward + // the UDP payload (BOOTP message) to the DHCP state machine so DORA can + // complete. Without this, the client never sees OFFER and DHCP stalls. + if (dport == 68) { + if (_onDhcp && udlen >= 8 && (size_t)udlen <= ipLen - ihl) + _onDhcp(udp + 8, udlen - 8); + return; + } + if (dport != 5600) return; // video port + uint16_t ulen = udlen; + if (ulen < 8 || (size_t)ulen > ipLen - ihl) return; + const uint8_t* rtp = udp + 8; size_t rtpLen = ulen - 8; + if (rtpLen && _onRtp) { + // RTP-SEQUENCE REORDER: deliver the :5600 payload strictly in RTP-seq order so the + // depacketizer (not reorder-tolerant) stops seeing scrambled/complete-but-gapped frames. + // Keyed on the RTP seq (not the 802.11 MPDU seq — that was wrong for this VTX and buffered + // everything). Gated _reorderOn. + if (_reorderOn) { + uint16_t rtpSeq = (uint16_t)((rtp[2] << 8) | rtp[3]); + processReorderRtp(rtpSeq, rtp, rtpLen); + } else { + _onRtp(rtp, rtpLen); + } + fwdFrames++; fwdBytes += rtpLen; + } + // Layer health every 1 second + auto now = std::chrono::steady_clock::now(); + auto elapsed = std::chrono::duration_cast(now - layerClock).count(); + if (elapsed >= 1000) { + float secs = (float)elapsed / 1000.0f; + __android_log_print(4, "rx-layer", + "IN=%d/s FWD=%d/s (%.1f%%) decOK=%d/s | GOODPUT=%.1f Mbps", + (int)(inFrames/secs), (int)(fwdFrames/secs), + (float)fwdFrames*100.f/(float)(inFrames+1), + (int)(decryptOk/secs), + (float)fwdBytes * 8.0f / (secs * 1e6f)); + inFrames=fwdFrames=decryptOk=0; fwdBytes=0; layerClock = now; + } + // (removed: per-frame t0..t3 timing + rxd-diag breakdown — 4 steady_clock::now()/frame of + // pure profiling overhead; the 1s rx-layer GOODPUT line above is enough.) + }; // end deliverMsdu + + if (amsdu) { + // Walk A-MSDU subframes: [DA(6)|SA(6)|len(2)|MSDU(len bytes)|pad to 4B] (no pad on last). + // Each MSDU is its own LLC/SNAP packet — the AP coalesced several into one ACKed MPDU. + _dbgAmsdu++; + size_t off = 0; + while (off + 14 <= llcLen) { + uint16_t sub = (uint16_t)((llc[off + 12] << 8) | llc[off + 13]); + if (sub < 8 || off + 14 + sub > llcLen) break; // truncated/garbage -> stop + _dbgAmsduSub++; + deliverMsdu(llc + off + 14, sub); // MSDU starts with its LLC/SNAP + off = (off + 14 + sub + 3) & ~size_t(3); // next subframe is 4-byte aligned + } + } else { + deliverMsdu(llc, llcLen); // normal single-MSDU frame + } +} + +// Extract UDP/5600 RTP payload from a decrypted MSDU (SNAP + IPv4 + UDP) and emit it. +void RxDeframe::emitReorderRtp(const uint8_t* llc, size_t llcLen) { + if (llcLen < 8 || !_onRtp) return; + const uint8_t* ip = llc + 8; size_t ipl = llcLen - 8; // skip 8B SNAP + if (ipl < 20 || (ip[0] >> 4) != 4 || ip[9] != 17) return; + size_t ihl = (ip[0] & 0x0f) * 4; + if (ipl < ihl + 8) return; + const uint8_t* u = ip + ihl; + if (((u[2] << 8) | u[3]) != 5600) return; + uint16_t udpLen = (u[4] << 8) | u[5]; + if (udpLen < 8) return; + size_t rtpLen = (size_t)udpLen - 8; // UDP len includes the 8B UDP header + if (rtpLen == 0 || rtpLen > ipl - ihl - 8) return; + _onRtp(u + 8, rtpLen); +} + +// Reorder release-timer: how long to hold a gap waiting for a late/retransmitted seq before +// skipping it. Too short = skip a recoverable retransmit (residual loss); too long = latency. +// Env DEVOURER_REORDER_MS to sweep toward kernel parity. +static int64_t reorderTimeoutMs() { + static int64_t v = [] { + if (const char* e = std::getenv("DEVOURER_REORDER_MS")) { int n = std::atoi(e); if (n > 0) return (int64_t)n; } +#if defined(__ANDROID__) + // Live-tunable without a rebuild (adb shell setprop debug.pixelpilot.reorderms N). Added + // to test whether real compressed-BA-style retransmits under accept-BA (DEVOURER_ENABLE_BA + // / debug.pixelpilot.ba=1) need longer than the kernel's 50ms REORDER_WAIT_TIME to arrive — + // under real A-MPDU aggregation the AP has more airtime contention per retransmit cycle + // than the decline-BA/no-aggregation case this constant was originally tuned against. + char v2[PROP_VALUE_MAX] = {0}; + if (__system_property_get("debug.pixelpilot.reorderms", v2) > 0) { + int n = std::atoi(v2); + if (n > 0) return (int64_t)n; + } +#endif + return (int64_t)50; // kernel REORDER_WAIT_TIME (include/rtw_recv.h); bench 60≈50 best + }(); + return v; +} +#define kReorderTimeoutMs reorderTimeoutMs() +static int64_t nowMsSteady() { + return std::chrono::duration_cast( + std::chrono::steady_clock::now().time_since_epoch()).count(); +} + +// RTP-SEQUENCE reorder for the :5600 video flow. Buffers the RTP payload keyed by its 16-bit +// (mod 65536) sequence number and emits strictly in order via _onRtp. Mirrors the proven +// wrap-safe drain of processReorder but in the RTP-seq domain the depacketizer actually needs. +// A release timer (kReorderTimeoutMs) force-flushes a permanently-missing seq so a lost packet +// skips forward instead of stalling the drain forever (and leaking the pending map). +bool RxDeframe::processReorderRtp(uint16_t rtpSeq, const uint8_t* rtp, size_t rtpLen) { + auto& rc = _reorderRtp; + if (rtpLen == 0 || rtpLen > 1500) return false; + int64_t now = nowMsSteady(); + if (!rc.enable) { + rc.enable = true; rc.indicate = rtpSeq; rc.pending.clear(); + rc.lastFlushMs = now; rc.wsize = 256; + } + // signed 16-bit distance in [-32768, 32767] + auto sdiff = [](uint16_t a, uint16_t b) { int d = ((int)a - (int)b) & 0xffff; return d >= 32768 ? d - 65536 : d; }; + auto drain = [&]() { + for (auto it = rc.pending.find(rc.indicate); it != rc.pending.end(); + it = rc.pending.find(rc.indicate)) { + _onRtp(it->second.data(), it->second.size()); + rc.pending.erase(it); + rc.indicate = (uint16_t)(rc.indicate + 1); + rc.lastFlushMs = now; + } + }; + auto lowestPending = [&]() -> uint16_t { + uint16_t best = rc.indicate; int bestd = 0x7fffffff; + for (auto& kv : rc.pending) { int dd = sdiff(kv.first, rc.indicate); + if (dd >= 0 && dd < bestd) { bestd = dd; best = kv.first; } } + return best; + }; + static long cIn = 0, cBuf = 0, cDrain = 0, cDropOld = 0, cFarJump = 0, cFarSkip = 0, cToFlush = 0, cToSkip = 0, cLog = 0; + size_t pendBefore = rc.pending.size(); + + int d = sdiff(rtpSeq, rc.indicate); + if (d < 0) { cDropOld++; return false; } // already passed (dup/retransmit) + if (d >= rc.wsize) { // far ahead: shift window, flush below + uint16_t newInd = (uint16_t)((rtpSeq - rc.wsize + 1) & 0xffff); + cFarJump++; cFarSkip += (long)sdiff(newInd, rc.indicate); + while (rc.indicate != newInd) { + auto it = rc.pending.find(rc.indicate); + if (it != rc.pending.end()) { _onRtp(it->second.data(), it->second.size()); rc.pending.erase(it); } + rc.indicate = (uint16_t)(rc.indicate + 1); + } + rc.lastFlushMs = now; drain(); d = sdiff(rtpSeq, rc.indicate); + } + if (d == 0) { // in order + cIn++; _onRtp(rtp, rtpLen); rc.indicate = (uint16_t)(rc.indicate + 1); rc.lastFlushMs = now; + size_t pb = rc.pending.size(); drain(); cDrain += (long)(pb - rc.pending.size()); + } else { // ahead, in window — buffer + if (rc.pending.count(rtpSeq) == 0) { rc.pending[rtpSeq] = std::vector(rtp, rtp + rtpLen); cBuf++; } + } + // release timer: a gap at indicate_seq persisting past the timeout -> skip the lost seq + if (!rc.pending.empty() && (now - rc.lastFlushMs) > kReorderTimeoutMs) { + uint16_t lp = lowestPending(); + cToFlush++; cToSkip += (long)sdiff(lp, rc.indicate); + rc.indicate = lp; rc.lastFlushMs = now; drain(); + } + (void)pendBefore; + if ((++cLog % 4000) == 0) + __android_log_print(ANDROID_LOG_INFO, "rxd-reo", + "RTP inOrder=%ld buffered=%zu drained(rec)=%ld dropOld=%ld | farJump=%ld farSkip=%ld | toFlush=%ld toSkip=%ld pend=%zu", + cIn, rc.pending.size(), cDrain, cDropOld, cFarJump, cFarSkip, cToFlush, cToSkip, rc.pending.size()); + return true; +} + +// A-MPDU per-TID reorder buffer — faithful port of the kernel's recv_indicatepkt_reorder + +// rtw_reordering_ctrl_timeout_handler (release timer). QoS-data A-MPDU subframes/retransmits +// arrive out-of-order; the depacketizer (ParseRTP) is NOT reorder-tolerant, so out-of-order RTP +// makes it declare complete frames "missing". This delivers strictly in-order within a 64-seq +// window, with a release-timer flush so a permanently-lost seq skips forward instead of stalling. +// 12-bit 802.11 sequence space (mod 4096). kReorderTimeoutMs matches the kernel's ~sortterm. + +bool RxDeframe::processReorder(uint8_t tid, uint16_t seq, const uint8_t* llc, size_t llcLen) { + if (tid >= 16) return false; + auto& rc = _reorder[tid]; + uint16_t sq = seq & 0xfff; + int64_t now = nowMsSteady(); + + // Reorder window. Kernel uses the BA bufsz (64) because its DMA RX arrives ~in-order; our USB + // URB delivery reorders frames in large chunks (measured ~37% buffered), so 64 overruns and + // force-skips recoverable frames. A larger window absorbs the USB out-of-order spread. + static int win = [] { const char* e = std::getenv("DEVOURER_REORDER_WIN"); int n = e ? std::atoi(e) : 0; + return (n >= 16 && n <= 2000) ? n : 64; }(); + if (!rc.enable) { rc.enable = true; rc.indicate_seq = sq; rc.pending.clear(); rc.lastFlushMs = now; rc.wsize_b = (uint16_t)win; } + + // signed 12-bit distance sq - indicate_seq in [-2048, 2047] + auto sdiff = [](uint16_t a, uint16_t b) { int d = ((int)a - (int)b) & 0xfff; return d >= 2048 ? d - 4096 : d; }; + // Wrap-safe: deliver the EXACT next expected seq via find() — NOT begin() (numeric-min of the + // std::map), which returns the wrong entry across the 12-bit seq wrap (~every 0.8s at 120fps). + // The old begin()==indicate_seq test stalled the drain whenever a wrapped seq sorted below + // indicate_seq, so pending piled up until the far-ahead branch fired and SKIPPED recoverable + // frames (the farSkip that made this reorder net-harmful and got it disabled). + auto drain = [&]() { + for (auto it = rc.pending.find(rc.indicate_seq); it != rc.pending.end(); + it = rc.pending.find(rc.indicate_seq)) { + emitReorderRtp(it->second.data(), it->second.size()); + rc.pending.erase(it); + rc.indicate_seq = (rc.indicate_seq + 1) & 0xfff; + rc.lastFlushMs = now; + } + }; + // Wrap-safe lowest buffered seq at/ahead of indicate_seq (min non-negative 12-bit distance). + auto lowestPending = [&]() -> uint16_t { + uint16_t best = rc.indicate_seq; int bestd = 0x7fffffff; + for (auto& kv : rc.pending) { int dd = sdiff(kv.first, rc.indicate_seq); + if (dd >= 0 && dd < bestd) { bestd = dd; best = kv.first; } } + return best; + }; + + // DIAG counters to find WHERE the residual loss comes from. + static long cIn=0, cBuf=0, cDrain=0, cDropOld=0, cFarJump=0, cFarSkip=0, cToFlush=0, cToSkip=0, cLog=0; + size_t pendBefore = rc.pending.size(); + + int d = sdiff(sq, rc.indicate_seq); + if (d < 0) { cDropOld++; return false; } // old / duplicate — already passed + if (d >= (int)rc.wsize_b) { + // Far ahead — kernel advances the window to (sq - wsize + 1), FLUSHING everything below it + // strictly in order first (so nothing is emitted out-of-order), then the frame is in-window. + uint16_t newInd = (uint16_t)((sq - rc.wsize_b + 1) & 0xfff); + cFarJump++; cFarSkip += sdiff(newInd, rc.indicate_seq); // seqs skipped by the window shift + // Flush strictly in order from indicate_seq up to newInd (wrap-safe walk): deliver any + // present, skip the truly-missing. begin()-based iteration was wrong across the wrap. + while (rc.indicate_seq != newInd) { + auto it = rc.pending.find(rc.indicate_seq); + if (it != rc.pending.end()) { emitReorderRtp(it->second.data(), it->second.size()); rc.pending.erase(it); } + rc.indicate_seq = (rc.indicate_seq + 1) & 0xfff; + } + rc.lastFlushMs = now; + drain(); // deliver consecutive from newInd + d = sdiff(sq, rc.indicate_seq); // recompute; now in-window + } + if (d == 0) { // in-order + cIn++; + emitReorderRtp(llc, llcLen); + rc.indicate_seq = (rc.indicate_seq + 1) & 0xfff; + rc.lastFlushMs = now; + size_t pb = rc.pending.size(); drain(); cDrain += (long)(pb - rc.pending.size()); + } else { // ahead, in window — buffer + if (rc.pending.count(sq) == 0) { rc.pending[sq] = std::vector(llc, llc + llcLen); cBuf++; } + } + + // RELEASE TIMER: if a gap at indicate_seq persists past the timeout, skip the (lost) seq — + // advance to the lowest buffered frame and drain. Prevents a permanent-loss stall/freeze. + if (!rc.pending.empty() && (now - rc.lastFlushMs) > kReorderTimeoutMs) { + uint16_t lp = lowestPending(); // wrap-safe true-next buffered (not numeric-min) + cToFlush++; cToSkip += sdiff(lp, rc.indicate_seq); // gap seqs skipped + rc.indicate_seq = lp; + rc.lastFlushMs = now; + drain(); + } + (void)pendBefore; + if ((++cLog % 4000) == 0) + __android_log_print(ANDROID_LOG_INFO, "rxd-reo", + "inOrder=%ld buffered=%ld drained(recovered)=%ld dropOld(dup)=%ld | farJump=%ld farSkip=%ld | toFlush=%ld toSkip=%ld | pend=%zu", + cIn, cBuf, cDrain, cDropOld, cFarJump, cFarSkip, cToFlush, cToSkip, rc.pending.size()); + return true; +} + +// Kernel rtw_process_bar_frame: a BlockAckReq advances the reorder window to start_seq. +void RxDeframe::processBar(uint8_t tid, uint16_t start_seq) { + if (tid >= 16) return; + auto& rc = _reorder[tid]; + if (!rc.enable) return; + uint16_t ss = start_seq & 0xfff; + int64_t now = nowMsSteady(); + // deliver everything below start_seq (in order) then set the window to start_seq + for (auto it = rc.pending.begin(); it != rc.pending.end(); ) { + int d = ((int)it->first - (int)ss) & 0xfff; if (d >= 2048) d -= 4096; + if (d < 0) { emitReorderRtp(it->second.data(), it->second.size()); it = rc.pending.erase(it); } + else break; + } + rc.indicate_seq = ss; + rc.lastFlushMs = now; + while (!rc.pending.empty() && rc.pending.begin()->first == rc.indicate_seq) { + auto it = rc.pending.begin(); + emitReorderRtp(it->second.data(), it->second.size()); + rc.indicate_seq = (rc.indicate_seq + 1) & 0xfff; + rc.pending.erase(it); + } +} + +} // namespace apfpv diff --git a/src/RxDeframe.h b/src/RxDeframe.h new file mode 100644 index 0000000..233b4e1 --- /dev/null +++ b/src/RxDeframe.h @@ -0,0 +1,92 @@ +#pragma once +#include +#include +#include +#include +#include "jaguar1/FrameParser.h" +#include "Wpa2Supplicant.h" +#include "LqFeedback.h" +namespace apfpv { +using Mac = std::array; +class ApfpvStation; // fwd: liveness/deauth notifications for the reconnect supervisor +class RxDeframe { +public: + using OnRtpFn = std::function; + RxDeframe(const Mac& self, const Mac& bssid, Wpa2Supplicant* wpa, LqFeedback* lq, OnRtpFn onRtp); + void onPacket(const Packet& pkt); + void setStation(ApfpvStation* st) { _station = st; } + // DHCP reply hook: received UDP -> port 68 (BOOTP client) forwarded here so + // the DHCP state machine can advance OFFER->REQUEST->ACK->Bound. + using OnDhcpFn = std::function; + void setDhcpSink(OnDhcpFn fn) { _onDhcp = std::move(fn); } + // General-IP sink (the VpnService TUN): the FULL decrypted IPv4 packet, so SSH/any + // TCP+UDP traverses the dongle — not just RTP. The OS then routes it (RTP->5600, SSH->22). + using OnIpFn = std::function; + void setIpSink(OnIpFn fn) { _onIp = std::move(fn); } + // ARP responder: answer "who has ?" so peers (the VTX unicasting RTP video, SSH, + // ...) keep a FRESH ARP entry for us — without it a STALE entry fails to re-validate and + // the unicast stream stops after a frame or two. send() gets the encrypted ARP-reply MPDU. + void setArp(uint32_t ourIp, std::function&)> send) { + _ourIp = ourIp; _arpSend = std::move(send); + } +private: + static int toDbm(uint8_t r); + Mac _self, _bssid; Wpa2Supplicant* _wpa; LqFeedback* _lq; OnRtpFn _onRtp; + OnDhcpFn _onDhcp; OnIpFn _onIp; + uint32_t _ourIp = 0; + std::function&)> _arpSend; + ApfpvStation* _station = nullptr; + // Per-payload-type recent-RTP-seq window for dropping 802.11-retransmit duplicates + // (reordered retransmits need a window, not just the previous seq) + debug counters. + uint16_t _seqHist[128][128] = {}; + bool _seqValid[128][128] = {}; + uint8_t _seqPos[128] = {}; + int _dbgRx = 0, _dbgDrop = 0; + // RX-health instrumentation (logged to logcat tag "rxd-health"): decrypt failures + RTP-seq-gap + // loss, to diagnose dongle jumps/rewinds (loss vs duplication vs decrypt failure). + int _dbgDecFail = 0, _dbgLoss = 0, _dbgOoo = 0; + int _dbgAmsdu = 0, _dbgAmsduSub = 0; // A-MSDU frames seen + total subframes de-aggregated + uint16_t _lastSeq[128] = {}; + bool _lastSeqV[128] = {}; + // Bottleneck diag: cumulative time (ns) spent in onPacket phases per interval + int64_t _diagTotalNs = 0, _diagDecNs = 0, _diagFwdNs = 0; + int _diagPkts = 0; + // A-MPDU RX reorder buffer (kernel: recv_indicatepkt_reorder). + // Per-TID sliding window for in-order delivery of A-MPDU subframes. + // Without this, out-of-order subframes are dropped -> video corruption. + struct ReorderCtl { + bool enable = false; + uint16_t indicate_seq = 0xffff; // next expected seq (mod 4096) + uint16_t wsize_b = 64; // window size in frames (USB needs > kernel's BA bufsz 64) + std::map> pending; // seq -> plaintext frame + int64_t lastFlushMs = 0; // kernel reorder release-timer: force-flush a stuck gap + }; + ReorderCtl _reorder[16]; // one per TID (0-15) + // Process a decrypted QoS-data frame through the reorder buffer for TID `tid`. + // Delivers frames in-order via onRtpFn. Returns true if delivered, false if queued/dropped. + bool processReorder(uint8_t tid, uint16_t seq, const uint8_t* llc, size_t llcLen); + // Kernel rtw_process_bar_frame: on a BlockAckReq, advance the reorder window to start_seq + // (flush everything below it). Keeps the SW reorder from stalling when the AP moves on. + void processBar(uint8_t tid, uint16_t start_seq); + // Emit one MSDU's RTP payload to _onRtp (shared by direct + reorder paths). + void emitReorderRtp(const uint8_t* llc, size_t llcLen); + // RTP-SEQUENCE reorder: the depacketizer consumes UDP/5600 RTP in arrival order, but the + // dongle's USB/A-MPDU delivery hands packets up OUT of RTP-seq order. Re-keying the reorder + // on the 802.11 MPDU seq was WRONG for this VTX (A-MSDU/constant MPDU seq) — it buffered + // ~everything and skipped more than it delivered (rxd-reo: buffered>>inOrder, farSkip>>0), + // which is net-harmful. The unit the depacketizer actually needs in order is the RTP seq, so + // buffer the RTP payload keyed by its 16-bit seq and emit strictly in order. Returns true if + // delivered (in order or recovered), false if queued/old. + bool processReorderRtp(uint16_t rtpSeq, const uint8_t* rtp, size_t rtpLen); + // Per-stream RTP-seq reorder state (mod 65536). One slot: the single :5600 video flow. + struct RtpReorderCtl { + bool enable = false; + uint16_t indicate = 0; // next expected RTP seq + int wsize = 256; // ~4 video frames of slack for the bursty USB delivery + std::map> pending; // rtpSeq -> payload + int64_t lastFlushMs = 0; + }; + RtpReorderCtl _reorderRtp{}; + bool _reorderOn = false; // gated: DEVOURER_REORDER / debug.pixelpilot.reorder +}; +} diff --git a/src/UsbTransport.h b/src/UsbTransport.h index d235614..7ed7761 100644 --- a/src/UsbTransport.h +++ b/src/UsbTransport.h @@ -38,6 +38,8 @@ class UsbTransport final : public IRtlTransport { bool is_usb() const override { return true; } + int reset() override { return libusb_reset_device(_dev_handle); } + uint8_t read8(uint16_t reg) override { return ctrl_read(reg); } uint16_t read16(uint16_t reg) override { return ctrl_read(reg); } uint32_t read32(uint16_t reg) override { return ctrl_read(reg); } diff --git a/src/Wpa2Authenticator.cpp b/src/Wpa2Authenticator.cpp new file mode 100644 index 0000000..d7c148b --- /dev/null +++ b/src/Wpa2Authenticator.cpp @@ -0,0 +1,132 @@ +// ============================================================================ +// Wpa2Authenticator.cpp — AP-side WPA2-PSK 4-way (the mirror of Wpa2Supplicant). +// Sends M1(ANonce) + M3(GTK, KEK-wrapped); verifies the station's M2/M4 MICs. +// ============================================================================ +#include "Wpa2Authenticator.h" +#include "Dot11Frames.h" +#include "crypto/AesCcm.h" +#include +#include + +#ifdef _WIN32 +extern "C" __declspec(dllimport) long __stdcall + BCryptGenRandom(void*, unsigned char*, unsigned long, unsigned long); +#endif +static void ap_secure_rand(uint8_t* b, size_t n) { +#ifdef _WIN32 + if (BCryptGenRandom(nullptr, b, (unsigned long)n, 0x00000002u) == 0) return; +#else + FILE* u = fopen("/dev/urandom","rb"); if (u) { size_t r=fread(b,1,n,u); fclose(u); if (r==n) return; } +#endif + for (size_t i=0;i& pmk, const Mac& ap, const Mac& sta, + const std::array& gtk, uint8_t gtkKeyId) { + _pmk = pmk; _ap = ap; _sta = sta; _gtk = gtk; _gtkKeyId = gtkKeyId; + _state = State::Idle; _replay = 0; +} + +// Identical PRF input to Wpa2Supplicant::derivePtk (min/max MAC, min/max nonce) so keys agree. +void Wpa2Authenticator::derivePtk(const uint8_t* sNonce) { + std::memcpy(_sNonce.data(), sNonce, 32); + uint8_t data[76]; + const Mac& aa = _ap; const Mac& sa = _sta; + bool aaFirst = std::memcmp(aa.data(), sa.data(), 6) < 0; + std::memcpy(data + 0, (aaFirst?aa:sa).data(), 6); + std::memcpy(data + 6, (aaFirst?sa:aa).data(), 6); + bool aFirst = std::memcmp(_aNonce.data(), _sNonce.data(), 32) < 0; + std::memcpy(data + 12, aFirst?_aNonce.data():_sNonce.data(), 32); + std::memcpy(data + 44, aFirst?_sNonce.data():_aNonce.data(), 32); + auto ptk = _c.prf(_pmk.data(), 32, "Pairwise key expansion", data, sizeof(data), 48); + std::memcpy(_kck.data(), ptk.data() + 0, 16); + std::memcpy(_kek.data(), ptk.data() + 16, 16); + std::memcpy(_tk.data(), ptk.data() + 32, 16); +} + +std::vector Wpa2Authenticator::buildM1() { + std::vector kb(99, 0); + kb[0]=0x02; kb[1]=0x03; + uint16_t bl=(uint16_t)(kb.size()-4); kb[2]=bl>>8; kb[3]=bl&0xff; + kb[4]=0x02; // RSN descriptor + uint16_t info=0x008A; kb[5]=info>>8; kb[6]=info&0xff; // ver2 + pairwise + ACK (no MIC) + kb[7]=0x00; kb[8]=0x10; // key length 16 + for(int i=0;i<8;i++) kb[9+i]=(uint8_t)(_replay>>((7-i)*8)); // replay counter (BE) + std::memcpy(kb.data()+17, _aNonce.data(), 32); + return BuildEapolKeyFromAp(_ap, _sta, kb); +} + +std::vector Wpa2Authenticator::buildM3() { + // Key Data = RSN IE (matches the beacon) + GTK KDE, then AES-key-wrapped under the KEK. + std::vector kd; + static const uint8_t rsn[] = {0x30,0x14, 0x01,0x00, 0x00,0x0f,0xac,0x04, + 0x01,0x00, 0x00,0x0f,0xac,0x04, 0x01,0x00, 0x00,0x0f,0xac,0x02, 0x00,0x00}; + kd.insert(kd.end(), rsn, rsn+sizeof(rsn)); // 22 + kd.push_back(0xdd); kd.push_back(22); // GTK KDE: dd len + kd.push_back(0x00); kd.push_back(0x0f); kd.push_back(0xac); kd.push_back(0x01); + kd.push_back(_gtkKeyId & 0x03); kd.push_back(0x00); // keyid | reserved + kd.insert(kd.end(), _gtk.begin(), _gtk.end()); // +24 => 46 + if (kd.size() % 8 != 0) { kd.push_back(0xdd); while (kd.size() % 8) kd.push_back(0x00); } + std::vector wrapped(kd.size()+8); + crypto::aes_key_wrap(_kek.data(), kd.data(), kd.size(), wrapped.data()); + + std::vector kb(99 + wrapped.size(), 0); + kb[0]=0x02; kb[1]=0x03; + uint16_t bl=(uint16_t)(kb.size()-4); kb[2]=bl>>8; kb[3]=bl&0xff; + kb[4]=0x02; + uint16_t info=0x134A; kb[5]=info>>8; kb[6]=info&0xff; // ver2+pairwise+install+ack+mic+secure+enc + kb[7]=0x00; kb[8]=0x10; + for(int i=0;i<8;i++) kb[9+i]=(uint8_t)(_replay>>((7-i)*8)); + std::memcpy(kb.data()+17, _aNonce.data(), 32); + uint16_t kdl=(uint16_t)wrapped.size(); kb[97]=kdl>>8; kb[98]=kdl&0xff; + std::memcpy(kb.data()+99, wrapped.data(), wrapped.size()); + auto mic = _c.eapol_mic(_kck.data(), kb.data(), kb.size()); // MIC over the frame (MIC=0) + std::memcpy(kb.data()+81, mic.data(), 16); + return BuildEapolKeyFromAp(_ap, _sta, kb); +} + +bool Wpa2Authenticator::verifyMic(const uint8_t* body, size_t len) const { + if (len < 97) return false; // need the 16-byte MIC field at [81..96] + std::vector m(body, body+len); + uint8_t rx[16]; std::memcpy(rx, body+81, 16); + std::memset(m.data()+81, 0, 16); + auto calc = _c.eapol_mic(_kck.data(), m.data(), m.size()); + return std::memcmp(calc.data(), rx, 16) == 0; +} + +void Wpa2Authenticator::startHandshake() { + ap_secure_rand(_aNonce.data(), 32); + _replay = 1; + _send(buildM1()); + _state = State::WaitM2; + fprintf(stderr, "[ap-wpa] sent M1 (ANonce) -> WaitM2\n"); +} + +bool Wpa2Authenticator::onEapolKey(const uint8_t* body, size_t len) { + if (len < 95) return false; + uint16_t info = (body[5]<<8)|body[6]; + bool hasMic = info & 0x0100, secure = info & 0x0200; + if (_state == State::WaitM2 && hasMic && !secure) { // M2: SNonce + MIC + derivePtk(body + 17); + if (!verifyMic(body, len)) { fprintf(stderr, "[ap-wpa] M2 MIC mismatch\n"); return false; } + _replay++; // M3 uses the next counter + _send(buildM3()); + _state = State::WaitM4; + fprintf(stderr, "[ap-wpa] M2 ok -> sent M3\n"); + return false; + } + if (_state == State::WaitM4 && hasMic && secure) { // M4: confirm + if (!verifyMic(body, len)) { fprintf(stderr, "[ap-wpa] M4 MIC mismatch\n"); return false; } + _state = State::Done; + fprintf(stderr, "[ap-wpa] M4 ok -> 4-way COMPLETE (station authenticated)\n"); + return true; + } + return false; +} + +} // namespace apfpv diff --git a/src/Wpa2Authenticator.h b/src/Wpa2Authenticator.h new file mode 100644 index 0000000..3ed055d --- /dev/null +++ b/src/Wpa2Authenticator.h @@ -0,0 +1,36 @@ +#pragma once +#include +#include +#include +#include +#include "Wpa2Crypto.h" +namespace apfpv { +using Mac = std::array; +// AP-side WPA2-PSK 4-way handshake (Authenticator) — the mirror of Wpa2Supplicant. One +// instance per associating station. The AP sends M1 (ANonce) + M3 (GTK), and verifies the +// station's M2/M4 MICs. Same crypto (PRF-SHA1 PTK, HMAC-SHA1 MIC) so keys agree with any +// standard supplicant. +class Wpa2Authenticator { +public: + using SendFn = std::function&)>; + Wpa2Authenticator(Crypto c, SendFn send); + void begin(const std::array& pmk, const Mac& ap, const Mac& sta, + const std::array& gtk, uint8_t gtkKeyId); + void startHandshake(); // generate ANonce + send M1 + bool onEapolKey(const uint8_t* body, size_t len); // M2 -> M3, M4 -> Done (returns true on M4) + bool ready() const { return _state == State::Done; } +private: + enum class State { Idle, WaitM2, WaitM4, Done }; + void derivePtk(const uint8_t* sNonce); + std::vector buildM1(); + std::vector buildM3(); + bool verifyMic(const uint8_t* body, size_t len) const; + Crypto _c; SendFn _send; + Mac _ap{}, _sta{}; + std::array _pmk{}, _aNonce{}, _sNonce{}; + std::array _kck{}, _kek{}, _tk{}, _gtk{}; + uint8_t _gtkKeyId = 1; + State _state = State::Idle; + uint64_t _replay = 0; +}; +} diff --git a/src/apfpv/Dot11Frames.cpp b/src/apfpv/Dot11Frames.cpp new file mode 100644 index 0000000..26d0617 --- /dev/null +++ b/src/apfpv/Dot11Frames.cpp @@ -0,0 +1,252 @@ +// ============================================================================ +// Dot11Frames.cpp — 802.11 management + EAPOL frame construction +// ============================================================================ +// Builds the raw frames StationMode injects via RtlJaguarDevice::send_packet(). +// These are TX of management/EAPOL frames — latency-tolerant (no SIFS deadline +// applies to our *sending*; the SIFS deadline is on ACK-ing received unicast, +// which is the hardware's job, not ours). +// +// Frame layout we emit to send_packet(): the device's TX expects a radiotap +// header (devourer prepends/handles the TX descriptor); here we build the +// 802.11 MPDU. Wiring of the radiotap/TX-desc prefix matches what upstream +// devourer already does for monitor injection (see RtlJaguarDevice TX path). +// +// Scope of this file: Auth (open seq1), Assoc-Request (with SSID + rates + +// RSN IE for WPA2), and the EAPOL-Key frame shell for the 4-way handshake. +// ============================================================================ + +#include +#include +#include +#include +#include + +#include "Dot11Frames.h" +namespace apfpv { + +using Mac = std::array; + +// ---- 802.11 frame-control builders ---------------------------------------- +static constexpr uint16_t FC_MGMT = 0x0000; +static constexpr uint16_t FC_SUB_ASSOC_REQ = 0x0000; // mgmt subtype 0 +static constexpr uint16_t FC_SUB_AUTH = 0x00B0; // mgmt subtype 11 (<<4) +static constexpr uint16_t FC_SUB_BEACON = 0x0080; // mgmt subtype 8 (<<4) +static constexpr uint16_t FC_DATA = 0x0008; // type data +static constexpr uint16_t FC_TODS = 0x0100; // to-DS (station->AP) +static constexpr uint16_t FC_FROMDS = 0x0200; // from-DS (AP->station) + +// Generic 802.11 MAC header (24 bytes, 3-address) +static void put_hdr(std::vector& f, uint16_t fc, + const Mac& a1, const Mac& a2, const Mac& a3, + uint16_t seq = 0) { + f.push_back(fc & 0xFF); f.push_back(fc >> 8); // frame control + f.push_back(0); f.push_back(0); // duration (HW fills) + f.insert(f.end(), a1.begin(), a1.end()); // addr1 = RA (dest) + f.insert(f.end(), a2.begin(), a2.end()); // addr2 = TA (us) + f.insert(f.end(), a3.begin(), a3.end()); // addr3 = BSSID + f.push_back((seq << 4) & 0xFF); f.push_back((seq << 4) >> 8); // seq ctrl +} + +static void put_le16(std::vector& f, uint16_t v){ f.push_back(v&0xFF); f.push_back(v>>8); } + +// ---- Auth, open-system, sequence 1 ----------------------------------------- +std::vector BuildAuthOpenSeq1(const Mac& self, const Mac& bssid) { + std::vector f; + put_hdr(f, FC_MGMT | FC_SUB_AUTH, bssid, self, bssid); + put_le16(f, 0); // auth algorithm = Open System + put_le16(f, 1); // auth sequence = 1 + put_le16(f, 0); // status code = 0 + return f; +} + +// ---- Beacon (open, for the VRX EIRP-calibration mode) ---------------------- +// Broadcast beacon so a phone Wi-Fi scan lists the dongle as an AP and can read +// its RSSI (-> EIRP). Open network (no privacy bit): we never associate, this is +// a measurement target only. +// WPA2-PSK/CCMP RSN IE (byte-identical to the station assoc-req). Appended to beacon/probe-resp. +static void appendRsnIe(std::vector& f) { + static const uint8_t rsn[] = { + 0x30,0x14, 0x01,0x00, 0x00,0x0f,0xac,0x04, + 0x01,0x00, 0x00,0x0f,0xac,0x04, 0x01,0x00, 0x00,0x0f,0xac,0x02, 0x00,0x00 }; + f.insert(f.end(), rsn, rsn+sizeof(rsn)); +} + +std::vector BuildBeacon(const Mac& self, const std::string& ssid, uint8_t channel, bool wpa2) { + std::vector f; + static const Mac bcast = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF}; + put_hdr(f, FC_MGMT | FC_SUB_BEACON, bcast, self, self); // addr3=BSSID=self + for (int i = 0; i < 8; ++i) f.push_back(0); // timestamp (driver/HW may fill) + put_le16(f, 100); // beacon interval (100 TU ~= 102.4 ms) + put_le16(f, wpa2 ? 0x0011 : 0x0001); // capability: ESS [+ Privacy when WPA2] + f.push_back(0x00); f.push_back((uint8_t)ssid.size()); // SSID + f.insert(f.end(), ssid.begin(), ssid.end()); + static const uint8_t rates[] = {0x82,0x84,0x8b,0x96,0x0c,0x12,0x18,0x24}; + f.push_back(0x01); f.push_back(sizeof(rates)); // Supported Rates + f.insert(f.end(), rates, rates+sizeof(rates)); + f.push_back(0x03); f.push_back(0x01); f.push_back(channel); // DS Param = channel + if (wpa2) appendRsnIe(f); // RSN IE for WPA2-PSK + return f; +} + +// ---- AP-side responses: Auth (open seq2), Assoc Response, Probe Response ---- +std::vector BuildAuthResp(const Mac& ap, const Mac& sta) { + std::vector f; + put_hdr(f, FC_MGMT | FC_SUB_AUTH, sta, ap, ap); // a1=STA a2=AP a3=BSSID + put_le16(f, 0); put_le16(f, 2); put_le16(f, 0); // open system, seq 2, status success + return f; +} +std::vector BuildAssocResp(const Mac& ap, const Mac& sta, uint16_t aid) { + std::vector f; + put_hdr(f, FC_MGMT | 0x0010, sta, ap, ap); // mgmt subtype 1 = Assoc Response + put_le16(f, 0x0011); // capability (ESS+Privacy) + put_le16(f, 0); // status = success + put_le16(f, 0xC000 | (aid & 0x3FFF)); // AID (two MSBs set per spec) + static const uint8_t rates[] = {0x82,0x84,0x8b,0x96,0x0c,0x12,0x18,0x24}; + f.push_back(0x01); f.push_back(sizeof(rates)); + f.insert(f.end(), rates, rates+sizeof(rates)); + return f; +} +std::vector BuildProbeResp(const Mac& ap, const Mac& sta, const std::string& ssid, + uint8_t channel, bool wpa2) { + std::vector f; + put_hdr(f, FC_MGMT | 0x0050, sta, ap, ap); // mgmt subtype 5 = Probe Response + for (int i = 0; i < 8; ++i) f.push_back(0); + put_le16(f, 100); put_le16(f, wpa2 ? 0x0011 : 0x0001); + f.push_back(0x00); f.push_back((uint8_t)ssid.size()); + f.insert(f.end(), ssid.begin(), ssid.end()); + static const uint8_t rates[] = {0x82,0x84,0x8b,0x96,0x0c,0x12,0x18,0x24}; + f.push_back(0x01); f.push_back(sizeof(rates)); + f.insert(f.end(), rates, rates+sizeof(rates)); + f.push_back(0x03); f.push_back(0x01); f.push_back(channel); + if (wpa2) appendRsnIe(f); + return f; +} + +// ---- Association Request (SSID + supported rates + RSN IE for WPA2) --------- +std::vector BuildAssocRequest(const Mac& self, const Mac& bssid, + const std::string& ssid, uint16_t rsnCaps) { + std::vector f; + put_hdr(f, FC_MGMT | FC_SUB_ASSOC_REQ, bssid, self, bssid); + + put_le16(f, 0x1531); // capability info (match kernel assoc-req: ESS+Privacy+ShortPre/Slot+SpectrumMgmt) + put_le16(f, 3); // listen interval + + // IE: SSID (0) + f.push_back(0x00); f.push_back((uint8_t)ssid.size()); + f.insert(f.end(), ssid.begin(), ssid.end()); + + // IE: Supported Rates (1). On 5GHz, CCK rates (1/2/5.5/11) are INVALID and make the AP + // classify us as a legacy/2.4GHz client → conservative HT-1SS rate control (the MCS2 pin). + // DEVOURER_OFDM_RATES=1 sends the kernel's exact 5GHz OFDM-only set (6/9/12/18/24/36/48/54, + // basic bits on 6/12/24) in a single tag-1 IE with NO Extended-Rates — matching the live + // kernel assoc-req byte-for-byte. Default keeps the 2.4GHz-compatible CCK+OFDM split. + if (std::getenv("DEVOURER_OFDM_RATES")) { + static const uint8_t rates5[] = {0x8c,0x12,0x98,0x24,0xb0,0x48,0x60,0x6c}; + f.push_back(0x01); f.push_back(sizeof(rates5)); + f.insert(f.end(), rates5, rates5+sizeof(rates5)); + } else { + static const uint8_t rates[] = {0x82,0x84,0x8b,0x96,0x0c,0x12,0x18,0x24}; + f.push_back(0x01); f.push_back(sizeof(rates)); + f.insert(f.end(), rates, rates+sizeof(rates)); + // IE: Extended Supported Rates (50) — 24/36/48/54 Mbps. + static const uint8_t extRates[] = {0x30,0x48,0x60,0x6c}; + f.push_back(0x32); f.push_back(sizeof(extRates)); + f.insert(f.end(), extRates, extRates+sizeof(extRates)); + } + + // IE: RSN (48) — WPA2-PSK / CCMP. Required so the AP starts the 4-way HS. + // version 1; group=CCMP; pairwise=CCMP; akm=PSK + static const uint8_t rsn[] = { + 0x01,0x00, // RSN version 1 + 0x00,0x0f,0xac,0x04, // group cipher = CCMP (00-0F-AC-4) + 0x01,0x00, 0x00,0x0f,0xac,0x04, // 1 pairwise = CCMP + 0x01,0x00, 0x00,0x0f,0xac,0x02, // 1 AKM = PSK (00-0F-AC-2) + 0x00,0x00 // RSN capabilities + }; + f.push_back(0x30); f.push_back(sizeof(rsn)); + f.insert(f.end(), rsn, rsn+sizeof(rsn)); + f[f.size()-2] = (uint8_t)(rsnCaps & 0xff); // RSN capabilities (PMF: MFPC b7 / MFPR b6) + f[f.size()-1] = (uint8_t)(rsnCaps >> 8); + + // IEs the kernel's 88XXau assoc-request includes that a modern HT/VHT AP needs + // to ACCEPT the association and start the 4-way. Missing HT Capabilities in + // particular makes the AP reject/mis-associate us and NEVER send EAPOL M1 — our + // exact symptom (reach Handshaking, no M1). Bytes are the kernel's exact values + // for this 8812 (captured via usbmon on a working wpa_supplicant connect). + static const uint8_t tail[] = { + 0xdd,0x07, 0x00,0x50,0xf2,0x02,0x00,0x01,0x00, // WMM Information Element + // HT Capabilities. A-MPDU Params byte was 0x1f = MaxLenExp 3 (64KB) + MinMPDUSpacing 7 + // (16µs) — we advertised 16µs spacing, which makes the AP pad/cap A-MPDUs to us and holds + // throughput at ~30Mbps. 0x13 = MaxLenExp 3 (64KB) + MinMPDUSpacing 4 (2µs): tighter packing + // the 8812 RX handles, so the AP can build deep aggregates (the 30→65 lever). + // HT capinfo byte0 = 0x2c. NOTE (2026-07-14): the LIVE kernel advertises 0x6e (40MHz=1, + // SGI40=1); ours omits both. Advertising kernel-match caps + OMN=40MHz (0x11) + OFDM-only + // 5GHz rates was A/B-tested on the 40MHz OpenIPC VTX and did NOT change the AP's rate pin + // (still HT-1SS MCS2, no aggregation), so it's NOT the rate-control lever. Left at 0x2c / + // OMN 0x12 to avoid regressing 80MHz APs (needs a bandwidth-aware caps rewrite, not a + // hardcode). The self-consistency issue is real but orthogonal to the MCS2 pin. + 0x2d,0x1a, 0x2c,0x19,0x13,0xff,0xff, // HT Capabilities — A-MPDU density 2µs (was 16µs) + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, + 0xbf,0x0c, 0xa2,0x31,0xc0,0x03,0xfa,0xff,0x63,0x03,0xfa,0xff,0x63,0x03, // VHT Caps (12B) + 0xc7,0x01, 0x12, // Operating Mode Notification: 80MHz, 2SS + 0x7f,0x08, 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x40 // Extended Capabilities + }; + f.insert(f.end(), tail, tail+sizeof(tail)); + + return f; +} + +// ---- EAPOL-Key frame shell (for WPA2 4-way handshake; msg 2/4 from us) ------ +// The KCK/KEK come from the PTK (derived in Wpa2Supplicant). This builds the +// 802.11 DATA + LLC/SNAP(0x888E) + EAPOL-Key body; MIC is filled by caller +// after it computes HMAC over the assembled buffer with MIC field zeroed. +std::vector BuildEapolKey(const Mac& self, const Mac& bssid, + const std::vector& keyBody) { + std::vector f; + put_hdr(f, FC_DATA | FC_TODS, bssid, self, bssid); + // LLC/SNAP for EAPOL + static const uint8_t snap[] = {0xaa,0xaa,0x03,0x00,0x00,0x00,0x88,0x8e}; + f.insert(f.end(), snap, snap+sizeof(snap)); + f.insert(f.end(), keyBody.begin(), keyBody.end()); + return f; +} + +// AP-side EAPOL-Key (from-DS): a1=DA=STA, a2=BSSID=AP, a3=SA=AP. For Authenticator M1/M3. +std::vector BuildEapolKeyFromAp(const Mac& ap, const Mac& sta, + const std::vector& keyBody) { + std::vector f; + put_hdr(f, FC_DATA | FC_FROMDS, sta, ap, ap); + static const uint8_t snap[] = {0xaa,0xaa,0x03,0x00,0x00,0x00,0x88,0x8e}; + f.insert(f.end(), snap, snap+sizeof(snap)); + f.insert(f.end(), keyBody.begin(), keyBody.end()); + return f; +} + + +// Negotiated RSN: build the IE from the scanned pairwise/group cipher suites. +std::vector BuildAssocRequest(const Mac& self, const Mac& bssid, + const std::string& ssid, + uint32_t pairwiseCipher, uint32_t groupCipher, + uint16_t rsnCaps) { + // Reuse the base frame, then append an RSN IE with the negotiated ciphers. + std::vector f = BuildAssocRequest(self, bssid, ssid, rsnCaps); + // strip the trailing hardcoded RSN (last sizeof(rsn)+2 bytes) and re-add. + // Simpler: the base already appended a CCMP/PSK RSN; if negotiated == CCMP + // it is identical, so only rebuild when different. + if (pairwiseCipher == 0x000FAC04 && groupCipher == 0x000FAC04) return f; + // remove last RSN IE (0x30 ... ) — find last 0x30 tag + for (size_t i = f.size(); i-- > 0; ) { + if (f[i] == 0x30 && i + 1 < f.size()) { f.resize(i); break; } + } + auto be32 = [&](uint32_t v){ f.push_back(v>>24); f.push_back(v>>16); f.push_back(v>>8); f.push_back(v); }; + f.push_back(0x30); f.push_back(20); // RSN IE, len 20 + f.push_back(0x01); f.push_back(0x00); // version 1 + be32(groupCipher); // group + f.push_back(0x01); f.push_back(0x00); be32(pairwiseCipher); // 1 pairwise + f.push_back(0x01); f.push_back(0x00); be32(0x000FAC02); // 1 AKM = PSK + f.push_back((uint8_t)(rsnCaps & 0xff)); f.push_back((uint8_t)(rsnCaps >> 8)); // RSN caps (PMF) + return f; +} + +} // namespace apfpv diff --git a/src/apfpv/Dot11Frames.h b/src/apfpv/Dot11Frames.h new file mode 100644 index 0000000..a68b4a0 --- /dev/null +++ b/src/apfpv/Dot11Frames.h @@ -0,0 +1,28 @@ +#pragma once +#include +#include +#include +#include +namespace apfpv { +using Mac = std::array; +std::vector BuildAuthOpenSeq1(const Mac& self, const Mac& bssid); +// Open (no-privacy) beacon for the EIRP-calibration "VRX beacon" mode: lets a +// phone Wi-Fi scan see the dongle as an AP so its EIRP can be measured. +std::vector BuildBeacon(const Mac& self, const std::string& ssid, uint8_t channel, + bool wpa2 = false); +// AP-side mgmt responses + AP EAPOL-Key (from-DS) for the WPA2 Authenticator. +std::vector BuildAuthResp(const Mac& ap, const Mac& sta); +std::vector BuildAssocResp(const Mac& ap, const Mac& sta, uint16_t aid); +std::vector BuildProbeResp(const Mac& ap, const Mac& sta, const std::string& ssid, + uint8_t channel, bool wpa2 = false); +std::vector BuildEapolKeyFromAp(const Mac& ap, const Mac& sta, + const std::vector& keyBody); +std::vector BuildAssocRequest(const Mac& self, const Mac& bssid, const std::string& ssid, + uint16_t rsnCaps = 0); +// Negotiated variant: RSN IE reflects the cipher chosen from the AP's beacon. +// rsnCaps carries the RSN capabilities (802.11w MFPC bit7 / MFPR bit6); 0 = no PMF. +std::vector BuildAssocRequest(const Mac& self, const Mac& bssid, const std::string& ssid, + uint32_t pairwiseCipher, uint32_t groupCipher, + uint16_t rsnCaps = 0); +std::vector BuildEapolKey(const Mac& self, const Mac& bssid, const std::vector& keyBody); +} diff --git a/src/apfpv/ScanProbe.cpp b/src/apfpv/ScanProbe.cpp new file mode 100644 index 0000000..734c78d --- /dev/null +++ b/src/apfpv/ScanProbe.cpp @@ -0,0 +1,69 @@ +// ============================================================================ +// ScanProbe.cpp — beacon parser: BSSID + channel + RSN negotiation +// Removes the hardcoded-channel / empty-BSSID / fixed-cipher assumptions. +// ============================================================================ +#include "ScanProbe.h" +#include +namespace apfpv { + +static uint32_t suite(const uint8_t* p){ return (p[0]<<24)|(p[1]<<16)|(p[2]<<8)|p[3]; } + +// Walk a beacon/probe-resp's IEs once, extracting SSID + channel + RSN. Returns +// false if the frame isn't a valid mgmt beacon/probe-resp. ssidOut may be empty +// (hidden SSID). Shared by parseBeacon (target filter) and parseAnyBeacon. +bool ScanProbe::parseAnyBeacon(const uint8_t* f, size_t len, std::string& ssidOut, ApInfo& out) { + if (len < 36) return false; + uint16_t fc = f[0] | (f[1] << 8); + if (((fc >> 2) & 0x3) != 0x0) return false; // mgmt + uint8_t sub = (fc >> 4) & 0xF; + if (sub != 0x8 && sub != 0x5) return false; // beacon(8) or probe-resp(5) + + // addr3 = BSSID; beacon fixed params = 12 bytes (ts8 + interval2 + caps2) + Mac bssid; std::memcpy(bssid.data(), f + 16, 6); + const uint8_t* ie = f + 24 + 12; + size_t ieLen = len - (24 + 12); + + std::string foundSsid; int channel = 0; ApInfo tmp; tmp.bssid = bssid; + size_t i = 0; + while (i + 2 <= ieLen) { + uint8_t id = ie[i], l = ie[i+1]; + if (i + 2 + l > ieLen) break; + const uint8_t* d = ie + i + 2; + if (id == 0) { // SSID + foundSsid.assign((const char*)d, l); + } else if (id == 3 && l >= 1) { // DS param = channel + channel = d[0]; + } else if (id == 48 && l >= 8) { // RSN IE + tmp.rsnPresent = true; + size_t o = 2; // skip version + tmp.groupCipher = suite(d + o); o += 4; + uint16_t pc = d[o] | (d[o+1] << 8); o += 2; + for (int k = 0; k < pc && o + 4 <= l; k++) { tmp.pairwise.push_back(suite(d+o)); o += 4; } + if (o + 2 <= l) { uint16_t ac = d[o] | (d[o+1] << 8); o += 2; + for (int k = 0; k < ac && o + 4 <= l; k++) { tmp.akm.push_back(suite(d+o)); o += 4; } } + } + i += 2 + l; + } + tmp.channel = channel; tmp.found = true; + ssidOut = foundSsid; + out = tmp; + return true; +} + +bool ScanProbe::parseBeacon(const uint8_t* f, size_t len, const std::string& ssid, ApInfo& out) { + std::string found; ApInfo tmp; + if (!parseAnyBeacon(f, len, found, tmp)) return false; + if (found != ssid) return false; + out = tmp; + return true; +} + +uint32_t ScanProbe::chooseCipher(const std::vector& adv) { + // Prefer CCMP (00-0F-AC-04); accept GCMP-256 (00-0F-AC-09) if offered. + for (uint32_t s : adv) if (s == 0x000FAC04) return s; // CCMP-128 + for (uint32_t s : adv) if (s == 0x000FAC09) return s; // GCMP-256 + for (uint32_t s : adv) if (s == 0x000FAC04 || s == 0x000FAC0A) return s; + return adv.empty() ? 0x000FAC04 : adv[0]; // fallback CCMP +} + +} // namespace apfpv diff --git a/src/apfpv/ScanProbe.h b/src/apfpv/ScanProbe.h new file mode 100644 index 0000000..aa7c3aa --- /dev/null +++ b/src/apfpv/ScanProbe.h @@ -0,0 +1,36 @@ +#pragma once +#include +#include +#include +#include +#include +namespace apfpv { +using Mac = std::array; +// Beacon/probe-response scanner: discovers the AP's BSSID, channel, and its +// advertised RSN (cipher/AKM) so we associate with NEGOTIATED parameters +// instead of hardcoded assumptions — security + discovery parity with the +// kernel wpa_supplicant the VRX uses. +struct ApInfo { + Mac bssid{}; + int channel = 0; + bool found = false; + // negotiated RSN, parsed from the beacon's RSN IE: + uint32_t groupCipher = 0; // e.g. 0x000FAC04 (CCMP) + std::vector pairwise; // advertised pairwise ciphers + std::vector akm; // advertised AKM suites (PSK = 0x000FAC02) + bool rsnPresent = false; + int rssi = -127; +}; +class ScanProbe { +public: + // Parse a received beacon/probe-resp frame for the target SSID. Returns true + // and fills 'out' when the SSID matches. Feed RX mgmt frames here during scan. + static bool parseBeacon(const uint8_t* frame, size_t len, const std::string& ssid, ApInfo& out); + // Parse ANY beacon/probe-resp (no target filter) for an all-SSID scan: + // returns true for a valid mgmt beacon with a non-empty SSID and fills + // ssidOut + out (bssid / channel / RSN). Used by ApfpvStation::scanAll. + static bool parseAnyBeacon(const uint8_t* frame, size_t len, std::string& ssidOut, ApInfo& out); + // Pick the strongest pairwise cipher we support from the AP's advertised set. + static uint32_t chooseCipher(const std::vector& advertised); +}; +} diff --git a/src/apfpv/StationMode.cpp b/src/apfpv/StationMode.cpp new file mode 100644 index 0000000..660bc54 --- /dev/null +++ b/src/apfpv/StationMode.cpp @@ -0,0 +1,672 @@ +// ============================================================================ +// StationMode.cpp — station-mode arming + ACK-survival probe (impl) +// Declarations in StationMode.h. The auto-ACK arming sequence is ported from +// the Linux driver hw_var_set_opmode STATION path (see docs/ARMING_SEQUENCE.md): +// MACID -> MSR=STATION(0x02) -> BSSID -> RCR|=RCR_CBSSID_DATA(BIT6) +// All register symbols confirmed in devourer hal/hal_com_reg.h. +// ============================================================================ +#include "StationMode.h" +#include "Dot11Frames.h" +#include "StationTxDesc.h" +#include "jaguar1/PhydmWatchdog.h" +#if defined(__ANDROID__) +#include +#endif +#include "hal_com_reg.h" + +#include +#include +#include +#include +#include +#include +#include +#include "jaguar1/RadioManagementModule.h" +// Platform-agnostic logging (NDK on Android, compat stderr shim on host builds). +#include +#define SMLOG(...) __android_log_print(ANDROID_LOG_INFO, "apfpv-arm", __VA_ARGS__) + +namespace apfpv { + +static constexpr uint8_t HW_STATE_STATION = 0x02; // MSR_INFRA +static constexpr int TXDESC_8812 = 40; + +StationMode::~StationMode() { if (_alivePtr) _alivePtr->store(false); } +StationMode::StationMode(RtlAdapter& dev, RadioManagementModule& rm, SendFrameFn send) + : _dev(dev), _rm(rm), _send(std::move(send)) {} + +void StationMode::arm(const MacAddr& self, const MacAddr& bssid) { +#if defined(__ANDROID__) + __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", + "arm ENTRY: current_channel=%d", (int)_rm.current_channel()); +#endif + // Desensitize the receiver's DIG for the AP's ACTUAL signal strength BEFORE auth. Until link-up + // the DIG sits on monitor bounds [0x1c,0x2a] with the gain near its floor; at close range (a + // strong beacon RSSI) that OVERLOADS the front-end -> false-alarm storm (fa>1000) -> the AP's + // auth-response is never cleanly demodulated ("NO auth-resp" -> TXFAIL_NoAuth reconnect loop). + // Feeding the measured beacon RSSI engages the connected DIG bounds [0x20,0x3e] and snaps IGI to + // clamp(rssi+100, 0x20, 0x3e) — e.g. -20 dBm -> 0x3e (fully desensitized) — so a point-blank AP + // can be received. On fail/disconnect the supervisor restores monitor mode via SetUnlinked(). + { + int apRssi = _lastBeaconRssi.load(); // stable last-heard beacon RSSI (NOT _scanResult, reset per-channel) + if (apRssi < 0) { + // SetLinkRssi internally caps the derived IGI at the kernel's operating point (~0x37) so + // a point-blank signal desensitizes enough to kill the saturation false-alarm storm + // without going deaf (feeding raw -20 dBm would snap IGI to the 0x3e max = no RX). + PhydmWatchdog::SetLinkRssi(apRssi); +#if defined(__ANDROID__) + __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", + "arm: DIG desensitize for beacon RSSI=%ddBm (anti-saturation, IGI capped ~0x37)", apRssi); +#endif + } + } + // (0) **SEQUENCE FIX — IQK on the OPERATING channel.** Faithful to the kernel + // join order (rtw_mlme_ext.c join_cmd_hdl): HW_VAR_DO_IQK(TRUE) -> set_channel_ + // bwmode(final channel) so the IQK runs on the channel we will actually use, + // -> MSR=STATION -> issue_auth. The devourer only ran the IQK during init on + // the init/scan channel; the IQK output (per-channel TX/RX IQ calibration) is + // then WRONG for the AP's channel, so the auth radiates mis-calibrated and the + // AP can't decode it. Re-arm + re-run the channel-set here, on the channel the + // scan settled on, immediately before auth. (TXPAUSE that the IQK/LCK leave is + // cleared by step (4c) below, which runs after this.) + if (!std::getenv("DEVOURER_SKIP_JOIN_IQK")) { + // The async RX is drained by the caller (runProbe) for the ENTIRE arm, so the + // IQK/LCK cal AND the MAC/MSR/RCR/BSSID writes below all run sequentially with + // the device quiet — like the kernel. The OLD code paused RX only around the IQK + // here and RESUMED mid-arm, so the RX-filter writes that follow still raced the + // RX daemon -> corrupted filter -> auth-response dropped. Whole-arm serialization + // measured 3/5 vs 1/5 (DEVOURER_NO_PAUSE_CAL disables the runProbe-level pause). + _rm.ArmIQKOnNextChannelSet(); + // 40 MHz REQUIRES the HT40 prime-channel offset (LOWER/UPPER): with 0 (DONT_CARE) the + // chip's subcarrier mapping is undefined ("SCMapping: DONOT CARE Mode Setting") and RX of + // the AP's primary-channel frames is garbage — 40 MHz "connected but barely any frames". + uint8_t off40 = (_connectWidth == CHANNEL_WIDTH_40) + ? _rm.prime_offset_40mhz(_rm.current_channel()) : 0; + _rm.set_channel_bwmode(_rm.current_channel(), off40, _connectWidth); // 20 or 40, via setConnectWidth + SMLOG("arm: channel=%d offset=%d width=%s", (int)_rm.current_channel(), (int)off40, + _connectWidth == CHANNEL_WIDTH_80 ? "80MHz" : _connectWidth == CHANNEL_WIDTH_40 ? "40MHz" : "20MHz"); +#if defined(__ANDROID__) + __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", + "arm set bandwidth: ch=%d off=%d width=%s", + (int)_rm.current_channel(), (int)off40, + _connectWidth == CHANNEL_WIDTH_80 ? "80MHz" : _connectWidth == CHANNEL_WIDTH_40 ? "40MHz" : "20MHz"); +#endif + } + // (1) own MAC -> REG_MACID + for (int i = 0; i < 4; ++i) _dev.rtw_write8(REG_MACID + i, self.b[i]); + _dev.rtw_write16(REG_MACID + 4, (uint16_t)(self.b[4] | (self.b[5] << 8))); + // (2) MSR -> STATION before auth. Faithful to the kernel's start_clnt_join, + // which calls Set_MSR(WIFI_FW_STATION_STATE)=0x02 BEFORE start_clnt_auth/ + // issue_auth (rtw_mlme_ext.c). With MSR=STATION the AP's BSSID is the chip's + // own network, so the HW MAC filters/auto-ACKs the AP during the auth/assoc + // handshake. (The earlier "kernel auths in NO_LINK" theory was wrong — the + // kernel source shows STATION pre-auth.) DEVOURER_AUTH_NOLINK forces the old + // NO_LINK behaviour for A/B bisection. + if (!std::getenv("DEVOURER_SKIP_MSR")) { + uint8_t netType = std::getenv("DEVOURER_AUTH_NOLINK") ? 0x00 : HW_STATE_STATION; + uint8_t msr = (uint8_t)((_dev.rtw_read8(MSR) & 0x0C) | netType); + _dev.rtw_write8(MSR, msr); + } + // (3) AP BSSID -> REG_BSSID + for (int i = 0; i < 4; ++i) _dev.rtw_write8(REG_BSSID + i, bssid.b[i]); + _dev.rtw_write16(REG_BSSID + 4, (uint16_t)(bssid.b[4] | (bssid.b[5] << 8))); + // (4) RCR: station receive config. CRITICAL FIX (usbmon diff vs kernel 88XXau): + // the previous value (0x000000ce) omitted RCR_AMF, so the HW MAC dropped EVERY + // management frame — the AP's auth/assoc RESPONSES never reached the driver + // (only beacons slipped through via CBSSID_BCN). It also dropped the + // APP_PHYST/APPFCS bits that define the RX buffer layout the FrameParser + // expects (same as monitor mode), so the few frames that did arrive misparsed. + // This now matches the kernel station RCR exactly (0xf40060ce) and the + // devourer's own HalModule init RCR: + // RCR_AMF accept management frames (auth/assoc/deauth responses) + // RCR_APP_PHYST PHY status prepended -> parser/RSSI (monitor-mode layout) + // RCR_APPFCS FCS appended -> parser frame length + // FORCEACK HW auto-ACKs unicast addressed to us (AP keeps the link) + if (!std::getenv("DEVOURER_SKIP_RCR")) { + // RCR_ADF (accept DATA frames) is ESSENTIAL: the WPA 4-way handshake + // (EAPOL-Key M1..M4) rides in DATA frames, not management. Without it the HW + // MAC drops EAPOL M1 -> onEapolKey is never called -> the 4-way times out -> + // FailAuth. The kernel's 8812a RCR (rtl8812a_hal_init.c:4163) sets RCR_ADF; + // RCR_ACF (control) matches its connect-mode RCR. This was THE 4-way bug. + uint32_t rcr = RCR_APM | RCR_AM | RCR_AB | RCR_ADF | RCR_ACF | + RCR_CBSSID_DATA | RCR_CBSSID_BCN | + RCR_APP_ICV | RCR_AMF | RCR_HTC_LOC_CTRL | RCR_APP_MIC | + RCR_APP_PHYST_RXFF | RCR_APPFCS | FORCEACK | + // RCR_VHT_DACK (BIT26)=1: reply to VHT single-MPDU data with a normal + // ACK (kernel default), not a BlockAck. With it 0 the AP got no ACK and + // retransmitted every frame ~5× (measured). See RadioManagementModule. + RCR_VHT_DACK; + _dev.rtw_write32(REG_RCR, rcr); + // CRITICAL: accept ALL mgmt frame subtypes during arm phase. + // Without this, auth/assoc responses are HW-filtered. + _dev.rtw_write16(REG_RXFLTMAP0, 0xFFFF); + _dev.rtw_write16(REG_RXFLTMAP2, 0xFFFF); + } + // (4a) Enable HW TSF update for the STA. Faithful to the kernel's + // rtw_iface_enable_tsf_update -> rtw_hal_set_tsf_update(1) at join, which clears + // DIS_TSF_UDT (BIT4) of REG_BCN_CTRL (0x550) so the HW syncs its TSF timer to the + // AP's beacons/probe-resp (needed for TBTT/PS keepalive — else the AP drops us + // after assoc). The devourer's monitor init left REG_BCN_CTRL = DIS_TSF_UDT + // (HalModule). The kernel gates this on RCR_CBSSID_BCN, which we set above. + // DEVOURER_SKIP_TSF disables for A/B. + if (!std::getenv("DEVOURER_SKIP_TSF")) { + _dev.rtw_write8(0x0550, (uint8_t)(_dev.rtw_read8(0x0550) & ~0x10)); + } + // (4b) Join-time MAC timing the kernel 88XXau re-tunes on association but the + // devourer's init left at monitor/default values (usbmon diff). Most important: + // our VO/mgmt EDCA had CWmax=1023 (ECWmax 0xa) vs the kernel's 7 (0x3) — a huge + // random backoff before every management TX. Port the kernel's exact values so + // the auth/assoc TX is scheduled like the kernel's. Env-gated for A/B. + if (!std::getenv("DEVOURER_SKIP_EDCA")) { + _dev.rtw_write32(0x0500, 0x002f3222); // REG_EDCA_VO_PARAM (mgmt/voice) + _dev.rtw_write32(0x0504, 0x005e4322); // REG_EDCA_VI_PARAM + _dev.rtw_write32(0x0508, 0x005ea42b); // REG_EDCA_BE_PARAM + _dev.rtw_write32(0x050c, 0x0000a44f); // REG_EDCA_BK_PARAM + _dev.rtw_write8(0x0607, 0x07); // REG_RRSR byte3 (response-rate ctrl) + } + // (4c) **THE TX-SILENCE FIX.** The IQK (Iqk8812a::ConfigureMac, run on every + // channel-set) writes REG_TXPAUSE (0x522) = 0x3f to PAUSE all 6 TX queues during + // calibration, but the devourer's IQK port NEVER restores it to 0 (the kernel's + // _PHY_ReloadMACRegisters does). usbmon diff of the kernel's full init vs ours: + // kernel TXPAUSE=0x00, devourer=0x3f. With all queues paused the chip ACCEPTS + // the bulk-OUT into the TX FIFO ("70 bytes OK") but never transmits it -> the + // auth/assoc never radiates and the AP never replies. Clear it here (and it is + // the proper place: after the last channel-set/IQK, before the first TX). + if (!std::getenv("DEVOURER_SKIP_TXPAUSE_CLR")) { + _dev.rtw_write8(0x0522, 0x00); // REG_TXPAUSE — release all TX queues + SMLOG("TXPAUSE(0x522) cleared to 0x%02x", _dev.rtw_read8(0x0522)); + } + // **THE TX-RADIATION FIX (RFE / RF front-end).** usbmon diff of the kernel's + // full init vs ours: the kernel sets REG_RFE_CTRL 0xcb8/0xeb8 = 0x42825000 + // (routes the external PA + the antenna TX/RX switch for the TX path); the + // devourer left them at 0x00000000. Result: the MAC transmits the frame + // (TXPKT_EMPTY drains, "OK 70 bytes") but the signal never reaches the antenna + // — the AP hears nothing — while RX keeps working through the LNA path. This + // is the real cause of the "on-air silence". Also restore the TX-path AGC + // 0xc1c/0xe1c to the kernel's value. Env-gated for A/B. + if (std::getenv("DEVOURER_TRY_RFE")) { // opt-in: a lone 0xcb8 write breaks RX; + // needs the full RFE-config sequence. + // 0xcb8 is PAGED: Page C = rA_RFE_Jaguar (RF front-end / PA+antenna TR + // switch), Page C1 = rOFDM0_TxCoeff6 (a TX-IQK coefficient). Iqk8812a leaves + // the BB in Page C1 and writes 0xcb8=0 there, so the RFE is never set to the + // operational value AND a naive write here would corrupt the TX-IQK coeff. + // Select Page C, then set RFE to the kernel's 8812 value (0x42825000) — our + // 8814-derived PHY table wrongly used 0x00500000 and the IQK zeroed it. + _dev.phy_set_bb_reg(0x82c, 0x80000000, 0x0); // BB Page C (bit31=0) + _dev.rtw_write32(0x0cb8, 0x42825000); // rA_RFE_Jaguar + _dev.rtw_write32(0x0eb8, 0x42825000); // rB_RFE_Jaguar + SMLOG("RFE set (Page C) 0xcb8/0xeb8=0x42825000"); + } + // NOTE: the firmware "connected" H2C (MACID_CFG 0x40 / MEDIA_STATUS 0x01 / + // RSSI 0x42) is deliberately NOT sent here. The kernel sends it only AFTER the + // assoc response — see becomeStation(). Sending it pre-auth told the firmware + // that MACID 0 was already a connected peer. DEVOURER_AUTH_STA reverts to the + // old (pre-auth-H2C) behaviour for A/B. + if (std::getenv("DEVOURER_AUTH_STA") && !std::getenv("DEVOURER_SKIP_H2C")) + sendStationH2C(/*macid=*/0, bssid); + _armed = true; +} + +// Post-association transition: NOW the station is associated, so switch the chip +// to STATION network type and register the AP as a firmware peer (MACID/RA + +// media-status + RSSI H2C). This is the kernel's order — do it only after the +// assoc response, never before auth. +void StationMode::becomeStation(const MacAddr& bssid) { + if (!std::getenv("DEVOURER_SKIP_MSR")) { + uint8_t msr = (uint8_t)((_dev.rtw_read8(MSR) & 0x0C) | HW_STATE_STATION); + _dev.rtw_write8(MSR, msr); + } + // Switch the RX filter from monitor (promiscuous, no ACK) to real-station (our-MAC + FORCEACK) + // so the chip HW-ACKs the AP's unicast RTP — without this the AP retransmits every frame and the + // link is choppy. Env-gated for A/B (DEVOURER_NO_STATION_ACK keeps the old monitor RX filter). + // Windows sequence: H2C 0x34 (FW join) fires BEFORE RCR is set. The firmware + // needs to enter offload mode before the station RX filter activates. + if (!std::getenv("DEVOURER_SKIP_H2C")) sendStationH2C(/*macid=*/0, bssid); + if (!std::getenv("DEVOURER_NO_STATION_ACK")) _rm.SetStationRxFilter(); + SMLOG("becomeStation: MSR->STATION + connected H2C sent"); +} + +// Port of the Linux 88XXau station-association H2C firmware sequence, captured by +// usbmon-diffing a working wpa_supplicant association to OpenIPC against the +// devourer's arm(). The register arming above (MACID/MSR/BSSID/RCR) is NOT enough: +// the firmware must also have the AP registered as a *peer* (MACID) with a rate +// table + RSSI, or the chip's TX scheduler cannot pick a rate for it and the +// bulk-OUT TX FIFO wedges after the first frame (the "first TX OK, then +// -7/timeout" station-mode symptom). Each command is the elementID + params the +// kernel writes to the HMEBOX mailbox (primary box 0x1D0 + ext box 0x1F0). +// +// Individual commands are env-gated so each can be isolated on the native probe: +// DEVOURER_SKIP_MACIDCFG / DEVOURER_SKIP_MEDIASTATUS / DEVOURER_SKIP_RSSI. +void StationMode::sendStationH2C(uint8_t macid, const MacAddr& bssid) { + (void)bssid; + // H2C_MACID_CFG / PHYDM_H2C_RA_MASK (0x40): rate-adaptation + aggregation config. + // Kernel PHYDM format (phydm_rainfo.c:phydm_ra_h2c): 7-byte payload with macid, + // rate_id, SGI, init_ra_lv, bw_mode, VHT-enable, LDPC, dis_ra, dis_pt, + 32-bit mask. + // The old simplified format (macid, 0xac=dis_ra=1, ...) told firmware to DISABLE RA + // (and implicitly Block-Ack) for this macid. The PHYDM format arms the full engine. + // rate_id = PHYDM_ARFR0_AC_2SS = 9 (VHT 2SS 5GHz initial rate). + // bw_mode = 2 (80MHz), VHT=1, LDPC=1, dis_ra=0, dis_pt=0, SGI=1, init_ra_lv=0. + // ra_mask = 0x3FFFFFFF (OFDM + HT-MCS 1/2SS + VHT 1SS partial, generous initial mask). + // Register-based H2C (vendor format). PKT H2C (32-byte rtw88 format) was + // tested, did not change BACAM_CMD=0 — the v52.14 firmware binary only + // accepts the register-based (HMEBOX ElementID + buffer) format. + bool h2cRa = true; + if (!std::getenv("DEVOURER_SKIP_MACIDCFG")) { + // RA H2C (peer rate-adaptation config). Kernel usbmon payload = 00 a9 12 10 f0 ff ff + // (init_ra_lv=1, LDPC=0, ra_mask=0xfffff010). Matched init_ra_lv/LDPC to the kernel, but + // the aggressive VHT ra_mask is OPT-IN (DEVOURER_RA_KERNEL): forcing the top VHT-2SS rates + // for OUR uplink ACKs/BAs is only safe if our uplink is as strong as the AP's downlink; + // when it isn't, the FW picks an unreliable TX rate and the BlockAcks are lost (measured: + // 65Mbps run degraded with it on). The conservative mask is the safe default; the RA H2C + // did NOT change the AP's downlink bw=0/20MHz anyway — that's gated on the HW BlockAck. + uint8_t rate_id = 9; uint8_t init_ra_lv = 1; uint8_t sgi = 1; + uint8_t bw_mode = 2; uint8_t vht_en = 1; uint8_t ldpc_en = 0; + uint8_t dis_pt = 0; uint8_t dis_ra = 0; + // Kernel-exact ra_mask (0xfffff010): OFDM-6M fallback (bit4) + ALL HT/VHT rates incl the + // top VHT-2SS MCS8/9 (bits 30/31). Our old 0x3FFFFFFF included invalid 5GHz CCK (bits 0-3) + // AND dropped the top VHT bits — the wrong peer profile for the FW. Affects our TX-data + // rate only (the HW BlockAck radiates at a basic rate), so it's safe for RX video. + // DEVOURER_RA_CONSERVATIVE falls back to the old mask if a weak uplink needs it. + // ★ KERNEL-VERBATIM MACID_CFG (reconstructed from the kernel's live usbmon H2C box: + // payload = [00, 89, 1a, 00, 80, ff, ff]). Our prior "kernel-exact" values were WRONG — + // init_ra_lv=1 (s/b 0 -> byte1 0xA9 vs kernel 0x89), byte2 0x12 (s/b 0x1a, kernel sets + // bit3), ra_mask 0xfffff010 (s/b 0xffff8000). The firmware uses this per-peer profile to + // pick the rate for the compressed BA it auto-transmits; a wrong profile sends the BA at a + // rate the AP misses -> AP shrinks its A-MPDU + DELBAs -> the ~28Mbps cap. DEVOURER_RA_OLD + // restores the prior (wrong) values for A/B. + uint8_t macidCfg[7]; + if (std::getenv("DEVOURER_RA_OLD")) { + uint32_t ra_mask = std::getenv("DEVOURER_RA_CONSERVATIVE") ? 0x3FFFFFFF : 0xfffff010; + macidCfg[0] = macid; + macidCfg[1] = (uint8_t)((rate_id & 0x1f) | ((init_ra_lv & 0x3) << 5) | (sgi << 7)); + macidCfg[2] = (uint8_t)(bw_mode | (ldpc_en << 2) | (vht_en << 4) | (dis_pt << 6) | (dis_ra << 7)); + macidCfg[3] = (uint8_t)(ra_mask & 0xff); macidCfg[4] = (uint8_t)((ra_mask >> 8) & 0xff); + macidCfg[5] = (uint8_t)((ra_mask >> 16) & 0xff); macidCfg[6] = (uint8_t)((ra_mask >> 24) & 0xff); + } else { + macidCfg[0] = macid; // macid 0 (AP peer) + macidCfg[1] = 0x89; // rate_id=9, init_ra_lv=0, sgi=1 + macidCfg[2] = 0x1a; // bw80 + VHT + bit3 + macidCfg[3] = 0x00; macidCfg[4] = 0x80; macidCfg[5] = 0xff; macidCfg[6] = 0xff; // ra_mask 0xffff8000 + } + h2cRa = _dev.fillH2CCmd(0x40, 7, macidCfg); + } + bool h2cMs = true; + // The kernel does NOT send H2C 0x01 (media-status) for the 8812 station (usbmon: 0 writes; + // MSR is set via REG_CR instead). We sent byte0=0x21 — an extra command that may park the FW + // peer state wrong. Default OFF now; DEVOURER_MEDIASTATUS re-enables for A/B. + if (std::getenv("DEVOURER_MEDIASTATUS")) { + uint8_t msrParm[3] = { 0x21, macid, macid }; + h2cMs = _dev.fillH2CCmd(0x01, 3, msrParm); + } + bool h2cRssi = true; + if (!std::getenv("DEVOURER_SKIP_RSSI")) { + // Kernel-verbatim H2C_RSSI_SETTING: payload [macid, 0x00, rssi] (3 bytes); rssi ~0x2e at + // -54dBm. Was [macid,0,0x39,0x06] (4 bytes, static, wrong). DEVOURER_RA_OLD keeps the old. + if (std::getenv("DEVOURER_RA_OLD")) { + uint8_t rssiSet[4] = { macid, 0x00, 0x39, 0x06 }; + h2cRssi = _dev.fillH2CCmd(0x42, 4, rssiSet); + } else { + uint8_t rssiSet[3] = { macid, 0x00, 0x2e }; + h2cRssi = _dev.fillH2CCmd(0x42, 3, rssiSet); + } + } + SMLOG("station H2C: MACID_CFG=%d MEDIA_STATUS=%d RSSI=%d", + h2cRa ? 1 : 0, h2cMs ? 1 : 0, h2cRssi ? 1 : 0); + // Hal_PatchwithJaguar_8812 (kernel: hal_com.c:4355-4363, called after + // H2C_MEDIA_STATUS_RPT for RTL8812 in station mode). Configures VHT LSIG + // length and BW indication for non-Jaguar AP peers. Without this the chip + // may mis-handle VHT A-MPDU aggregates. PLATFORM-AGNOSTIC: these are plain + // USB register writes — was incorrectly gated #ifdef __ANDROID__, which + // skipped the VHT A-MPDU patch on native Windows/Linux hosts and let the AP + // DELBA the session (no compressed BA for the aggregate). Apply everywhere. + // rVhtlen_Use_Lsig_Jaguar = 0x8c3 (BB register), 0x3F for non-Jaguar AP + _dev.rtw_write8(0x8c3, 0x3F); + // REG_TCR (0x0604) + 3 = 0x0607: set bits 0,1,2 for VHT A-MPDU timing + uint8_t tcr3 = _dev.rtw_read8(0x0607); + _dev.rtw_write8(0x0607, (uint8_t)(tcr3 | 0x07)); // BIT0|BIT1|BIT2 + // rBWIndication_Jaguar (0x834) + 3 = 0x837: clear BIT2 for non-Jaguar AP + uint8_t bw3 = _dev.rtw_read8(0x837); + _dev.rtw_write8(0x837, (uint8_t)(bw3 & ~0x04)); // ~BIT2 +} + +// Management-frame TX rate, BAND-AWARE. Faithful to the kernel +// init_mlme_ext_priv_value (rtw_mlme_ext.c): a 5 GHz channel (ch>14) has no CCK +// PHY, so auth/assoc MUST go at OFDM-6M (DESC_RATE6M=0x04); 2.4 GHz uses the +// robust CCK-1M (DESC_RATE1M=0x00). Sending CCK-1M on 5 GHz is un-transmittable — +// the frame drains from the TX FIFO ("OK 70 bytes") but never radiates, which was +// our 5 GHz on-air silence. DEVOURER_MGMT_RATE overrides for A/B. +static uint8_t mgmtTxRate(int channel) { + if (const char* r = std::getenv("DEVOURER_MGMT_RATE")) + return (uint8_t)strtoul(r, nullptr, 0); + return (channel > 14) ? 0x04 : 0x00; // OFDM-6M (5 GHz) | CCK-1M (2.4 GHz) +} +// Kernel 88XXau auth/assoc TX descriptor uses MACID=1 + RAID=8 (usbmon diff), +// NOT MACID=0/RAID=0. MACID 0 is special on Jaguar; the TX scheduler treats a +// real AP peer as a non-zero MACID. Env-overridable for A/B bisection. +static uint8_t mgmtMacId() { + if (const char* m = std::getenv("DEVOURER_MGMT_MACID")) + return (uint8_t)strtoul(m, nullptr, 0); + return 1; +} +// Rate-id for the auth/assoc TX descriptor, BAND-AWARE — matches the kernel +// rtw_get_mgntframe_raid: non-CCK (5 GHz) => RATEID_IDX_G(7), 11B (2.4 GHz) => +// RATEID_IDX_B(8). Pairs with mgmtTxRate(). DEVOURER_MGMT_RAID overrides for A/B. +static uint8_t mgmtRaid(int channel) { + if (const char* r = std::getenv("DEVOURER_MGMT_RAID")) + return (uint8_t)strtoul(r, nullptr, 0); + return (channel > 14) ? 7 : 8; // RATEID_IDX_G (5 GHz) | RATEID_IDX_B (2.4 GHz) +} +// Diagnostic: send auth/assoc as BMC=1 (broadcast TX desc — the monitor path that +// is PROVEN to radiate) while still addressed to the AP. If the AP then replies, +// the BMC=0 unicast TX path (ACK-wait/retry) was the non-radiating culprit. +static apfpv::StationFrameKind mgmtKind() { + if (std::getenv("DEVOURER_AUTH_BMC")) + return apfpv::StationFrameKind::BroadcastMgmt; + return apfpv::StationFrameKind::Mgmt; +} + +bool StationMode::sendAuthOpenSeq1(const MacAddr& self, const MacAddr& bssid) { + Mac s, b; std::copy(self.b.begin(), self.b.end(), s.begin()); + std::copy(bssid.b.begin(), bssid.b.end(), b.begin()); + auto mpdu = BuildAuthOpenSeq1(s, b); + std::vector frame(TXDESC_8812 + mpdu.size(), 0); + std::memcpy(frame.data() + TXDESC_8812, mpdu.data(), mpdu.size()); + FillStationTxDesc(frame.data(), (uint16_t)mpdu.size(), TXDESC_8812, + mgmtMacId(), mgmtKind(), + mgmtRaid(_rm.current_channel()), mgmtTxRate(_rm.current_channel())); + if (std::getenv("DEVOURER_LOG_AUTHTX")) { + std::fprintf(stderr, "[authtx] mpdu %zuB:", mpdu.size()); + for (size_t i = 0; i < mpdu.size(); ++i) std::fprintf(stderr, " %02x", mpdu[i]); + std::fprintf(stderr, "\n"); + } + return _send(frame); +} + +bool StationMode::sendAssocRequest(const MacAddr& self, const MacAddr& bssid, const char* ssid) { + Mac s, b; std::copy(self.b.begin(), self.b.end(), s.begin()); + std::copy(bssid.b.begin(), bssid.b.end(), b.begin()); + uint16_t rsnCaps = (uint16_t)((_pmf >= 1 ? 0x0080 : 0) | (_pmf >= 2 ? 0x0040 : 0)); // MFPC|MFPR + auto mpdu = BuildAssocRequest(s, b, std::string(ssid), _pairwise, _group, rsnCaps); + std::vector frame(TXDESC_8812 + mpdu.size(), 0); + std::memcpy(frame.data() + TXDESC_8812, mpdu.data(), mpdu.size()); + FillStationTxDesc(frame.data(), (uint16_t)mpdu.size(), TXDESC_8812, + mgmtMacId(), mgmtKind(), + mgmtRaid(_rm.current_channel()), mgmtTxRate(_rm.current_channel())); + return _send(frame); +} + +static inline uint8_t fc_type(uint16_t fc) { return (fc >> 2) & 0x3; } +static inline uint8_t fc_subtype(uint16_t fc) { return (fc >> 4) & 0xF; } + +void StationMode::onMgmtFrame(uint16_t fc, const MacAddr& a1, const MacAddr& a2) { + if (fc_type(fc) != 0x0) return; // mgmt only + uint8_t sub = fc_subtype(fc); + if (sub != 0x8) { + SMLOG("rx mgmt subtype 0x%X", sub); // log non-beacon mgmt (auth/assoc/deauth) + if (std::getenv("DEVOURER_LOG_MGMT")) + std::fprintf(stderr, "[mgmt-rx] sub=0x%X a1=%02x:%02x:%02x:%02x:%02x:%02x " + "a2=%02x:%02x:%02x:%02x:%02x:%02x\n", sub, + a1.b[0],a1.b[1],a1.b[2],a1.b[3],a1.b[4],a1.b[5], + a2.b[0],a2.b[1],a2.b[2],a2.b[3],a2.b[4],a2.b[5]); + } + // Only believe a response actually addressed to us (a1=self) FROM our AP (a2=bssid). + // Overheard auth/assoc frames between other stations share the subtype and would + // otherwise false-trigger the join (confirmed root cause of the bogus "Associating"). + if (!(a1.b == _expectSelf.b && a2.b == _expectBssid.b)) { + if (std::getenv("DEVOURER_LOG_MGMT")) + std::fprintf(stderr, "[mgmt-rx] ^ ignored (not addressed to us from our AP)\n"); + return; + } + switch (sub) { + case 0xB: _gotAuthResp = true; break; // Auth + case 0x1: _gotAssocOk = true; break; // Assoc-Resp + case 0xC: case 0xA: _gotDeauth = true; break; // Deauth/Disassoc + default: break; + } +} + +StationMode::Result +StationMode::runProbe(const MacAddr& self, const MacAddr& bssid, + const char* ssid, int holdSeconds) { + using namespace std::chrono; + _expectSelf = self; _expectBssid = bssid; // onMgmtFrame matches a1==self && a2==bssid + // DAEMON-vs-DIRECT-CALL FIX: the IQK/LCK inside arm() does dozens of register reads + // (control transfers). With the async RX worker thread live, those control reads + // serialize behind the pending bulk-IN URBs on vhci/WinUSB (same root cause as the + // bulk-OUT wedge) -> the IQK ready-poll is slowed/garbled -> the cal converges only + // ~2/5. Drain the RX pool for the cal so its control I/O is clean, like the kernel + // (which runs cal with no userspace RX thread contending). Resume re-arms RX so the + // auth-response that follows is still caught. + bool pauseForCal = !std::getenv("DEVOURER_NO_PAUSE_CAL"); + { + // RAII, scoped to just the cal: arm()/RunIQK() below do raw register I/O + // (rtw_read32/write32) that CAN throw (control-transfer failure, replug mid-cal). + // The old code called resumeAsyncRx() only after arm() returned normally, so a + // throw left the async RX pool paused AND _rxQuiesce stuck true forever — the next + // connect's stopAsyncRx() then waits on an event thread that's permanently yielding + // (per pauseAsyncRx's _rxQuiesce contract), times out, and frees URBs libusb still + // owns -> UAF/destroyed-mutex SIGABRT. Guarantee resumeAsyncRx() runs on every exit + // path instead — scoped to this block so RX resumes here, same as before, not at + // runProbe's return (which would leave RX paused through the auth-response wait). + struct ResumeGuard { + RtlAdapter& dev; bool active; + ~ResumeGuard() { if (active) dev.resumeAsyncRx(); } + } resumeGuard{_dev, pauseForCal}; + if (pauseForCal) _dev.pauseAsyncRx(); + arm(self, bssid); + // IQK CONVERGENCE-VERIFIED RETRY: the path-A TX I/Q cal converges only ~2/5 of the + // time (worse under USB latency, e.g. a VM's emulated xHCI), and when it falls back + // to the IQK-default matrix (TXiqcX=0x200, TXiqcY=0x000) the auth radiates + // mis-modulated and the AP can't decode it -> no auth-resp -> stuck Authenticating. + // Read the post-cal path-A TX-IQC and re-run the IQK (RX still paused for clean + // control I/O) until it converges off the default, up to a bounded number of tries. + auto readTxIqcX = [&]() -> uint32_t { + try { + uint32_t pc = _dev.rtw_read32(0x082c); + _dev.rtw_write32(0x082c, pc | 0x80000000u); // Page C1 -> TX-IQC matrix + uint32_t x = _dev.rtw_read32(0x0cd4) & 0x7ff; + _dev.rtw_write32(0x082c, pc); + return x; + } catch (...) { return 0x200; } + }; + int iqkTries = 6; + if (const char* e = std::getenv("DEVOURER_IQK_RETRIES")) iqkTries = atoi(e); + uint32_t iqcx0 = readTxIqcX(); + fprintf(stderr, "[iqk-verify] post-arm TXiqcX=0x%03x (0x200=default/not-converged)\n", iqcx0); + for (int it = 0; it < iqkTries && readTxIqcX() == 0x200; ++it) { + fprintf(stderr, "[iqk-retry] TX-cal did not converge, re-running IQK (try %d)\n", it + 1); + try { _rm.RunIQK(); } catch (...) {} + fprintf(stderr, "[iqk-retry] -> TXiqcX=0x%03x\n", readTxIqcX()); + } + } // ResumeGuard fires resumeAsyncRx() here — same position as the original explicit call. + // ACK-DRIVEN VERIFY: read back the RX-filter state the chip holds after arm(). The auth + // radiates every run (FIFO=4095) yet the AP reply is missed in silent runs — a raced + // RCR/MSR/BSSID write would silently drop that reply. Compare silent vs connecting. + try { + uint32_t rcr=_dev.rtw_read32(0x608); uint8_t msr=_dev.rtw_read8(0x102); + uint8_t b0=_dev.rtw_read8(0x618),b1=_dev.rtw_read8(0x619),b2=_dev.rtw_read8(0x61a), + b3=_dev.rtw_read8(0x61b),b4=_dev.rtw_read8(0x61c),b5=_dev.rtw_read8(0x61d); + uint8_t m0=_dev.rtw_read8(0x610),m1=_dev.rtw_read8(0x611),m2=_dev.rtw_read8(0x612), + m3=_dev.rtw_read8(0x613),m4=_dev.rtw_read8(0x614),m5=_dev.rtw_read8(0x615); + fprintf(stderr,"[arm-verify] RCR=0x%08x MSR=0x%02x BSSID=%02x:%02x:%02x:%02x:%02x:%02x " + "MACID=%02x:%02x:%02x:%02x:%02x:%02x SELF=%02x:%02x:%02x:%02x:%02x:%02x\n", + rcr,msr,b0,b1,b2,b3,b4,b5,m0,m1,m2,m3,m4,m5, + self.b[0],self.b[1],self.b[2],self.b[3],self.b[4],self.b[5]); + } catch (...) {} + SMLOG("arm: self %02x:%02x:%02x:%02x:%02x:%02x -> bssid %02x:%02x:%02x:%02x:%02x:%02x", + self.b[0],self.b[1],self.b[2],self.b[3],self.b[4],self.b[5], + bssid.b[0],bssid.b[1],bssid.b[2],bssid.b[3],bssid.b[4],bssid.b[5]); + if (!_armed) return Result::Error; + // CRITICAL TX-POWER FIX: arm()'s channel-set runs PHY_SetTxPowerLevel8812, which resets + // power to the EEPROM by-rate index (~0 here = on-air SILENCE). Any earlier SetTxPower + // (ApfpvStation connect-path) is therefore overridden, so the auth keyed the PA at zero + // power and the AP's hostapd NEVER heard it (proven via AP-side capture), while the + // beacon path radiated at -52 dBm because its SetTxPower is the last power write. + // Re-apply a solid uniform index AFTER the channel-set so the auth actually radiates. + { uint8_t p = 40; if (const char* e = std::getenv("DEVOURER_TX_PWR_IDX")) p = (uint8_t)atoi(e); + try { _rm.SetTxPower(p); } catch (...) {} } + // DEAUTH-BEFORE-AUTH: clear any stale association the AP still holds for our MAC from a + // prior (possibly incomplete) attempt. Without it the AP ignores our re-auth (it still + // thinks we're associated) and we stall at Authenticating on alternate back-to-back + // connects. This is exactly what wpa_supplicant sends on (re)connect. + if (!std::getenv("DEVOURER_SKIP_DEAUTH")) { + uint8_t m[26] = {0xC0,0x00, 0x00,0x00}; // FC=mgmt/deauth, duration + std::memcpy(m+4, bssid.b.data(), 6); // a1 = AP + std::memcpy(m+10, self.b.data(), 6); // a2 = self + std::memcpy(m+16, bssid.b.data(), 6); // a3 = AP + m[24]=0x03; m[25]=0x00; // reason 3 (STA leaving) + std::vector df(TXDESC_8812 + sizeof(m), 0); + std::memcpy(df.data()+TXDESC_8812, m, sizeof(m)); + FillStationTxDesc(df.data(), (uint16_t)sizeof(m), TXDESC_8812, mgmtMacId(), mgmtKind(), + mgmtRaid(_rm.current_channel()), mgmtTxRate(_rm.current_channel())); + _send(df); + std::this_thread::sleep_for(milliseconds(60)); // let the AP drop the old STA + } + if (_onPhase) _onPhase(1); // -> Authenticating + bool sent = sendAuthOpenSeq1(self, bssid); + // RF-front-end + TX-path state at the auth TX (fires every arm, sent or not) to + // catch the "drains FIFO but nothing radiates" silence: RFE (antenna TR switch, + // BB Page C1 via 0x82c bit31), ANT sel, TXPAUSE, CCA, BB path-A TX enable. + try { + uint32_t pc = _dev.rtw_read32(0x082c); + _dev.rtw_write32(0x082c, pc & ~0x80000000u); + uint32_t rfeA = _dev.rtw_read32(0x0cb8); + _dev.rtw_write32(0x082c, pc | 0x80000000u); // Page C1 for TX-IQC matrix + uint32_t txY = _dev.rtw_read32(0x0ccc) & 0x7ff, txX = _dev.rtw_read32(0x0cd4) & 0x7ff; + _dev.rtw_write32(0x082c, pc); + // TXiqcX=0x200,TXiqcY=0x0 = IQK-default (path-A TX-cal did NOT converge this arm). + fprintf(stderr, "[auth-tx-rf] sent=%d CR=0x%04x TXPAUSE=0x%02x ANT808=0x%02x CCA838=0x%08x RFEcb8=0x%08x TXiqcX=0x%03x TXiqcY=0x%03x BBtxA=0x%08x\n", + (int)sent, _dev.rtw_read16(0x0100), _dev.rtw_read8(0x0522), _dev.rtw_read8(0x0808), + _dev.rtw_read32(0x0838), rfeA, txX, txY, _dev.rtw_read32(0x0c04)); + } catch (...) { fprintf(stderr, "[auth-tx-rf] register read THREW\n"); } + // A single sent=0 (bulk-OUT flicker) used to hard-Error here, killing the arm before the + // retransmit loop below ever ran. Instead retry the send a few times immediately; the OUT + // path recovers on the next attempt. Only if EVERY attempt fails do we bail (and then as + // TXFAIL_NoAuthResp, which the supervisor retries cleanly — not the hard Error that the + // scan/arm loop treats as fatal). + if (!sent) { + for (int i = 0; i < 5 && !sent; ++i) { + std::this_thread::sleep_for(milliseconds(60)); + sent = sendAuthOpenSeq1(self, bssid); + } + SMLOG("auth-req initial sent=0 -> retried, now sent=%d", sent ? 1 : 0); + } + SMLOG("auth-req tx sent=%d, waiting 3s for auth-resp...", sent ? 1 : 0); + + auto t0 = steady_clock::now(); + auto lastTx = t0; + int retx = 0; + while (!_gotAuthResp && !_gotDeauth && steady_clock::now() - t0 < seconds(3)) { + std::this_thread::sleep_for(milliseconds(50)); + // Retransmit the auth like the kernel MLME (it retries ~3x at ~200ms). If our + // single auth-req flickered off-air on this arm, another frame may radiate and + // reach the AP. Cheap, kernel-faithful, and directly tests per-frame TX flicker. + if (!_gotAuthResp && retx < 8 && + steady_clock::now() - lastTx > milliseconds(300)) { + sendAuthOpenSeq1(self, bssid); + lastTx = steady_clock::now(); + ++retx; + } + } + if (!_gotAuthResp) { + try { + fprintf(stderr, "[auth-fail] CR=0x%04x TXPKT_EMPTY=0x%04x HISR=0x%08x FIFOPAGE0x200=0x%08x REG0x10c=0x%04x\n", + _dev.rtw_read16(0x0100), _dev.rtw_read16(0x041a), _dev.rtw_read32(0x00b4), + _dev.rtw_read32(0x0200), _dev.rtw_read16(0x010c)); + // RF front-end state — RFE (antenna TR switch, BB Page C1 via 0x82c bit31), + // antenna sel, TXPAUSE, CCA. 0 RFE = "drains FIFO but nothing radiates". + uint32_t pc = _dev.rtw_read32(0x082c); + _dev.rtw_write32(0x082c, pc & ~0x80000000u); + uint32_t rfeA = _dev.rtw_read32(0x0cb8), rfeB = _dev.rtw_read32(0x0eb8); + _dev.rtw_write32(0x082c, pc); + fprintf(stderr, "[auth-fail-rf] TXPAUSE0x522=0x%02x ANT808=0x%02x CCA838=0x%08x RFEcb8=0x%08x RFEeb8=0x%08x BBtxA_c04=0x%08x\n", + _dev.rtw_read8(0x0522), _dev.rtw_read8(0x0808), _dev.rtw_read32(0x0838), + rfeA, rfeB, _dev.rtw_read32(0x0c04)); + } catch (...) { fprintf(stderr, "[auth-fail] chip register read THREW -> whole USB wedged (not just bulk-OUT)\n"); } + SMLOG("NO auth-resp (deauth=%d)", _gotDeauth.load()); return Result::TXFAIL_NoAuthResp; + } + SMLOG("got auth-resp! sending assoc-req..."); + + if (_onPhase) _onPhase(2); // -> Associating + std::this_thread::sleep_for(milliseconds(100)); + if (!sendAssocRequest(self, bssid, ssid)) return Result::Error; + + auto t1 = steady_clock::now(); + while (!_gotAssocOk && !_gotDeauth && steady_clock::now() - t1 < seconds(3)) + std::this_thread::sleep_for(milliseconds(50)); + if (_gotDeauth) return Result::NOGO_Deauthed; + if (!_gotAssocOk) return Result::NOGO_NoAssocResp; + + // Associated! NOW become a connected station (MSR->STATION + connected H2C), + // matching the kernel's post-assoc order. The 4-way handshake + data follow. + becomeStation(bssid); + + auto h0 = steady_clock::now(); + while (steady_clock::now() - h0 < seconds(holdSeconds)) { + if (_gotDeauth) return Result::NOGO_Deauthed; + std::this_thread::sleep_for(milliseconds(100)); + } + return Result::GO_LinkHeld; +} + + +// ---- beacon scan: discover BSSID + channel + RSN (security/discovery parity) - +void StationMode::onScanFrame(const uint8_t* frame, size_t len) { + // RX thread. _scanSsid/_scanResult are also touched by scanForSsid on the scan + // thread — snapshot the SSID under the lock (a torn std::string read during a + // concurrent reassign crashes parseBeacon's compare), parse off-lock, then + // update the result under the lock. + try { + std::string ssid; + { std::lock_guard lk(_scanMtx); ssid = _scanSsid; } + ApInfo info; + if (ScanProbe::parseBeacon(frame, len, ssid, info)) { + std::lock_guard lk(_scanMtx); + if (!_scanResult.found || info.rssi > _scanResult.rssi) _scanResult = info; + if (info.rssi > -127) _lastBeaconRssi.store(info.rssi); // stable across per-channel resets (for arm DIG) + } + } catch (...) { /* garbled frame at high rate */ } +} + +ApInfo StationMode::scanForSsid(const char* ssid, int channelHint, int perChannelMs) { + using namespace std::chrono; + // Restore MONITOR-mode DIG bounds (sensitive) for the scan. arm() switches DIG to connected + // bounds (desensitized) for the strong-signal auth via SetLinkRssi, and on a FAILED arm the + // reconnect loop comes straight back here with _s_linked still TRUE — leaving the scan + // desensitized so it can't hear a weaker/normal-range AP -> FAIL_NO_AP loop. Reset each scan. + PhydmWatchdog::SetUnlinked(); + { std::lock_guard lk(_scanMtx); _scanSsid = ssid; _scanResult = ApInfo{}; } + // Order = likelihood for a LEGAL APFPV link first, then catch-all: + // 1) hint (the configured/last APFPV channel) + // 2) 5.2 GHz UNII-1 (36/40/44/48) — the legal DE 200 mW @ 20 MHz APFPV band + // (PSD cap 10 mW/MHz x 20 MHz = 200 mW). A compliant VTX lives here. + // 3) the rest of 5 GHz (UNII-2A/2C DFS, UNII-3 149-165) + common 2.4 GHz, + // so we can still find a test hotspot/router parked off the legal band. + int channels[] = { channelHint, + 36, 40, 44, 48, // UNII-1 — legal DE 200 mW APFPV + 1, 6, 11, // 2.4 GHz common (test hotspots) + 52, 56, 60, 64, // UNII-2A (DFS) + 149, 153, 157, 161, 165 }; // UNII-3 + for (int ch : channels) { + if (ch <= 0) continue; + { std::lock_guard lk(_scanMtx); _scanResult = ApInfo{}; } // reset per channel — don't carry a bleed match over + _rm.set_channel_bwmode((uint8_t)ch, 0, CHANNEL_WIDTH_20); // tune radio (devourer API) + auto t0 = steady_clock::now(); + while (steady_clock::now() - t0 < milliseconds(perChannelMs)) { + // Accept only a CLEAN on-channel reception: the beacon's DS-param + // channel must equal the tuned channel. This rejects 2.4GHz adjacent- + // channel bleed (hearing a ch6 AP while tuned to ch1) that otherwise + // returns the WRONG channel -> we arm off-channel -> auth never ACKs. + { + std::lock_guard lk(_scanMtx); + if (_scanResult.found && (_scanResult.channel == ch || _scanResult.channel == 0)) { + _scanResult.channel = ch; return _scanResult; + } + } + std::this_thread::sleep_for(milliseconds(20)); + } + } + { std::lock_guard lk(_scanMtx); return _scanResult; } // .found == false if not seen +} + +} // namespace apfpv \ No newline at end of file diff --git a/src/apfpv/StationMode.h b/src/apfpv/StationMode.h new file mode 100644 index 0000000..7336cc8 --- /dev/null +++ b/src/apfpv/StationMode.h @@ -0,0 +1,64 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include "RtlAdapter.h" +#include "jaguar1/RadioManagementModule.h" +#include "ScanProbe.h" +namespace apfpv { +struct MacAddr { std::array b; }; +class StationMode { +public: + using SendFrameFn = std::function&)>; + StationMode(RtlAdapter& dev, RadioManagementModule& rm, SendFrameFn send); + ~StationMode(); + void arm(const MacAddr& self, const MacAddr& bssid); + bool sendAuthOpenSeq1(const MacAddr& self, const MacAddr& bssid); + bool sendAssocRequest(const MacAddr& self, const MacAddr& bssid, const char* ssid); + // Beacon scan: sweep channel hint + 5GHz UNII-1 (36..48), collect beacons, + // return the matching AP (BSSID/channel/RSN). Channel set on the radio. + ApInfo scanForSsid(const char* ssid, int channelHint, int perChannelMs); + // Cipher negotiated from the AP's beacon, used to build the assoc RSN IE. + void setNegotiatedCipher(uint32_t pairwise, uint32_t group) { _pairwise = pairwise; _group = group; } + void setPmf(int p) { _pmf = p; } // 0=off 1=capable 2=required (RSN caps in assoc-req) + enum class Result { GO_LinkHeld, NOGO_Deauthed, NOGO_NoAssocResp, TXFAIL_NoAuthResp, Error }; + Result runProbe(const MacAddr& self, const MacAddr& bssid, const char* ssid, int holdSeconds = 10); + void onMgmtFrame(uint16_t fc, const MacAddr& a1, const MacAddr& a2); + // Feed raw mgmt frames during scan (beacon parsing). + void onScanFrame(const uint8_t* frame, size_t len); + // Optional UI funnel hook: runProbe reports 1=authenticating, 2=associating. + void setPhaseCb(std::function cb) { _onPhase = std::move(cb); } + // Port of the kernel station-association H2C firmware sequence (MACID_CFG / + // MEDIA_STATUS_RPT / RSSI_SETTING). Registers the AP as a firmware peer. + void sendStationH2C(uint8_t macid, const MacAddr& bssid); + // Post-assoc transition: MSR->STATION + connected H2C (kernel order). + void becomeStation(const MacAddr& bssid); + // Connect-time channel width (20/40); default 20 MHz (legal DE). Fed from ApfpvStation::Params. + void setConnectWidth(ChannelWidth_t w) { _connectWidth = w; } +private: + std::function _onPhase; + RtlAdapter& _dev; RadioManagementModule& _rm; SendFrameFn _send; + bool _armed = false; + uint32_t _pairwise = 0x000FAC04, _group = 0x000FAC04; // default CCMP + int _pmf = 0; // 802.11w mode (RSN caps) + ChannelWidth_t _connectWidth = CHANNEL_WIDTH_20; // connect-time bandwidth (20/40), settable + std::string _scanSsid; ApInfo _scanResult; + // Last matched-beacon RSSI (dBm), kept STABLE across the per-channel _scanResult resets so arm() + // can feed it to the DIG for anti-saturation. _scanResult.rssi is reset to -127 per channel, so + // arm() must NOT read that; it reads this instead. 0 = no beacon heard yet. + std::atomic _lastBeaconRssi{0}; + std::mutex _scanMtx; +public: + std::atomic _alive{true}; + std::shared_ptr> _alivePtr; // TOCTOU-safe: set by dispatch lambda, survives destruction + std::atomic _gotAuthResp{false}, _gotAssocOk{false}, _gotDeauth{false}; + // The self MAC + AP BSSID we are joining. onMgmtFrame MUST match a1==self && a2==bssid + // before believing an auth/assoc-resp — otherwise overheard frames from OTHER stations + // on a shared/congested channel false-trigger the join (confirmed: a ch1 auth between + // two unrelated devices flipped _gotAuthResp and produced a bogus "Associating"). + MacAddr _expectSelf{}, _expectBssid{}; +}; +} diff --git a/src/apfpv/StationTxDesc.cpp b/src/apfpv/StationTxDesc.cpp new file mode 100644 index 0000000..5b161bf --- /dev/null +++ b/src/apfpv/StationTxDesc.cpp @@ -0,0 +1,70 @@ +// ============================================================================ +// StationTxDesc.cpp — unicast station TX descriptor (impl) +// KEY FIX vs devourer monitor path: BMC=0 (unicast, enables ACK handshake). +// Reuses devourer's working HWSEQ/retry/rate/checksum machinery. +// Declarations in StationTxDesc.h. +// ============================================================================ +#include "basic_types.h" +#include "jaguar1/FrameParser.h" +#include "StationTxDesc.h" +#include +#if defined(__ANDROID__) +#include +#endif + +namespace apfpv { + +// Firmware-RA uplink toggle. ON by default. Disable with DEVOURER_NO_FW_RA (host) or, on Android +// where env isn't settable, `setprop debug.pixelpilot.fwra 0`. Disabling reverts DATA frames to a +// fixed low uplink rate → the AP stops aggregating (declines-BA clean path) → no A-MPDU PN-replay +// loss (smoother for low-bitrate video; loses the VHT throughput headroom). +static bool fwRaEnabled() { + if (std::getenv("DEVOURER_NO_FW_RA") != nullptr) return false; +#if defined(__ANDROID__) + char v[PROP_VALUE_MAX] = {0}; + if (__system_property_get("debug.pixelpilot.fwra", v) > 0 && v[0] == '0') return false; +#endif + return true; +} + +void FillStationTxDesc(uint8_t* txdesc, uint16_t payloadLen, uint8_t descOffset, + uint8_t macId, StationFrameKind kind, uint8_t rateId, uint8_t txRate) { + SET_TX_DESC_PKT_SIZE_8812(txdesc, payloadLen); + SET_TX_DESC_OFFSET_8812(txdesc, descOffset); + SET_TX_DESC_FIRST_SEG_8812(txdesc, 1); + SET_TX_DESC_LAST_SEG_8812(txdesc, 1); + SET_TX_DESC_OWN_8812(txdesc, 1); + // Broadcast mgmt (beacon) must set BMC=1 so the chip does NOT wait for an ACK + // and retry — otherwise the frame never radiates. Unicast station frames keep + // BMC=0 (the ACK-handshake fix). + // Beacon + broadcast mgmt are BMC (no ACK wait / no retry). Beacon additionally goes to the + // BEACON queue (QSEL 0x10) so that in AP/master mode (MSR=AP + ENSWBCN) the MAC actually + // radiates it at TBTT — the normal mgmt queue (0x12) is NOT transmitted once MSR=AP. + bool bmc = (kind == StationFrameKind::BroadcastMgmt || kind == StationFrameKind::Beacon); + SET_TX_DESC_BMC_8812(txdesc, bmc ? 1 : 0); + SET_TX_DESC_MACID_8812(txdesc, macId); + SET_TX_DESC_QUEUE_SEL_8812(txdesc, + (kind == StationFrameKind::Beacon) ? 0x10 : + (kind == StationFrameKind::Mgmt || kind == StationFrameKind::BroadcastMgmt) ? 0x12 : 0x00); + SET_TX_DESC_SEC_TYPE_8812(txdesc, 0); // SW-CCMP / cleartext mgmt + SET_TX_DESC_HWSEQ_EN_8812(txdesc, 1); + SET_TX_DESC_RATE_ID_8812(txdesc, rateId); + // ⭐ Uplink rate control — THE VHT lever (validated on the VM kernel-diff rig 2026-07-14). + // mgmt/EAPOL/handshake use fixed USE_RATE=1 (reliable — they must not ride an un-converged + // firmware RA and fail to associate). DATA frames use USE_RATE=0 = firmware rate-adaptation + // (per-macid RA, ramps with link quality), matching the kernel which NEVER pins a fixed + // uplink rate. Root cause of the old ~22Mbps ceiling: the RTL8812 AP mirrors our uplink + // rate onto its downlink RA, so a fixed-low (6Mbps) uplink pinned us at HT-1SS-MCS2 with NO + // aggregation. Firmware-RA uplink → AP ramps to VHT-2SS/80MHz AND auto-emits the compressed + // BlockAck (A-MPDU 99.8%): 22 → 48-53 Mbps (2.2-2.4x), the wall 10+ prior sessions couldn't + // pass. DEFAULT ON for data; DEVOURER_NO_FW_RA reverts to fixed-rate for A/B / if an AP + // dislikes it. (HW-decrypt + BA-accept are NOT required — SW decrypt handles the A-MPDU.) + bool fwRa = (kind == StationFrameKind::CcmpData) && fwRaEnabled(); + SET_TX_DESC_USE_RATE_8812(txdesc, fwRa ? 0 : 1); + if (!fwRa) SET_TX_DESC_TX_RATE_8812(txdesc, txRate); + SET_TX_DESC_RETRY_LIMIT_ENABLE_8812(txdesc, bmc ? 0 : 1); // no retry for broadcast + SET_TX_DESC_DATA_RETRY_LIMIT_8812(txdesc, bmc ? 0 : 12); + rtl8812a_cal_txdesc_chksum(txdesc); +} + +} // namespace apfpv diff --git a/src/apfpv/StationTxDesc.h b/src/apfpv/StationTxDesc.h new file mode 100644 index 0000000..0a77130 --- /dev/null +++ b/src/apfpv/StationTxDesc.h @@ -0,0 +1,7 @@ +#pragma once +#include +namespace apfpv { +enum class StationFrameKind { Mgmt, EapolData, CcmpData, BroadcastMgmt, Beacon }; +void FillStationTxDesc(uint8_t* txdesc, uint16_t payloadLen, uint8_t descOffset, + uint8_t macId, StationFrameKind kind, uint8_t rateId, uint8_t txRate); +} diff --git a/src/apfpv/Wpa2Crypto.cpp b/src/apfpv/Wpa2Crypto.cpp new file mode 100644 index 0000000..47b24c4 --- /dev/null +++ b/src/apfpv/Wpa2Crypto.cpp @@ -0,0 +1,80 @@ +// ============================================================================ +// Wpa2Crypto.cpp — WPA2 crypto backend, now BACKED BY VENDORED PRIMITIVES +// (src/crypto/Sha1Hmac + AesCcm). No external deps; builds on NDK/Linux. +// Fills the Crypto vtable Wpa2Supplicant consumes. +// ============================================================================ +#include "Wpa2Crypto.h" +#include "crypto/Sha1Hmac.h" +#include "crypto/AesCcm.h" +#include + +namespace apfpv { + +static std::array pbkdf2_pmk_impl(const std::string& pass,const uint8_t* ssid,size_t sl){ + std::array pmk{}; + crypto::pbkdf2_sha1(pass.c_str(), ssid, sl, 4096, pmk.data(), 32); + return pmk; +} + +// IEEE 802.11i PRF: R = HMAC-SHA1(K, label||0x00||data||i) concatenated. +static std::vector prf_impl(const uint8_t* key,size_t kl,const std::string& label, + const uint8_t* data,size_t dl,size_t outLen){ + std::vector out; out.reserve(((outLen+19)/20)*20); + for(uint8_t i=0; out.size() buf(label.begin(),label.end()); + buf.push_back(0x00); buf.insert(buf.end(),data,data+dl); buf.push_back(i); + uint8_t mac[20]; crypto::hmac_sha1(key,kl,buf.data(),buf.size(),mac); + out.insert(out.end(),mac,mac+20); + } + out.resize(outLen); return out; +} + +static std::array eapol_mic_impl(const uint8_t* kck,const uint8_t* msg,size_t len){ + uint8_t full[20]; crypto::hmac_sha1(kck,16,msg,len,full); + std::array mic{}; std::memcpy(mic.data(),full,16); return mic; // AES/SHA1 -> first 16 +} + +static bool ccmp_decrypt_impl(const uint8_t* tk,const uint8_t* nonce,const uint8_t* in,size_t inLen, + const uint8_t* aad,size_t aadLen,std::vector& out){ + out.resize(inLen>8?inLen-8:0); + // DATA RX fast path: ARM Crypto Extensions AES (~20 cycles/block vs ~1500 SW). At 65Mbps + // (5600 pkt/s) SW AES (~180µs/pkt) SATURATES one core -> backlog -> dropped frames. ARM-CE + // (~5µs/pkt) matches the kernel and removes the ceiling. Returns false on MIC mismatch -> + // SW fallback. Handshake EAPOL always uses SW. The earlier SIGABRTs were the handshake Rcon + // bug + onScanFrame mutex (both fixed) — NOT this path; and aes_enc_ce was missing its final + // AddRoundKey (now fixed) which had made every CE block wrong -> silent SW fallback. +#if defined(__aarch64__) || defined(__x86_64__) || defined(_M_X64) || defined(__i386__) || defined(_M_IX86) + // Hardware AES (ARM-CE / x86 AES-NI). Returns false -> SW fallback (handshake, + // or an x86 CPU without AES-NI). On native x86 this lifts CCMP decrypt from + // ~355us/pkt (software) to ~10us, removing the host-side throughput cap. + if (crypto::aes_ccm_decrypt_ce(tk,nonce,13,aad,aadLen,in,inLen,out.data())) return true; +#endif + // SW fallback: handshake EAPOL + data if CE unavailable / MIC mismatch + static thread_local uint8_t cacheKey[16] = {}; + static thread_local crypto::Aes cacheAes; + static thread_local bool cacheValid = false; + if (!cacheValid || std::memcmp(cacheKey, tk, 16) != 0) { + crypto::aes_key_expand(tk, cacheAes); + std::memcpy(cacheKey, tk, 16); + cacheValid = true; + } + return crypto::aes_ccm_decrypt(tk,nonce,13,aad,aadLen,in,inLen,out.data(),&cacheAes); +} +static std::vector ccmp_encrypt_impl(const uint8_t* tk,const uint8_t* nonce,const uint8_t* in, + size_t inLen,const uint8_t* aad,size_t aadLen){ + std::vector out(inLen+8); + crypto::aes_ccm_encrypt(tk,nonce,13,aad,aadLen,in,inLen,out.data()); + return out; +} + +Crypto MakeWpa2Crypto(){ + Crypto c; + c.pbkdf2_pmk = [](const std::string& p,const uint8_t* s,size_t n){ return pbkdf2_pmk_impl(p,s,n); }; + c.prf = [](const uint8_t* k,size_t kl,const std::string& l,const uint8_t* d,size_t dl,size_t o){ return prf_impl(k,kl,l,d,dl,o); }; + c.eapol_mic = [](const uint8_t* k,const uint8_t* m,size_t l){ return eapol_mic_impl(k,m,l); }; + c.ccmp_decrypt = [](const uint8_t* tk,const uint8_t* n,const uint8_t* in,size_t il,const uint8_t* a,size_t al,std::vector& o){ return ccmp_decrypt_impl(tk,n,in,il,a,al,o); }; + c.ccmp_encrypt = [](const uint8_t* tk,const uint8_t* n,const uint8_t* in,size_t il,const uint8_t* a,size_t al){ return ccmp_encrypt_impl(tk,n,in,il,a,al); }; + return c; +} + +} // namespace apfpv diff --git a/src/apfpv/Wpa2Crypto.h b/src/apfpv/Wpa2Crypto.h new file mode 100644 index 0000000..3d1d4a3 --- /dev/null +++ b/src/apfpv/Wpa2Crypto.h @@ -0,0 +1,16 @@ +#pragma once +#include +#include +#include +#include +#include +namespace apfpv { +struct Crypto { + std::function(const std::string&, const uint8_t*, size_t)> pbkdf2_pmk; + std::function(const uint8_t*, size_t, const std::string&, const uint8_t*, size_t, size_t)> prf; + std::function(const uint8_t*, const uint8_t*, size_t)> eapol_mic; + std::function&)> ccmp_decrypt; + std::function(const uint8_t*, const uint8_t*, const uint8_t*, size_t, const uint8_t*, size_t)> ccmp_encrypt; +}; +Crypto MakeWpa2Crypto(); +} diff --git a/src/apfpv/Wpa2Supplicant.cpp b/src/apfpv/Wpa2Supplicant.cpp new file mode 100644 index 0000000..e5089c9 --- /dev/null +++ b/src/apfpv/Wpa2Supplicant.cpp @@ -0,0 +1,418 @@ +// ============================================================================ +// Wpa2Supplicant.cpp — WPA2-PSK 4-way handshake + CCMP (impl) +// Declarations in Wpa2Supplicant.h. Pure userspace protocol logic; crypto via +// injected Crypto backend (Wpa2Crypto.cpp). Not timing-critical. +// ============================================================================ +#include "Wpa2Supplicant.h" +#include "Dot11Frames.h" +#include "crypto/AesCcm.h" +#include "crypto/AesCmac.h" +#include +#include +#include +#if defined(__ANDROID__) +#include +#define WPALOG(...) __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", __VA_ARGS__) +#else +#define WPALOG(...) do{}while(0) +#endif +#include + +#ifdef _WIN32 +extern "C" __declspec(dllimport) long __stdcall + BCryptGenRandom(void* hAlg, unsigned char* pb, unsigned long cb, unsigned long flags); +#endif +// Cryptographically-secure RNG for the SNonce (must be unpredictable): Windows CNG +// (BCryptGenRandom, system-preferred DRBG), else /dev/urandom (Linux/Android), else a +// weak last-resort mix. Replaces the bare std::rand() Windows fallback. +static void secure_random(uint8_t* buf, size_t n) { +#ifdef _WIN32 + if (BCryptGenRandom(nullptr, buf, (unsigned long)n, 0x00000002u /*USE_SYSTEM_PREFERRED_RNG*/) == 0) return; +#else + FILE* u = fopen("/dev/urandom", "rb"); + if (u) { size_t r = fread(buf, 1, n, u); fclose(u); if (r == n) return; } +#endif + for (size_t i = 0; i < n; ++i) buf[i] = (uint8_t)((std::rand() >> 3) ^ (i * 131)); +} + +namespace apfpv { + +Wpa2Supplicant::Wpa2Supplicant(Crypto c, SendFn send) + : _c(std::move(c)), _send(std::move(send)) {} + +void Wpa2Supplicant::begin(const std::string& passphrase, const std::string& ssid, + const Mac& self, const Mac& bssid) { + _self = self; _bssid = bssid; + _pmk = _c.pbkdf2_pmk(passphrase, + reinterpret_cast(ssid.data()), ssid.size()); + _state = State::WaitMsg1; + // fresh session: clear replay/PN windows + GTK slots so a reconnect can't be falsely + // rejected by stale counters from the previous association. + _replay = 0; _txPn = 1; _rxPnPair = 0; + _rxPnGtk[0]=_rxPnGtk[1]=_rxPnGtk[2]=_rxPnGtk[3]=0; + winClear(_rxPnPairWin); for(int _k=0;_k<4;_k++) winClear(_rxPnGtkWin[_k]); + _gtkKeyId = 0xff; _gtkPrevId = 0xff; +} + +void Wpa2Supplicant::beginCached(const std::array& pmk, + const Mac& self, const Mac& bssid) { + _self = self; _bssid = bssid; + _pmk = pmk; // pre-computed — no PBKDF2 here (keeps the M1 window) + _state = State::WaitMsg1; + // fresh session: clear replay/PN windows + GTK slots so a reconnect can't be falsely + // rejected by stale counters from the previous association. + _replay = 0; _txPn = 1; _rxPnPair = 0; + _rxPnGtk[0]=_rxPnGtk[1]=_rxPnGtk[2]=_rxPnGtk[3]=0; + winClear(_rxPnPairWin); for(int _k=0;_k<4;_k++) winClear(_rxPnGtkWin[_k]); + _gtkKeyId = 0xff; _gtkPrevId = 0xff; +} + +bool Wpa2Supplicant::ready() const { return _state == State::Done; } + +void Wpa2Supplicant::derivePtk(const uint8_t* aNonce) { + std::memcpy(_aNonce.data(), aNonce, 32); + // SNonce: cryptographically-secure + unpredictable (CNG on Windows, /dev/urandom on + // Linux/Android). MUST be fresh per 4-way (incl. PTK rekey) or PTK derivation is predictable. + secure_random(_sNonce.data(), 32); + uint8_t data[76]; + const Mac& aa = _bssid; const Mac& sa = _self; + bool aaFirst = std::memcmp(aa.data(), sa.data(), 6) < 0; + std::memcpy(data + 0, (aaFirst?aa:sa).data(), 6); + std::memcpy(data + 6, (aaFirst?sa:aa).data(), 6); + bool aFirst = std::memcmp(aNonce, _sNonce.data(), 32) < 0; + std::memcpy(data + 12, aFirst?aNonce:_sNonce.data(), 32); + std::memcpy(data + 44, aFirst?_sNonce.data():aNonce, 32); + auto ptk = _c.prf(_pmk.data(), 32, "Pairwise key expansion", data, sizeof(data), 48); + std::memcpy(_kck.data(), ptk.data() + 0, 16); + std::memcpy(_kek.data(), ptk.data() + 16, 16); + std::memcpy(_tk.data(), ptk.data() + 32, 16); +} + +// Build an EAPOL-Key body (key info, nonce, MIC). MIC computed over the body +// with the MIC field zeroed, using KCK, then patched in. Returns full 802.11 +// data frame via BuildEapolKey. +std::vector Wpa2Supplicant::buildMsg2() { + // Canonical 802.1X EAPOL-Key frame (WPA2/RSN, Key Descriptor Version 2 = AES): + // [0] EAPOL Protocol Version (0x02) + // [1] EAPOL Packet Type (0x03 = EAPOL-Key) + // [2-3] EAPOL Body Length (BE) = frame length - 4 + // [4] Descriptor Type (0x02 = RSN) + // [5-6] Key Information (BE) + // [7-8] Key Length (BE) + // [9-16] Key Replay Counter (echo the AP's) + // [17-48] Key Nonce (SNonce) + // [49-64] EAPOL Key IV (0) | [65-72] Key RSC (0) | [73-80] Reserved (0) + // [81-96] Key MIC (zeroed for the HMAC, then patched in) + // [97-98] Key Data Length (BE) + // [99+] Key Data = STA RSN IE (MUST match the Assoc-Req RSN IE) + // The old code omitted the 4-byte EAPOL header + the RSN-IE Key Data and + // computed the MIC over that non-canonical buffer, so the AP's MIC check + // always failed and it never sent M3. + static const uint8_t rsnIe[] = { // == Dot11Frames BuildAssocRequest RSN IE + 0x30,0x14, 0x01,0x00, 0x00,0x0f,0xac,0x04, + 0x01,0x00, 0x00,0x0f,0xac,0x04, 0x01,0x00, 0x00,0x0f,0xac,0x02, 0x00,0x00 }; + const size_t kdLen = sizeof(rsnIe); // 22 + std::vector kb(99 + kdLen, 0); + kb[0] = 0x02; kb[1] = 0x03; + uint16_t bodyLen = (uint16_t)(kb.size() - 4); + kb[2] = bodyLen >> 8; kb[3] = bodyLen & 0xFF; + kb[4] = 0x02; // descriptor type = RSN + uint16_t info = 0x010A; // ver2(AES) + pairwise + MIC + kb[5] = info >> 8; kb[6] = info & 0xFF; + kb[7] = 0x00; kb[8] = 0x10; // key length = 16 (CCMP pairwise) + std::memcpy(kb.data() + 9, _replayCtr, 8); // echo the AP's replay counter + std::memcpy(kb.data() + 17, _sNonce.data(), 32); + kb[97] = (uint8_t)(kdLen >> 8); kb[98] = (uint8_t)(kdLen & 0xFF); + std::memcpy(kb.data() + 99, rsnIe, kdLen); + { uint16_t caps = (uint16_t)((_pmf>=1?0x0080:0)|(_pmf>=2?0x0040:0)); // must match the assoc-req RSN caps + kb[99 + kdLen - 2] = (uint8_t)(caps & 0xff); kb[99 + kdLen - 1] = (uint8_t)(caps >> 8); } + // MIC over the WHOLE frame with the MIC field [81..96] zeroed. + auto mic = _c.eapol_mic(_kck.data(), kb.data(), kb.size()); + std::memcpy(kb.data() + 81, mic.data(), 16); + return BuildEapolKey(_self, _bssid, kb); +} + +std::vector Wpa2Supplicant::buildMsg4() { + // Canonical EAPOL-Key, no Key Data, no SNonce, secure bit set. Same layout as M2. + std::vector kb(99, 0); + kb[0] = 0x02; kb[1] = 0x03; + uint16_t bodyLen = (uint16_t)(kb.size() - 4); + kb[2] = bodyLen >> 8; kb[3] = bodyLen & 0xFF; + kb[4] = 0x02; // descriptor type = RSN + uint16_t info = 0x030A; // ver2 + pairwise + MIC + secure + kb[5] = info >> 8; kb[6] = info & 0xFF; + // key length 0, nonce 0, key data length 0 for M4. + std::memcpy(kb.data() + 9, _replayCtr, 8); // echo M3's replay counter + auto mic = _c.eapol_mic(_kck.data(), kb.data(), kb.size()); + std::memcpy(kb.data() + 81, mic.data(), 16); + return BuildEapolKey(_self, _bssid, kb); +} + +// Unwrap the EAPOL-Key Data (NIST AES-key-wrap with the KEK) and install the GTK from its +// KDE. Shared by M3 (initial 4-way) AND the periodic group rekey. Tracks the GTK key id. +bool Wpa2Supplicant::installGtkFromKeyData(const uint8_t* body, size_t len) { + uint16_t kdLen = (len >= 99) ? ((body[97] << 8) | body[98]) : 0; + if (kdLen < 24 || (kdLen % 8) != 0 || len < (size_t)(99 + kdLen)) return false; + std::vector kp(kdLen, 0); + if (!crypto::aes_key_unwrap(_kek.data(), body + 99, kdLen, kp.data())) return false; + size_t plen = kdLen - 8; + bool gotGtk = false; + for (size_t i = 0; i + 2 <= plen; ) { + uint8_t t = kp[i], l = kp[i+1]; + if (t == 0 || l == 0) break; + if (t == 0xdd && (size_t)(i + 2 + l) <= plen && + kp[i+2]==0x00 && kp[i+3]==0x0f && kp[i+4]==0xac) { + if (kp[i+5]==0x01 && l >= 22) { // GTK KDE: + uint8_t newId = kp[i+6] & 0x03; + if (_gtkKeyId != 0xff && _gtkKeyId != newId) { _gtkPrev = _gtk; _gtkPrevId = _gtkKeyId; } + _gtkKeyId = newId; + std::memcpy(_gtk.data(), &kp[i+8], 16); + _rxPnGtk[newId & 3] = 0; winClear(_rxPnGtkWin[newId & 3]); // fresh key: reset RX replay window + fprintf(stderr, "[wpa] GTK installed keyid=%d (%02x%02x..)\n", _gtkKeyId, _gtk[0], _gtk[1]); + gotGtk = true; + } else if (kp[i+5]==0x09 && l >= 24) { // IGTK KDE (802.11w): + _igtkKeyId = (uint16_t)(kp[i+6] | (kp[i+7] << 8)); + std::memcpy(_igtk.data(), &kp[i+14], 16); + _igtkSet = true; + fprintf(stderr, "[wpa] IGTK installed keyid=%u\n", _igtkKeyId); + } + } + i += 2 + l; + } + return gotGtk; +} + +// Group-rekey reply (M2 of the 2-way group handshake): Key Type=Group, MIC+Secure set, +// no Key Data, echo the AP's replay counter, MIC over the frame with the KCK. +std::vector Wpa2Supplicant::buildGroupMsg2() { + std::vector kb(99, 0); + kb[0] = 0x02; kb[1] = 0x03; + uint16_t bodyLen = (uint16_t)(kb.size() - 4); + kb[2] = bodyLen >> 8; kb[3] = bodyLen & 0xFF; + kb[4] = 0x02; // descriptor type = RSN + uint16_t info = 0x0302; // ver2(AES) + GROUP(pairwise=0) + MIC + secure + kb[5] = info >> 8; kb[6] = info & 0xFF; + kb[7] = 0x00; kb[8] = 0x10; // key length = 16 (CCMP group) + std::memcpy(kb.data() + 9, _replayCtr, 8); + auto mic = _c.eapol_mic(_kck.data(), kb.data(), kb.size()); + std::memcpy(kb.data() + 81, mic.data(), 16); + return BuildEapolKey(_self, _bssid, kb); +} + +// 802.11w BIP-CMAC-128 verify of a protected broadcast/multicast Deauth/Disassoc. True only if +// the trailing MME's MIC == AES-CMAC(IGTK, maskedFC|A1|A2|A3|body-with-MME-MIC-zeroed). Per +// 802.11-2016 §12.5.4; best-effort (validate against a real PMF AP). On any mismatch we return +// false and the caller ignores the frame — the safe default (a genuine deauth still trips the +// RX-silence watchdog), so an imperfect BIP can never cause a spurious disconnect. +bool Wpa2Supplicant::verifyProtectedMgmt(const uint8_t* frame, size_t len) const { + if (!_igtkSet || len < 24 + 18) return false; + size_t mmeOff = len - 18; // MME = 0x4c len(16) keyid(2) ipn(6) mic(8) + if (frame[mmeOff] != 0x4c || frame[mmeOff+1] != 16) return false; + const uint8_t* mme = frame + mmeOff; + if ((uint16_t)(mme[2] | (mme[3] << 8)) != _igtkKeyId) return false; + std::vector buf; buf.reserve(14 + (len - 24)); + uint16_t mfc = (uint16_t)((frame[0] | (frame[1] << 8)) & ~0x7800); // mask Retry/PwrMgmt/MoreData/Protected + buf.push_back(mfc & 0xff); buf.push_back(mfc >> 8); + buf.insert(buf.end(), frame + 4, frame + 16); // A1 | A2 | A3 (duration excluded) + buf.insert(buf.end(), frame + 24, frame + len); // body incl. the MME + for (int k = 0; k < 8; ++k) buf[buf.size() - 8 + k] = 0; // zero the MME MIC for the calc + uint8_t mic[8]; crypto::bip_cmac_128(_igtk.data(), buf.data(), buf.size(), mic); + return std::memcmp(mic, mme + 10, 8) == 0; +} + +bool Wpa2Supplicant::onEapolKey(const uint8_t* body, size_t len) { + WPALOG("[wpa] onEapolKey len=%zu state=%d", len, (int)_state); + if (len < 95) { WPALOG("[wpa] EAPOL too short (len=%zu <95) — dropped", len); return false; } + // EAPOL anti-replay: the AP's 8-byte replay counter (offset 9) must not go backwards once + // the handshake is up — rejects replayed EAPOL-Key frames (KRACK-class). Only enforced past + // Done so a reconnect's fresh (possibly lower) M1 counter is never rejected; begin() resets. + uint64_t rc = 0; for (int i = 0; i < 8; ++i) rc = (rc << 8) | body[9 + i]; + if (_state == State::Done && rc < _replay) { + fprintf(stderr, "[wpa] EAPOL-Key replay dropped (rc < %llu)\n", (unsigned long long)_replay); + return false; + } + if (rc > _replay) _replay = rc; + // Capture so our M2/M4 echo the AP's counter (else the AP rejects them as replay-mismatched). + std::memcpy(_replayCtr, body + 9, 8); + const uint8_t* keyInfo = body + 5; + uint16_t info = (keyInfo[0] << 8) | keyInfo[1]; + const bool hasMic = info & 0x0100; + const bool isPairwise = info & 0x0008; + const uint8_t* nonce = body + 17; + fprintf(stderr, "[wpa] EAPOL-Key rx len=%zu info=0x%04x mic=%d pairwise=%d state=%d\n", + len, info, (int)hasMic, (int)isPairwise, (int)_state); + WPALOG("[wpa] EAPOL-Key info=0x%04x mic=%d pairwise=%d state=%d", info, (int)hasMic, (int)isPairwise, (int)_state); + switch (_state) { + case State::WaitMsg1: + if (!hasMic && isPairwise) { + derivePtk(nonce); + bool sent = _send(buildMsg2()); + _state = State::WaitMsg3; + fprintf(stderr, "[wpa] M1 -> built+sent M2 (sent=%d) -> WaitMsg3\n", (int)sent); + WPALOG("[wpa] M1 rx -> M2 sent=%d -> WaitMsg3", (int)sent); + } else { + WPALOG("[wpa] WaitMsg1 but frame not M1 (mic=%d pairwise=%d) — ignored", (int)hasMic, (int)isPairwise); + } + return false; + case State::WaitMsg3: + if (hasMic && isPairwise) { + // M3 carries the new GTK as a KEK-wrapped KDE in the Key Data field. + installGtkFromKeyData(body, len); + bool sent4 = _send(buildMsg4()); + _state = State::Done; + fprintf(stderr, "[wpa] M3 -> built+sent M4 (sent=%d) -> DONE\n", (int)sent4); + WPALOG("[wpa] M3 rx -> M4 sent=%d -> DONE (keys installed)", (int)sent4); + return true; + } + return false; + default: + // POST-HANDSHAKE REKEYS — a real station MUST service these or its keys go + // stale and decryption silently dies mid-session (the ~10-min "stream stops" bug). + if (hasMic && !isPairwise) { // GROUP rekey (2-way): AP rotates the GTK + bool ok = installGtkFromKeyData(body, len); + bool sent = _send(buildGroupMsg2()); + fprintf(stderr, "[wpa] GROUP REKEY: GTK %s, M2 sent=%d\n", ok?"reinstalled":"PARSE-FAIL", (int)sent); + return ok; + } + if (!hasMic && isPairwise) { // PTK rekey: AP restarts the 4-way + derivePtk(nonce); + _txPn = 1; _rxPnPair = 0; winClear(_rxPnPairWin); // fresh pairwise key -> fresh PN windows + _send(buildMsg2()); + _state = State::WaitMsg3; + fprintf(stderr, "[wpa] PTK REKEY: re-derived PTK, sent M2 -> WaitMsg3\n"); + return false; + } + return false; + } +} + +// Build the CCMP AAD per IEEE 802.11: masked FC (clear subtype b4-6 + Retry/PwrMgt/MoreData, +// set Protected) | A1 | A2 | A3 | masked SC (fragment only, sequence zeroed) [ | QoS TID ]. +// The DURATION field is EXCLUDED. The old code passed the raw 24-byte header (duration in, +// FC/SC unmasked) so the MIC never matched and NO encrypted frame decrypted. +static size_t ccmpAad(const uint8_t* hdr, bool qos, uint8_t* aad) { + uint16_t fc = hdr[0] | (hdr[1] << 8); + uint16_t mfc = (fc & ~0x3870) | 0x4000; + aad[0] = mfc & 0xff; aad[1] = (mfc >> 8) & 0xff; + std::memcpy(aad + 2, hdr + 4, 18); // A1 | A2 | A3 (skip duration hdr[2:4]) + aad[20] = hdr[22] & 0x0f; aad[21] = 0x00; // SC: fragment number only + if (qos) { aad[22] = hdr[24] & 0x0f; aad[23] = 0x00; return 24; } + return 22; +} + +bool Wpa2Supplicant::decryptData(const uint8_t* frame, size_t len, std::vector& plain) { + if (_state != State::Done) return false; + if (len < 24 + 8) return false; + uint16_t fc = frame[0] | (frame[1] << 8); + size_t hdr = 24; + if (fc & 0x0080) hdr += 2; // QoS-data (subtype bit, NOT the Order flag + // 0x8000 — same bug that hid the handshake) + const uint8_t* ccmp = frame + hdr; // 8-byte CCMP header + bool qos = (fc & 0x0080) != 0; + // CCMP nonce: priority(1)=QoS TID (0 if non-QoS) | A2(6) | PN(6). + uint8_t pn[6] = { ccmp[7], ccmp[6], ccmp[5], ccmp[4], ccmp[1], ccmp[0] }; + uint8_t nonce[13]; nonce[0] = qos ? (frame[24] & 0x0f) : 0; + std::memcpy(nonce + 1, frame + 10, 6); // A2 + std::memcpy(nonce + 7, pn, 6); + const uint8_t* enc = ccmp + 8; + size_t encLen = len - hdr - 8; + // Proper CCMP AAD (masked, duration excluded). Group-addressed (broadcast/multicast: + // DHCP OFFER, ARP, multicast video) decrypts with the GTK; unicast with the pairwise TK. + uint8_t aad[30]; size_t aadLen = ccmpAad(frame, qos, aad); + // Group frames pick a GTK by the CCMP KeyID (two slots survive a rekey); unicast uses the TK. + uint8_t kid = (ccmp[3] >> 6) & 0x03; + const uint8_t* key; + if (frame[4] & 0x01) key = (kid == _gtkPrevId) ? _gtkPrev.data() : _gtk.data(); + else key = _tk.data(); + // The RX frame carries a trailing 4-byte 802.11 FCS (RCR APPFCS). It sits AFTER the CCMP MIC, + // so the FCS-INCLUSIVE length ALWAYS fails the MIC check. PERF FIX (2026-06-21): the old code + // tried the full length FIRST and only retried with -4 on failure — doing a WASTED full + // CTR+CBC-MAC decrypt on EVERY packet (the single RX worker was 93% CPU-bound, ~22Mbps cap, + // ~580µs/pkt). APPFCS is always on, so try the FCS-stripped length FIRST; the full-length + // fallback only runs if that fails (no-FCS edge case). Halves the per-packet AES work. + bool ok = (encLen > 12) && _c.ccmp_decrypt(key, nonce, enc, encLen - 4, aad, aadLen, plain); + if (!ok) ok = _c.ccmp_decrypt(key, nonce, enc, encLen, aad, aadLen, plain); + if (!ok) { + // MIC/CCM failure = wrong key (stale PTK/GTK after reconnect) or corrupt frame. + static int micFails = 0; + if ((++micFails % 200) == 1) + __android_log_print(ANDROID_LOG_WARN, "rxd-decfail", + "MIC-fail #%d grp=%d kid=%d (wrong key / corrupt)", micFails, (int)(frame[4]&1), (int)kid); + return false; + } + // RX anti-replay (post-MIC) with a 64-PN SLIDING WINDOW. A-MPDU subframes legitimately + // arrive out-of-order within the Block-Ack window (bufsz=64), so the old strict + // "pn <= lastPn -> drop" rejected ~53% of valid reordered subframes (the real A-MPDU + // throughput killer once the AP started aggregating). The window accepts any in-window, + // not-yet-seen PN — matching how the chip/kernel do CCMP replay under aggregation. + uint64_t pnVal = ((uint64_t)pn[0]<<40)|((uint64_t)pn[1]<<32)|((uint64_t)pn[2]<<24) + | ((uint64_t)pn[3]<<16)|((uint64_t)pn[4]<<8)|(uint64_t)pn[5]; + uint64_t& highest = (frame[4]&0x01) ? _rxPnGtk[kid] : _rxPnPair; + uint64_t* win = (frame[4]&0x01) ? _rxPnGtkWin[kid] : _rxPnPairWin; + if (!pnWindowCheck(highest, win, pnVal)) { + // True duplicate (an unacked-A-MPDU retransmit) or a PN below the 64-frame window. + static int pnReplays = 0; + if ((++pnReplays % 500) == 1) + __android_log_print(ANDROID_LOG_WARN, "rxd-decfail", + "PN-replay #%d grp=%d kid=%d pn=%llu high=%llu (dup or out-of-window)", + pnReplays, (int)(frame[4]&1), (int)kid, + (unsigned long long)pnVal, (unsigned long long)highest); + return false; + } + return true; +} + + +// Build an encrypted (CCMP) 802.11 QoS-data frame carrying an IPv4 payload. +// Frame: [802.11 hdr (to-DS)][CCMP hdr 8B][ enc{ LLC/SNAP | IPv4 } ][MIC 8B]. +std::vector Wpa2Supplicant::buildEncryptedData(const uint8_t* ip, size_t iplen, uint16_t ethertype) { + if (_state != State::Done) return {}; + // LLC/SNAP + payload = the plaintext to encrypt (ethertype 0x0800 IPv4, 0x0806 ARP, ...) + std::vector plain; + uint8_t snap[8] = {0xaa,0xaa,0x03,0x00,0x00,0x00,(uint8_t)(ethertype>>8),(uint8_t)(ethertype&0xff)}; + plain.insert(plain.end(), snap, snap+8); + plain.insert(plain.end(), ip, ip+iplen); + + // addr3 = the L2 destination (DA). A broadcast/multicast IP MUST map to the matching L2 + // group address; otherwise the AP receives an IP broadcast that is L2-unicast to itself + // and does NOT relay it to the DS/DHCP server — so a DHCP DISCOVER (dst 255.255.255.255) + // never reaches dnsmasq and no lease is offered. Unicast IP goes via the AP (A3=BSSID). + uint8_t a3[6]; + if (ethertype != 0x0800) // ARP etc. -> L2 broadcast + std::memset(a3, 0xff, 6); + else if (iplen >= 20 && ip[16]==0xff && ip[17]==0xff && ip[18]==0xff && ip[19]==0xff) + std::memset(a3, 0xff, 6); // limited broadcast + else if (iplen >= 20 && (ip[16] & 0xf0) == 0xe0) { // IPv4 multicast 224-239 + a3[0]=0x01; a3[1]=0x00; a3[2]=0x5e; a3[3]=ip[17]&0x7f; a3[4]=ip[18]; a3[5]=ip[19]; + } else std::memcpy(a3, _bssid.data(), 6); // unicast -> via AP + + // 802.11 data header, to-DS (STA->AP). addr1=BSSID(RA), addr2=self(TA), addr3=DA. + std::vector f; + f.push_back(0x08); f.push_back(0x01); // FC: data, to-DS, Protected + f.push_back(0x00); f.push_back(0x00); // duration + f.insert(f.end(), _bssid.begin(), _bssid.end()); // addr1 = BSSID (RA) + f.insert(f.end(), _self.begin(), _self.end()); // addr2 = SA (TA) + f.insert(f.end(), a3, a3 + 6); // addr3 = DA (L2 destination) + f.push_back(0x00); f.push_back(0x00); // seq ctrl (HW fills) + f[1] |= 0x40; // set Protected bit + + // CCMP header (8B): PN0 PN1 rsv keyid(ext) PN2..PN5 + uint64_t pn = _txPn++; + uint8_t pn0=pn&0xff, pn1=(pn>>8)&0xff, pn2=(pn>>16)&0xff, + pn3=(pn>>24)&0xff, pn4=(pn>>32)&0xff, pn5=(pn>>40)&0xff; + f.push_back(pn0); f.push_back(pn1); f.push_back(0x00); f.push_back(0x20); // keyid0, ext-IV + f.push_back(pn2); f.push_back(pn3); f.push_back(pn4); f.push_back(pn5); + + // CCMP nonce: priority(1)=0 | A2(6) | PN(6, big-endian) + uint8_t nonce[13]; nonce[0]=0; + std::memcpy(nonce+1, _self.data(), 6); + nonce[7]=pn5; nonce[8]=pn4; nonce[9]=pn3; nonce[10]=pn2; nonce[11]=pn1; nonce[12]=pn0; + // Proper CCMP AAD (masked, duration excluded) — the AP's MIC check requires this exact + // form, else our uplink (DHCP DISCOVER etc.) is dropped and we never get a lease. + uint8_t aad[30]; size_t aadLen = ccmpAad(f.data(), false, aad); + auto enc = _c.ccmp_encrypt(_tk.data(), nonce, plain.data(), plain.size(), aad, aadLen); + f.insert(f.end(), enc.begin(), enc.end()); // ciphertext + MIC + return f; +} + +} // namespace apfpv diff --git a/src/apfpv/Wpa2Supplicant.h b/src/apfpv/Wpa2Supplicant.h new file mode 100644 index 0000000..dd5f99b --- /dev/null +++ b/src/apfpv/Wpa2Supplicant.h @@ -0,0 +1,106 @@ +#pragma once +#include +#include +#include +#include +#include +#include "Wpa2Crypto.h" +namespace apfpv { +using Mac = std::array; +class Wpa2Supplicant { +public: + using SendFn = std::function&)>; + Wpa2Supplicant(Crypto c, SendFn send); + void replaceSend(SendFn send) { _send = std::move(send); } + int state() const { return (int)_state; } // 0=Idle 1=WaitMsg1 2=WaitMsg3 3=Done + void begin(const std::string& passphrase, const std::string& ssid, const Mac& self, const Mac& bssid); + // Fast path: skip the (slow ~seconds) PBKDF2 by injecting a pre-computed PMK. The PMK + // depends ONLY on passphrase+SSID, so it can be cached across connects. CRITICAL: the + // AP only advertises M1 for ~4s (4 retries); if begin() does PBKDF2 inline the RX + // doesn't switch to the EAPOL/streaming phase until after the AP gives up -> M1 missed. + void beginCached(const std::array& pmk, const Mac& self, const Mac& bssid); + const std::array& pmk() const { return _pmk; } + bool onEapolKey(const uint8_t* body, size_t len); + bool ready() const; + void setPmf(int p) { _pmf = p; } // 802.11w: 0=off 1=capable 2=required + bool pmfActive() const { return _pmf > 0 && _state == State::Done; } + // Verify a protected broadcast/multicast mgmt frame (Deauth/Disassoc carrying an MME) + // with BIP-CMAC-128 against the installed IGTK. false => forged/unverifiable -> ignore it. + bool verifyProtectedMgmt(const uint8_t* frame, size_t len) const; + bool decryptData(const uint8_t* frame, size_t len, std::vector& plain); + // Build an ENCRYPTED 802.11 data frame (CCMP) carrying an IP/UDP payload. + // Used to TX DHCP (and any station uplink) after the 4-way handshake. + // 'ipPayload' is a complete IPv4 packet (IP+UDP+BOOTP). Returns the full + // 802.11 frame ready for send_packet (caller prepends the TX descriptor). + std::vector buildEncryptedData(const uint8_t* ipPayload, size_t len, uint16_t ethertype = 0x0800); + const std::array& tk() const { return _tk; } + const std::array& gtk() const { return _gtk; } // Lever C.2: HW-CAM group key + uint8_t gtkKeyId() const { return _gtkKeyId; } +private: + enum class State { Idle, WaitMsg1, WaitMsg3, Done }; + void derivePtk(const uint8_t* aNonce); + std::vector buildMsg2(); + std::vector buildMsg4(); + std::vector buildGroupMsg2(); // group-rekey ACK (Key Type=Group) + bool installGtkFromKeyData(const uint8_t* body, size_t len); // unwrap+install GTK (M3 & rekey) + Crypto _c; SendFn _send; + Mac _self{}, _bssid{}; + std::array _pmk{}; std::array _kck{}, _kek{}, _tk{}, _gtk{}, _gtkPrev{}; + std::array _aNonce{}, _sNonce{}; + // Two GTK slots indexed by CCMP KeyID so frames in-flight on the OLD key still decrypt + // across a group rekey (else the ~1s around a rekey loses broadcast/multicast). + uint8_t _gtkKeyId = 0xff, _gtkPrevId = 0xff; // 0xff = unset + int _pmf = 0; // 802.11w mode (RSN caps in M2) + std::array _igtk{}; uint16_t _igtkKeyId = 0; bool _igtkSet = false; // BIP key + // CCMP RX replay windows (per key). A-MPDU subframes arrive OUT OF ORDER within the + // Block-Ack window (bufsz=64), so a strict "PN must increase" check wrongly rejects every + // reordered subframe as a replay (was ~53% drop at A-MPDU rates). Use a 64-PN SLIDING + // WINDOW per IEEE 802.11: _rxPn* = highest PN seen; _rxPn*Win = bitmap of the 64 PNs at/below + // it (bit i set = PN (highest-i) already received). Accept if newer, or in-window & not dup. + // 2048-PN window (32 × 64-bit words). Without HW Block-Ack the AP retransmits whole + // unacked A-MPDUs, so frames arrive reordered by up to ~1000 PNs (measured). A 64-PN + // window rejected the far-reordered-but-not-yet-seen frames as "replay", keeping them + // lost; a 2048 window recovers them (the AP's retransmit is our only copy of an + // over-air-lost subframe), while still dropping true duplicates (bit already set). + // 64-PN window (1 word): matches the Block-Ack reorder window (bufsz=64). Recovers + // legitimately-reordered A-MPDU subframes (the real fix vs the old strict pn<=last check) + // WITHOUT accepting ancient unacked-A-MPDU retransmits — those carry stale RTP seqs that + // arrive too late to render and only inflate the seq-gap "lost" counter (a 2048 window + // measured lost=554k vs the strict check's ~12k). 64 is the sweet spot. + static constexpr int PN_WIN_WORDS = 1; // 64 PNs = the BA reorder window (tuned: wider + // accepts ancient AP retransmits -> stale RTP seqs inflate "lost") + uint64_t _rxPnPair = 0; uint64_t _rxPnGtk[4] = {0,0,0,0}; + uint64_t _rxPnPairWin[PN_WIN_WORDS] = {0}; + uint64_t _rxPnGtkWin[4][PN_WIN_WORDS] = {{0}}; + static void winClear(uint64_t* w) { for (int i=0;i>6] >> (bit&63)) & 1; } + static void winSet(uint64_t* w, uint64_t bit) { w[bit>>6] |= (1ULL << (bit&63)); } + static void winShift(uint64_t* w, uint64_t n) { // shift the whole bitmap UP by n bits + if (n >= (uint64_t)PN_WIN_WORDS*64) { winClear(w); return; } + int words = (int)(n >> 6), bits = (int)(n & 63); + for (int i = PN_WIN_WORDS-1; i >= 0; --i) { + uint64_t v = (i-words >= 0) ? w[i-words] : 0; + uint64_t lo = (bits && i-words-1 >= 0) ? (w[i-words-1] >> (64-bits)) : 0; + w[i] = (bits ? (v << bits) : v) | lo; + } + } + // Returns true if PN `p` is fresh (accept) + updates the window; false if dup/too-old. + static bool pnWindowCheck(uint64_t& highest, uint64_t* win, uint64_t p) { + bool empty = true; for (int i=0;i highest) { + winShift(win, p - highest); + winSet(win, 0); // bit 0 = the new highest + highest = p; + return true; + } + uint64_t diff = highest - p; + if (diff >= (uint64_t)PN_WIN_WORDS*64) return false; // older than the window -> replay + if (winTest(win, diff)) return false; // already received -> duplicate + winSet(win, diff); + return true; + } + State _state = State::Idle; uint64_t _replay = 0; uint64_t _txPn = 1; uint8_t _replayCtr[8] = {0}; + std::vector _lastKeyFrame; +}; +} diff --git a/src/apfpv/compat/android/log.h b/src/apfpv/compat/android/log.h new file mode 100644 index 0000000..65c2799 --- /dev/null +++ b/src/apfpv/compat/android/log.h @@ -0,0 +1,45 @@ +// --------------------------------------------------------------------------- +// compat/android/log.h — drop-in shim for non-Android (native Windows/Linux) +// builds of the libusb driver. +// +// The driver's diagnostics call __android_log_print(prio, tag, fmt, ...) and +// #include directly. On Android the NDK provides the real +// header. On a host build this directory is added to the include path ONLY +// when NOT ANDROID (see CMakeLists), so resolves here and the +// same source compiles unchanged — diagnostics go to stderr instead of logcat. +// --------------------------------------------------------------------------- +#ifndef APFPV_COMPAT_ANDROID_LOG_H +#define APFPV_COMPAT_ANDROID_LOG_H + +#include +#include + +#ifndef ANDROID_LOG_VERBOSE +typedef enum android_LogPriority { + ANDROID_LOG_UNKNOWN = 0, + ANDROID_LOG_DEFAULT, + ANDROID_LOG_VERBOSE, + ANDROID_LOG_DEBUG, + ANDROID_LOG_INFO, + ANDROID_LOG_WARN, + ANDROID_LOG_ERROR, + ANDROID_LOG_FATAL, + ANDROID_LOG_SILENT, +} android_LogPriority; +#endif + +// printf-style sink. `inline` (not static) so all TUs share one definition and +// there's no "defined but not used" warning when a TU includes the header but +// doesn't log. +inline int __android_log_print(int prio, const char *tag, const char *fmt, ...) { + (void)prio; + va_list ap; + va_start(ap, fmt); + std::fprintf(stderr, "[%s] ", tag ? tag : "?"); + std::vfprintf(stderr, fmt, ap); + std::fputc('\n', stderr); + va_end(ap); + return 0; +} + +#endif /* APFPV_COMPAT_ANDROID_LOG_H */ diff --git a/src/apfpv/crypto/AesCcm.cpp b/src/apfpv/crypto/AesCcm.cpp new file mode 100644 index 0000000..8926eea --- /dev/null +++ b/src/apfpv/crypto/AesCcm.cpp @@ -0,0 +1,351 @@ +// Self-contained AES-128 + CCM (CCMP: M=8, L=2). Public-domain style AES core. +#include "AesCcm.h" +#include +#include // getenv (DEVOURER_FORCE_SW_AES A/B switch) +// x86 AES-NI intrinsics (must be included at file scope, NOT inside a namespace). +// Gives native Windows/Linux x86 hosts the same hardware AES the ARM phone gets +// from ARM-CE — without it, software AES runs ~8x slower (355us vs ~10us/pkt) and +// caps CCMP-decrypt throughput well below line rate. +#if defined(__x86_64__) || defined(_M_X64) || defined(__i386__) || defined(_M_IX86) +#include +#define APFPV_X86_AESNI 1 +#if defined(__GNUC__) +#include // __get_cpuid — reliable AES-NI detection on mingw/GCC +#endif +#endif +namespace apfpv { namespace crypto { + +static const uint8_t S[256]={ +0x63,0x7c,0x77,0x7b,0xf2,0x6b,0x6f,0xc5,0x30,0x01,0x67,0x2b,0xfe,0xd7,0xab,0x76, +0xca,0x82,0xc9,0x7d,0xfa,0x59,0x47,0xf0,0xad,0xd4,0xa2,0xaf,0x9c,0xa4,0x72,0xc0, +0xb7,0xfd,0x93,0x26,0x36,0x3f,0xf7,0xcc,0x34,0xa5,0xe5,0xf1,0x71,0xd8,0x31,0x15, +0x04,0xc7,0x23,0xc3,0x18,0x96,0x05,0x9a,0x07,0x12,0x80,0xe2,0xeb,0x27,0xb2,0x75, +0x09,0x83,0x2c,0x1a,0x1b,0x6e,0x5a,0xa0,0x52,0x3b,0xd6,0xb3,0x29,0xe3,0x2f,0x84, +0x53,0xd1,0x00,0xed,0x20,0xfc,0xb1,0x5b,0x6a,0xcb,0xbe,0x39,0x4a,0x4c,0x58,0xcf, +0xd0,0xef,0xaa,0xfb,0x43,0x4d,0x33,0x85,0x45,0xf9,0x02,0x7f,0x50,0x3c,0x9f,0xa8, +0x51,0xa3,0x40,0x8f,0x92,0x9d,0x38,0xf5,0xbc,0xb6,0xda,0x21,0x10,0xff,0xf3,0xd2, +0xcd,0x0c,0x13,0xec,0x5f,0x97,0x44,0x17,0xc4,0xa7,0x7e,0x3d,0x64,0x5d,0x19,0x73, +0x60,0x81,0x4f,0xdc,0x22,0x2a,0x90,0x88,0x46,0xee,0xb8,0x14,0xde,0x5e,0x0b,0xdb, +0xe0,0x32,0x3a,0x0a,0x49,0x06,0x24,0x5c,0xc2,0xd3,0xac,0x62,0x91,0x95,0xe4,0x79, +0xe7,0xc8,0x37,0x6d,0x8d,0xd5,0x4e,0xa9,0x6c,0x56,0xf4,0xea,0x65,0x7a,0xae,0x08, +0xba,0x78,0x25,0x2e,0x1c,0xa6,0xb4,0xc6,0xe8,0xdd,0x74,0x1f,0x4b,0xbd,0x8b,0x8a, +0x70,0x3e,0xb5,0x66,0x48,0x03,0xf6,0x0e,0x61,0x35,0x57,0xb9,0x86,0xc1,0x1d,0x9e, +0xe1,0xf8,0x98,0x11,0x69,0xd9,0x8e,0x94,0x9b,0x1e,0x87,0xe9,0xce,0x55,0x28,0xdf, +0x8c,0xa1,0x89,0x0d,0xbf,0xe6,0x42,0x68,0x41,0x99,0x2d,0x0f,0xb0,0x54,0xbb,0x16}; +static uint8_t xt(uint8_t x){ return (x<<1)^((x>>7)*0x1b); } +static void aes_key(Aes& a,const uint8_t k[16]){ + memcpy(a.rk,k,16); uint8_t rc=1; + for(int i=16;i<176;i+=4){ + uint8_t t[4]; memcpy(t,a.rk+i-4,4); + if(i%16==0){ uint8_t tmp=t[0];t[0]=S[t[1]]^rc;t[1]=S[t[2]];t[2]=S[t[3]];t[3]=S[tmp]; rc=xt(rc); } + for(int j=0;j<4;j++) a.rk[i+j]=a.rk[i-16+j]^t[j]; + } +} +static void aes_enc(const Aes& a,const uint8_t in[16],uint8_t out[16]){ + uint8_t s[16]; memcpy(s,in,16); for(int i=0;i<16;i++)s[i]^=a.rk[i]; + for(int r=1;r<10;r++){ + uint8_t t[16]; + for(int i=0;i<16;i++) t[i]=S[s[i]]; + uint8_t u[16]; + u[0]=t[0];u[4]=t[4];u[8]=t[8];u[12]=t[12]; + u[1]=t[5];u[5]=t[9];u[9]=t[13];u[13]=t[1]; + u[2]=t[10];u[6]=t[14];u[10]=t[2];u[14]=t[6]; + u[3]=t[15];u[7]=t[3];u[11]=t[7];u[15]=t[11]; + for(int c=0;c<4;c++){ uint8_t* col=u+c*4; uint8_t a0=col[0],a1=col[1],a2=col[2],a3=col[3]; + col[0]=xt(a0)^(xt(a1)^a1)^a2^a3; col[1]=a0^xt(a1)^(xt(a2)^a2)^a3; + col[2]=a0^a1^xt(a2)^(xt(a3)^a3); col[3]=(xt(a0)^a0)^a1^a2^xt(a3); } + for(int i=0;i<16;i++) s[i]=u[i]^a.rk[r*16+i]; + } + uint8_t t[16]; for(int i=0;i<16;i++)t[i]=S[s[i]]; + uint8_t u[16]; + u[0]=t[0];u[4]=t[4];u[8]=t[8];u[12]=t[12]; + u[1]=t[5];u[5]=t[9];u[9]=t[13];u[13]=t[1]; + u[2]=t[10];u[6]=t[14];u[10]=t[2];u[14]=t[6]; + u[3]=t[15];u[7]=t[3];u[11]=t[7];u[15]=t[11]; + for(int i=0;i<16;i++) out[i]=u[i]^a.rk[160+i]; +} +// CCM core: CBC-MAC over (B0|AAD|payload), CTR for encryption. M=8,L=2. +static void ccm_core(const Aes& a,const uint8_t* nonce,size_t nlen, + const uint8_t* aad,size_t aadlen, + const uint8_t* in,size_t inlen,uint8_t* out,uint8_t mic[8],bool enc){ + uint8_t L=2; uint8_t B[16]={0}; + B[0]=(uint8_t)((aadlen>0?0x40:0)|(((8-2)/2)<<3)|(L-1)); + memcpy(B+1,nonce,nlen); + B[14]=inlen>>8;B[15]=inlen&0xff; + uint8_t X[16]; aes_enc(a,B,X); + if(aadlen>0){ uint8_t hdr[16]={0}; size_t o=0; hdr[0]=aadlen>>8;hdr[1]=aadlen&0xff;o=2; + size_t i=0; while(i>8;A[15]=ctr&0xff; uint8_t Sx[16]; aes_enc(a,A,Sx); + size_t k=inlen-i<16?inlen-i:16; for(size_t j=0;j>8;A[15]=ctr&0xff; uint8_t Sx[16]; aes_enc(a,A,Sx); + size_t k=ptlen-i<16?ptlen-i:16; for(size_t j=0;jsizeof(tmp)) return false; + ccm_core(a,nonce,nlen,aad,aadlen,out,ptlen,tmp,mic,false); + return memcmp(mic,ct+ptlen,8)==0; +} +// AES key cache support: export expanded key + cached-decrypt overload +void aes_key_expand(const uint8_t key[16], Aes& a) { aes_key(a, key); } +bool aes_ccm_decrypt(const uint8_t key[16],const uint8_t* nonce,size_t nlen, + const uint8_t* aad,size_t aadlen,const uint8_t* ct,size_t ctlen,uint8_t* out, + const Aes* aes_cache){ + if(ctlen<8) return false; size_t ptlen=ctlen-8; + const Aes* a = aes_cache; + Aes local; if(!a){ aes_key(local,key); a=&local; } + uint8_t A[16]={0}; A[0]=1; memcpy(A+1,nonce,nlen); + uint16_t ctr=1; size_t i=0; + while(i>8;A[15]=ctr&0xff; uint8_t Sx[16]; aes_enc(*a,A,Sx); + size_t k=ptlen-i<16?ptlen-i:16; for(size_t j=0;jsizeof(tmp)) return false; + ccm_core(*a,nonce,nlen,aad,aadlen,out,ptlen,tmp,mic,false); + return memcmp(mic,ct+ptlen,8)==0; +} + +#if defined(__aarch64__) +#define APFPV_HAVE_AES_HW 1 +// ARM-CE accelerated decrypt — data RX only (NOT handshake). +// Uses inline ASM AESE/AESMC. Separate from SW AES above so the +// EAPOL/4-way handshake always uses the proven SW implementation. +struct AesCeKey { uint8_t rk[176]; }; +static void aes_ce_expand(const uint8_t k[16], AesCeKey& ce){ + static const uint8_t Rcon[11]={0,1,2,4,8,0x10,0x20,0x40,0x80,0x1b,0x36}; + uint8_t* rk=ce.rk; memcpy(rk,k,16); + for(int r=1;r<=10;r++){ + uint8_t prev[16]; memcpy(prev,rk+(r-1)*16,16); + uint8_t tmp[4]; tmp[0]=S[prev[13]]; tmp[1]=S[prev[14]]; tmp[2]=S[prev[15]]; tmp[3]=S[prev[12]]; + tmp[0]^=Rcon[r]; + for(int i=0;i<4;i++) rk[r*16+i]=rk[(r-1)*16+i]^tmp[i]; + for(int i=4;i<16;i++) rk[r*16+i]=rk[(r-1)*16+i]^rk[r*16+i-4]; + } +} +static void aes_enc_ce(const AesCeKey& ce,const uint8_t in[16],uint8_t out[16]){ + const uint8_t* rk=ce.rk; + __asm__ __volatile__( + ".arch_extension crypto\n\t" + "ld1 {v0.16b}, [%[in]]\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" "aesmc v0.16b, v0.16b\n\t" + "ld1 {v1.16b}, [%[pk]], #16\n\t" + "aese v0.16b, v1.16b\n\t" // round 10: SubBytes+ShiftRows, no MixColumns + "ld1 {v1.16b}, [%[pk]]\n\t" // rk[10] + "eor v0.16b, v0.16b, v1.16b\n\t" // FINAL AddRoundKey — was MISSING (broke every block) + "st1 {v0.16b}, [%[out]]\n\t" + : [pk] "+r"(rk) + : [in] "r"(in), [out] "r"(out) + : "v0","v1","v16","cc","memory" + ); +} +#elif defined(APFPV_X86_AESNI) +#define APFPV_HAVE_AES_HW 1 +// x86 AES-NI accelerated decrypt — data RX only (NOT handshake), mirrors the +// ARM-CE path. Same CCM logic; only the block cipher uses AES-NI intrinsics. +#if defined(__GNUC__) +#define APFPV_AES_TGT __attribute__((target("aes,sse2"))) +#else +#define APFPV_AES_TGT +#endif +struct AesCeKey { __m128i rk[11]; }; +APFPV_AES_TGT static inline __m128i x86_kexp(__m128i key, __m128i kga){ + kga = _mm_shuffle_epi32(kga, _MM_SHUFFLE(3,3,3,3)); + key = _mm_xor_si128(key, _mm_slli_si128(key,4)); + key = _mm_xor_si128(key, _mm_slli_si128(key,4)); + key = _mm_xor_si128(key, _mm_slli_si128(key,4)); + return _mm_xor_si128(key, kga); +} +APFPV_AES_TGT static void aes_ce_expand(const uint8_t k[16], AesCeKey& ce){ + ce.rk[0]=_mm_loadu_si128(reinterpret_cast(k)); + ce.rk[1]=x86_kexp(ce.rk[0],_mm_aeskeygenassist_si128(ce.rk[0],0x01)); + ce.rk[2]=x86_kexp(ce.rk[1],_mm_aeskeygenassist_si128(ce.rk[1],0x02)); + ce.rk[3]=x86_kexp(ce.rk[2],_mm_aeskeygenassist_si128(ce.rk[2],0x04)); + ce.rk[4]=x86_kexp(ce.rk[3],_mm_aeskeygenassist_si128(ce.rk[3],0x08)); + ce.rk[5]=x86_kexp(ce.rk[4],_mm_aeskeygenassist_si128(ce.rk[4],0x10)); + ce.rk[6]=x86_kexp(ce.rk[5],_mm_aeskeygenassist_si128(ce.rk[5],0x20)); + ce.rk[7]=x86_kexp(ce.rk[6],_mm_aeskeygenassist_si128(ce.rk[6],0x40)); + ce.rk[8]=x86_kexp(ce.rk[7],_mm_aeskeygenassist_si128(ce.rk[7],0x80)); + ce.rk[9]=x86_kexp(ce.rk[8],_mm_aeskeygenassist_si128(ce.rk[8],0x1b)); + ce.rk[10]=x86_kexp(ce.rk[9],_mm_aeskeygenassist_si128(ce.rk[9],0x36)); +} +APFPV_AES_TGT static void aes_enc_ce(const AesCeKey& ce,const uint8_t in[16],uint8_t out[16]){ + __m128i m=_mm_loadu_si128(reinterpret_cast(in)); + m=_mm_xor_si128(m,ce.rk[0]); + m=_mm_aesenc_si128(m,ce.rk[1]); m=_mm_aesenc_si128(m,ce.rk[2]); + m=_mm_aesenc_si128(m,ce.rk[3]); m=_mm_aesenc_si128(m,ce.rk[4]); + m=_mm_aesenc_si128(m,ce.rk[5]); m=_mm_aesenc_si128(m,ce.rk[6]); + m=_mm_aesenc_si128(m,ce.rk[7]); m=_mm_aesenc_si128(m,ce.rk[8]); + m=_mm_aesenc_si128(m,ce.rk[9]); + m=_mm_aesenclast_si128(m,ce.rk[10]); + _mm_storeu_si128(reinterpret_cast<__m128i*>(out),m); +} +#endif + +#ifdef APFPV_HAVE_AES_HW +// Data RX only: hardware-AES CCMP decrypt (ARM-CE or x86 AES-NI). Returns false +// to fall back to the SW path (e.g. an x86 CPU lacking AES-NI). Only aes_enc_ce +// differs per-arch; the CCM (CTR + CBC-MAC) logic below is shared. +bool aes_ccm_decrypt_ce(const uint8_t key[16],const uint8_t* nonce,size_t nlen, + const uint8_t* aad,size_t aadlen, + const uint8_t* ct,size_t ctlen,uint8_t* out){ + if(ctlen<8) return false; + // A/B knob: DEVOURER_FORCE_SW_AES=1 forces the software-AES fallback (the pre-fix path) + // so the AES-NI win can be measured back-to-back on the same link. + static const bool kForceSw = std::getenv("DEVOURER_FORCE_SW_AES") != nullptr; + if (kForceSw) return false; +#if defined(APFPV_X86_AESNI) && defined(__GNUC__) + // Robust AES-NI detection. __builtin_cpu_supports("aes") returns FALSE on mingw when + // __cpu_model isn't initialized -> silent software fallback (~290us/pkt) even on a CPU + // that HAS AES-NI -> single RX worker CPU-bound at ~39 Mbps. Use CPUID directly: + // leaf 1, ECX bit 25 = AES-NI. (This was the APFPV-vs-wfb RX gap: wfb has no CCMP.) + static const bool kHasAesNi = [](){ + unsigned a,b,c,d; + if (__get_cpuid(1,&a,&b,&c,&d)) return (c & (1u<<25)) != 0; + return false; + }(); + if(!kHasAesNi) return false; // genuinely no AES-NI -> SW fallback +#endif + // TLS cache per worker thread + static thread_local uint8_t ck[16]={}; static thread_local AesCeKey ce; static thread_local bool ok=false; + if(!ok||memcmp(ck,key,16)!=0){ aes_ce_expand(key,ce); memcpy(ck,key,16); ok=true; } + size_t ptlen=ctlen-8; + // CTR decrypt + uint8_t A[16]={0}; A[0]=1; memcpy(A+1,nonce,nlen); + uint16_t ctr=1; size_t i=0; + while(i>8;A[15]=ctr&0xff; uint8_t Sx[16]; aes_enc_ce(ce,A,Sx); + size_t k=ptlen-i<16?ptlen-i:16; for(size_t j=0;j0?0x40:0)|(((8-2)/2)<<3)|(L-1)); + memcpy(B+1,nonce,nlen); B[14]=ptlen>>8;B[15]=ptlen&0xff; + uint8_t X[16]; aes_enc_ce(ce,B,X); + if(aadlen>0){ uint8_t hdr[16]={0}; size_t o=0; hdr[0]=aadlen>>8;hdr[1]=aadlen&0xff;o=2; + size_t j=0; while(j>=1;} return p; } + +void aes128_ecb_encrypt(const uint8_t key[16],const uint8_t in[16],uint8_t out[16]){ + Aes a; aes_key(a,key); aes_enc(a,in,out); +} +void aes128_ecb_decrypt(const uint8_t key[16],const uint8_t in[16],uint8_t out[16]){ + build_isb(); Aes a; aes_key(a,key); + uint8_t s[16]; for(int i=0;i<16;i++) s[i]=in[i]^a.rk[160+i]; + for(int r=9;r>=1;r--){ + // inv shiftrows + uint8_t t[16]; + t[0]=s[0];t[4]=s[4];t[8]=s[8];t[12]=s[12]; + t[1]=s[13];t[5]=s[1];t[9]=s[5];t[13]=s[9]; + t[2]=s[10];t[6]=s[14];t[10]=s[2];t[14]=s[6]; + t[3]=s[7];t[7]=s[11];t[11]=s[15];t[15]=s[3]; + for(int i=0;i<16;i++) t[i]=ISB[t[i]]; // inv subbytes + for(int i=0;i<16;i++) t[i]^=a.rk[r*16+i]; // addroundkey + // inv mixcolumns + for(int c=0;c<4;c++){ uint8_t* col=t+c*4; uint8_t a0=col[0],a1=col[1],a2=col[2],a3=col[3]; + col[0]=mul(a0,14)^mul(a1,11)^mul(a2,13)^mul(a3,9); + col[1]=mul(a0,9)^mul(a1,14)^mul(a2,11)^mul(a3,13); + col[2]=mul(a0,13)^mul(a1,9)^mul(a2,14)^mul(a3,11); + col[3]=mul(a0,11)^mul(a1,13)^mul(a2,9)^mul(a3,14); } + for(int i=0;i<16;i++) s[i]=t[i]; + } + uint8_t t[16]; + t[0]=s[0];t[4]=s[4];t[8]=s[8];t[12]=s[12]; + t[1]=s[13];t[5]=s[1];t[9]=s[5];t[13]=s[9]; + t[2]=s[10];t[6]=s[14];t[10]=s[2];t[14]=s[6]; + t[3]=s[7];t[7]=s[11];t[11]=s[15];t[15]=s[3]; + for(int i=0;i<16;i++) out[i]=ISB[t[i]]^a.rk[i]; +} + +// RFC 3394 AES Key Unwrap. wrapped = (n+1)*8 bytes. out = n*8. +bool aes_key_unwrap(const uint8_t kek[16],const uint8_t* wrapped,size_t wlen,uint8_t* out){ + if(wlen<16 || (wlen%8)!=0) return false; + size_t n=(wlen/8)-1; + uint8_t A[8]; memcpy(A,wrapped,8); + memcpy(out,wrapped+8,n*8); + for(int j=5;j>=0;j--){ + for(size_t i=n;i>=1;i--){ + uint8_t blk[16]; memcpy(blk,A,8); memcpy(blk+8,out+(i-1)*8,8); + uint64_t t=n*(uint64_t)j+i; + for(int k=0;k<8;k++) blk[7-k]^=(uint8_t)(t>>(k*8)); + uint8_t dec[16]; aes128_ecb_decrypt(kek,blk,dec); + memcpy(A,dec,8); memcpy(out+(i-1)*8,dec+8,8); + if(i==1) {} // continue + } + } + static const uint8_t IV[8]={0xA6,0xA6,0xA6,0xA6,0xA6,0xA6,0xA6,0xA6}; + return memcmp(A,IV,8)==0; +} + +// RFC 3394 AES Key Wrap (inverse of unwrap) — wraps plen bytes (multiple of 8, >=16) into +// out[plen+8]. Used by the AP-side 4-way (M3) to deliver the GTK encrypted under the KEK. +void aes_key_wrap(const uint8_t kek[16],const uint8_t* plain,size_t plen,uint8_t* out){ + size_t n=plen/8; + static const uint8_t IV[8]={0xA6,0xA6,0xA6,0xA6,0xA6,0xA6,0xA6,0xA6}; + uint8_t A[8]; memcpy(A,IV,8); + memcpy(out+8,plain,plen); // R[1..n] = P + for(int j=0;j<=5;j++){ + for(size_t i=1;i<=n;i++){ + uint8_t blk[16]; memcpy(blk,A,8); memcpy(blk+8,out+i*8,8); + uint8_t enc[16]; aes128_ecb_encrypt(kek,blk,enc); + memcpy(A,enc,8); + uint64_t t=n*(uint64_t)j+i; + for(int k=0;k<8;k++) A[7-k]^=(uint8_t)(t>>(k*8)); // A = MSB64(B) XOR t + memcpy(out+i*8,enc+8,8); // R[i] = LSB64(B) + } + } + memcpy(out,A,8); // C[0] = A +} +}} diff --git a/src/apfpv/crypto/AesCcm.h b/src/apfpv/crypto/AesCcm.h new file mode 100644 index 0000000..92d69b9 --- /dev/null +++ b/src/apfpv/crypto/AesCcm.h @@ -0,0 +1,36 @@ +#pragma once +#include +#include +namespace apfpv { namespace crypto { +// AES-128 expanded key (cached by caller to avoid per-packet key expansion) +struct Aes { uint8_t rk[176]; }; +void aes_key_expand(const uint8_t key[16], Aes& out); +// AES-128-CCM as used by CCMP (8-byte MIC, 13-byte nonce, L=2). +// decrypt: returns true if MIC valid; out gets plaintext (clen-8 bytes). +// Pass cached expanded key (aes_key_expand) for the hot path, or nullptr. +bool aes_ccm_decrypt(const uint8_t key[16], const uint8_t* nonce, size_t nlen, + const uint8_t* aad, size_t aadlen, + const uint8_t* ct, size_t ctlen, uint8_t* out, + const Aes* aes_cache = nullptr); +// Hardware-AES accelerated decrypt — data RX only. ARM-CE on aarch64, AES-NI on +// x86. Returns false to fall back to the SW path. Mirrors APFPV_HAVE_AES_HW in the .cpp. +#if defined(__aarch64__) || defined(__x86_64__) || defined(_M_X64) || defined(__i386__) || defined(_M_IX86) +bool aes_ccm_decrypt_ce(const uint8_t key[16],const uint8_t* nonce,size_t nlen, + const uint8_t* aad,size_t aadlen, + const uint8_t* ct,size_t ctlen,uint8_t* out); +#endif +void aes_ccm_encrypt(const uint8_t key[16], const uint8_t* nonce, size_t nlen, + const uint8_t* aad, size_t aadlen, + const uint8_t* pt, size_t ptlen, uint8_t* out /* ptlen+8 */); +}} + +namespace apfpv { namespace crypto { +// AES-128 ECB single-block (for key-wrap). out=enc(in) under key. +void aes128_ecb_encrypt(const uint8_t key[16], const uint8_t in[16], uint8_t out[16]); +void aes128_ecb_decrypt(const uint8_t key[16], const uint8_t in[16], uint8_t out[16]); +// RFC 3394 AES Key Unwrap. n = number of 64-bit blocks in wrapped-8. Returns +// true if integrity check (A == A6A6A6A6A6A6A6A6) passes. out = n*8 bytes. +bool aes_key_unwrap(const uint8_t kek[16], const uint8_t* wrapped, size_t wlen, uint8_t* out); +// RFC 3394 AES Key Wrap (inverse). out = (plen+8) bytes. plen multiple of 8, >= 16. +void aes_key_wrap(const uint8_t kek[16], const uint8_t* plain, size_t plen, uint8_t* out); +}} diff --git a/src/apfpv/crypto/AesCmac.cpp b/src/apfpv/crypto/AesCmac.cpp new file mode 100644 index 0000000..e4845bb --- /dev/null +++ b/src/apfpv/crypto/AesCmac.cpp @@ -0,0 +1,66 @@ +// ============================================================================ +// AesCmac.cpp — AES-128-CMAC (RFC 4493) + BIP-CMAC-128 (802.11w), built on the +// vendored aes128_ecb_encrypt. No external deps. Self-tested with RFC 4493. +// ============================================================================ +#include "AesCmac.h" +#include "AesCcm.h" +#include + +namespace apfpv { namespace crypto { + +static void lshift1(const uint8_t in[16], uint8_t out[16]) { + uint8_t carry = 0; + for (int i = 15; i >= 0; --i) { uint16_t v = ((uint16_t)in[i] << 1) | carry; out[i] = (uint8_t)v; carry = (v >> 8) & 1; } +} + +void aes128_cmac(const uint8_t key[16], const uint8_t* msg, size_t len, uint8_t out[16]) { + const uint8_t Rb = 0x87; + uint8_t zero[16] = {0}, L[16], K1[16], K2[16]; + aes128_ecb_encrypt(key, zero, L); // L = AES(K, 0) + lshift1(L, K1); if (L[0] & 0x80) K1[15] ^= Rb; // subkey K1 + lshift1(K1, K2); if (K1[0] & 0x80) K2[15] ^= Rb; // subkey K2 + + size_t n = (len + 15) / 16; + bool complete; + if (n == 0) { n = 1; complete = false; } else complete = (len % 16) == 0; + + uint8_t Mlast[16]; + const uint8_t* last = msg + (n - 1) * 16; + if (complete) { + for (int i = 0; i < 16; ++i) Mlast[i] = last[i] ^ K1[i]; + } else { + uint8_t pad[16] = {0}; + size_t rem = len % 16; + if (rem) std::memcpy(pad, last, rem); + pad[rem] = 0x80; + for (int i = 0; i < 16; ++i) Mlast[i] = pad[i] ^ K2[i]; + } + + uint8_t X[16] = {0}, Y[16]; + for (size_t i = 0; i + 1 < n; ++i) { + for (int j = 0; j < 16; ++j) Y[j] = X[j] ^ msg[i * 16 + j]; + aes128_ecb_encrypt(key, Y, X); + } + for (int j = 0; j < 16; ++j) Y[j] = Mlast[j] ^ X[j]; + aes128_ecb_encrypt(key, Y, out); +} + +void bip_cmac_128(const uint8_t igtk[16], const uint8_t* msg, size_t len, uint8_t mic8[8]) { + uint8_t full[16]; + aes128_cmac(igtk, msg, len, full); + std::memcpy(mic8, full, 8); // BIP-CMAC-128 MIC = first 8 bytes +} + +bool aes_cmac_selftest() { + const uint8_t key[16] = {0x2b,0x7e,0x15,0x16,0x28,0xae,0xd2,0xa6,0xab,0xf7,0x15,0x88,0x09,0xcf,0x4f,0x3c}; + uint8_t mac[16]; + aes128_cmac(key, nullptr, 0, mac); // empty message + static const uint8_t exp0[16] = {0xbb,0x1d,0x69,0x29,0xe9,0x59,0x37,0x28,0x7f,0xa3,0x7d,0x12,0x9b,0x75,0x67,0x46}; + if (std::memcmp(mac, exp0, 16) != 0) return false; + const uint8_t m16[16] = {0x6b,0xc1,0xbe,0xe2,0x2e,0x40,0x9f,0x96,0xe9,0x3d,0x7e,0x11,0x73,0x93,0x17,0x2a}; + aes128_cmac(key, m16, 16, mac); // single full block + static const uint8_t exp16[16] = {0x07,0x0a,0x16,0xb4,0x6b,0x4d,0x41,0x44,0xf7,0x9b,0xdd,0x9d,0xd0,0x4a,0x28,0x7c}; + return std::memcmp(mac, exp16, 16) == 0; +} + +}} // namespace apfpv::crypto diff --git a/src/apfpv/crypto/AesCmac.h b/src/apfpv/crypto/AesCmac.h new file mode 100644 index 0000000..7f3f81f --- /dev/null +++ b/src/apfpv/crypto/AesCmac.h @@ -0,0 +1,12 @@ +#pragma once +#include +#include +namespace apfpv { namespace crypto { +// AES-128-CMAC (RFC 4493). Writes a 16-byte MAC of msg[0..len) under key into out. +void aes128_cmac(const uint8_t key[16], const uint8_t* msg, size_t len, uint8_t out[16]); +// BIP-CMAC-128 (802.11w): 8-byte MIC = first 8 bytes of AES-128-CMAC. Used to verify +// protected broadcast/multicast management frames against the IGTK. +void bip_cmac_128(const uint8_t igtk[16], const uint8_t* msg, size_t len, uint8_t mic8[8]); +// RFC 4493 known-answer self-test (empty + 16-byte message). True == correct. +bool aes_cmac_selftest(); +}} diff --git a/src/apfpv/crypto/Sha1Hmac.cpp b/src/apfpv/crypto/Sha1Hmac.cpp new file mode 100644 index 0000000..b542193 --- /dev/null +++ b/src/apfpv/crypto/Sha1Hmac.cpp @@ -0,0 +1,46 @@ +#include "Sha1Hmac.h" +#include +namespace apfpv { namespace crypto { +struct Sha1Ctx { uint32_t h[5]; uint64_t len; uint8_t buf[64]; size_t n; }; +static inline uint32_t rol(uint32_t v,int b){ return (v<>(32-b)); } +static void sha1_init(Sha1Ctx& c){ c.h[0]=0x67452301;c.h[1]=0xEFCDAB89;c.h[2]=0x98BADCFE;c.h[3]=0x10325476;c.h[4]=0xC3D2E1F0;c.len=0;c.n=0; } +static void sha1_block(Sha1Ctx& c,const uint8_t* p){ + uint32_t w[80]; + for(int i=0;i<16;i++) w[i]=(p[i*4]<<24)|(p[i*4+1]<<16)|(p[i*4+2]<<8)|p[i*4+3]; + for(int i=16;i<80;i++) w[i]=rol(w[i-3]^w[i-8]^w[i-14]^w[i-16],1); + uint32_t a=c.h[0],b=c.h[1],d=c.h[2],e=c.h[3],f=c.h[4]; + for(int i=0;i<80;i++){ uint32_t k,t; + if(i<20){k=0x5A827999;t=(b&d)|((~b)&e);} + else if(i<40){k=0x6ED9EBA1;t=b^d^e;} + else if(i<60){k=0x8F1BBCDC;t=(b&d)|(b&e)|(d&e);} + else{k=0xCA62C1D6;t=b^d^e;} + uint32_t tmp=rol(a,5)+t+f+k+w[i]; f=e;e=d;d=rol(b,30);b=a;a=tmp; } + c.h[0]+=a;c.h[1]+=b;c.h[2]+=d;c.h[3]+=e;c.h[4]+=f; +} +static void sha1_update(Sha1Ctx& c,const uint8_t* p,size_t len){ + c.len+=len; + while(len){ size_t k=64-c.n; if(k>len)k=len; memcpy(c.buf+c.n,p,k); c.n+=k;p+=k;len-=k; + if(c.n==64){sha1_block(c,c.buf);c.n=0;} } +} +static void sha1_final(Sha1Ctx& c,uint8_t out[20]){ + uint64_t bits=c.len*8; uint8_t pad=0x80; sha1_update(c,&pad,1); + uint8_t z=0; while(c.n!=56) sha1_update(c,&z,1); + uint8_t lb[8]; for(int i=0;i<8;i++) lb[i]=(uint8_t)(bits>>(56-i*8)); sha1_update(c,lb,8); + for(int i=0;i<5;i++){out[i*4]=c.h[i]>>24;out[i*4+1]=c.h[i]>>16;out[i*4+2]=c.h[i]>>8;out[i*4+3]=c.h[i];} +} +void sha1(const uint8_t* d,size_t l,uint8_t o[20]){ Sha1Ctx c; sha1_init(c); sha1_update(c,d,l); sha1_final(c,o); } +void hmac_sha1(const uint8_t* key,size_t kl,const uint8_t* msg,size_t ml,uint8_t out[20]){ + uint8_t k[64]={0}; if(kl>64){ sha1(key,kl,k);} else memcpy(k,key,kl); + uint8_t ip[64],op[64]; for(int i=0;i<64;i++){ip[i]=k[i]^0x36;op[i]=k[i]^0x5c;} + uint8_t ih[20]; Sha1Ctx c; sha1_init(c); sha1_update(c,ip,64); sha1_update(c,msg,ml); sha1_final(c,ih); + sha1_init(c); sha1_update(c,op,64); sha1_update(c,ih,20); sha1_final(c,out); +} +void pbkdf2_sha1(const char* pass,const uint8_t* salt,size_t sl,unsigned iters,uint8_t* out,size_t outlen){ + size_t pl=strlen(pass); unsigned block=1; + while(outlen){ uint8_t u[20],t[20],s2[256]; size_t s2l=sl; + memcpy(s2,salt,sl); s2[s2l++]=block>>24;s2[s2l++]=block>>16;s2[s2l++]=block>>8;s2[s2l++]=block; + hmac_sha1((const uint8_t*)pass,pl,s2,s2l,u); memcpy(t,u,20); + for(unsigned i=1;i +#include +namespace apfpv { namespace crypto { +void sha1(const uint8_t* data, size_t len, uint8_t out[20]); +void hmac_sha1(const uint8_t* key, size_t klen, const uint8_t* msg, size_t mlen, uint8_t out[20]); +void pbkdf2_sha1(const char* pass, const uint8_t* salt, size_t saltlen, + unsigned iters, uint8_t* out, size_t outlen); +}} diff --git a/src/jaguar1/PhydmWatchdog.cpp b/src/jaguar1/PhydmWatchdog.cpp index f0bd3a3..88dd5e7 100644 --- a/src/jaguar1/PhydmWatchdog.cpp +++ b/src/jaguar1/PhydmWatchdog.cpp @@ -179,6 +179,36 @@ void PhydmWatchdog::ResetFaCountersAc() { PhydmWatchdog::FaCnt PhydmWatchdog::LastFaCnt() const { return _lastFaCnt; } +// --------------------------------------------------------------------- +// APFPV additions -- static link-state feedback (see header for the why). +// --------------------------------------------------------------------- +std::atomic PhydmWatchdog::_s_linked{false}; +std::atomic PhydmWatchdog::_s_rssiDbm{-128}; +std::atomic PhydmWatchdog::_s_uplinkRate{-1}; + +void PhydmWatchdog::SetLinkRssi(int rssi_dbm) { + _s_rssiDbm.store(rssi_dbm, std::memory_order_relaxed); + _s_linked.store(true, std::memory_order_relaxed); +} + +void PhydmWatchdog::SetUnlinked() { + _s_linked.store(false, std::memory_order_relaxed); + _s_uplinkRate.store(-1, std::memory_order_relaxed); +} + +int PhydmWatchdog::RssiDbm() { + return _s_rssiDbm.load(std::memory_order_relaxed); +} + +void PhydmWatchdog::OnUplinkRate(uint8_t rate_idx) { + _s_uplinkRate.store((int)rate_idx, std::memory_order_relaxed); +} + +uint8_t PhydmWatchdog::UplinkRate() { + int r = _s_uplinkRate.load(std::memory_order_relaxed); + return r < 0 ? 0xff : static_cast(r); +} + void PhydmWatchdog::DigInit() { /* Port of `phydm_dig_init` (phydm_dig.c:726). Reads current * BB 0xc50 byte 0 as the initial IGI value. Bounds match the @@ -216,11 +246,45 @@ void PhydmWatchdog::DigTick(uint32_t fa_cnt) { constexpr uint8_t kStepDown = 2; /* fa < kFaTh0 */ /* Refresh bounds from abs_boundary_decision + dym_boundary_decision - * each tick (cheap, makes the !is_linked behaviour explicit). */ - _dm_dig_max = 0x26; - _dm_dig_min = 0x1c; - _rx_gain_range_max = _dig_max_of_min; - _rx_gain_range_min = _dm_dig_min; + * each tick. Two regimes: + * + * monitor (!is_linked, wfb-ng): coverage bounds — dm_dig_max=0x26, + * floor=0x1c, ceiling=dig_max_of_min(0x2a). Keeps gain high for + * long-range sensitivity at the cost of false alarms. + * + * connected (is_linked, station): phydm connected bounds — + * dm_dig_max=0x3e, dm_dig_min=0x20, and the dynamic floor tracks + * RSSI: rx_gain_range_min = clamp(rssi_dBm+100, 0x20, 0x3e). The + * floor-clamp below then snaps cur_ig straight to that operating + * point (e.g. -45 dBm → 0x37), matching the kernel 88XXau driver + * and avoiding the strong-signal over-gain that storms the FA + * counter and makes the AP rate-cap us. */ + if (_s_linked.load(std::memory_order_relaxed)) { + _dm_dig_max = 0x3e; + _dm_dig_min = 0x20; + int rssi_val = _s_rssiDbm.load(std::memory_order_relaxed) + 100; + if (rssi_val < _dm_dig_min) rssi_val = _dm_dig_min; + // Cap the RSSI-derived IGI FLOOR at the kernel's strong-signal operating point (~0x37). The + // dBm+100 map overshoots for a point-blank signal (-20 dBm -> 0x50 -> clamps to the 0x3e max), + // which forces the front-end DEAF (RXPKT_NUM=0, no data, DHCP/SSH fail) even though the kernel + // runs ~0x37 fine. Capping the FLOOR at 0x38 keeps enough desensitization to kill the close- + // range saturation FA storm while still hearing the AP. Ceiling stays 0x3e so a genuine FA + // storm can still walk higher; a clean strong signal settles at ~0x38. + constexpr int kStrongFloorCap = 0x38; + bool strongCapped = (rssi_val > kStrongFloorCap); + if (strongCapped) rssi_val = kStrongFloorCap; + _rx_gain_range_min = static_cast(rssi_val); + // For a STRONG signal, PIN the ceiling to the operating point too. Otherwise the FA-driven walk + // climbs IGI above the floor (0x38 -> 0x3c -> 0x3e) chasing a spurious close-range false-alarm + // count and goes DEAF (RXPKT_NUM=0) — even though 0x38 receives fine (DHCP/assoc succeed there). + // Weaker signals keep the full [floor,0x3e] range so the walk can still adapt to real FA. + _rx_gain_range_max = strongCapped ? static_cast(kStrongFloorCap) : _dm_dig_max; + } else { + _dm_dig_max = 0x26; + _dm_dig_min = 0x1c; + _rx_gain_range_max = _dig_max_of_min; + _rx_gain_range_min = _dm_dig_min; + } uint8_t new_igi = _cur_ig_value; if (fa_cnt > kFaTh2) { diff --git a/src/jaguar1/PhydmWatchdog.h b/src/jaguar1/PhydmWatchdog.h index 4b2797a..5fdbd37 100644 --- a/src/jaguar1/PhydmWatchdog.h +++ b/src/jaguar1/PhydmWatchdog.h @@ -73,7 +73,32 @@ class PhydmWatchdog { }; FaCnt LastFaCnt() const; + // --------------------------------------------------------------------- + // APFPV additions -- the close-range DIG-saturation fix. Upstream's + // DigTick is "always !is_linked since devourer doesn't track link state" + // (its own comment) -- correct for wfb-ng's monitor-mode-only use, but + // APFPV's station-mode path needs the DIG floor to track live RSSI once + // connected, or a close-range/strong signal storms the FA counter and the + // front-end goes deaf (RXPKT_NUM=0, no data). Static: DigTick is called + // from a shared watchdog tick path regardless of which station instance + // is live, matching the pre-rebuild WiFiDriver's own static design. + // Validated on real hardware: this exact fix (RSSI-derived floor + a + // strong-signal ceiling pin) took close-range connect from failing + // outright to a stable ~1-3m/SNR25-40 sweet spot. + // --------------------------------------------------------------------- + static void SetLinkRssi(int rssi_dbm); + static void SetUnlinked(); + static int RssiDbm(); // last RX RSSI in dBm (-128 if never set) + // Firmware-RA uplink rate feedback: payload[0] bit7=SGI, [6:0]=rate index, + // payload[1]=macid. FrameParser calls OnUplinkRate on the matching C2H + // report; 0xff means "none yet". + static void OnUplinkRate(uint8_t rate_idx); + static uint8_t UplinkRate(); + private: + static std::atomic _s_linked; + static std::atomic _s_rssiDbm; + static std::atomic _s_uplinkRate; void ThreadLoop(); /* Port of `phydm_fa_cnt_statistics_ac` (phydm_dig.c:1421). Reads * OFDM/CCK FA + CCA + CRC32 counters from page-F BB registers. */ diff --git a/src/jaguar1/RadioManagementModule.cpp b/src/jaguar1/RadioManagementModule.cpp index 9e690e4..0e127f1 100644 --- a/src/jaguar1/RadioManagementModule.cpp +++ b/src/jaguar1/RadioManagementModule.cpp @@ -254,6 +254,16 @@ static uint8_t rtw_get_center_ch(uint8_t channel, ChannelWidth_t chnl_bw, return center_ch; } +uint8_t RadioManagementModule::prime_offset_40mhz(uint8_t channel) const { + // Primary below the center => LOWER; above the center => UPPER. + // Matches WFB working 40MHz rule: ch44 center=46, primary(44)
LOWER. + // (The offset naming is about PRIMARY position relative to center, not secondary.) + int center = get_40mhz_center_channel(channel); + if (center > channel) return HAL_PRIME_CHNL_OFFSET_LOWER; // primary below center + if (center < channel) return HAL_PRIME_CHNL_OFFSET_UPPER; // primary above center + return HAL_PRIME_CHNL_OFFSET_DONT_CARE; +} + void RadioManagementModule::set_channel_bwmode(uint8_t channel, uint8_t channel_offset, ChannelWidth_t bwmode) { @@ -593,6 +603,22 @@ void RadioManagementModule::PHY_HandleSwChnlAndSetBW8812( _currentCenterFrequencyIndex = CenterFrequencyIndex1; } +#if defined(__ANDROID__) + // APFPV addition: verify actual bandwidth/channel by reading the RF + // synthesizer and BW registers back. _lastRfCh feeds rf_wedged() (0xea = + // the RF synth locked up and won't tune to any channel until a USB port + // reset) -- only meaningful after a tune on Android, where this readback + // runs (matches the pre-rebuild WiFiDriver's placement/behavior). + { + uint32_t bb834 = phy_query_bb_reg(0x0834, 0x00000003); + uint32_t rf_ch = phy_query_rf_reg(RfPath::RF_PATH_A, 0x18, 0x000000FF); + _lastRfCh = rf_ch; + __android_log_print(ANDROID_LOG_INFO, "apfpv-scan", + "RF VERIFY: primary=%d bw=%d | BB834[1:0]=%d RF_CH=0x%x", + (int)ChannelNum, (int)ChnlWidth, (int)bb834, (unsigned)rf_ch); + } +#endif + /* Switch workitem or set timer to do switch channel or setbandwidth operation */ phy_SwChnlAndSetBwMode8812(); @@ -681,7 +707,13 @@ void RadioManagementModule::phy_SwChnlAndSetBwMode8812() { * `HalModule::rtl8812au_hal_init` → `ArmIQKOnNextChannelSet`. * Optional override: `DEVOURER_FORCE_IQK=1` runs IQK on every * channel-set (used for canary-diff workflow against kernel). */ - if ((_needIQK || _tuning.force_iqk) && !_tuning.disable_iqk) { + // APFPV addition: _scanMode suppresses IQK during a scan sweep (fast + // unsettled retunes across channels/bands wedge the RF synth if IQK runs + // mid-sweep -- RF_CH reads back 0xea and stays wedged for the rest of the + // sweep). Listening for beacons needs no TX calibration; IQK still runs + // once the sweep settles on the real operating channel (setScanMode(false) + // before that final arm). + if ((_needIQK || _tuning.force_iqk) && !_tuning.disable_iqk && !_scanMode) { if (_eepromManager->version_id.ICType == CHIP_8812) { _iqk.Calibrate(_currentChannel, current_band_type, /*is_recovery=*/false); @@ -2979,3 +3011,86 @@ void RadioManagementModule::PHY_TxPowerTrainingByPath_8812(RfPath rfPath) { _device.phy_set_bb_reg(writeOffset, 0xffffff, writeData); } + +// --------------------------------------------------------------------------- +// APFPV addition -- see the header comment above SetStationRxFilter's +// declaration. Ported verbatim from the pre-rebuild WiFiDriver; only +// hw_var_rcr_config()/rtw_write16()/rtw_read32()/rtw_read8() call sites are +// this class's own (upstream already uses the same primitives internally). +// --------------------------------------------------------------------------- +void RadioManagementModule::SetStationRxFilter() { + // Kernel-exact station RCR (the A-MPDU fix). The kernel's usb_halinit.c + // station RCR is APM | AM | AB | CBSSID_DATA | CBSSID_BCN | HTC_LOC_CTRL | + // AMF | ... and explicitly does NOT set RCR_AAP. RCR_AAP (promiscuous + // "accept all unicast") puts the MAC in a monitor-style path that BYPASSES + // the infrastructure-station RX state machine -- including HW auto-Block- + // Ack generation for A-MPDU. With AAP the chip ACKs single frames + // (FORCEACK) but never auto-BAs aggregates, so the AP sends ADDBA, gets no + // BA, and tears the session down with DELBA -> single-frame fallback -> + // ~25 Mbps ceiling. CBSSID_DATA (match REG_BSSID, set to the AP) + APM + // (match our MAC) is the real station path that triggers auto-BA -> + // A-MPDU -> 65Mbps. DEVOURER_RCR_AAP=1 restores the old promiscuous + // behavior for A/B comparison. DEVOURER_RCR_WINDOWS enables the Windows + // RCR (FORCEACK=0, needs FWOffload) for A/B comparison. + uint32_t rcr; + if (std::getenv("DEVOURER_RCR_WINDOWS")) { + rcr = 0x3f6b00f4; // Windows RCR (FORCEACK=0, needs FWOffload) + } else if (std::getenv("DEVOURER_RCR_AAP")) { + rcr = RCR_AAP | RCR_APM | RCR_AM | RCR_AB | RCR_APWRMGT | + RCR_ADF | RCR_ACF | RCR_AMF | RCR_APP_PHYST_RXFF | RCR_APPFCS | FORCEACK; + } else { + rcr = RCR_APM | RCR_AM | RCR_AB | RCR_CBSSID_DATA | RCR_CBSSID_BCN | + RCR_HTC_LOC_CTRL | RCR_AMF | RCR_APP_PHYST_RXFF | RCR_APPFCS | FORCEACK | + // RCR_VHT_DACK (BIT26): response type for a VHT SINGLE-MPDU data + // packet -- 1 = reply with a normal ACK, 0 = reply with a + // BlockAck. The kernel sets 1 (its RCR=0xce6000f4); with 0 our HW + // answered the AP's single VHT MPDUs with a compressed BlockAck + // instead of the ACK the AP expects -> the AP saw no ack -> + // retransmitted each frame to the retry limit, the measured + // retransmit storm that collapsed paced throughput. + RCR_VHT_DACK; + // RCR_APP_MIC (BIT30) + RCR_APP_ICV (BIT29) -- the MAC retains the CCMP + // MIC/ICV at the bottom of each RX frame. Only needed when HW decrypt is + // on (RxDeframe's HW-decrypt path strips CCMP-hdr(8)+MIC(8)=16 bytes, + // which is only correct if the HW kept the MIC -- without this the -16 + // eats 8 bytes of real payload off every frame -> corrupt NALUs -> black + // screen). Default off (the proven SW-CCMP path). DEVOURER_RCR_APP or + // DEVOURER_HW_DECRYPT opts in. + bool rcrApp = std::getenv("DEVOURER_RCR_APP") != nullptr + || std::getenv("DEVOURER_HW_DECRYPT") != nullptr; +#if defined(__ANDROID__) + if (!rcrApp) { + char v[PROP_VALUE_MAX] = {0}; + if (__system_property_get("debug.pixelpilot.rcrapp", v) > 0 && v[0] != '0') rcrApp = true; + char h[PROP_VALUE_MAX] = {0}; + if (__system_property_get("debug.pixelpilot.hwdec", h) > 0 && h[0] != '0') rcrApp = true; + } +#endif + if (rcrApp) rcr |= (1u << 30) | (1u << 29); + } + hw_var_rcr_config(rcr); + // TEST 2 (DEVOURER_T2_KFLT): match the kernel's RX filter exactly. The + // kernel leaves RXFLTMAP0 at chip default and sets RXFLTMAP1 = BIT8|BIT10 + // (BAR + PS-Poll, not the BA subtype). Default keeps our permissive filter. + if (std::getenv("DEVOURER_T2_KFLT")) { + _device.rtw_write16(REG_RXFLTMAP2, 0xFFFF); + _device.rtw_write16(REG_RXFLTMAP1, BIT8 | BIT10); // kernel: BAR + PS-Poll only + } else { + // RXFLTMAP0: accept ALL management frame subtypes (auth, assoc, action, etc.) + _device.rtw_write16(REG_RXFLTMAP0, 0xFFFF); + _device.rtw_write16(REG_RXFLTMAP2, 0xFFFF); // keep accepting all data-frame subtypes + // RXFLTMAP1: accept BAR (subtype 8) + BA (subtype 9) + PS-Poll (subtype 10). + _device.rtw_write16(REG_RXFLTMAP1, BIT8 | BIT9 | BIT10); + } + // Read REG_RCR back to prove the filter stuck. + uint32_t rb = _device.rtw_read32(REG_RCR); + _logger->info("[SetStationRxFilter] RCR set=0x{:08x} readback=0x{:08x} AAP={} FORCEACK={}", + rcr, rb, (rb & RCR_AAP) ? 1 : 0, (rb & FORCEACK) ? 1 : 0); +#if defined(__ANDROID__) + // Mirror to logcat (apfpv-scan) -- the spdlog _logger doesn't reach logcat. + uint8_t m[6]; for (int i=0;i<6;i++) m[i]=_device.rtw_read8(0x0610+i); + __android_log_print(4, "apfpv-scan", + "[StationRxFilter] RCR=0x%08x AAP=%d FORCEACK=%d MACID=%02x:%02x:%02x:%02x:%02x:%02x", + rb, (rb&RCR_AAP)?1:0, (rb&FORCEACK)?1:0, m[0],m[1],m[2],m[3],m[4],m[5]); +#endif +} diff --git a/src/jaguar1/RadioManagementModule.h b/src/jaguar1/RadioManagementModule.h index e625e76..505153a 100644 --- a/src/jaguar1/RadioManagementModule.h +++ b/src/jaguar1/RadioManagementModule.h @@ -73,6 +73,11 @@ class RadioManagementModule { bool _warned_uncharacterized = false; /* one-shot extended-channel warning */ ChannelWidth_t _currentChannelBw; uint8_t _currentChannel; + // APFPV additions -- see rf_wedged()/setScanMode() below for what these + // gate. No upstream equivalent (no scan-mode/RF-wedge-recovery concept + // there); ported member state from the pre-rebuild WiFiDriver. + uint32_t _lastRfCh = 0; + bool _scanMode = false; BandType current_band_type; bool _swChannel = false; bool _channelBwInitialized = false; @@ -174,6 +179,37 @@ class RadioManagementModule { void ArmIQKOnNextChannelSet() { _needIQK = true; } void hw_var_rcr_config(uint32_t rcr); void SetMonitorMode(); + // APFPV addition: kernel-exact infrastructure-station RCR (the A-MPDU + // fix -- RCR_AAP/promiscuous bypasses the HW auto-Block-Ack engine for + // aggregates, causing an ADDBA/DELBA/single-frame-fallback ~25Mbps + // ceiling; APM+CBSSID_DATA routes through the real station RX path so + // the chip auto-BAs aggregates -> full-rate delivery). No upstream + // equivalent exists (no client-association station-mode flow there at + // all) -- built on the same hw_var_rcr_config()/rtw_write16() primitives + // upstream's own _InitWMACSetting_8812A() uses, logic unchanged from the + // pre-rebuild WiFiDriver. + void SetStationRxFilter(); + /* The channel the radio is currently tuned to (set by set_channel_bwmode). + * Used by StationMode to pick a band-correct mgmt TX rate: 5 GHz (ch>14) + * has no CCK, so auth/assoc must go at OFDM, like the kernel. */ + uint8_t current_channel() const { return _currentChannel; } + /* True when the RF synthesizer is wedged (RF 0x18 low byte reads 0xea after + * a tune) -- it won't lock to any channel until a USB port reset. _lastRfCh + * is refreshed by the RF-0x18 verify-read inside PHY_HandleSwChnlAndSetBW8812 + * (Android only, matching the pre-rebuild WiFiDriver's placement) -- reads as + * "not wedged" on non-Android builds, where that readback doesn't run. */ + bool rf_wedged() const { return (_lastRfCh & 0xFF) == 0xea; } + /* Suppress IQK during the scan sweep (fast unsettled retunes wedge the RF + * synth if IQK runs mid-sweep). Set true around the channel-hop loop, + * false before arming the real operating channel. Wired into the IQK + * trigger inside PHY_HandleSwChnlAndSetBW8812. */ + void setScanMode(bool s) { _scanMode = s; } + /* Primary-channel offset (LOWER/UPPER of the 40 MHz center) for a given + * 20 MHz primary. Required for 40 MHz HT: without it _cur40MhzPrimeSc stays + * DONT_CARE and the VHT/HT subcarrier mapping is undefined ("SCMapping: + * DONOT CARE Mode Setting" in the vendor log) -- RX of the AP's primary- + * channel frames comes back garbage. */ + uint8_t prime_offset_40mhz(uint8_t channel) const; void set_channel_bwmode(uint8_t channel, uint8_t channel_offset, ChannelWidth_t bwmode); /* Lean frequency-hop retune. Runs ONLY the RF channel switch (phy_SwChnl),