diff --git a/kernel/src/Api/Audio.hpp b/kernel/src/Api/Audio.hpp index 7aa2c34..bf495bf 100644 --- a/kernel/src/Api/Audio.hpp +++ b/kernel/src/Api/Audio.hpp @@ -56,7 +56,10 @@ namespace montauk::abi { (Drivers::Audio::Mixer::Output)value); if (cmd == AUDIO_CTL_BT_STATUS) { if (!Drivers::USB::Bluetooth::IsInitialized()) return 0; - return (int64_t)Drivers::USB::Bluetooth::A2dp::GetState(); + if (Drivers::USB::Bluetooth::A2dp::IsReady()) return 2; + return Drivers::USB::Bluetooth::A2dp::GetState() + == Drivers::USB::Bluetooth::A2dp::State::Idle + ? 0 : 1; } return (int64_t)Drivers::Audio::Mixer::Control(handle, cmd, value); diff --git a/kernel/src/Api/BuildNo.hpp b/kernel/src/Api/BuildNo.hpp index 7ede517..50f8da1 100644 --- a/kernel/src/Api/BuildNo.hpp +++ b/kernel/src/Api/BuildNo.hpp @@ -12,4 +12,4 @@ #pragma once -#define MONTAUK_BUILD_NUMBER 30 +#define MONTAUK_BUILD_NUMBER 33 diff --git a/kernel/src/Api/Syscall.hpp b/kernel/src/Api/Syscall.hpp index eb539aa..6753427 100644 --- a/kernel/src/Api/Syscall.hpp +++ b/kernel/src/Api/Syscall.hpp @@ -167,7 +167,7 @@ namespace montauk::abi { static constexpr int AUDIO_CTL_PAUSE = 3; static constexpr int AUDIO_CTL_GET_OUTPUT = 4; // 0=HDA, 1=Bluetooth static constexpr int AUDIO_CTL_SET_OUTPUT = 5; // Switch audio output - static constexpr int AUDIO_CTL_BT_STATUS = 6; // Get Bluetooth status + static constexpr int AUDIO_CTL_BT_STATUS = 6; // 0=unavailable, 1=setup, 2=ready static constexpr int AUDIO_CTL_SET_MASTER_VOLUME = 7; // 0-100 static constexpr int AUDIO_CTL_GET_MASTER_VOLUME = 8; static constexpr int AUDIO_CTL_SET_MUTE = 9; // 0/1, per-stream diff --git a/kernel/src/Drivers/Audio/Mixer.cpp b/kernel/src/Drivers/Audio/Mixer.cpp index f5b3491..85cbda8 100644 --- a/kernel/src/Drivers/Audio/Mixer.cpp +++ b/kernel/src/Drivers/Audio/Mixer.cpp @@ -9,6 +9,7 @@ #include "IntelHda.hpp" #include #include +#include #include #include #include @@ -142,10 +143,7 @@ namespace Drivers::Audio::Mixer { static bool BluetoothReady() { if (!Drivers::USB::Bluetooth::IsInitialized()) return false; - auto state = Drivers::USB::Bluetooth::A2dp::GetState(); - return state == Drivers::USB::Bluetooth::A2dp::State::Configured || - state == Drivers::USB::Bluetooth::A2dp::State::Open || - state == Drivers::USB::Bluetooth::A2dp::State::Streaming; + return Drivers::USB::Bluetooth::A2dp::IsReady(); } static void SyncHdaMasterMute() { @@ -664,6 +662,8 @@ namespace Drivers::Audio::Mixer { if (changed) BumpSerialLocked(); g_lock.Release(); + if (changed) + Drivers::USB::Bluetooth::Avrcp::NotifyVolumeChanged(percent); if (changed) Sched::WakeObjectWaiters((void*)&g_serial); } diff --git a/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp b/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp index 10cda25..aabfd6d 100644 --- a/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/A2dp.cpp @@ -35,6 +35,7 @@ namespace Drivers::USB::Bluetooth::A2dp { constexpr uint8_t AVDTP_SUSPEND = 0x09; constexpr uint8_t AVDTP_ABORT = 0x0A; constexpr uint8_t AVDTP_GET_ALL_CAPABILITIES = 0x0C; // AVDTP 1.3 + constexpr uint8_t AVDTP_DELAYREPORT = 0x0D; // AVDTP message types constexpr uint8_t MSG_COMMAND = 0x00; @@ -66,8 +67,9 @@ namespace Drivers::USB::Bluetooth::A2dp { // ========================================================================= static std::atomic g_state{State::Idle}; - static uint16_t g_sigCid = 0; // L2CAP CID for AVDTP signaling + static std::atomic g_sigCid{0}; // L2CAP CID for AVDTP signaling static std::atomic g_mediaCid{0}; // L2CAP CID for AVDTP media transport + static std::atomic g_aclHandle{0}; static uint8_t g_txLabel = 1; static uint8_t g_remoteSeid = 0; // Remote stream endpoint ID static uint8_t g_localSeid = 1; // Our local SEID @@ -87,8 +89,10 @@ namespace Drivers::USB::Bluetooth::A2dp { // on a response for. ProcessAvdtp only accepts a response that matches both, // so the headset's own AVDTP traffic (it opens channels and issues its own // commands) cannot be mistaken for our reply and desync the handshake. - static uint8_t g_expectLabel = 0xFF; - static uint8_t g_expectSignal = 0xFF; + static std::atomic g_expectLabel{0xFF}; + static std::atomic g_expectSignal{0xFF}; + static std::atomic g_peerConfigured{false}; + static std::atomic g_sourceSetupActive{false}; // SBC encoder static Sbc::SbcEncoder g_sbcEncoder = {}; @@ -138,22 +142,23 @@ namespace Drivers::USB::Bluetooth::A2dp { static std::atomic g_requestedVolume{-1}; // AVDTP response tracking - static volatile bool g_avdtpResponseReady = false; - static uint8_t g_avdtpResponseBuf[128] = {}; + static std::atomic g_avdtpResponseReady{false}; + static uint8_t g_avdtpResponseBuf[672] = {}; // default Basic L2CAP MTU static uint32_t g_avdtpResponseLen = 0; // SDP (service discovery) state. Many A2DP sinks refuse to engage AVDTP // until the source has queried their service record, so we do a minimal SDP // ServiceSearchAttribute query for the AudioSink service first. static uint16_t g_sdpCid = 0; - static volatile bool g_sdpRspReady = false; + static std::atomic g_sdpRspReady{false}; // ========================================================================= // AVDTP signaling helpers // ========================================================================= - static void SendAvdtpCommand(uint8_t signalId, const uint8_t* payload, uint16_t len) { + static bool SendAvdtpCommand(uint8_t signalId, const uint8_t* payload, uint16_t len) { uint8_t buf[128] = {}; + if (2u + len > sizeof(buf)) return false; // AVDTP single packet header uint8_t lbl = g_txLabel; @@ -167,17 +172,24 @@ namespace Drivers::USB::Bluetooth::A2dp { // Record what we're waiting for and discard any stale response, so only // the matching reply satisfies WaitAvdtpResponse (see g_expectLabel). - g_expectLabel = lbl; - g_expectSignal = signalId; - g_avdtpResponseReady = false; + g_expectLabel.store(lbl, std::memory_order_relaxed); + g_expectSignal.store(signalId, std::memory_order_relaxed); + g_avdtpResponseReady.store(false, std::memory_order_release); g_avdtpResponseLen = 0; - L2cap::SendData(g_sigCid, buf, 2 + len); + if (!L2cap::SendData(g_sigCid.load(std::memory_order_acquire), + buf, 2 + len)) { + g_expectLabel.store(0xFF, std::memory_order_relaxed); + g_expectSignal.store(0xFF, std::memory_order_relaxed); + return false; + } + return true; } - static void SendAvdtpResponse(uint8_t txLabel, uint8_t signalId, + static void SendAvdtpResponse(uint16_t cid, uint8_t txLabel, uint8_t signalId, const uint8_t* payload, uint16_t len) { uint8_t buf[128] = {}; + if (2u + len > sizeof(buf)) return; buf[0] = (txLabel << 4) | (PKT_SINGLE << 2) | MSG_RESPONSE_ACCEPT; buf[1] = signalId; @@ -186,7 +198,7 @@ namespace Drivers::USB::Bluetooth::A2dp { memcpy(&buf[2], payload, len); } - L2cap::SendData(g_sigCid, buf, 2 + len); + L2cap::SendData(cid, buf, 2 + len); } // ========================================================================= @@ -194,17 +206,29 @@ namespace Drivers::USB::Bluetooth::A2dp { // ========================================================================= static bool WaitAvdtpResponse(uint32_t timeoutMs = 3000) { - g_avdtpResponseReady = false; + // SendAvdtpCommand() clears this flag *before* putting the command on + // the wire. Clearing it again here loses a fast response delivered by + // another core between SendAvdtpCommand() and this function -- the + // resulting timeout made AVDTP setup depend on scheduling luck. uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); Hci::DrainEvents(); // AVDTP responses arrive as ACL data - if (g_avdtpResponseReady) return true; + if (g_avdtpResponseReady.load(std::memory_order_acquire)) { + // Freeze this mailbox before returning to the caller. A late + // duplicate with the same label must not overwrite the response + // while the negotiation routine is parsing it. + g_expectLabel.store(0xFF, std::memory_order_release); + g_expectSignal.store(0xFF, std::memory_order_release); + return true; + } for (int j = 0; j < 100; j++) { asm volatile("" ::: "memory"); } } + g_expectLabel.store(0xFF, std::memory_order_release); + g_expectSignal.store(0xFF, std::memory_order_release); return false; } @@ -221,7 +245,7 @@ namespace Drivers::USB::Bluetooth::A2dp { // ========================================================================= static bool AvdtpDiscover() { - SendAvdtpCommand(AVDTP_DISCOVER, nullptr, 0); + if (!SendAvdtpCommand(AVDTP_DISCOVER, nullptr, 0)) return false; if (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover timeout"; @@ -291,7 +315,7 @@ namespace Drivers::USB::Bluetooth::A2dp { g_sinkSbcCaps[0] = g_sinkSbcCaps[1] = g_sinkSbcCaps[2] = g_sinkSbcCaps[3] = 0; uint8_t payload[1] = {(uint8_t)(seid << 2)}; - SendAvdtpCommand(AVDTP_GET_CAPABILITIES, payload, 1); + if (!SendAvdtpCommand(AVDTP_GET_CAPABILITIES, payload, 1)) return false; if (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP GetCapabilities timeout (SEID=" << (uint64_t)seid << ")"; @@ -317,13 +341,19 @@ namespace Drivers::USB::Bluetooth::A2dp { if (off + 2 + (uint32_t)losc > g_avdtpResponseLen) break; const uint8_t* content = &g_avdtpResponseBuf[off + 2]; if (cat == 0x08) g_sinkDelayReporting = true; // Delay Reporting - if (cat == 0x04 && losc >= 2) { // Content Protection (e.g. SCMS-T) + if (cat == 0x04 && losc >= 2 + && content[0] == 0x02 && content[1] == 0x00) { + // We implement only SCMS-T (CP_TYPE 0x0002). Treating an + // arbitrary advertised protection scheme as SCMS-T made us add + // the wrong one-byte media header and corrupt every packet. g_sinkContentProtection = true; g_sinkCpType[0] = content[0]; g_sinkCpType[1] = content[1]; cl << " [CP " << (uint64_t)content[0] << " " << (uint64_t)content[1] << "]"; } - if (cat == CAT_MEDIA_CODEC && losc >= 6 && content[1] == CODEC_SBC) { + if (cat == CAT_MEDIA_CODEC && losc >= 6 + && ((content[0] >> 4) & 0x0F) == MEDIA_AUDIO + && content[1] == CODEC_SBC) { g_sinkSbcCaps[0] = content[2]; g_sinkSbcCaps[1] = content[3]; g_sinkSbcCaps[2] = content[4]; @@ -344,19 +374,29 @@ namespace Drivers::USB::Bluetooth::A2dp { // values gets UNSUPPORTED_CONFIGURATION (0x29). uint8_t oct0, oct1, minBP, maxBP; if (g_haveSinkSbcCaps) { - static const uint8_t freqPref[4] = {0x10, 0x20, 0x40, 0x80}; // 48,44.1,32,16 kHz + // The system mixer produces 48 kHz PCM and this path has no sample + // rate converter. SBC sinks are required to support 48 kHz; do not + // claim 44.1/32/16 and then feed 48 kHz samples under that header. + if (!(g_sinkSbcCaps[0] & 0x10) || !(g_sinkSbcCaps[1] & 0x04)) { + KernelLogStream(WARNING, "BT-A2DP") + << "Sink lacks required 48kHz/8-subband SBC mode"; + return false; + } static const uint8_t modePref[4] = {0x01, 0x02, 0x04, 0x08}; // Joint,Stereo,Dual,Mono static const uint8_t blkPref[4] = {0x10, 0x20, 0x40, 0x80}; // 16,12,8,4 blocks - static const uint8_t subPref[2] = {0x04, 0x08}; // 8,4 subbands static const uint8_t allocPref[2] = {0x01, 0x02}; // Loudness,SNR - oct0 = PickBit(g_sinkSbcCaps[0] & 0xF0, freqPref, 4) - | PickBit(g_sinkSbcCaps[0] & 0x0F, modePref, 4); + oct0 = 0x10 | PickBit(g_sinkSbcCaps[0] & 0x0F, modePref, 4); oct1 = PickBit(g_sinkSbcCaps[1] & 0xF0, blkPref, 4) - | PickBit(g_sinkSbcCaps[1] & 0x0C, subPref, 2) + | 0x04 | PickBit(g_sinkSbcCaps[1] & 0x03, allocPref, 2); minBP = g_sinkSbcCaps[2] < 2 ? 2 : g_sinkSbcCaps[2]; maxBP = g_sinkSbcCaps[3] > 53 ? 53 : g_sinkSbcCaps[3]; // cap to our quality target - if (maxBP < minBP) maxBP = minBP; + if ((oct0 & 0x0F) == 0 || (oct1 & 0xF0) == 0 + || (oct1 & 0x03) == 0 || maxBP < minBP) { + KernelLogStream(WARNING, "BT-A2DP") + << "Sink advertised an unusable SBC configuration"; + return false; + } } else { oct0 = 0x11; oct1 = 0x15; minBP = 2; maxBP = 53; // mandatory SBC baseline } @@ -392,7 +432,8 @@ namespace Drivers::USB::Bluetooth::A2dp { << (uint64_t)maxBP << (g_sinkContentProtection ? " +scms-t" : "") << (g_sinkDelayReporting ? " +delay" : ""); - SendAvdtpCommand(AVDTP_SET_CONFIGURATION, payload, (uint16_t)n); + if (!SendAvdtpCommand(AVDTP_SET_CONFIGURATION, payload, (uint16_t)n)) + return false; if (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP SetConfiguration timeout"; @@ -408,13 +449,14 @@ namespace Drivers::USB::Bluetooth::A2dp { } g_state = State::Configured; + g_peerConfigured.store(false, std::memory_order_release); KernelLogStream(OK, "BT-A2DP") << "Stream configured"; return true; } static bool AvdtpOpen() { uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; - SendAvdtpCommand(AVDTP_OPEN, payload, 1); + if (!SendAvdtpCommand(AVDTP_OPEN, payload, 1)) return false; if (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Open timeout"; @@ -436,7 +478,7 @@ namespace Drivers::USB::Bluetooth::A2dp { uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; for (int attempt = 0; attempt < 2; attempt++) { - SendAvdtpCommand(AVDTP_START, payload, 1); + if (!SendAvdtpCommand(AVDTP_START, payload, 1)) continue; if (!WaitAvdtpResponse()) { // A lost response is recoverable: re-issue once. AVDTP START @@ -474,6 +516,36 @@ namespace Drivers::USB::Bluetooth::A2dp { // OnChannelReady — called by L2CAP when an AVDTP channel is configured // ========================================================================= + void OnConnected(uint16_t aclHandle) { + // Establish the reset boundary at ACL connection creation, before the + // peer can open L2CAP channels. StartSource() used to reset this state + // much later, after authentication and a settle delay; that erased + // perfectly valid inbound AVDTP channels opened during the delay and + // caused duplicate outbound dials with connRsp=0xFFFF. + g_aclHandle.store(aclHandle, std::memory_order_release); + g_sigCid.store(0, std::memory_order_release); + g_mediaCid.store(0, std::memory_order_release); + g_sdpCid = 0; + g_state.store(State::Idle, std::memory_order_release); + g_txLabel = 1; + g_remoteSeid = 0; + g_peerConfigured.store(false, std::memory_order_release); + g_numSinkSeids = 0; + g_haveSinkSbcCaps = false; + g_sinkDelayReporting = false; + g_sinkContentProtection = false; + memset(g_sinkSbcCaps, 0, sizeof(g_sinkSbcCaps)); + memset(g_cfgSbc, 0, sizeof(g_cfgSbc)); + g_expectLabel.store(0xFF, std::memory_order_relaxed); + g_expectSignal.store(0xFF, std::memory_order_relaxed); + g_avdtpResponseLen = 0; + g_avdtpResponseReady.store(false, std::memory_order_release); + g_sdpRspReady.store(false, std::memory_order_release); + g_sbcInitialized.store(false, std::memory_order_release); + g_ringTail.store(g_ringHead.load(std::memory_order_relaxed), + std::memory_order_release); + } + void OnChannelReady(uint16_t l2capCid) { // Invoked from L2CAP inside the ACL receive path -- i.e. NESTED under // Xhci::PollEvents. PollEvents is non-reentrant (re-entry guard), so we @@ -481,14 +553,54 @@ namespace Drivers::USB::Bluetooth::A2dp { // would stall and every WaitAvdtpResponse() would time out. Just record // the channel; StartSource() drives the handshakes from top-level // (process) context where polling is free to run. - if (g_sigCid == 0) { - g_sigCid = l2capCid; + uint16_t sigCid = g_sigCid.load(std::memory_order_acquire); + if (sigCid == 0) { + g_sigCid.store(l2capCid, std::memory_order_release); KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready: CID=" << (uint64_t)l2capCid; - } else if (g_mediaCid == 0) { - g_mediaCid = l2capCid; + } else if (sigCid == l2capCid) { + return; + } else if ((g_state == State::Open || g_state == State::Streaming) + && g_mediaCid.load(std::memory_order_acquire) == 0) { + g_mediaCid.store(l2capCid, std::memory_order_release); + g_routeChanged.store(true, std::memory_order_release); KernelLogStream(OK, "BT-A2DP") << "AVDTP media channel ready: CID=" << (uint64_t)l2capCid; + } else { + // A second PSM-AVDTP channel before OPEN is a crossed/duplicate + // signaling connection, not media. Keeping it would make replies + // go out on a different channel and make the eventual real media + // channel the untracked third channel. + KernelLogStream(WARNING, "BT-A2DP") + << "Closing duplicate pre-OPEN AVDTP channel CID=" + << (uint64_t)l2capCid; + L2cap::FreeChannel(l2capCid); + } + } + + void OnChannelClosed(uint16_t localCid) { + if (localCid == g_sigCid.load(std::memory_order_acquire)) { + uint16_t mediaCid = g_mediaCid.load(std::memory_order_acquire); + g_sigCid.store(0, std::memory_order_release); + g_mediaCid.store(0, std::memory_order_release); + g_state.store(State::Idle, std::memory_order_release); + g_peerConfigured.store(false, std::memory_order_release); + g_sbcInitialized.store(false, std::memory_order_release); + g_routeChanged.store(true, std::memory_order_release); + if (mediaCid != 0) L2cap::FreeChannel(mediaCid); + } else if (localCid == g_mediaCid.load(std::memory_order_acquire)) { + State oldState = g_state.load(std::memory_order_acquire); + g_mediaCid.store(0, std::memory_order_release); + // A raw media-transport disconnect does not close the SEP. Keep it + // Open so a manual retry can re-establish only the media channel. + if ((oldState == State::Open || oldState == State::Streaming) + && g_sigCid.load(std::memory_order_acquire) != 0) + g_state.store(State::Open, std::memory_order_release); + else if (oldState == State::Idle + || g_sigCid.load(std::memory_order_acquire) == 0) + g_state.store(State::Idle, std::memory_order_release); + g_sbcInitialized.store(false, std::memory_order_release); + g_routeChanged.store(true, std::memory_order_release); } } @@ -637,15 +749,21 @@ namespace Drivers::USB::Bluetooth::A2dp { return n; } - // Lenient (ANY-of) match: strict SDP semantics demand the record contain - // ALL pattern UUIDs, but headsets bundle service+protocol UUIDs in one - // pattern; returning a near-match record beats returning nothing. + // An SDP ServiceSearchPattern is an AND: the record must contain every UUID + // in the pattern. The old ANY match returned the AVRCP record for an + // AudioSource+L2CAP query merely because both records contain L2CAP, which + // can make a sink reject the service database as internally inconsistent. static bool SdpRecordMatches(const SdpRecordDef& r, const uint16_t* uuids, uint32_t n) { - for (uint32_t i = 0; i < n; i++) - for (uint8_t j = 0; j < r.NumUuids; j++) - if (uuids[i] == r.Uuids[j]) return true; - return false; + if (n == 0) return false; + for (uint32_t i = 0; i < n; i++) { + bool found = false; + for (uint8_t j = 0; j < r.NumUuids; j++) { + if (uuids[i] == r.Uuids[j]) { found = true; break; } + } + if (!found) return false; + } + return true; } // Attribute-id ranges requested in an AttributeIDList data element. @@ -736,9 +854,17 @@ namespace Drivers::USB::Bluetooth::A2dp { if (len < 5) return; uint8_t pdu = d[0]; uint16_t tid = ((uint16_t)d[1] << 8) | d[2]; + uint16_t declaredLen = ((uint16_t)d[3] << 8) | d[4]; uint8_t params[200] = {}; uint16_t n = 0; + if (declaredLen != (uint16_t)(len - 5)) { + params[n++] = 0x00; + params[n++] = 0x03; // invalid request syntax + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + // Pattern match (search PDUs only) against every record we serve. uint16_t pat[8] = {}; uint32_t numPat = (pdu == 0x02 || pdu == 0x06) @@ -768,6 +894,11 @@ namespace Drivers::USB::Bluetooth::A2dp { uint32_t at = 5; const SdpRecordDef* attrRec = nullptr; // 0x04's addressed record if (pdu == 0x06) { + if (numPat == 0) { + params[n++] = 0x00; params[n++] = 0x03; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } uint32_t po, pl; if (!SdpDeHeader(d, len, 5, &po, &pl)) return; at = po + pl; @@ -785,23 +916,44 @@ namespace Drivers::USB::Bluetooth::A2dp { at = 9; } - uint16_t maxBytes = 0xFFFF; + uint16_t maxBytes = 0; AttrRange ranges[8]; uint32_t numRanges = 0; uint16_t resumeOff = 0; bool isCont = false; - if (at + 2 <= len) { - maxBytes = (uint16_t)(((uint16_t)d[at] << 8) | d[at + 1]); - at += 2; - numRanges = SdpAttrRanges(d, len, at, ranges, 8); - uint32_t avo, avl; // step past the id list to - if (SdpDeHeader(d, len, at, &avo, &avl)) at = avo + avl; - // Continuation state: 1-byte length + that many opaque bytes. - // Ours is always 2 bytes (the resume offset we handed out). - if (at + 3 <= len && d[at] == 0x02) { - resumeOff = (uint16_t)(((uint16_t)d[at + 1] << 8) | d[at + 2]); - isCont = true; - } + if (at + 2 > len) { + params[n++] = 0x00; params[n++] = 0x03; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + maxBytes = (uint16_t)(((uint16_t)d[at] << 8) | d[at + 1]); + at += 2; + uint32_t avo, avl; + if (!SdpDeHeader(d, len, at, &avo, &avl) + || (d[at] & 0xF8) != 0x30) { + params[n++] = 0x00; params[n++] = 0x03; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + numRanges = SdpAttrRanges(d, len, at, ranges, 8); + at = avo + avl; + // Continuation state: 1-byte length + opaque state. We issue + // exactly two bytes containing the response-body resume offset. + if (at >= len) { + params[n++] = 0x00; params[n++] = 0x03; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + uint8_t contLen = d[at++]; + if (contLen == 0 && at == len) { + isCont = false; + } else if (contLen == 2 && at + 2 == len) { + resumeOff = (uint16_t)(((uint16_t)d[at] << 8) | d[at + 1]); + isCont = true; + } else { + params[n++] = 0x00; params[n++] = 0x05; + SdpServerSend(cid, 0x01, tid, params, n); + return; } if (maxBytes < 7) maxBytes = 7; // spec minimum @@ -887,16 +1039,61 @@ namespace Drivers::USB::Bluetooth::A2dp { } SdpServerSend(cid, (pdu == 0x06) ? 0x07 : 0x05, tid, params, n); } else if (pdu == 0x02) { // ServiceSearchRequest -> 0x03 - params[n++] = 0x00; params[n++] = (uint8_t)numMatch; // total count - params[n++] = 0x00; params[n++] = (uint8_t)numMatch; // current count - for (int r = 0; r < kNumSdpRecords; r++) { + uint32_t po, pl; + if (numPat == 0 || !SdpDeHeader(d, len, 5, &po, &pl)) { + params[n++] = 0x00; params[n++] = 0x03; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + uint32_t at = po + pl; + if (at + 3 > len) { + params[n++] = 0x00; params[n++] = 0x03; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + uint16_t maxRecords = (uint16_t)(((uint16_t)d[at] << 8) | d[at + 1]); + at += 2; + if (maxRecords == 0) { + params[n++] = 0x00; params[n++] = 0x03; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + + uint8_t resume = 0; + uint8_t contLen = d[at++]; + if (contLen == 1 && at < len) resume = d[at++]; + else if (contLen != 0) { + params[n++] = 0x00; params[n++] = 0x05; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + if (at != len || resume > numMatch) { + params[n++] = 0x00; params[n++] = 0x05; + SdpServerSend(cid, 0x01, tid, params, n); + return; + } + + uint16_t current = (uint16_t)(numMatch - resume); + if (current > maxRecords) current = maxRecords; + params[n++] = 0x00; params[n++] = (uint8_t)numMatch; + params[n++] = (uint8_t)(current >> 8); + params[n++] = (uint8_t)current; + uint8_t skipped = 0, emitted = 0; + for (int r = 0; r < kNumSdpRecords && emitted < current; r++) { if (!match[r]) continue; + if (skipped++ < resume) continue; params[n++] = (uint8_t)(kSdpRecords[r].Handle >> 24); params[n++] = (uint8_t)(kSdpRecords[r].Handle >> 16); params[n++] = (uint8_t)(kSdpRecords[r].Handle >> 8); params[n++] = (uint8_t)(kSdpRecords[r].Handle & 0xFF); + emitted++; + } + if ((uint16_t)(resume + current) < numMatch) { + params[n++] = 0x01; + params[n++] = (uint8_t)(resume + current); + } else { + params[n++] = 0x00; } - params[n++] = 0x00; SdpServerSend(cid, 0x03, tid, params, n); } else { params[n++] = 0x00; params[n++] = 0x03; // invalid request syntax @@ -916,7 +1113,8 @@ namespace Drivers::USB::Bluetooth::A2dp { SdpHandleRequest(localCid, data, len); return; } - if (localCid == g_sdpCid) g_sdpRspReady = true; + if (localCid == g_sdpCid) + g_sdpRspReady.store(true, std::memory_order_release); } // Minimal SDP: open PSM 0x0001, send a ServiceSearchAttributeRequest for the @@ -925,12 +1123,14 @@ namespace Drivers::USB::Bluetooth::A2dp { // query. Returns true if the SDP channel configured (i.e. ACL data flows). static bool DoSdpQuery(uint32_t timeoutMs) { g_sdpCid = 0; - g_sdpRspReady = false; + g_sdpRspReady.store(false, std::memory_order_release); uint16_t cid = L2cap::Connect(L2cap::PSM_SDP); if (!cid || !L2cap::WaitConfigured(cid, timeoutMs)) { KernelLogStream(WARNING, "BT-A2DP") << "SDP channel setup failed (connRsp=" - << base::hex << (uint64_t)L2cap::LastConnRspResult() << base::dec << ")"; + << base::hex << (uint64_t)L2cap::ConnRspResult(cid) + << base::dec << ")"; + if (cid) L2cap::FreeChannel(cid); return false; } g_sdpCid = cid; @@ -954,7 +1154,10 @@ namespace Drivers::USB::Bluetooth::A2dp { while (Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); Hci::DrainEvents(); - if (g_sdpRspReady) { answered = true; break; } + if (g_sdpRspReady.load(std::memory_order_acquire)) { + answered = true; + break; + } for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } @@ -987,7 +1190,7 @@ namespace Drivers::USB::Bluetooth::A2dp { Xhci::PollEvents(); Hci::DrainEvents(); Hci::ProcessPendingCommands(); - auto* c = Hci::GetActiveConnection(); + auto* c = Hci::GetConnection(g_aclHandle.load(std::memory_order_acquire)); if (c && c->Encrypted) return true; for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } @@ -997,6 +1200,44 @@ namespace Drivers::USB::Bluetooth::A2dp { bool StartSource(uint32_t timeoutMs) { constexpr int kMaxAttempts = 4; + uint16_t aclHandle = g_aclHandle.load(std::memory_order_acquire); + if (aclHandle == 0 || aclHandle != L2cap::GetAclHandle() + || !Hci::GetConnection(aclHandle)) { + KernelLogStream(WARNING, "BT-A2DP") + << "Cannot start source without an active ACL link"; + return false; + } + + // A repeated manual Connect on an ACL link whose A2DP setup already + // succeeded is a cheap success, not a second configuration attempt. + State initialState = g_state.load(std::memory_order_acquire); + if (g_sigCid.load(std::memory_order_acquire) != 0 + && g_mediaCid.load(std::memory_order_acquire) != 0 + && (initialState == State::Open || initialState == State::Streaming)) { + return true; + } + + bool expectedSetup = false; + if (!g_sourceSetupActive.compare_exchange_strong( + expectedSetup, true, std::memory_order_acquire)) { + // AVDTP has one transaction-label space per signaling channel. A + // second setup caller must not interleave Discover/Open commands + // with the in-flight handshake; wait for that owner to finish. + uint64_t waitStart = Timekeeping::GetMilliseconds(); + while (g_sourceSetupActive.load(std::memory_order_acquire) + && Timekeeping::GetMilliseconds() - waitStart < timeoutMs) { + Xhci::PollEvents(); + Hci::DrainEvents(); + for (int j = 0; j < 100; j++) asm volatile("pause" ::: "memory"); + } + return IsReady(); + } + struct SetupGuard { + ~SetupGuard() { + g_sourceSetupActive.store(false, std::memory_order_release); + } + } setupGuard; + if (WaitEncrypted(3000)) { KernelLogStream(OK, "BT-A2DP") << "Link encrypted; dialing channels"; } else { @@ -1012,24 +1253,21 @@ namespace Drivers::USB::Bluetooth::A2dp { // dialed attempt, zero inbound traffic... because we never stopped // transmitting long enough to receive). Listen briefly before // dialing; fresh pairings are source-driven and just spend the wait. - g_sigCid = 0; - g_mediaCid = 0; - g_state = State::Idle; - g_txLabel = 1; - g_avdtpResponseReady = false; - { + if (g_sigCid.load(std::memory_order_acquire) == 0) { uint64_t lStart = Timekeeping::GetMilliseconds(); - while (Timekeeping::GetMilliseconds() - lStart < 2500) { + while (Timekeeping::GetMilliseconds() - lStart < 1000) { Xhci::PollEvents(); Hci::DrainEvents(); Hci::ProcessPendingCommands(); - if (g_sigCid != 0) break; + if (g_sigCid.load(std::memory_order_acquire) != 0) break; for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } } - if (g_sigCid != 0) { + if (g_sigCid.load(std::memory_order_acquire) != 0) { KernelLogStream(OK, "BT-A2DP") << "Sink opened AVDTP to us (cid=" - << base::hex << (uint64_t)g_sigCid << base::dec << ")"; + << base::hex + << (uint64_t)g_sigCid.load(std::memory_order_acquire) + << base::dec << ")"; } // Connection phase, retried. A sink commonly ignores the very first @@ -1044,16 +1282,20 @@ namespace Drivers::USB::Bluetooth::A2dp { // Dial only while no signaling channel exists in either direction; // an inbound one (from the listen phase above, or landing between // retries) short-circuits straight to negotiation. - for (int attempt = 0; attempt < kMaxAttempts && g_sigCid == 0; attempt++) { - g_mediaCid = 0; - g_state = State::Idle; + for (int attempt = 0; attempt < kMaxAttempts + && g_sigCid.load(std::memory_order_acquire) == 0; attempt++) { g_txLabel = 1; - g_avdtpResponseReady = false; + g_avdtpResponseReady.store(false, std::memory_order_release); // SDP service query: best effort, FIRST attempt only -- some sinks // gate AVDTP on a prior SDP query; repeating it on retries only burns // a channel slot and 2s with no benefit. - if (attempt == 0) DoSdpQuery(2000); + if (attempt == 0) DoSdpQuery(1000); + + // The SDP wait services inbound traffic. A sink may have opened + // its own AVDTP signaling channel while we queried it; do not race + // that channel with a duplicate outgoing connection. + if (g_sigCid.load(std::memory_order_acquire) != 0) break; // AVDTP signaling channel (PSM 0x0019). Dial out, then wait for a // channel to become ready in EITHER direction (OnChannelReady sets @@ -1065,13 +1307,29 @@ namespace Drivers::USB::Bluetooth::A2dp { << " (acl=" << (uint64_t)L2cap::GetAclHandle() << "), waiting..."; uint64_t sigStart = Timekeeping::GetMilliseconds(); - while (Timekeeping::GetMilliseconds() - sigStart < timeoutMs) { + uint32_t attemptWaitMs = timeoutMs < 1500 ? timeoutMs : 1500; + uint32_t waitBudgetMs = attemptWaitMs; + while (Timekeeping::GetMilliseconds() - sigStart < waitBudgetMs) { Xhci::PollEvents(); Hci::DrainEvents(); - if (g_sigCid != 0) break; + if (g_sigCid.load(std::memory_order_acquire) != 0) break; + // PENDING is explicitly non-final. A peer performing service + // authorization gets the caller's full timeout on this same + // channel; only a completely ignored dial uses the short retry + // timer. + if (L2cap::ConnRspResult(sig) == L2cap::CONN_PENDING) + waitBudgetMs = timeoutMs; for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } - if (g_sigCid != 0) break; // signaling channel up -> proceed + uint16_t readySig = g_sigCid.load(std::memory_order_acquire); + if (readySig != 0) { + // A crossed inbound channel may win while our dial is still + // pending. Retire the losing dial now; otherwise the media + // phase can mistake that second active PSM-AVDTP channel for + // the media transport opened after AVDTP_OPEN. + if (sig != 0 && sig != readySig) L2cap::FreeChannel(sig); + break; + } auto* ch = sig ? L2cap::GetChannel(sig) : nullptr; KernelLogStream(WARNING, "BT-A2DP") << "AVDTP signaling attempt " @@ -1079,13 +1337,15 @@ namespace Drivers::USB::Bluetooth::A2dp { << base::hex << (uint64_t)(ch ? ch->RemoteCid : 0) << base::dec << " localCfg=" << (uint64_t)(ch ? ch->LocalConfigDone : 0) << " remoteCfg=" << (uint64_t)(ch ? ch->RemoteConfigDone : 0) - << " connRsp=" << base::hex << (uint64_t)L2cap::LastConnRspResult() + << " connRsp=" << base::hex << (uint64_t)L2cap::ConnRspResult(sig) << base::dec << ")"; // Retry only if the remote IGNORED our CONN_REQ entirely. If it - // answered (connRsp != 0xFFFF) the stall is in the config exchange, - // not the dial -- don't loop; let the config-phase logs speak. - if (L2cap::LastConnRspResult() != 0xFFFF) { + // answered (connRsp != 0xFFFF), the extended PENDING/config wait + // above has already expired; close that half-open channel and + // surface the failure rather than churning more CIDs automatically. + if (L2cap::ConnRspResult(sig) != 0xFFFF) { + if (sig) L2cap::FreeChannel(sig); return false; } // Free our unanswered dialed channel so repeated retries don't leak @@ -1100,16 +1360,43 @@ namespace Drivers::USB::Bluetooth::A2dp { } } - if (g_sigCid == 0) { + if (g_sigCid.load(std::memory_order_acquire) == 0) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP signaling setup gave up after retries"; return false; } KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready, cid=" - << base::hex << (uint64_t)g_sigCid << base::dec; + << base::hex << (uint64_t)g_sigCid.load(std::memory_order_acquire) + << base::dec; - // 2. Negotiate the SBC stream (top-level: polling is free to run). - g_state = State::Discovering; - if (!AvdtpDiscover()) return false; + // If the sink chose to initiate AVDTP, let its transaction own the + // state machine. Starting our own Discover/SetConfiguration in parallel + // produces two initiators configuring the same SEP and usually ends in + // BAD_STATE. Give its OPEN command time to arrive. + if (g_peerConfigured.load(std::memory_order_acquire)) { + uint64_t peerStart = Timekeeping::GetMilliseconds(); + while (Timekeeping::GetMilliseconds() - peerStart < timeoutMs) { + Xhci::PollEvents(); + Hci::DrainEvents(); + State s = g_state.load(std::memory_order_acquire); + if (s == State::Open || s == State::Streaming) break; + for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); + } + State s = g_state.load(std::memory_order_acquire); + if (s != State::Open && s != State::Streaming) { + KernelLogStream(WARNING, "BT-A2DP") + << "Peer-driven AVDTP setup stopped before OPEN"; + return false; + } + } else if (g_state.load(std::memory_order_acquire) == State::Configured) { + // A prior attempt may have configured the SEP but lost/timed out the + // OPEN response. Resume at OPEN instead of re-running SetConfig on + // an in-use endpoint. + if (!AvdtpOpen()) return false; + } else if (g_state.load(std::memory_order_acquire) != State::Open + && g_state.load(std::memory_order_acquire) != State::Streaming) { + // 2. Negotiate the SBC stream (top-level: polling is free to run). + g_state = State::Discovering; + if (!AvdtpDiscover()) return false; // Probe each advertised audio sink and configure the first that offers // SBC. Each SEP carries a single codec, so we cannot assume the first @@ -1118,28 +1405,29 @@ namespace Drivers::USB::Bluetooth::A2dp { // sink, so a usable endpoint is always present once we look past the // first. AvdtpGetCapabilities sets g_haveSinkSbcCaps when the probed // SEID advertises SBC; we keep that endpoint's caps for SetConfiguration. - bool pickedSbc = false; - for (uint32_t i = 0; i < g_numSinkSeids; i++) { - if (!AvdtpGetCapabilities(g_sinkSeids[i])) continue; // skip endpoints that error - if (g_haveSinkSbcCaps) { - g_remoteSeid = g_sinkSeids[i]; - pickedSbc = true; - KernelLogStream(OK, "BT-A2DP") << "Selected SBC sink SEID=" - << (uint64_t)g_remoteSeid; - break; + bool pickedSbc = false; + for (uint32_t i = 0; i < g_numSinkSeids; i++) { + if (!AvdtpGetCapabilities(g_sinkSeids[i])) continue; + if (g_haveSinkSbcCaps) { + g_remoteSeid = g_sinkSeids[i]; + pickedSbc = true; + KernelLogStream(OK, "BT-A2DP") << "Selected SBC sink SEID=" + << (uint64_t)g_remoteSeid; + break; + } + KernelLogStream(INFO, "BT-A2DP") << "SEID=" << (uint64_t)g_sinkSeids[i] + << " is not SBC, trying next"; + } + if (!pickedSbc) { + KernelLogStream(WARNING, "BT-A2DP") << "No SBC-capable sink endpoint found"; + return false; } - KernelLogStream(INFO, "BT-A2DP") << "SEID=" << (uint64_t)g_sinkSeids[i] - << " is not SBC, trying next"; - } - if (!pickedSbc) { - KernelLogStream(WARNING, "BT-A2DP") << "No SBC-capable sink endpoint found"; - return false; - } - if (!AvdtpSetConfiguration()) return false; // -> Configured + if (!AvdtpSetConfiguration()) return false; // -> Configured - // 3. Open the stream endpoint. - if (!AvdtpOpen()) return false; // -> Open + // 3. Open the stream endpoint. + if (!AvdtpOpen()) return false; // -> Open + } // 4. Media transport channel: a SECOND PSM 0x0019 L2CAP channel the AVDTP // initiator opens after AVDTP_OPEN. Dial ONCE, immediately (inside the @@ -1156,25 +1444,53 @@ namespace Drivers::USB::Bluetooth::A2dp { // have been answered (see the SDP server above). Miss either and the // sink pends this channel forever (connRsp=1 status=2, authorization // pending) -- the HW failure on builds without the SDP server. - g_mediaCid = 0; constexpr uint32_t kMediaWaitMs = 8000; - uint16_t media = L2cap::Connect(L2cap::PSM_AVDTP); - KernelLogStream(INFO, "BT-A2DP") << "AVDTP media: dialed cid=" - << base::hex << (uint64_t)media << base::dec << ", holding channel..."; + uint16_t media = g_mediaCid.load(std::memory_order_acquire); + if (media == 0) + media = L2cap::FindConfiguredAvdtpChannelExcept( + g_sigCid.load(std::memory_order_acquire)); + if (media == 0) + media = L2cap::FindAvdtpChannelExcept( + g_sigCid.load(std::memory_order_acquire)); + if (media != 0) { + auto* existingMedia = L2cap::GetChannel(media); + if (existingMedia && existingMedia->Configured) { + g_mediaCid.store(media, std::memory_order_release); + KernelLogStream(OK, "BT-A2DP") + << "Using sink-opened AVDTP media channel cid=" + << base::hex << (uint64_t)media << base::dec; + } else { + KernelLogStream(INFO, "BT-A2DP") + << "Reusing pending AVDTP media channel cid=" + << base::hex << (uint64_t)media << base::dec; + } + } else { + media = L2cap::Connect(L2cap::PSM_AVDTP); + KernelLogStream(INFO, "BT-A2DP") << "AVDTP media: dialed cid=" + << base::hex << (uint64_t)media << base::dec << ", holding channel..."; + } uint64_t mStart = Timekeeping::GetMilliseconds(); - while (Timekeeping::GetMilliseconds() - mStart < kMediaWaitMs) { + while (g_mediaCid.load(std::memory_order_acquire) == 0 + && Timekeeping::GetMilliseconds() - mStart < kMediaWaitMs) { Xhci::PollEvents(); Hci::DrainEvents(); auto* ch = media ? L2cap::GetChannel(media) : nullptr; - if (ch && ch->Configured) { g_mediaCid = media; break; } // PENDING->SUCCESS->configured - uint16_t other = L2cap::FindConfiguredAvdtpChannelExcept(g_sigCid); - if (other) { g_mediaCid = other; break; } // sink opened it inbound + if (ch && ch->Configured) { + g_mediaCid.store(media, std::memory_order_release); + break; + } + uint16_t other = L2cap::FindConfiguredAvdtpChannelExcept( + g_sigCid.load(std::memory_order_acquire)); + if (other) { + g_mediaCid.store(other, std::memory_order_release); + break; + } for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } - if (g_mediaCid == 0) { + if (g_mediaCid.load(std::memory_order_acquire) == 0) { // Do NOT FreeChannel here -- a late SUCCESS would then match no // active channel. Leave it; the next connection's Initialize resets // the table. Log the held channel's state for diagnosis. @@ -1183,13 +1499,15 @@ namespace Drivers::USB::Bluetooth::A2dp { << base::hex << (uint64_t)(ch ? ch->RemoteCid : 0) << base::dec << " localCfg=" << (uint64_t)(ch ? ch->LocalConfigDone : 0) << " remoteCfg=" << (uint64_t)(ch ? ch->RemoteConfigDone : 0) - << " connRsp=" << base::hex << (uint64_t)L2cap::LastConnRspResult() + << " connRsp=" << base::hex << (uint64_t)L2cap::ConnRspResult(media) + << " status=" << (uint64_t)L2cap::ConnRspStatus(media) << base::dec << ")"; return false; } KernelLogStream(OK, "BT-A2DP") << "A2DP source ready (signaling + media), cid=" - << base::hex << (uint64_t)g_mediaCid << base::dec << " state=Open"; + << base::hex << (uint64_t)g_mediaCid.load(std::memory_order_acquire) + << base::dec << " state=Open"; // 5. AVRCP control channel (PSM 0x17), best effort, AFTER the stream // exists -- the order every phone uses. Dialed before the media @@ -1205,8 +1523,9 @@ namespace Drivers::USB::Bluetooth::A2dp { << base::hex << (uint64_t)avrcp << base::dec; } else { KernelLogStream(INFO, "BT-A2DP") << "AVRCP dial not configured (connRsp=" - << base::hex << (uint64_t)L2cap::LastConnRspResult() << base::dec + << base::hex << (uint64_t)L2cap::ConnRspResult(avrcp) << base::dec << ", continuing)"; + if (avrcp) L2cap::FreeChannel(avrcp); } } return true; @@ -1216,23 +1535,40 @@ namespace Drivers::USB::Bluetooth::A2dp { // ProcessAvdtp — handle AVDTP signaling packets // ========================================================================= - void ProcessAvdtp(const uint8_t* data, uint16_t len) { + void ProcessAvdtp(uint16_t localCid, const uint8_t* data, uint16_t len) { if (len < 2) return; + // AVDTP signaling and media share the same PSM. RTP/SBC packets on the + // media channel are not signaling packets; parsing their RTP header as + // an AVDTP command generated a bogus General Reject for each packet. + if (localCid != g_sigCid.load(std::memory_order_acquire)) return; + uint8_t txLabel = (data[0] >> 4) & 0x0F; uint8_t pktType = (data[0] >> 2) & 0x03; uint8_t msgType = data[0] & 0x03; uint8_t signalId = data[1] & 0x3F; - if (msgType == MSG_RESPONSE_ACCEPT || msgType == MSG_RESPONSE_REJECT) { + // All procedures we issue fit in a SINGLE packet. Do not misinterpret + // START/CONTINUE/END headers (whose byte 1 has a different meaning) as + // a complete response or command. + if (pktType != PKT_SINGLE) { + KernelLogStream(WARNING, "BT-A2DP") + << "Ignoring unsupported fragmented AVDTP signaling packet"; + return; + } + + if (msgType == MSG_RESPONSE_ACCEPT || msgType == MSG_RESPONSE_REJECT + || msgType == MSG_GENERAL_REJECT) { // Only accept the response to the command we are actually waiting on // (matching transaction label AND signal id). Otherwise the headset's // own responses/duplicates could be read as ours and desync the chain. - if (txLabel == g_expectLabel && signalId == g_expectSignal) { + if (txLabel == g_expectLabel.load(std::memory_order_relaxed) + && signalId == g_expectSignal.load(std::memory_order_relaxed) + && !g_avdtpResponseReady.load(std::memory_order_acquire)) { uint32_t cp = (len > sizeof(g_avdtpResponseBuf)) ? sizeof(g_avdtpResponseBuf) : len; memcpy(g_avdtpResponseBuf, data, cp); - g_avdtpResponseLen = len; - g_avdtpResponseReady = true; + g_avdtpResponseLen = cp; + g_avdtpResponseReady.store(true, std::memory_order_release); } return; } @@ -1245,7 +1581,7 @@ namespace Drivers::USB::Bluetooth::A2dp { uint8_t rsp[2] = {}; rsp[0] = (g_localSeid << 2); // SEID, not in use rsp[1] = (MEDIA_AUDIO << 4) | 0x00; // Audio, Source - SendAvdtpResponse(txLabel, AVDTP_DISCOVER, rsp, 2); + SendAvdtpResponse(localCid, txLabel, AVDTP_DISCOVER, rsp, 2); break; } @@ -1254,6 +1590,14 @@ namespace Drivers::USB::Bluetooth::A2dp { // Respond with our SBC capabilities. GET_ALL_CAPABILITIES // (AVDTP 1.3) must be answered too -- our SDP record // advertises 1.3, and silence to it stalls the peer. + if (len < 3 || ((data[2] >> 2) & 0x3F) != g_localSeid) { + uint8_t rej[3] = { + (uint8_t)((txLabel << 4) | MSG_RESPONSE_REJECT), + signalId, 0x12 // BAD_ACP_SEID + }; + L2cap::SendData(localCid, rej, sizeof(rej)); + break; + } uint8_t rsp[10] = {}; rsp[0] = CAT_MEDIA_TRANSPORT; rsp[1] = 0; @@ -1269,7 +1613,7 @@ namespace Drivers::USB::Bluetooth::A2dp { rsp[7] = 0x15; // 16 blocks (b4) | 8 subbands (b2) | Loudness (b0) rsp[8] = 2; // Min bitpool rsp[9] = 53; // Max bitpool - SendAvdtpResponse(txLabel, signalId, rsp, 10); + SendAvdtpResponse(localCid, txLabel, signalId, rsp, 10); break; } @@ -1279,18 +1623,92 @@ namespace Drivers::USB::Bluetooth::A2dp { // record THEIRS; our later START must address the sink's // SEID or it rejects with BAD_ACP_SEID (0x12). if (len >= 4) { - g_remoteSeid = (data[3] >> 2) & 0x3F; + uint8_t acpSeid = (data[2] >> 2) & 0x3F; + uint8_t intSeid = (data[3] >> 2) & 0x3F; + bool transport = false, sbc = false, malformed = false; + bool scmsT = false, delayReporting = false; + uint8_t sbcCfg[4] = {}; + uint32_t off = 4; + while (off + 2 <= len) { + uint8_t cat = data[off]; + uint8_t losc = data[off + 1]; + if (off + 2u + losc > len) { malformed = true; break; } + const uint8_t* content = &data[off + 2]; + if (cat == CAT_MEDIA_TRANSPORT && losc == 0) { + transport = true; + } else if (cat == CAT_MEDIA_CODEC && losc >= 6 + && ((content[0] >> 4) & 0x0F) == MEDIA_AUDIO + && content[1] == CODEC_SBC) { + memcpy(sbcCfg, &content[2], 4); + // A configuration selects exactly one bit from + // each SBC capability field and a valid bitpool. + // Our advertised local SEP is deliberately a + // fixed 48k/joint/16-block/8-subband/loudness + // configuration, so an initiator must select it. + if (sbcCfg[0] == 0x11 && sbcCfg[1] == 0x15 + && sbcCfg[2] >= 2 && sbcCfg[2] <= 53 + && sbcCfg[3] >= sbcCfg[2] && sbcCfg[3] <= 53) { + sbc = true; + } + } else if (cat == 0x04 && losc >= 2 + && content[0] == 0x02 && content[1] == 0x00) { + scmsT = true; + } else if (cat == 0x08 && losc == 0) { + delayReporting = true; + } + off += 2u + losc; + } + if (off != len) malformed = true; + + if (acpSeid != g_localSeid || intSeid == 0 + || malformed || !transport || !sbc) { + uint8_t reject[2] = { + (uint8_t)(!transport ? CAT_MEDIA_TRANSPORT : CAT_MEDIA_CODEC), + (uint8_t)(acpSeid != g_localSeid ? 0x12 : 0x29) + }; + uint8_t buf[4] = { + (uint8_t)((txLabel << 4) | MSG_RESPONSE_REJECT), + AVDTP_SET_CONFIGURATION, reject[0], reject[1] + }; + L2cap::SendData(localCid, buf, sizeof(buf)); + break; + } + + g_remoteSeid = intSeid; + memcpy(g_cfgSbc, sbcCfg, sizeof(g_cfgSbc)); + g_sinkContentProtection = scmsT; + g_sinkDelayReporting = delayReporting; + g_peerConfigured.store(true, std::memory_order_release); g_state = State::Configured; - SendAvdtpResponse(txLabel, AVDTP_SET_CONFIGURATION, nullptr, 0); + SendAvdtpResponse(localCid, txLabel, + AVDTP_SET_CONFIGURATION, nullptr, 0); KernelLogStream(OK, "BT-A2DP") << "Remote configured stream, SEID=" << (uint64_t)g_remoteSeid; + } else { + uint8_t rej[4] = { + (uint8_t)((txLabel << 4) | MSG_RESPONSE_REJECT), + AVDTP_SET_CONFIGURATION, + CAT_MEDIA_TRANSPORT, 0x11 // BAD_LENGTH + }; + L2cap::SendData(localCid, rej, sizeof(rej)); } break; } case AVDTP_OPEN: { + uint8_t seid = len >= 3 ? ((data[2] >> 2) & 0x3F) : 0; + if (seid != g_localSeid + || g_state.load(std::memory_order_acquire) != State::Configured) { + uint8_t rej[3] = { + (uint8_t)((txLabel << 4) | MSG_RESPONSE_REJECT), + AVDTP_OPEN, + (uint8_t)(seid != g_localSeid ? 0x12 : 0x31) + }; + L2cap::SendData(localCid, rej, sizeof(rej)); + break; + } g_state = State::Open; - SendAvdtpResponse(txLabel, AVDTP_OPEN, nullptr, 0); + SendAvdtpResponse(localCid, txLabel, AVDTP_OPEN, nullptr, 0); KernelLogStream(OK, "BT-A2DP") << "Remote opened stream"; // The media transport channel will be set up via L2CAP after this @@ -1298,9 +1716,21 @@ namespace Drivers::USB::Bluetooth::A2dp { } case AVDTP_START: { + uint8_t seid = len >= 3 ? ((data[2] >> 2) & 0x3F) : 0; + if (seid != g_localSeid + || g_state.load(std::memory_order_acquire) != State::Open + || g_mediaCid.load(std::memory_order_acquire) == 0) { + uint8_t rej[4] = { + (uint8_t)((txLabel << 4) | MSG_RESPONSE_REJECT), + AVDTP_START, (uint8_t)(seid << 2), + (uint8_t)(seid != g_localSeid ? 0x12 : 0x31) + }; + L2cap::SendData(localCid, rej, sizeof(rej)); + break; + } g_state = State::Streaming; ResetMediaClock(); - SendAvdtpResponse(txLabel, AVDTP_START, nullptr, 0); + SendAvdtpResponse(localCid, txLabel, AVDTP_START, nullptr, 0); KernelLogStream(OK, "BT-A2DP") << "Remote started streaming"; break; } @@ -1311,27 +1741,74 @@ namespace Drivers::USB::Bluetooth::A2dp { // with no kernel log output at all). case AVDTP_CLOSE: { g_state = State::Idle; + g_peerConfigured.store(false, std::memory_order_release); g_routeChanged.store(true, std::memory_order_release); - SendAvdtpResponse(txLabel, AVDTP_CLOSE, nullptr, 0); + SendAvdtpResponse(localCid, txLabel, AVDTP_CLOSE, nullptr, 0); + uint16_t mediaCid = g_mediaCid.load(std::memory_order_acquire); + if (mediaCid != 0) L2cap::FreeChannel(mediaCid); + else g_sbcInitialized.store(false, std::memory_order_release); KernelLogStream(WARNING, "BT-A2DP") << "Remote CLOSED stream"; break; } case AVDTP_SUSPEND: { g_state = State::Open; - SendAvdtpResponse(txLabel, AVDTP_SUSPEND, nullptr, 0); + SendAvdtpResponse(localCid, txLabel, AVDTP_SUSPEND, nullptr, 0); KernelLogStream(WARNING, "BT-A2DP") << "Remote SUSPENDED stream"; break; } case AVDTP_ABORT: { g_state = State::Idle; + g_peerConfigured.store(false, std::memory_order_release); g_routeChanged.store(true, std::memory_order_release); - SendAvdtpResponse(txLabel, AVDTP_ABORT, nullptr, 0); + SendAvdtpResponse(localCid, txLabel, AVDTP_ABORT, nullptr, 0); + uint16_t mediaCid = g_mediaCid.load(std::memory_order_acquire); + if (mediaCid != 0) L2cap::FreeChannel(mediaCid); + else g_sbcInitialized.store(false, std::memory_order_release); KernelLogStream(WARNING, "BT-A2DP") << "Remote ABORTED stream"; break; } + case AVDTP_GET_CONFIGURATION: { + uint8_t seid = len >= 3 ? ((data[2] >> 2) & 0x3F) : 0; + if (seid != g_localSeid || g_state == State::Idle + || g_remoteSeid == 0) { + uint8_t rej[3] = { + (uint8_t)((txLabel << 4) | MSG_RESPONSE_REJECT), + AVDTP_GET_CONFIGURATION, + (uint8_t)(seid != g_localSeid ? 0x12 : 0x31) + }; + L2cap::SendData(localCid, rej, sizeof(rej)); + break; + } + uint8_t rsp[18] = {}; + uint16_t n = 0; + rsp[n++] = CAT_MEDIA_TRANSPORT; rsp[n++] = 0; + rsp[n++] = CAT_MEDIA_CODEC; rsp[n++] = 6; + rsp[n++] = (MEDIA_AUDIO << 4); rsp[n++] = CODEC_SBC; + memcpy(&rsp[n], g_cfgSbc, 4); n += 4; + if (g_sinkContentProtection) { + rsp[n++] = 0x04; rsp[n++] = 2; + rsp[n++] = 0x02; rsp[n++] = 0x00; + } + if (g_sinkDelayReporting) { + rsp[n++] = 0x08; rsp[n++] = 0; + } + SendAvdtpResponse(localCid, txLabel, + AVDTP_GET_CONFIGURATION, rsp, n); + break; + } + + case AVDTP_DELAYREPORT: + // Delay reports are advisory for a source without a local + // presentation clock. Acknowledge them when the sink uses + // the category we negotiated instead of stalling its AVDTP + // transaction state machine. + SendAvdtpResponse(localCid, txLabel, + AVDTP_DELAYREPORT, nullptr, 0); + break; + default: { // AVDTP General Reject -- silence to an unknown command can // stall the peer's signaling state machine. @@ -1341,7 +1818,7 @@ namespace Drivers::USB::Bluetooth::A2dp { (uint8_t)((txLabel << 4) | (PKT_SINGLE << 2) | MSG_GENERAL_REJECT), signalId }; - L2cap::SendData(g_sigCid, rej, 2); + L2cap::SendData(localCid, rej, 2); break; } } @@ -1424,8 +1901,8 @@ namespace Drivers::USB::Bluetooth::A2dp { if (!AcquireMediaService()) return false; if (g_state == State::Streaming) { uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; - SendAvdtpCommand(AVDTP_SUSPEND, payload, 1); - WaitAvdtpResponse(1000); + if (SendAvdtpCommand(AVDTP_SUSPEND, payload, 1)) + WaitAvdtpResponse(1000); g_state = State::Open; } if (flushQueued) { @@ -1460,8 +1937,12 @@ namespace Drivers::USB::Bluetooth::A2dp { } uint32_t samplesPerFrame = Sbc::GetSamplesPerFrame(&g_sbcEncoder); - uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2; + // The mixer ring is always 16-bit stereo, even when a rare sink selects + // SBC mono. Consume stereo frames and downmix immediately before encode + // rather than interpreting alternating L/R samples as a mono timeline. + uint32_t bytesPerFrame = samplesPerFrame * 2 * 2; int16_t framePcm[512]; + int16_t monoPcm[256]; // Bundle as many SBC frames as fit in the media channel's MTU into // each RTP packet. One frame per packet means 375 packets/s at @@ -1490,10 +1971,13 @@ namespace Drivers::USB::Bluetooth::A2dp { if (fill < bytesPerFrame) break; // ring dry, nothing to send if (audioMs >= elapsed + LEAD_MS) break; // sink lead is full if (!Hci::AclTxReady()) { - // Normal credit pacing most of the time. A credit pool stuck - // for 250+ ms with the USB side fully drained means NOCP - // events were lost -- reset and carry on. - if (now - g_lastSendMs > 250 && Hci::AclTxInFlight() == 0) { + // Normal credit pacing most of the time. RF retransmissions, + // coexistence windows, and multipoint scheduling can all delay + // legitimate completions for hundreds of milliseconds; do not + // erase the controller's hard credit accounting that quickly. + // A full second with the USB side drained is much stronger + // evidence that NOCP events were actually lost. + if (now - g_lastSendMs > 1000 && Hci::AclTxInFlight() == 0) { KernelLogStream(WARNING, "BT-A2DP") << "media credit stall (seq=" << (uint64_t)g_seqNum << "); resetting credits"; @@ -1531,13 +2015,17 @@ namespace Drivers::USB::Bluetooth::A2dp { uint32_t sbcHdrPos = hdr++; // SBC payload header: frame count // Pull frames from the ring (may wrap), apply volume, encode. - // Frame size is constant for a fixed config; the size of the - // first encode bounds whether the next one still fits. + // Frame size is constant for a fixed configuration. uint32_t off = hdr; uint32_t nFrames = 0; uint32_t frameLen = 0; + uint32_t packetTail = g_ringTail.load(std::memory_order_relaxed); + Sbc::SbcEncoder encoderBefore = g_sbcEncoder; while (nFrames < 15) { - if (frameLen != 0 && off + frameLen > maxPayload) break; + // Frame size is fixed for the negotiated SBC configuration. + // Check even the first frame before consuming PCM or writing + // beyond the peer's media MTU. + if (off + g_sbcEncoder.FrameSize > maxPayload) break; uint32_t avail = g_ringHead.load(std::memory_order_acquire) - g_ringTail.load(std::memory_order_relaxed); if (avail < bytesPerFrame) break; @@ -1555,14 +2043,31 @@ namespace Drivers::USB::Bluetooth::A2dp { memset(framePcm, 0, bytesPerFrame); } - frameLen = Sbc::Encode(&g_sbcEncoder, framePcm, &mediaPkt[off]); + const int16_t* encodePcm = framePcm; + if (g_sbcEncoder.Channels == 1) { + for (uint32_t s = 0; s < samplesPerFrame; s++) { + int32_t mixed = (int32_t)framePcm[s * 2] + + (int32_t)framePcm[s * 2 + 1]; + monoPcm[s] = (int16_t)(mixed / 2); + } + encodePcm = monoPcm; + } + frameLen = Sbc::Encode(&g_sbcEncoder, encodePcm, &mediaPkt[off]); off += frameLen; nFrames++; } if (nFrames == 0) break; mediaPkt[sbcHdrPos] = (uint8_t)nFrames; - L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)off); + if (!L2cap::SendData(g_mediaCid.load(std::memory_order_acquire), + mediaPkt, (uint16_t)off)) { + // The TX ring can still lose a race to signaling traffic after + // AclTxReady(). Put this packet's PCM back instead of silently + // dropping it and advancing RTP sequence/timestamp state. + g_sbcEncoder = encoderBefore; + g_ringTail.store(packetTail, std::memory_order_release); + break; + } g_seqNum++; g_timestamp += samplesPerFrame * nFrames; @@ -1637,9 +2142,15 @@ namespace Drivers::USB::Bluetooth::A2dp { } void OnDisconnected(uint16_t aclHandle) { - if (aclHandle != L2cap::GetAclHandle()) return; + if (aclHandle != g_aclHandle.load(std::memory_order_acquire)) return; g_state.store(State::Idle, std::memory_order_release); + g_aclHandle.store(0, std::memory_order_release); + g_sigCid.store(0, std::memory_order_release); g_mediaCid.store(0, std::memory_order_release); + g_sdpCid = 0; + g_peerConfigured.store(false, std::memory_order_release); + g_avdtpResponseReady.store(false, std::memory_order_release); + g_sdpRspReady.store(false, std::memory_order_release); g_sbcInitialized.store(false, std::memory_order_release); g_ringTail.store(g_ringHead.load(std::memory_order_relaxed), std::memory_order_release); @@ -1658,6 +2169,13 @@ namespace Drivers::USB::Bluetooth::A2dp { return g_state.load(std::memory_order_acquire); } + bool IsReady() { + State state = g_state.load(std::memory_order_acquire); + return g_sigCid.load(std::memory_order_acquire) != 0 + && g_mediaCid.load(std::memory_order_acquire) != 0 + && (state == State::Open || state == State::Streaming); + } + bool IsStreaming() { return g_state.load(std::memory_order_acquire) == State::Streaming; } diff --git a/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp b/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp index 9f34c05..d153dff 100644 --- a/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/A2dp.hpp @@ -32,11 +32,22 @@ namespace Drivers::USB::Bluetooth::A2dp { // handshakes), NOT from an event/interrupt path. Leaves the stream Open. bool StartSource(uint32_t timeoutMs = 5000); + // Reset all per-link AVDTP state for a newly established ACL connection. + // This is deliberately separate from StartSource(): the peer may open its + // AVDTP channels during authentication/the post-encryption settle window, + // and StartSource must preserve those already-live inbound channels. + void OnConnected(uint16_t aclHandle); + // Called by L2CAP when an AVDTP channel becomes ready void OnChannelReady(uint16_t l2capCid); // Process an AVDTP signaling packet - void ProcessAvdtp(const uint8_t* data, uint16_t len); + void ProcessAvdtp(uint16_t localCid, const uint8_t* data, uint16_t len); + + // L2CAP channel teardown can happen while the ACL link remains alive. + // Clear stale signaling/media CIDs so subsequent writes do not target a + // dead channel and a manual reconnect can rebuild the A2DP path. + void OnChannelClosed(uint16_t localCid); // Process an SDP packet (on any SDP L2CAP channel). Dispatches by PDU: // requests (the headset querying OUR services) are answered by the minimal @@ -80,6 +91,10 @@ namespace Drivers::USB::Bluetooth::A2dp { // Get current state State GetState(); + // True only when signaling and media transports are both live and the SEP + // has reached a state from which audio can be started. + bool IsReady(); + // Check if currently streaming bool IsStreaming(); diff --git a/kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp b/kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp index 0b97934..e4dd459 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/Avrcp.cpp @@ -10,6 +10,7 @@ #include #include #include +#include #include using namespace Kt; @@ -28,6 +29,7 @@ namespace Drivers::USB::Bluetooth::Avrcp { constexpr uint8_t AVC_RSP_ACCEPTED = 0x9; constexpr uint8_t AVC_RSP_REJECTED = 0xA; constexpr uint8_t AVC_RSP_STABLE = 0xC; + constexpr uint8_t AVC_RSP_CHANGED = 0xD; constexpr uint8_t AVC_RSP_INTERIM = 0xF; // AV/C opcodes @@ -43,33 +45,35 @@ namespace Drivers::USB::Bluetooth::Avrcp { constexpr uint8_t PDU_SET_ABS_VOLUME = 0x50; // AVRCP notification events - constexpr uint8_t EVT_PLAYBACK_STATUS = 0x01; - constexpr uint8_t EVT_TRACK_CHANGED = 0x02; constexpr uint8_t EVT_VOLUME_CHANGED = 0x0D; + static kcp::Spinlock g_notifyLock; + static uint16_t g_volumeNotifyCid = 0; + static uint8_t g_volumeNotifyTransaction = 0; + // ========================================================================= // Send helpers // ========================================================================= // AVCTP single-packet response: byte0 = transaction<<4 | pktType<<2 | // C/R(1=response)<<1 | IPID; bytes 1-2 = PID big-endian. - static void SendAvctp(uint16_t cid, uint8_t transaction, uint16_t pid, + static bool SendAvctp(uint16_t cid, uint8_t transaction, uint16_t pid, bool invalidPid, const uint8_t* avc, uint16_t avcLen) { uint8_t buf[96] = {}; - if (3u + avcLen > sizeof(buf)) return; + if (3u + avcLen > sizeof(buf)) return false; buf[0] = (uint8_t)((transaction << 4) | 0x02 | (invalidPid ? 0x01 : 0)); buf[1] = (uint8_t)(pid >> 8); buf[2] = (uint8_t)(pid & 0xFF); if (avc && avcLen) memcpy(&buf[3], avc, avcLen); - L2cap::SendData(cid, buf, (uint16_t)(3 + avcLen)); + return L2cap::SendData(cid, buf, (uint16_t)(3 + avcLen)); } // AVRCP vendor-dependent response frame: // [rsp][subunit 0x48 panel][opcode 0x00][company 00 19 58][pdu][pkt 0][len BE][params] - static void SendVendorRsp(uint16_t cid, uint8_t transaction, uint8_t rspCode, + static bool SendVendorRsp(uint16_t cid, uint8_t transaction, uint8_t rspCode, uint8_t pdu, const uint8_t* p, uint16_t n) { uint8_t avc[64] = {}; - if (10u + n > sizeof(avc)) return; + if (10u + n > sizeof(avc)) return false; avc[0] = rspCode; avc[1] = 0x48; // PANEL subunit avc[2] = AVC_OP_VENDOR; @@ -79,7 +83,8 @@ namespace Drivers::USB::Bluetooth::Avrcp { avc[8] = (uint8_t)(n >> 8); avc[9] = (uint8_t)(n & 0xFF); if (p && n) memcpy(&avc[10], p, n); - SendAvctp(cid, transaction, 0x110E, false, avc, (uint16_t)(10 + n)); + return SendAvctp(cid, transaction, 0x110E, false, + avc, (uint16_t)(10 + n)); } // ========================================================================= @@ -126,14 +131,13 @@ namespace Drivers::USB::Bluetooth::Avrcp { } case AVC_OP_PASSTHROUGH: { - // Accept every pass-through (play/pause/etc.). Echo the frame - // with the response code swapped in. Wiring the operation ids - // to the Music app is a later feature; ACCEPTED keeps the - // controller-side state machine happy. + // No media-session control bridge exists yet. Claiming these + // commands were ACCEPTED made headset buttons appear broken and + // left the controller believing an action had happened. uint8_t rsp[16] = {}; uint16_t cp = (alen > sizeof(rsp)) ? (uint16_t)sizeof(rsp) : alen; memcpy(rsp, avc, cp); - rsp[0] = AVC_RSP_ACCEPTED; + rsp[0] = AVC_RSP_NOT_IMPL; SendAvctp(localCid, transaction, pid, false, rsp, cp); if (alen >= 4) { KernelLogStream(INFO, "BT-AVRCP") << "pass-through op=" @@ -156,7 +160,12 @@ namespace Drivers::USB::Bluetooth::Avrcp { uint8_t pdu = avc[6]; uint16_t plen = ((uint16_t)avc[8] << 8) | avc[9]; const uint8_t* p = &avc[10]; - if (10u + plen > alen) plen = (uint16_t)(alen - 10); + if (10u + plen > alen) { + uint8_t err = 0x01; // invalid parameter + SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED, + pdu, &err, 1); + break; + } if (pdu == PDU_GET_CAPABILITIES && ctype == AVC_CTYPE_STATUS && plen >= 1) { if (p[0] == 0x02) { // CompanyID list @@ -164,8 +173,7 @@ namespace Drivers::USB::Bluetooth::Avrcp { SendVendorRsp(localCid, transaction, AVC_RSP_STABLE, pdu, rp, sizeof(rp)); } else if (p[0] == 0x03) { // EventsSupported - uint8_t rp[5] = {0x03, 0x03, EVT_PLAYBACK_STATUS, - EVT_TRACK_CHANGED, EVT_VOLUME_CHANGED}; + uint8_t rp[3] = {0x03, 0x01, EVT_VOLUME_CHANGED}; SendVendorRsp(localCid, transaction, AVC_RSP_STABLE, pdu, rp, sizeof(rp)); } else { @@ -181,22 +189,18 @@ namespace Drivers::USB::Bluetooth::Avrcp { pdu, rp, sizeof(rp)); } else if (pdu == PDU_REGISTER_NOTIFY && ctype == AVC_CTYPE_NOTIFY && plen >= 1) { // INTERIM response with the current value. (A CHANGED - // follow-up on actual change is a later feature.) + // response is sent by NotifyVolumeChanged; registrations + // are one-shot as required by AVRCP.) if (p[0] == EVT_VOLUME_CHANGED) { uint8_t rp[2] = {EVT_VOLUME_CHANGED, (uint8_t)((Drivers::Audio::Mixer::GetMasterVolume() * 127) / 100)}; - SendVendorRsp(localCid, transaction, AVC_RSP_INTERIM, - pdu, rp, sizeof(rp)); - } else if (p[0] == EVT_PLAYBACK_STATUS) { - uint8_t rp[2] = {EVT_PLAYBACK_STATUS, - (uint8_t)(A2dp::IsStreaming() ? 0x01 : 0x00)}; - SendVendorRsp(localCid, transaction, AVC_RSP_INTERIM, - pdu, rp, sizeof(rp)); - } else if (p[0] == EVT_TRACK_CHANGED) { - uint8_t rp[9] = {EVT_TRACK_CHANGED, - 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}; - SendVendorRsp(localCid, transaction, AVC_RSP_INTERIM, - pdu, rp, sizeof(rp)); + g_notifyLock.Acquire(); + if (SendVendorRsp(localCid, transaction, AVC_RSP_INTERIM, + pdu, rp, sizeof(rp))) { + g_volumeNotifyCid = localCid; + g_volumeNotifyTransaction = transaction; + } + g_notifyLock.Release(); } else { uint8_t err = 0x01; SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED, @@ -228,4 +232,28 @@ namespace Drivers::USB::Bluetooth::Avrcp { } } + void NotifyVolumeChanged(int percent) { + if (percent < 0) percent = 0; + if (percent > 100) percent = 100; + + g_notifyLock.Acquire(); + uint16_t cid = g_volumeNotifyCid; + uint8_t transaction = g_volumeNotifyTransaction; + g_volumeNotifyCid = 0; // one-shot, regardless of transport outcome + if (cid != 0) { + uint8_t rp[2] = { + EVT_VOLUME_CHANGED, (uint8_t)((percent * 127) / 100) + }; + SendVendorRsp(cid, transaction, AVC_RSP_CHANGED, + PDU_REGISTER_NOTIFY, rp, sizeof(rp)); + } + g_notifyLock.Release(); + } + + void OnChannelClosed(uint16_t localCid) { + g_notifyLock.Acquire(); + if (g_volumeNotifyCid == localCid) g_volumeNotifyCid = 0; + g_notifyLock.Release(); + } + } diff --git a/kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp b/kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp index 6ecc241..cb3e621 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/Avrcp.hpp @@ -10,10 +10,18 @@ namespace Drivers::USB::Bluetooth::Avrcp { // Process an AVCTP packet arriving on an AVRCP control channel (PSM 0x0017). - // Implements a minimal AVRCP Target: unit/subunit info, pass-through accept, - // GetCapabilities, RegisterNotification (interim), absolute volume. Runs + // Implements a minimal AVRCP Target: unit/subunit info, honest rejection of + // unimplemented pass-through controls, capabilities, one-shot absolute- + // volume notification, and absolute-volume control. Runs // from the ACL receive path (DrainEvents, top-level) -- sending is safe // there, blocking waits are not (none are used). void ProcessAvctp(uint16_t localCid, const uint8_t* data, uint16_t len); + // Complete an outstanding EVENT_VOLUME_CHANGED registration. AVRCP + // notifications are one-shot: after CHANGED the controller registers again. + void NotifyVolumeChanged(int percent); + + // Drop registrations tied to a channel that L2CAP has torn down. + void OnChannelClosed(uint16_t localCid); + } diff --git a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp index 42da36a..1bbf639 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.cpp @@ -157,58 +157,45 @@ namespace Drivers::USB::Bluetooth { // Intel Bluetooth firmware detection // ========================================================================= - static bool InitIntelBluetooth(uint8_t slotId) { + static bool InitIntelBluetooth() { KernelLogStream(INFO, "BT") << "Intel Bluetooth adapter detected"; - // Intel BT controllers require HCI Reset before they respond to - // vendor-specific commands. This mirrors the Linux btintel driver - // sequence: Reset → Read Version → (firmware load) → Reset. - if (!Hci::Reset()) { - KernelLogStream(ERROR, "BT") << "Initial HCI Reset failed"; - return false; - } - - // Read standard HCI version -- if this fails, the controller is likely - // in bootloader mode where only vendor commands are accepted. - Hci::LocalVersion lver = {}; - bool hciVersionOk = Hci::ReadLocalVersion(&lver); - if (hciVersionOk) { - KernelLogStream(INFO, "BT") << "HCI version=" << (uint64_t)lver.HciVersion - << " rev=" << base::hex << (uint64_t)lver.HciRevision - << " LMP=" << (uint64_t)lver.LmpVersion - << " manufacturer=" << (uint64_t)lver.Manufacturer - << " subver=" << (uint64_t)lver.LmpSubversion << base::dec; - } - - // Read legacy Intel version for diagnostics (TLV parts return this in - // a different layout; the authoritative state check happens inside the - // firmware download path below via the TLV version). - Hci::IntelVersion ver = {}; - if (!Hci::ReadIntelVersion(&ver)) { - KernelLogStream(WARNING, "BT") << "Failed to read Intel BT version"; - } else { - KernelLogStream(INFO, "BT") << "Intel BT: HW variant=" << (uint64_t)ver.HwVariant - << " FW variant=" << base::hex << (uint64_t)ver.FwVariant - << " FW rev=" << (uint64_t)ver.FwRevision << "." - << (uint64_t)ver.FwBuildNum << base::dec; - } - - // Run the firmware download path. This reads the TLV version, and if - // the controller is in bootloader mode, loads the matching .sfi image - // from the ramdisk, secure-sends it, boots the operational firmware + // Read the Intel TLV version first. It is the mode probe accepted by + // both Intel's bootloader and operational firmware. A standard HCI + // Reset must not precede it: the bootloader answers Reset with status + // 0x01 (Unknown HCI Command), which is normal rather than a fatal + // transport failure. + // + // If the controller is in bootloader mode, this loads the matching SFI + // image from the ramdisk, secure-sends it, boots operational firmware, // and applies DDC parameters. Returns true if the controller ends up // operational (either already loaded, or freshly downloaded). if (!DownloadIntelFirmware()) { - KernelLogStream(WARNING, "BT") - << "Intel BT firmware not loaded; limited functionality"; - // Standard init already issued an HCI Reset above. - return true; + // Older Intel parts may lack the TLV command while already running + // usable operational firmware. Preserve that compatibility only + // when standard HCI proves it is genuinely operational. + if (Hci::Reset()) { + KernelLogStream(WARNING, "BT") + << "Intel firmware query failed; continuing with operational HCI"; + return true; + } + KernelLogStream(ERROR, "BT") + << "Intel controller remains in bootloader mode"; + return false; } - // The operational firmware just (re)booted. Give it a clean reset and - // re-enable the Intel vendor event mask before the generic HCI setup. - Hci::Reset(); - Hci::IntelSetEventMask(); + // Whether it was already present or was just booted, operational + // firmware must now accept standard HCI. Do not mark the adapter ready + // if this transition did not actually happen. + if (!Hci::Reset()) { + KernelLogStream(ERROR, "BT") + << "Operational firmware did not accept HCI Reset"; + return false; + } + if (!Hci::IntelSetEventMask()) { + KernelLogStream(WARNING, "BT") + << "Intel vendor event mask was not accepted"; + } return true; } @@ -272,10 +259,15 @@ namespace Drivers::USB::Bluetooth { // Intel-specific initialization (firmware download + HCI Reset) bool didReset = false; if (IsIntelBt(dev->VendorId, dev->ProductId)) { - if (InitIntelBluetooth(g_slotId)) { + if (InitIntelBluetooth()) { didReset = true; // InitIntelBluetooth already sent HCI Reset } else { - KernelLogStream(WARNING, "BT") << "Intel BT init failed, continuing with basic HCI"; + // A recognized Intel part that failed its vendor initialization + // is normally still a bootloader, where a second standard Reset + // only repeats status 0x01. Stop with an accurate failure + // instead of pretending a basic-HCI fallback exists. + KernelLogStream(ERROR, "BT") << "Intel BT initialization failed"; + return; } } @@ -315,6 +307,10 @@ namespace Drivers::USB::Bluetooth { KernelLogStream(INFO, "BT") << "ACL buffer: " << (uint64_t)aclLen << " bytes x " << (uint64_t)aclNum; } + // Bulk IN was armed before Intel firmware loading, but bootloader runts + // are not HCI ACL traffic and an absorbed firmware-phase USB error may + // have stopped the endpoint. Start framed ACL reception only now. + Hci::EnableAclDataReception(); // Set local name Hci::WriteLocalName("MontaukOS"); @@ -363,10 +359,15 @@ namespace Drivers::USB::Bluetooth { // Request (0x33), Simple Pairing Complete (0x36) all live there. It was // 0x00 -> the controller started SSP but the IO-Capability Request event // never reached us, so pairing always timed out with auth failure 0x05. - uint8_t eventMask[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F, 0xFF, 0x20}; + uint8_t eventMask[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x20}; Hci::SendCommand(Hci::OP_SET_EVENT_MASK, eventMask, 8); Hci::WaitCommandComplete(Hci::OP_SET_EVENT_MASK); + // Request Extended Inquiry Results so scan entries carry EIR names and + // RSSI. Older controllers may support only mode 1 (RSSI); the parser + // handles both result layouts and the fallback preserves discovery. + if (!Hci::WriteInquiryMode(2)) Hci::WriteInquiryMode(1); + // Enable inquiry + page scan (discoverable and connectable) Hci::WriteScanEnable(0x03); @@ -420,8 +421,11 @@ namespace Drivers::USB::Bluetooth { void ServiceEvents() { if (!g_initialized) return; if (Xhci::InPollContext()) return; // never nest under PollEvents - Hci::ProcessPendingCommands(); A2dp::ServiceMedia(); // reap events and feed queued media + // DrainEvents may just have queued an accept/auth/encryption reply. + // Send it in this same service pass instead of adding a scheduler-turn + // delay to the controller's security timeout. + Hci::ProcessPendingCommands(); Drivers::Audio::Mixer::OnBluetoothWritable(); int requestedVolume; if (A2dp::ConsumeVolumeRequest(&requestedVolume)) @@ -472,13 +476,22 @@ namespace Drivers::USB::Bluetooth { } } // Let the disconnection(s) complete before reprogramming the address. + // A fixed 300 ms pause was merely hopeful and could change the identity + // under a still-live encrypted link. if (droppedLink) { uint64_t t0 = Timekeeping::GetMilliseconds(); - while (Timekeeping::GetMilliseconds() - t0 < 300) { + bool anyActive = true; + while (anyActive && Timekeeping::GetMilliseconds() - t0 < 2000) { Xhci::PollEvents(); Hci::DrainEvents(); + anyActive = false; + for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) { + auto* conn = Hci::GetConnectionByIndex(i); + if (conn && conn->Active) { anyActive = true; break; } + } for (int k = 0; k < 200; k++) asm volatile("pause" ::: "memory"); } + if (anyActive) return false; } // Program the new address. Do NOT issue an HCI Reset afterwards: the @@ -515,9 +528,10 @@ namespace Drivers::USB::Bluetooth { Hci::ClearInquiryResults(); // Convert timeout to 1.28s units (min 1, max 30) - uint8_t duration = (uint8_t)(timeoutMs / 1280); - if (duration < 1) duration = 1; - if (duration > 30) duration = 30; + uint32_t durationUnits = timeoutMs / 1280; + if (durationUnits < 1) durationUnits = 1; + if (durationUnits > 30) durationUnits = 30; + uint8_t duration = (uint8_t)durationUnits; if (!Hci::StartInquiry(duration)) return -1; @@ -534,7 +548,21 @@ namespace Drivers::USB::Bluetooth { // Cancel if still running if (Hci::IsInquiryActive()) { - Hci::CancelInquiry(); + // A failed cancel must not be hidden: Create Connection while the + // controller is still in Inquiry is commonly rejected as Command + // Disallowed. Give a command that briefly lost HCI ownership one + // retry, continuing to service the completion event in between. + if (!Hci::CancelInquiry()) { + uint64_t cancelStart = Timekeeping::GetMilliseconds(); + while (Hci::IsInquiryActive() + && Timekeeping::GetMilliseconds() - cancelStart < 250) { + Xhci::PollEvents(); + Hci::DrainEvents(); + for (int j = 0; j < 100; j++) + asm volatile("pause" ::: "memory"); + } + if (Hci::IsInquiryActive() && !Hci::CancelInquiry()) return -1; + } } return Hci::GetInquiryResults(buf, maxCount); @@ -544,89 +572,82 @@ namespace Drivers::USB::Bluetooth { // Connect — initiate ACL connection // ========================================================================= + static bool SameAddress(const uint8_t* a, const uint8_t* b) { + if (!a || !b) return false; + for (int i = 0; i < 6; i++) if (a[i] != b[i]) return false; + return true; + } + + static Hci::ConnectionInfo* FindAclConnection(const uint8_t* bdAddr) { + for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) { + auto* conn = Hci::GetConnectionByIndex(i); + if (conn && conn->Active && conn->LinkType == 0x01 + && SameAddress(conn->BdAddr, bdAddr)) return conn; + } + return nullptr; + } + int Connect(const uint8_t* bdAddr, uint32_t timeoutMs) { if (!g_initialized || !bdAddr) return -1; - if (!Hci::CreateConnection(bdAddr)) return -1; + // A previous attempt can leave a healthy encrypted ACL link with A2DP + // incomplete. Treat another click as an A2DP repair attempt on that + // link; issuing HCI Create Connection again just returns "connection + // already exists" and made manual recovery impossible. + Hci::ConnectionInfo* target = FindAclConnection(bdAddr); + bool reusedAcl = target != nullptr; + + if (!target && !Hci::CreateConnection(bdAddr)) return -1; // Wait for Connection Complete event uint64_t start = Timekeeping::GetMilliseconds(); - while (Timekeeping::GetMilliseconds() - start < timeoutMs) { + while (!target && Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); Hci::DrainEvents(); - - // Check connection table for matching BD_ADDR - for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) { - auto* conn = Hci::GetConnectionByIndex(i); - if (conn && conn->Active) { - bool match = true; - for (int j = 0; j < 6; j++) { - if (conn->BdAddr[j] != bdAddr[j]) { match = false; break; } - } - if (match) { - // The headset (in pairing mode) drives Secure Simple - // Pairing itself right after the ACL link comes up. Let - // authentication + encryption finish BEFORE opening any - // L2CAP/AVDTP channels: doing A2DP on a not-yet- - // authenticated link races with the pairing handshake and - // the headset drops us (reason 0x05). Drain events here - // so the IO-capability / user-confirm / link-key / encrypt - // events all get serviced. - // - // We are the initiator: request authentication so the - // controller starts Secure Simple Pairing (Link Key - // Request -> our negative reply -> IO Capability Request - // -> ... ). The headset does not start this on its own. - // NB this only works now that octet 6 of the event mask - // is enabled so the IO-Capability Request event actually - // reaches us; before that this produced 03 17 06 05. - Hci::AuthenticateLink(conn->Handle); - - uint64_t t0 = Timekeeping::GetMilliseconds(); - while (Timekeeping::GetMilliseconds() - t0 < 5000) { - Xhci::PollEvents(); - Hci::DrainEvents(); - // Send queued pairing replies reliably (top-level, - // not nested under PollEvents). - Hci::ProcessPendingCommands(); - if (!conn->Active) break; // link dropped during pairing - if (conn->Encrypted) break; // authenticated + encrypted -> ready - for (int k = 0; k < 200; k++) asm volatile("pause" ::: "memory"); - } - - // Bring up the A2DP source stream (signaling + media - // channels, SBC negotiation) only once the link is - // secured. Without a media stream the headset also drops - // the link (reason 0x13), so this keeps it engaged too. - if (conn->Active) { - // Let the link settle after Encryption Change before - // dialing L2CAP: some sinks ignore a CONN_REQ that - // arrives the instant encryption completes. Drain - // (don't blind-sleep) so the ACL RX ring stays live. - uint64_t st = Timekeeping::GetMilliseconds(); - while (Timekeeping::GetMilliseconds() - st < 300) { - Xhci::PollEvents(); - Hci::DrainEvents(); - for (int k = 0; k < 200; k++) asm volatile("pause" ::: "memory"); - } - A2dp::StartSource(); - Drivers::Audio::Mixer::OnBluetoothStateChanged(); - } - // Persist any new link key now (process context), even if - // the link later dropped, so the disk write never stalls - // the nested pairing event handler. - Hci::FlushLinkKeys(); - return 0; - } - } - } - + target = FindAclConnection(bdAddr); for (int j = 0; j < 200; j++) { asm volatile("pause" ::: "memory"); } } + if (!target) return -1; - return -1; // Timeout + // Connection Complete queues authentication for both incoming and + // outgoing ACLs. Deliver that common request and wait for encryption; + // a manual A2DP repair on an encrypted ACL skips this entire wait. + if (!target->Encrypted) { + // A link that predates this syscall may have exhausted or missed its + // earlier security attempt; explicitly restart it. A freshly-created + // link already has the request queued by Connection Complete. + if (reusedAcl) Hci::AuthenticateLink(target->Handle); + uint64_t t0 = Timekeeping::GetMilliseconds(); + while (Timekeeping::GetMilliseconds() - t0 < 5000) { + Xhci::PollEvents(); + Hci::DrainEvents(); + Hci::ProcessPendingCommands(); + if (!target->Active || !SameAddress(target->BdAddr, bdAddr)) break; + if (target->Encrypted) break; + for (int k = 0; k < 200; k++) asm volatile("pause" ::: "memory"); + } + } + + bool targetAlive = target->Active + && SameAddress(target->BdAddr, bdAddr); + bool a2dpReady = false; + if (targetAlive) { + a2dpReady = A2dp::StartSource(); + Drivers::Audio::Mixer::OnBluetoothStateChanged(); + } + + // Persist any new link key now (process context), even if the link later + // dropped, so disk I/O never stalls the nested pairing event handler. + Hci::FlushLinkKeys(); + if (!target->Active || !SameAddress(target->BdAddr, bdAddr)) return -1; + if (!a2dpReady) { + KernelLogStream(WARNING, "BT") + << "ACL connected but A2DP source setup failed"; + return -2; + } + return 0; } // ========================================================================= @@ -640,11 +661,7 @@ namespace Drivers::USB::Bluetooth { for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) { auto* conn = Hci::GetConnectionByIndex(i); if (conn && conn->Active) { - bool match = true; - for (int j = 0; j < 6; j++) { - if (conn->BdAddr[j] != bdAddr[j]) { match = false; break; } - } - if (match) { + if (SameAddress(conn->BdAddr, bdAddr)) { Hci::Disconnect(conn->Handle, 0x13); // 0x13 = Remote User Terminated return 0; } diff --git a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp index d70f628..699dcb3 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/Bluetooth.hpp @@ -49,7 +49,8 @@ namespace Drivers::USB::Bluetooth { int Scan(Hci::InquiryDevice* buf, int maxCount, uint32_t timeoutMs); // Initiate connection to a remote device by BD_ADDR - // Returns 0 on success (connection established), -1 on failure + // Returns 0 when ACL + A2DP are ready, -1 when the ACL connection failed, + // or -2 when ACL connected but A2DP setup did not complete. int Connect(const uint8_t* bdAddr, uint32_t timeoutMs = 10000); // Disconnect a device by BD_ADDR diff --git a/kernel/src/Drivers/USB/Bluetooth/Hci.cpp b/kernel/src/Drivers/USB/Bluetooth/Hci.cpp index 94c5c64..40870c3 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Hci.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/Hci.cpp @@ -31,21 +31,26 @@ namespace Drivers::USB::Bluetooth::Hci { static bool g_initialized = false; // Event receive buffer (filled by xHCI interrupt IN callback) - static uint8_t g_eventBuf[256] = {}; - static volatile uint32_t g_eventLen = 0; - static volatile bool g_eventReady = false; + static uint8_t g_eventBuf[260] = {}; + static uint32_t g_eventLen = 0; + static std::atomic g_eventReady{false}; + static kcp::Spinlock g_eventMailboxLock; - // Continuation bytes still expected for the event in g_eventBuf. An HCI - // event larger than the interrupt endpoint's max packet (64) arrives as - // several USB packets; only the first starts with the event header. The - // mailbox is marked ready ONLY once the full declared length has arrived. - // This matters beyond correctness of the data: sending the NEXT command - // while the previous response is still mid-transmission wedges the AX211 - // bootloader into permanently ignoring commands. (For months the boot - // console's slow flanterm rendering accidentally paced commands past this; - // any log-suppressed/deferred bring-up hit it deterministically at the - // 96-byte FC05 TLV response.) - static volatile uint32_t g_eventRemaining = 0; + // USB interrupt transfers are endpoint-max-packet sized, while one HCI event + // may be up to 257 bytes. Reassemble the byte stream for *all* event types, + // then route complete Command Complete/Status events to the command mailbox + // and asynchronous events to a process-context ring. The previous code + // reassembled command events only; Extended Inquiry Result was processed + // from its first 64-byte fragment and the remaining fragments were mistaken + // for unrelated event headers. + static uint8_t g_eventAssembly[260] = {}; + static uint16_t g_eventAssemblyLen = 0; + static uint16_t g_eventAssemblyExpected = 0; + static constexpr int ASYNC_EVENT_SLOTS = 32; + static uint8_t g_asyncEventRing[ASYNC_EVENT_SLOTS][260] = {}; + static uint16_t g_asyncEventLens[ASYNC_EVENT_SLOTS] = {}; + static std::atomic g_asyncEventHead{0}; + static std::atomic g_asyncEventTail{0}; // Firmware-phase diagnostics. g_fwTrace turns on a bounded per-completion // trace of the interrupt IN pipe (SetFwTrace, driven by the bring-up); @@ -103,11 +108,31 @@ namespace Drivers::USB::Bluetooth::Hci { // bursts many ACL packets at once -- the single buffer was overwriting and // dropping the L2CAP Config Response, leaving our channel half-configured. static constexpr int ACL_RX_SLOTS = 32; - static constexpr int ACL_RX_SLOT_SIZE = 1024; + static constexpr int ACL_RX_SLOT_SIZE = 2048; static uint8_t g_aclRxRing[ACL_RX_SLOTS][ACL_RX_SLOT_SIZE] = {}; - static volatile uint16_t g_aclRxLens[ACL_RX_SLOTS] = {}; - static volatile uint8_t g_aclRxHead = 0; - static volatile uint8_t g_aclRxTail = 0; + static uint16_t g_aclRxLens[ACL_RX_SLOTS] = {}; + static std::atomic g_aclRxHead{0}; + static std::atomic g_aclRxTail{0}; + + // A USB bulk completion is only a fragment of an HCI ACL packet when the + // controller's ACL length exceeds the endpoint max packet. Assemble the + // HCI packet before putting it on the RX ring. Reception is enabled only + // after firmware setup, so Intel bootloader runts cannot poison framing. + static uint8_t g_aclUsbAssembly[ACL_RX_SLOT_SIZE] = {}; + static uint16_t g_aclUsbAssemblyLen = 0; + static uint16_t g_aclUsbAssemblyExpected = 0; + static std::atomic g_aclDataEnabled{false}; + static std::atomic g_aclPipeNeedsRecovery{false}; + static std::atomic g_aclTxPipeNeedsRecovery{false}; + static std::atomic g_aclTxTransportError{false}; + static std::atomic g_eventPipeNeedsRecovery{false}; + + // HCI ACL fragmentation sits below L2CAP: the first fragment contains the + // L2CAP header and PB=continuing fragments contain raw continuation bytes. + static uint8_t g_l2capAssembly[4096] = {}; + static uint16_t g_l2capAssemblyLen = 0; + static uint16_t g_l2capAssemblyExpected = 0; + static uint16_t g_l2capAssemblyHandle = 0; // ACL transmit DMA buffers (a ring, not one): SendAcl queues an async bulk // OUT transfer, so two sends in quick succession (e.g. our Config Request @@ -129,6 +154,20 @@ namespace Drivers::USB::Bluetooth::Hci { // HCI command DMA buffer (separate from ACL to avoid conflicts) static uint8_t* g_cmdDmaBuf = nullptr; static uint64_t g_cmdDmaBufPhys = 0; + // Owns both the shared command DMA buffer and the command-response + // transaction. The xHCI control-transfer lock alone is too late: two + // callers could overwrite g_cmdDmaBuf before either entered that lock. + // Zero means idle; otherwise this is the opcode whose Complete/Status is + // awaited. Intel Secure Send and Intel Reset intentionally have no HCI + // command response and release ownership after the USB transfer itself. + static std::atomic g_commandOpcode{0}; + + static void FinishCommand(uint16_t opcode) { + uint16_t expected = opcode; + g_commandOpcode.compare_exchange_strong(expected, 0, + std::memory_order_release, + std::memory_order_relaxed); + } // Connection table static ConnectionInfo g_connections[MAX_CONNECTIONS] = {}; @@ -153,8 +192,8 @@ namespace Drivers::USB::Bluetooth::Hci { // Inquiry results static InquiryDevice g_inquiryResults[MAX_INQUIRY_RESULTS] = {}; - static volatile int g_inquiryResultCount = 0; - static volatile bool g_inquiryActive = false; + static std::atomic g_inquiryResultCount{0}; + static std::atomic g_inquiryActive{false}; // Set when the Intel "bootup" vendor event arrives after a firmware boot. // Written from the USB transfer callback (ProcessEvent), polled by @@ -192,17 +231,23 @@ namespace Drivers::USB::Bluetooth::Hci { // fresh pairings (whose replies fit) ever worked. struct PendingHciCmd { uint16_t opcode; uint8_t len; uint8_t params[22]; }; static PendingHciCmd g_pending[16] = {}; - static volatile uint8_t g_pendingHead = 0; - static volatile uint8_t g_pendingTail = 0; + static std::atomic g_pendingHead{0}; + static std::atomic g_pendingTail{0}; static void EnqueueHciCmd(uint16_t opcode, const uint8_t* params, uint8_t len) { - uint8_t next = (uint8_t)((g_pendingHead + 1) & 15); - if (next == g_pendingTail) return; // full -> drop (should never happen) + uint8_t head = g_pendingHead.load(std::memory_order_relaxed); + uint8_t next = (uint8_t)((head + 1) & 15); + if (next == g_pendingTail.load(std::memory_order_acquire)) { + KernelLogStream(ERROR, "BT-HCI") + << "Pending command queue full; dropping opcode=" + << base::hex << (uint64_t)opcode << base::dec; + return; + } if (len > sizeof(g_pending[0].params)) len = sizeof(g_pending[0].params); - g_pending[g_pendingHead].opcode = opcode; - g_pending[g_pendingHead].len = len; - for (uint8_t i = 0; i < len; i++) g_pending[g_pendingHead].params[i] = params[i]; - g_pendingHead = next; + g_pending[head].opcode = opcode; + g_pending[head].len = len; + for (uint8_t i = 0; i < len; i++) g_pending[head].params[i] = params[i]; + g_pendingHead.store(next, std::memory_order_release); } // ========================================================================= @@ -224,13 +269,15 @@ namespace Drivers::USB::Bluetooth::Hci { }; static StoredLinkKey g_bonds[MAX_BONDS] = {}; static bool g_bondsDirty = false; + static kcp::Spinlock g_bondsLock; static bool AddrEq(const uint8_t* a, const uint8_t* b) { for (int i = 0; i < 6; i++) if (a[i] != b[i]) return false; return true; } - static int FindBondIndex(const uint8_t* addr) { + // Caller holds g_bondsLock. + static int FindBondIndexLocked(const uint8_t* addr) { for (int i = 0; i < MAX_BONDS; i++) { if (g_bonds[i].valid && AddrEq(g_bonds[i].addr, addr)) return i; } @@ -238,7 +285,8 @@ namespace Drivers::USB::Bluetooth::Hci { } static void StoreLinkKey(const uint8_t* addr, const uint8_t* key) { - int idx = FindBondIndex(addr); + g_bondsLock.Acquire(); + int idx = FindBondIndexLocked(addr); if (idx < 0) { for (int i = 0; i < MAX_BONDS; i++) { if (!g_bonds[i].valid) { idx = i; break; } @@ -249,6 +297,7 @@ namespace Drivers::USB::Bluetooth::Hci { memcpy(g_bonds[idx].key, key, 16); g_bonds[idx].valid = true; g_bondsDirty = true; // FlushLinkKeys() persists from safe context + g_bondsLock.Release(); } // On-disk layout: [magic u32][MAX_BONDS x { addr[6], key[16], valid[1] }]. @@ -261,26 +310,39 @@ namespace Drivers::USB::Bluetooth::Hci { if (size < LINK_KEY_BLOB_SIZE) { Fs::Vfs::CloseBackendFile(f); return; } uint8_t blob[LINK_KEY_BLOB_SIZE]; - Fs::Vfs::ReadBackendFile(f, blob, 0, LINK_KEY_BLOB_SIZE); + int bytesRead = Fs::Vfs::ReadBackendFile(f, blob, 0, LINK_KEY_BLOB_SIZE); Fs::Vfs::CloseBackendFile(f); + if (bytesRead != (int)LINK_KEY_BLOB_SIZE) return; uint32_t magic = (uint32_t)blob[0] | ((uint32_t)blob[1] << 8) | ((uint32_t)blob[2] << 16) | ((uint32_t)blob[3] << 24); if (magic != LINK_KEY_MAGIC) return; + StoredLinkKey loaded[MAX_BONDS] = {}; int off = 4, n = 0; for (int i = 0; i < MAX_BONDS; i++) { - memcpy(g_bonds[i].addr, &blob[off], 6); off += 6; - memcpy(g_bonds[i].key, &blob[off], 16); off += 16; - g_bonds[i].valid = blob[off++] != 0; - if (g_bonds[i].valid) n++; + memcpy(loaded[i].addr, &blob[off], 6); off += 6; + memcpy(loaded[i].key, &blob[off], 16); off += 16; + loaded[i].valid = blob[off++] != 0; + if (loaded[i].valid) n++; } + g_bondsLock.Acquire(); + memcpy(g_bonds, loaded, sizeof(g_bonds)); g_bondsDirty = false; + g_bondsLock.Release(); KernelLogStream(INFO, "BT-HCI") << "Loaded " << (uint64_t)n << " bonded device key(s)"; } void FlushLinkKeys() { - if (!g_bondsDirty) return; + StoredLinkKey snapshot[MAX_BONDS]; + g_bondsLock.Acquire(); + if (!g_bondsDirty) { + g_bondsLock.Release(); + return; + } + memcpy(snapshot, g_bonds, sizeof(snapshot)); + g_bondsDirty = false; + g_bondsLock.Release(); uint8_t blob[LINK_KEY_BLOB_SIZE] = {}; blob[0] = (uint8_t)(LINK_KEY_MAGIC); @@ -289,24 +351,34 @@ namespace Drivers::USB::Bluetooth::Hci { blob[3] = (uint8_t)(LINK_KEY_MAGIC >> 24); int off = 4; for (int i = 0; i < MAX_BONDS; i++) { - memcpy(&blob[off], g_bonds[i].addr, 6); off += 6; - memcpy(&blob[off], g_bonds[i].key, 16); off += 16; - blob[off++] = g_bonds[i].valid ? 1 : 0; + memcpy(&blob[off], snapshot[i].addr, 6); off += 6; + memcpy(&blob[off], snapshot[i].key, 16); off += 16; + blob[off++] = snapshot[i].valid ? 1 : 0; } Fs::Vfs::BackendFile f; if (Fs::Vfs::CreateBackendFile("0:/os/btkeys.bin", f) < 0) { + g_bondsLock.Acquire(); + g_bondsDirty = true; + g_bondsLock.Release(); KernelLogStream(WARNING, "BT-HCI") << "Could not open link key store for writing"; return; } - Fs::Vfs::WriteBackendFile(f, blob, 0, LINK_KEY_BLOB_SIZE); + int bytesWritten = Fs::Vfs::WriteBackendFile(f, blob, 0, LINK_KEY_BLOB_SIZE); Fs::Vfs::CloseBackendFile(f); - g_bondsDirty = false; + if (bytesWritten != (int)LINK_KEY_BLOB_SIZE) { + g_bondsLock.Acquire(); + g_bondsDirty = true; + g_bondsLock.Release(); + KernelLogStream(WARNING, "BT-HCI") << "Link key store write was incomplete"; + return; + } KernelLogStream(OK, "BT-HCI") << "Link key store persisted"; } int ListBonds(BondInfo* buf, int maxCount) { if (!buf || maxCount <= 0) return 0; + g_bondsLock.Acquire(); int n = 0; for (int i = 0; i < MAX_BONDS && n < maxCount; i++) { if (g_bonds[i].valid) { @@ -314,21 +386,159 @@ namespace Drivers::USB::Bluetooth::Hci { n++; } } + g_bondsLock.Release(); return n; } bool ForgetBond(const uint8_t* addr) { if (!addr) return false; - int idx = FindBondIndex(addr); - if (idx < 0) return false; + g_bondsLock.Acquire(); + int idx = FindBondIndexLocked(addr); + if (idx < 0) { + g_bondsLock.Release(); + return false; + } g_bonds[idx].valid = false; memset(g_bonds[idx].key, 0, 16); memset(g_bonds[idx].addr, 0, 6); g_bondsDirty = true; + g_bondsLock.Release(); FlushLinkKeys(); // persist removal now (caller is process context) return true; } + static void QueueCompleteHciEvent(const uint8_t* event, uint16_t len) { + if (len < 2) return; + uint8_t code = event[0]; + if (code == EVT_COMMAND_COMPLETE || code == EVT_COMMAND_STATUS) { + // HCI commands are serialized. Normally preserve an unconsumed + // reply. The exception is a late response from a command that + // already timed out: if the newly-arrived event matches the live + // owner while the mailbox does not, replace the stale entry so the + // current command's only response is not dropped. + g_eventMailboxLock.Acquire(); + bool storeEvent = !g_eventReady.load(std::memory_order_relaxed); + uint16_t liveOpcode = g_commandOpcode.load(std::memory_order_acquire); + if (!storeEvent && liveOpcode != 0) { + uint16_t newOpcode = 0, oldOpcode = 0; + if (code == EVT_COMMAND_COMPLETE && len >= 5) + newOpcode = (uint16_t)event[3] | ((uint16_t)event[4] << 8); + else if (code == EVT_COMMAND_STATUS && len >= 6) + newOpcode = (uint16_t)event[4] | ((uint16_t)event[5] << 8); + if (g_eventBuf[0] == EVT_COMMAND_COMPLETE && g_eventLen >= 5) + oldOpcode = (uint16_t)g_eventBuf[3] | ((uint16_t)g_eventBuf[4] << 8); + else if (g_eventBuf[0] == EVT_COMMAND_STATUS && g_eventLen >= 6) + oldOpcode = (uint16_t)g_eventBuf[4] | ((uint16_t)g_eventBuf[5] << 8); + storeEvent = newOpcode == liveOpcode && oldOpcode != liveOpcode; + } + if (storeEvent) { + memcpy(g_eventBuf, event, len); + g_eventLen = len; + g_eventReady.store(true, std::memory_order_release); + } + g_eventMailboxLock.Release(); + return; + } + + uint8_t head = g_asyncEventHead.load(std::memory_order_relaxed); + uint8_t next = (uint8_t)((head + 1) % ASYNC_EVENT_SLOTS); + if (next == g_asyncEventTail.load(std::memory_order_acquire)) return; + memcpy(g_asyncEventRing[head], event, len); + g_asyncEventLens[head] = len; + g_asyncEventHead.store(next, std::memory_order_release); + } + + static bool PopCommandEvent(uint8_t* event, uint16_t* len) { + if (!event || !len) return false; + + // Keep the mailbox marked occupied until the bytes are private. If it + // were cleared first, an interrupt on another core could overwrite the + // shared buffer while the waiter was still parsing it. + g_eventMailboxLock.Acquire(); + if (!g_eventReady.load(std::memory_order_acquire)) { + g_eventMailboxLock.Release(); + return false; + } + uint16_t n = (uint16_t)g_eventLen; + if (n > sizeof(g_eventBuf)) n = sizeof(g_eventBuf); + memcpy(event, g_eventBuf, n); + *len = n; + g_eventReady.store(false, std::memory_order_release); + g_eventMailboxLock.Release(); + return true; + } + + static void FeedHciEventBytes(const uint8_t* data, uint32_t length) { + uint32_t off = 0; + while (off < length) { + if (g_eventAssemblyExpected == 0) { + while (g_eventAssemblyLen < 2 && off < length) + g_eventAssembly[g_eventAssemblyLen++] = data[off++]; + if (g_eventAssemblyLen < 2) return; + g_eventAssemblyExpected = (uint16_t)(2u + g_eventAssembly[1]); + if (g_eventAssemblyExpected > sizeof(g_eventAssembly)) { + g_eventAssemblyLen = 0; + g_eventAssemblyExpected = 0; + return; + } + } + + uint16_t need = (uint16_t)(g_eventAssemblyExpected - g_eventAssemblyLen); + uint32_t take = length - off; + if (take > need) take = need; + memcpy(&g_eventAssembly[g_eventAssemblyLen], &data[off], take); + g_eventAssemblyLen = (uint16_t)(g_eventAssemblyLen + take); + off += take; + + if (g_eventAssemblyLen == g_eventAssemblyExpected) { + QueueCompleteHciEvent(g_eventAssembly, g_eventAssemblyLen); + g_eventAssemblyLen = 0; + g_eventAssemblyExpected = 0; + } + } + } + + static void QueueCompleteAclPacket(const uint8_t* packet, uint16_t len) { + uint8_t head = g_aclRxHead.load(std::memory_order_relaxed); + uint8_t next = (uint8_t)((head + 1) % ACL_RX_SLOTS); + if (next == g_aclRxTail.load(std::memory_order_acquire)) return; + memcpy(g_aclRxRing[head], packet, len); + g_aclRxLens[head] = len; + g_aclRxHead.store(next, std::memory_order_release); + } + + static void FeedAclUsbBytes(const uint8_t* data, uint32_t length) { + uint32_t off = 0; + while (off < length) { + if (g_aclUsbAssemblyExpected == 0) { + while (g_aclUsbAssemblyLen < sizeof(AclHeader) && off < length) + g_aclUsbAssembly[g_aclUsbAssemblyLen++] = data[off++]; + if (g_aclUsbAssemblyLen < sizeof(AclHeader)) return; + auto* hdr = (const AclHeader*)g_aclUsbAssembly; + uint32_t total = sizeof(AclHeader) + hdr->DataLength; + if (total < sizeof(AclHeader) || total > sizeof(g_aclUsbAssembly)) { + g_aclUsbAssemblyLen = 0; + g_aclUsbAssemblyExpected = 0; + return; + } + g_aclUsbAssemblyExpected = (uint16_t)total; + } + + uint16_t need = (uint16_t)(g_aclUsbAssemblyExpected - g_aclUsbAssemblyLen); + uint32_t take = length - off; + if (take > need) take = need; + memcpy(&g_aclUsbAssembly[g_aclUsbAssemblyLen], &data[off], take); + g_aclUsbAssemblyLen = (uint16_t)(g_aclUsbAssemblyLen + take); + off += take; + + if (g_aclUsbAssemblyLen == g_aclUsbAssemblyExpected) { + QueueCompleteAclPacket(g_aclUsbAssembly, g_aclUsbAssemblyLen); + g_aclUsbAssemblyLen = 0; + g_aclUsbAssemblyExpected = 0; + } + } + } + // ========================================================================= // USB transfer callback // ========================================================================= @@ -365,85 +575,52 @@ namespace Drivers::USB::Bluetooth::Hci { } } - if (data && length > 0) { - // HCI Event received on interrupt IN. - // Dispatch asynchronous events (inquiry results, connection - // events, etc.) immediately so they are never lost. Only - // buffer Command Complete / Command Status events — those are - // consumed by WaitCommandComplete / WaitCommandStatus. - uint8_t evtCode = data[0]; + if (data && length > 0) FeedHciEventBytes(data, length); - if (g_eventRemaining > 0) { - // Continuation packet of a buffered multi-packet event: - // append; ready only once the declared length is in. - uint32_t take = length; - if (take > g_eventRemaining) take = g_eventRemaining; - uint32_t room = sizeof(g_eventBuf) - g_eventLen; - uint32_t copyLen = (take < room) ? take : room; - if (copyLen > 0) { - memcpy(g_eventBuf + g_eventLen, data, copyLen); - g_eventLen = g_eventLen + copyLen; - } - g_eventRemaining = g_eventRemaining - take; - if (g_eventRemaining == 0) g_eventReady = true; - } else if (evtCode == EVT_COMMAND_COMPLETE || evtCode == EVT_COMMAND_STATUS) { - uint32_t copyLen = length; - if (copyLen > sizeof(g_eventBuf)) copyLen = sizeof(g_eventBuf); - memcpy(g_eventBuf, data, copyLen); - g_eventLen = copyLen; - - // Total event size = header (2) + declared parameter len. - // Larger than this packet -> more packets follow. - uint32_t total = (length >= 2) ? (2u + data[1]) : length; - if (total > length) { - g_eventRemaining = total - length; - } else { - g_eventReady = true; - } - } else { - // Process immediately (inquiry results, connection events, etc.) - ProcessEvent(data, length); - } + // During Intel firmware loading the event and bulk-IN pipes must + // keep cycling even across the controller's transient cc=4. The + // AX211 bootloader is known to inject one near the end of the SFI + // upload; stopping here makes every later command time out, while + // immediately posting the next TRB is the proven bootloader path. + // Once ACL reception is enabled, use full endpoint recovery for a + // genuine operational-mode halt. + if (data || !g_aclDataEnabled.load(std::memory_order_acquire)) { + Xhci::QueueInterruptTransfer(slotId); + } else { + g_eventPipeNeedsRecovery.store(true, std::memory_order_release); } - - // ALWAYS re-queue, including error (data == nullptr) and 0-length - // (ZLP) completions. Re-arming only on data>0 meant a single such - // completion silently killed the event pipe for good -- every - // later command then "timed out" with no xHCI error in sight. - // Mirrors the bulk-IN rule below ("must keep cycling"). - Xhci::QueueInterruptTransfer(slotId); } else if (epDci == bulkInDci) { - // ACL data received on bulk IN -> copy into the ring; processed by - // DrainEvents() at top level (do NOT process here, nested). - // Only enqueue a packet big enough to carry an L2CAP header (ACL - // header + L2CAP header = 8 bytes). Smaller completions are ZLP / - // re-arm artifacts and the firmware-phase bulk-IN runts (4-7 bytes, - // the old "rx flood") -- ProcessPacket would reject them anyway, and - // dropping them here keeps the ring + rx stats clean. We STILL - // re-arm on every completion below (the bulk IN must keep cycling to - // absorb the device's ~635 KB cc=4 glitch during firmware download). - if (data && length >= sizeof(AclHeader) + 4) { - uint8_t next = (uint8_t)((g_aclRxHead + 1) % ACL_RX_SLOTS); - if (next != g_aclRxTail) { // ring not full (else drop: overran) - uint32_t copyLen = length; - if (copyLen > ACL_RX_SLOT_SIZE) copyLen = ACL_RX_SLOT_SIZE; - memcpy(g_aclRxRing[g_aclRxHead], data, copyLen); - g_aclRxLens[g_aclRxHead] = (uint16_t)copyLen; - g_aclRxHead = next; - } - } + if (data && length > 0 + && g_aclDataEnabled.load(std::memory_order_acquire)) + FeedAclUsbBytes(data, length); - // Re-queue bulk IN transfer (only on a real success/short completion; - // the error path passes data==nullptr and is handled elsewhere). - if (data) { + // Bulk IN deliberately remains armed throughout Intel firmware + // loading even though its bytes are not ACL yet. It absorbs the + // bootloader's upload-era cc=4 so that the interrupt event pipe + // survives to receive the secure-send and bootup events. Preserve + // the original immediate rearm in that phase; after operational + // HCI starts, defer a real endpoint reset on errors. + if (data || !g_aclDataEnabled.load(std::memory_order_acquire)) { Xhci::QueueBulkInTransfer(slotId, nullptr, 0, dev->BulkInMaxPacket); + } else { + // An xHCI transaction error stops this endpoint and the callback + // used to leave it dead forever. Recover from process context; + // endpoint reset issues xHCI commands and cannot run nested here. + g_aclPipeNeedsRecovery.store(true, std::memory_order_release); } } else if (epDci == (dev->BulkOutEpNum ? (uint8_t)(dev->BulkOutEpNum * 2) : (uint8_t)0)) { - // Bulk OUT completion: the packet was DMA'd to the controller and - // its TX ring slot is free again. Controller buffer credits - // (g_aclPendingCount) are NOT released here -- only the - // Number-Of-Completed-Packets event does that. - g_aclTxDoneCount.fetch_add(1, std::memory_order_relaxed); + if (completionCode == Xhci::CC_SUCCESS + || completionCode == Xhci::CC_SHORT_PACKET) { + // The packet was DMA'd to the controller and its TX ring slot + // is free again. Controller buffer credits are NOT released + // here -- only Number-Of-Completed-Packets does that. + g_aclTxDoneCount.fetch_add(1, std::memory_order_relaxed); + } else { + // Transfer errors halt the endpoint. Recovery issues xHCI + // commands, so defer it out of this PollEvents callback. + g_aclTxTransportError.store(true, std::memory_order_release); + g_aclTxPipeNeedsRecovery.store(true, std::memory_order_release); + } } } @@ -463,6 +640,7 @@ namespace Drivers::USB::Bluetooth::Hci { uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < ms) { Xhci::PollEvents(); + DrainEvents(); for (int j = 0; j < 100; j++) { asm volatile("" ::: "memory"); } @@ -488,9 +666,9 @@ namespace Drivers::USB::Bluetooth::Hci { g_aclTxRingPhys[i] = Memory::SubHHDM(g_aclTxRing[i]); } - // NOTE: Do NOT queue interrupt IN or bulk IN transfers here. - // The BT controller is not yet HCI-initialized and may misbehave. - // Call StartEventPipe() after HCI Reset and initial setup. + // NOTE: Do not queue receive transfers until RegisterAdapter has given + // the configured USB function its settling delay. StartEventPipe then + // arms both receive endpoints before the first bootloader command. g_initialized = true; KernelLogStream(OK, "BT-HCI") << "HCI transport initialized on slot " << (uint64_t)slotId; @@ -521,6 +699,12 @@ namespace Drivers::USB::Bluetooth::Hci { KernelLogStream(INFO, "BT-HCI") << "Event pipe started (interrupt IN + bulk IN)"; } + void EnableAclDataReception() { + g_aclUsbAssemblyLen = 0; + g_aclUsbAssemblyExpected = 0; + g_aclDataEnabled.store(true, std::memory_order_release); + } + // ========================================================================= // SendCommand — via USB control transfer on EP0 // ========================================================================= @@ -528,6 +712,12 @@ namespace Drivers::USB::Bluetooth::Hci { bool SendCommand(uint16_t opcode, const uint8_t* params, uint8_t paramLen) { if (!g_initialized || !g_cmdDmaBuf) return false; + uint16_t expected = 0; + if (!g_commandOpcode.compare_exchange_strong(expected, opcode, + std::memory_order_acquire, + std::memory_order_relaxed)) + return false; + // HCI command packet: opcode (2) + paramLen (1) + params // USB-BT spec: HCI commands are sent via control transfer // bmRequestType = 0x20 (Host-to-device, Class, Device) @@ -535,10 +725,23 @@ namespace Drivers::USB::Bluetooth::Hci { // wValue = 0, wIndex = 0 // wLength = sizeof(CommandHeader) + paramLen - // A new command abandons any half-assembled response from the previous - // one (only possible if the controller died mid-event) -- otherwise a - // stale g_eventRemaining would swallow this command's reply. - g_eventRemaining = 0; + // A complete unconsumed command response means another transaction is + // still in flight. Overwriting it cross-wires opcodes and turns one + // scheduling race into a cascade of timeouts. + // With command ownership acquired there cannot be a live waiter for an + // older mailbox entry. Such an entry is a late response to a command + // that already timed out (or an unexpected completion for an Intel + // fire-and-forget command); leaving it occupied would reject every + // future command permanently. + if (g_eventReady.load(std::memory_order_acquire)) { + g_eventMailboxLock.Acquire(); + if (g_eventReady.load(std::memory_order_relaxed)) + g_eventReady.store(false, std::memory_order_release); + g_eventMailboxLock.Release(); + KernelLogStream(WARNING, "BT-HCI") + << "Discarded stale command response before opcode=" + << base::hex << (uint64_t)opcode << base::dec; + } // Use DMA-allocated buffer (not stack) for the command data. // xHCI reads from this buffer via DMA for OUT transfers. @@ -564,11 +767,15 @@ namespace Drivers::USB::Bluetooth::Hci { g_lastControlCC = cc; if (cc != Xhci::CC_SUCCESS) { + FinishCommand(opcode); KernelLogStream(WARNING, "BT-HCI") << "SendCommand failed, opcode=" << base::hex << (uint64_t)opcode << " cc=" << base::dec << (uint64_t)cc; return false; } + if (opcode == OP_INTEL_SECURE_SEND || opcode == OP_INTEL_RESET) + FinishCommand(opcode); + return true; } @@ -582,40 +789,44 @@ namespace Drivers::USB::Bluetooth::Hci { // cannot wait -- a nested PollEvents is a no-op, so the Command Complete // is reaped by the active PollEvents after we return. The command was // already submitted (fire-and-forget); report success. - if (Xhci::InPollContext()) return true; + if (Xhci::InPollContext()) { + FinishCommand(opcode); + return true; + } uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); + DrainEvents(); - if (g_eventReady) { - g_eventReady = false; + uint8_t event[sizeof(g_eventBuf)]; + uint16_t eventLen = 0; + if (PopCommandEvent(event, &eventLen)) { - if (g_eventLen >= 2) { - uint8_t evtCode = g_eventBuf[0]; - uint8_t evtParamLen = g_eventBuf[1]; + if (eventLen >= 6) { + uint8_t evtCode = event[0]; + uint8_t evtParamLen = event[1]; - if (evtCode == EVT_COMMAND_COMPLETE && evtParamLen >= 3) { + if (evtCode == EVT_COMMAND_COMPLETE && evtParamLen >= 4 + && 2u + evtParamLen <= eventLen) { // Command Complete: NumPkts(1) + Opcode(2) + Status(1) + Params - uint16_t evtOpcode = (uint16_t)g_eventBuf[3] | ((uint16_t)g_eventBuf[4] << 8); + uint16_t evtOpcode = (uint16_t)event[3] | ((uint16_t)event[4] << 8); if (evtOpcode == opcode) { if (outParams && maxLen > 0) { - // Copy params starting after the status byte - uint8_t availLen = (evtParamLen > 4) ? (evtParamLen - 4) : 0; - uint8_t copyLen = (availLen < maxLen) ? availLen : maxLen; // Include status byte + return params - copyLen = (evtParamLen > 3) ? (evtParamLen - 3) : 0; + uint8_t copyLen = (evtParamLen > 3) ? (evtParamLen - 3) : 0; if (copyLen > maxLen) copyLen = maxLen; - memcpy(outParams, &g_eventBuf[5], copyLen); + memcpy(outParams, &event[5], copyLen); } // Check status - uint8_t status = g_eventBuf[5]; + uint8_t status = event[5]; if (status != 0) { KernelLogStream(WARNING, "BT-HCI") << "Command Complete status=" << (uint64_t)status << " opcode=" << base::hex << (uint64_t)opcode; } - return true; + FinishCommand(opcode); + return status == 0; } } @@ -629,6 +840,7 @@ namespace Drivers::USB::Bluetooth::Hci { KernelLogStream(WARNING, "BT-HCI") << "WaitCommandComplete timeout, opcode=" << base::hex << (uint64_t)opcode; + FinishCommand(opcode); return false; } @@ -638,24 +850,31 @@ namespace Drivers::USB::Bluetooth::Hci { bool WaitCommandStatus(uint16_t opcode, uint32_t timeoutMs) { // See WaitCommandComplete: cannot wait when nested under PollEvents. - if (Xhci::InPollContext()) return true; + if (Xhci::InPollContext()) { + FinishCommand(opcode); + return true; + } uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); + DrainEvents(); - if (g_eventReady) { - g_eventReady = false; + uint8_t event[sizeof(g_eventBuf)]; + uint16_t eventLen = 0; + if (PopCommandEvent(event, &eventLen)) { - if (g_eventLen >= 2) { - uint8_t evtCode = g_eventBuf[0]; - uint8_t evtParamLen = g_eventBuf[1]; + if (eventLen >= 6) { + uint8_t evtCode = event[0]; + uint8_t evtParamLen = event[1]; - if (evtCode == EVT_COMMAND_STATUS && evtParamLen >= 4) { - uint8_t status = g_eventBuf[2]; - uint16_t evtOpcode = (uint16_t)g_eventBuf[4] | ((uint16_t)g_eventBuf[5] << 8); + if (evtCode == EVT_COMMAND_STATUS && evtParamLen >= 4 + && 2u + evtParamLen <= eventLen) { + uint8_t status = event[2]; + uint16_t evtOpcode = (uint16_t)event[4] | ((uint16_t)event[5] << 8); if (evtOpcode == opcode) { + FinishCommand(opcode); return (status == 0); } } @@ -668,6 +887,7 @@ namespace Drivers::USB::Bluetooth::Hci { } } + FinishCommand(opcode); return false; } @@ -676,11 +896,19 @@ namespace Drivers::USB::Bluetooth::Hci { // ========================================================================= bool SendAcl(uint16_t handle, uint16_t pbFlag, const uint8_t* data, uint16_t len) { - if (!g_initialized || !g_aclTxRing[0]) return false; + if (!g_initialized || !g_aclTxRing[0] || !GetConnection(handle)) return false; if (len + sizeof(AclHeader) > 4096) return false; // Single page DMA buffer g_aclTxLock.Acquire(); + // HCI host flow control is a hard limit, not merely a media-pacing + // hint. Signaling can race the media pumper after AclTxReady(), so + // enforce it again under the same lock that reserves the TX slot. + if (g_aclMaxNum != 0 && AclPendingCount() >= g_aclMaxNum) { + g_aclTxLock.Release(); + return false; + } + // Every slot still owned by the xHCI: reusing one would let this // packet's memcpy race the in-flight DMA. Dropping is recoverable // (L2CAP peers retransmit signaling; media just skips a frame), @@ -730,10 +958,12 @@ namespace Drivers::USB::Bluetooth::Hci { // A send is safe only when BOTH resources have room: // - a controller ACL buffer credit (NOCP-tracked), and // - a TX DMA ring slot not still queued in the xHCI transfer ring. - // Leave one TX slot of headroom so a signaling reply fired from the - // event path (AVDTP/AVRCP response) can always go out. + // Leave one controller credit AND one TX slot of headroom so a + // signaling reply fired from the event path (AVDTP/AVRCP response) can + // go out without violating HCI host flow control. uint16_t maxOut = g_aclMaxNum ? g_aclMaxNum : 4; - if (AclPendingCount() >= maxOut) return false; + uint16_t mediaLimit = maxOut > 1 ? (uint16_t)(maxOut - 1) : maxOut; + if (AclPendingCount() >= mediaLimit) return false; if (AclTxInFlight() >= (uint32_t)(ACL_TX_SLOTS - 1)) return false; return true; } @@ -750,11 +980,27 @@ namespace Drivers::USB::Bluetooth::Hci { // ProcessEvent — handle HCI events // ========================================================================= + static InquiryDevice* UpsertInquiryResult(const uint8_t* bdAddr) { + int count = g_inquiryResultCount.load(std::memory_order_relaxed); + for (int i = 0; i < count; i++) { + if (AddrEq(g_inquiryResults[i].BdAddr, bdAddr)) + return &g_inquiryResults[i]; + } + if (count >= MAX_INQUIRY_RESULTS) return nullptr; + InquiryDevice* result = &g_inquiryResults[count]; + memset(result, 0, sizeof(*result)); + memcpy(result->BdAddr, bdAddr, 6); + result->Rssi = -128; + g_inquiryResultCount.store(count + 1, std::memory_order_release); + return result; + } + void ProcessEvent(const uint8_t* data, uint32_t len) { if (len < 2) return; uint8_t evtCode = data[0]; uint8_t evtParamLen = data[1]; + if (2u + evtParamLen > len) return; const uint8_t* params = data + 2; switch (evtCode) { @@ -764,26 +1010,82 @@ namespace Drivers::USB::Bluetooth::Hci { uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8); const uint8_t* bdAddr = ¶ms[3]; uint8_t linkType = params[9]; + bool encrypted = params[10] != 0; KernelLogStream(INFO, "BT-HCI") << "Connection Complete: status=" << (uint64_t)status << " handle=" << (uint64_t)handle << " link=" << (uint64_t)linkType; if (status == 0) { + // L2CAP/A2DP state is intentionally single-link today. + // A second ACL must not silently reset the first link's + // channel table (the old behavior killed a playing + // headset as soon as another device connected). + auto* owner = GetConnection(L2cap::GetAclHandle()); + if (owner && owner->Handle == handle) { + KernelLogStream(INFO, "BT-HCI") + << "Ignoring duplicate Connection Complete event"; + break; + } + if (linkType == 0x01 && owner && owner->Handle != handle) { + uint8_t drop[3] = { + (uint8_t)handle, (uint8_t)(handle >> 8), + 0x0D // connection rejected due to limited resources + }; + EnqueueHciCmd(OP_DISCONNECT, drop, sizeof(drop)); + KernelLogStream(WARNING, "BT-HCI") + << "Rejecting second ACL link while A2DP link is active"; + break; + } + // Find empty connection slot + bool stored = false; for (int i = 0; i < MAX_CONNECTIONS; i++) { if (!g_connections[i].Active) { - g_connections[i].Active = true; g_connections[i].Handle = handle; memcpy(g_connections[i].BdAddr, bdAddr, 6); g_connections[i].LinkType = linkType; - g_connections[i].Encrypted = false; + g_connections[i].Encrypted = encrypted; + // Publish the slot last. Readers use Active as + // the validity gate and must never observe a + // newly-active entry with the prior handle or + // address still in it. + std::atomic_thread_fence(std::memory_order_release); + g_connections[i].Active = true; + stored = true; break; } } - // Initialize L2CAP for this connection - L2cap::Initialize(handle); + if (stored && linkType == 0x01) { + // Reset AVDTP before L2CAP can expose any inbound + // channels. StartSource runs later and must preserve + // everything the peer opens in the meantime. + A2dp::OnConnected(handle); + L2cap::Initialize(handle); + g_l2capAssemblyLen = 0; + g_l2capAssemblyExpected = 0; + g_l2capAssemblyHandle = handle; + + // Drive security for both locally-created and + // headset-originated ACL links. Previously only + // Bluetooth::Connect requested authentication, so + // an accepted inbound reconnect could remain + // unauthenticated and never reach A2DP. + if (!encrypted) { + uint8_t auth[2] = { + (uint8_t)handle, (uint8_t)(handle >> 8) + }; + EnqueueHciCmd(OP_AUTH_REQUESTED, auth, sizeof(auth)); + } + } else if (!stored) { + uint8_t drop[3] = { + (uint8_t)handle, (uint8_t)(handle >> 8), 0x0D + }; + EnqueueHciCmd(OP_DISCONNECT, drop, sizeof(drop)); + KernelLogStream(WARNING, "BT-HCI") + << "Connection table full; disconnecting untracked link"; + } } } break; @@ -797,6 +1099,7 @@ namespace Drivers::USB::Bluetooth::Hci { KernelLogStream(INFO, "BT-HCI") << "Disconnection: handle=" << (uint64_t)handle << " reason=" << (uint64_t)reason; A2dp::OnDisconnected(handle); + L2cap::OnDisconnected(handle); for (int i = 0; i < MAX_CONNECTIONS; i++) { if (g_connections[i].Active && g_connections[i].Handle == handle) { @@ -813,11 +1116,17 @@ namespace Drivers::USB::Bluetooth::Hci { // context); the dirty flag gets flushed from // process context. if (reason == 0x05) { - int idx = FindBondIndex(g_connections[i].BdAddr); + bool droppedBond = false; + g_bondsLock.Acquire(); + int idx = FindBondIndexLocked(g_connections[i].BdAddr); if (idx >= 0) { g_bonds[idx].valid = false; memset(g_bonds[idx].key, 0, 16); g_bondsDirty = true; + droppedBond = true; + } + g_bondsLock.Release(); + if (droppedBond) { KernelLogStream(WARNING, "BT-HCI") << "Auth failure with stored key: bond dropped;" << " next connect will re-pair"; @@ -839,9 +1148,19 @@ namespace Drivers::USB::Bluetooth::Hci { KernelLogStream(INFO, "BT-HCI") << "Connection Request: link=" << (uint64_t)linkType; - // Auto-accept ACL connections + // Auto-accept ACL connections. ProcessEvent runs from the + // top-level event drainer, but enqueue this like every other + // security-path reply so command transactions stay serialized. if (linkType == 0x01) { - AcceptConnection(bdAddr, 0x01); // Role = slave + uint8_t reply[7]; + memcpy(reply, bdAddr, 6); + if (GetConnection(L2cap::GetAclHandle())) { + reply[6] = 0x0D; // limited resources + EnqueueHciCmd(OP_REJECT_CONN_REQ, reply, sizeof(reply)); + } else { + reply[6] = 0x01; // become peripheral; role switch allowed later + EnqueueHciCmd(OP_ACCEPT_CONN_REQ, reply, sizeof(reply)); + } } } break; @@ -850,7 +1169,8 @@ namespace Drivers::USB::Bluetooth::Hci { case EVT_NUM_COMPLETED_PACKETS: { if (evtParamLen >= 1) { uint8_t numHandles = params[0]; - for (int i = 0; i < numHandles && (3 + i * 4) < evtParamLen; i++) { + for (int i = 0; i < numHandles + && 5 + i * 4 <= evtParamLen; i++) { uint16_t completed = (uint16_t)params[3 + i * 4] | ((uint16_t)params[4 + i * 4] << 8); int32_t v = g_aclPendingCount.fetch_sub(completed, @@ -891,13 +1211,21 @@ namespace Drivers::USB::Bluetooth::Hci { case EVT_LINK_KEY_REQUEST: { if (evtParamLen >= 6) { - int idx = FindBondIndex(¶ms[0]); + uint8_t key[16]; + bool foundKey = false; + g_bondsLock.Acquire(); + int idx = FindBondIndexLocked(¶ms[0]); if (idx >= 0) { + memcpy(key, g_bonds[idx].key, sizeof(key)); + foundKey = true; + } + g_bondsLock.Release(); + if (foundKey) { // We remember this device: hand back the stored key so // authentication succeeds without re-pairing. uint8_t reply[22]; memcpy(reply, ¶ms[0], 6); - memcpy(&reply[6], g_bonds[idx].key, 16); + memcpy(&reply[6], key, 16); EnqueueHciCmd(OP_LINK_KEY_REQ_REPLY, reply, 22); KernelLogStream(INFO, "BT-HCI") << "Link key request: stored key sent"; } else { @@ -934,18 +1262,27 @@ namespace Drivers::USB::Bluetooth::Hci { EnqueueHciCmd(OP_SET_CONN_ENCRYPT, enc, 3); } else if (evtParamLen >= 3) { // Failed authentication was previously swallowed silently. - // Drop the stale bond here too (the remote may not follow - // up with a reason-5 disconnect on every firmware). + // Drop a stored bond only for Authentication Failure / Key + // Missing; timeouts and transient controller errors do not + // prove the key is stale. KernelLogStream(WARNING, "BT-HCI") << "Authentication failed, status=" << (uint64_t)params[0]; uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8); for (int i = 0; i < MAX_CONNECTIONS; i++) { if (g_connections[i].Active && g_connections[i].Handle == handle) { - int idx = FindBondIndex(g_connections[i].BdAddr); - if (idx >= 0) { - g_bonds[idx].valid = false; - memset(g_bonds[idx].key, 0, 16); - g_bondsDirty = true; + bool droppedBond = false; + if (params[0] == 0x05 || params[0] == 0x06) { + g_bondsLock.Acquire(); + int idx = FindBondIndexLocked(g_connections[i].BdAddr); + if (idx >= 0) { + g_bonds[idx].valid = false; + memset(g_bonds[idx].key, 0, 16); + g_bondsDirty = true; + droppedBond = true; + } + g_bondsLock.Release(); + } + if (droppedBond) { KernelLogStream(WARNING, "BT-HCI") << "Stale bond dropped; next connect will re-pair"; } @@ -956,10 +1293,25 @@ namespace Drivers::USB::Bluetooth::Hci { break; } + case EVT_SIMPLE_PAIRING_COMPLETE: { + if (evtParamLen >= 7) { + KernelLogStream(params[0] == 0 ? OK : WARNING, "BT-HCI") + << "Simple Pairing Complete: status=" + << base::hex << (uint64_t)params[0] << base::dec; + } + break; + } + case EVT_INQUIRY_COMPLETE: { - g_inquiryActive = false; - KernelLogStream(INFO, "BT-HCI") << "Inquiry complete, " - << (uint64_t)g_inquiryResultCount << " device(s) found"; + if (evtParamLen < 1) break; + // Publish all preceding result-table writes before waking the + // blocking Scan reader on another core. + g_inquiryActive.store(false, std::memory_order_release); + KernelLogStream(params[0] == 0 ? INFO : WARNING, "BT-HCI") + << "Inquiry complete, status=" << base::hex + << (uint64_t)params[0] << base::dec << ", " + << (uint64_t)g_inquiryResultCount.load(std::memory_order_acquire) + << " device(s) found"; break; } @@ -967,35 +1319,45 @@ namespace Drivers::USB::Bluetooth::Hci { // Standard inquiry result: NumResp(1) + per-device(14 bytes each) if (evtParamLen >= 1) { uint8_t numResp = params[0]; - for (int i = 0; i < numResp && g_inquiryResultCount < MAX_INQUIRY_RESULTS; i++) { + for (int i = 0; i < numResp + && 1 + (i + 1) * 14 <= evtParamLen; i++) { const uint8_t* entry = ¶ms[1 + i * 14]; - auto& dev = g_inquiryResults[g_inquiryResultCount]; - memset(&dev, 0, sizeof(dev)); - memcpy(dev.BdAddr, entry, 6); - dev.ClassOfDevice = (uint32_t)entry[9] - | ((uint32_t)entry[10] << 8) - | ((uint32_t)entry[11] << 16); - dev.Rssi = -128; // Unknown for standard inquiry - g_inquiryResultCount++; + auto* dev = UpsertInquiryResult(entry); + if (!dev) continue; + dev->ClassOfDevice = (uint32_t)entry[9] + | ((uint32_t)entry[10] << 8) + | ((uint32_t)entry[11] << 16); } } break; } case EVT_INQUIRY_RESULT_RSSI: { - // Inquiry Result with RSSI: NumResp(1) + per-device(15 bytes each) + // Controllers use either inquiry_info_rssi (14 bytes) or the + // legacy inquiry_info_rssi_pscan form (15 bytes). Their class + // and RSSI offsets differ by one; hard-coding 15 silently lost + // results from controllers emitting the common 14-byte form. if (evtParamLen >= 1) { uint8_t numResp = params[0]; - for (int i = 0; i < numResp && g_inquiryResultCount < MAX_INQUIRY_RESULTS; i++) { - const uint8_t* entry = ¶ms[1 + i * 15]; - auto& dev = g_inquiryResults[g_inquiryResultCount]; - memset(&dev, 0, sizeof(dev)); - memcpy(dev.BdAddr, entry, 6); - dev.ClassOfDevice = (uint32_t)entry[9] - | ((uint32_t)entry[10] << 8) - | ((uint32_t)entry[11] << 16); - dev.Rssi = (int8_t)entry[14]; - g_inquiryResultCount++; + uint16_t resultBytes = (uint16_t)(evtParamLen - 1); + uint8_t stride = 0; + if (numResp != 0 && resultBytes == (uint16_t)(numResp * 14u)) + stride = 14; + else if (numResp != 0 + && resultBytes == (uint16_t)(numResp * 15u)) + stride = 15; + if (stride == 0) break; + for (int i = 0; i < numResp + && 1 + (i + 1) * stride <= evtParamLen; i++) { + const uint8_t* entry = ¶ms[1 + i * stride]; + uint8_t classOff = stride == 14 ? 8 : 9; + uint8_t rssiOff = stride == 14 ? 13 : 14; + auto* dev = UpsertInquiryResult(entry); + if (!dev) continue; + dev->ClassOfDevice = (uint32_t)entry[classOff] + | ((uint32_t)entry[classOff + 1] << 8) + | ((uint32_t)entry[classOff + 2] << 16); + dev->Rssi = (int8_t)entry[rssiOff]; } } break; @@ -1004,14 +1366,13 @@ namespace Drivers::USB::Bluetooth::Hci { case EVT_EXTENDED_INQUIRY_RESULT: { // Extended Inquiry Result: NumResp(1) + BD_ADDR(6) + PSRM(1) + reserved(1) // + CoD(3) + ClockOff(2) + RSSI(1) + EIR(240) - if (evtParamLen >= 15 && g_inquiryResultCount < MAX_INQUIRY_RESULTS) { - auto& dev = g_inquiryResults[g_inquiryResultCount]; - memset(&dev, 0, sizeof(dev)); - memcpy(dev.BdAddr, ¶ms[1], 6); - dev.ClassOfDevice = (uint32_t)params[9] - | ((uint32_t)params[10] << 8) - | ((uint32_t)params[11] << 16); - dev.Rssi = (int8_t)params[14]; + if (evtParamLen >= 15 && params[0] != 0) { + auto* dev = UpsertInquiryResult(¶ms[1]); + if (!dev) break; + dev->ClassOfDevice = (uint32_t)params[9] + | ((uint32_t)params[10] << 8) + | ((uint32_t)params[11] << 16); + dev->Rssi = (int8_t)params[14]; // Parse EIR data for device name const uint8_t* eir = ¶ms[15]; @@ -1026,28 +1387,32 @@ namespace Drivers::USB::Bluetooth::Hci { if (type == 0x08 || type == 0x09) { int nameLen = len - 1; if (nameLen > 63) nameLen = 63; - memcpy(dev.Name, &eir[pos + 2], nameLen); - dev.Name[nameLen] = '\0'; + memcpy(dev->Name, &eir[pos + 2], nameLen); + dev->Name[nameLen] = '\0'; } pos += 1 + len; } - - g_inquiryResultCount++; } break; } case EVT_ENCRYPT_CHANGE: { if (evtParamLen >= 4) { + uint8_t status = params[0]; uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8); uint8_t encryption = params[3]; for (int i = 0; i < MAX_CONNECTIONS; i++) { if (g_connections[i].Active && g_connections[i].Handle == handle) { - g_connections[i].Encrypted = (encryption != 0); + g_connections[i].Encrypted = status == 0 && encryption != 0; break; } } + if (status != 0) { + KernelLogStream(WARNING, "BT-HCI") + << "Encryption Change failed: status=" + << base::hex << (uint64_t)status << base::dec; + } } break; } @@ -1095,9 +1460,46 @@ namespace Drivers::USB::Bluetooth::Hci { uint16_t dataLen = hdr->DataLength; if (dataLen + sizeof(AclHeader) > len) return; + if (!GetConnection(handle)) return; // stale data queued before disconnect - // Dispatch to L2CAP - L2cap::ProcessPacket(handle, data + sizeof(AclHeader), dataLen); + const uint8_t* payload = data + sizeof(AclHeader); + + if (pbFlag == ACL_PB_CONTINUING) { + if (g_l2capAssemblyExpected == 0 || handle != g_l2capAssemblyHandle) + return; + uint32_t room = g_l2capAssemblyExpected - g_l2capAssemblyLen; + if (dataLen > room) { + g_l2capAssemblyLen = 0; + g_l2capAssemblyExpected = 0; + return; + } + memcpy(&g_l2capAssembly[g_l2capAssemblyLen], payload, dataLen); + g_l2capAssemblyLen = (uint16_t)(g_l2capAssemblyLen + dataLen); + if (g_l2capAssemblyLen == g_l2capAssemblyExpected) { + L2cap::ProcessPacket(handle, g_l2capAssembly, + g_l2capAssemblyExpected); + g_l2capAssemblyLen = 0; + g_l2capAssemblyExpected = 0; + } + return; + } + + // A new first fragment supersedes an incomplete old PDU. + g_l2capAssemblyLen = 0; + g_l2capAssemblyExpected = 0; + g_l2capAssemblyHandle = handle; + if (dataLen < sizeof(L2cap::L2capHeader)) return; + auto* l2 = (const L2cap::L2capHeader*)payload; + uint32_t expected = sizeof(L2cap::L2capHeader) + l2->Length; + if (expected > sizeof(g_l2capAssembly) || dataLen > expected) return; + if (dataLen == expected) { + L2cap::ProcessPacket(handle, payload, dataLen); + return; + } + + memcpy(g_l2capAssembly, payload, dataLen); + g_l2capAssemblyLen = dataLen; + g_l2capAssemblyExpected = (uint16_t)expected; } // ========================================================================= @@ -1222,17 +1624,20 @@ namespace Drivers::USB::Bluetooth::Hci { uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < 2000) { Xhci::PollEvents(); + DrainEvents(); - if (g_eventReady) { - g_eventReady = false; - if (g_eventLen >= 5 && g_eventBuf[0] == EVT_COMMAND_COMPLETE) { - uint16_t op = (uint16_t)g_eventBuf[3] | ((uint16_t)g_eventBuf[4] << 8); + uint8_t event[sizeof(g_eventBuf)]; + uint16_t eventLen = 0; + if (PopCommandEvent(event, &eventLen)) { + if (eventLen >= 5 && event[0] == EVT_COMMAND_COMPLETE) { + uint16_t op = (uint16_t)event[3] | ((uint16_t)event[4] << 8); if (op == OP_INTEL_READ_VERSION) { // Return params begin at byte 5 (status, then TLVs). - int avail = (int)g_eventLen - 5; + int avail = (int)eventLen - 5; if (avail < 0) avail = 0; int n = (avail < maxLen) ? avail : maxLen; - memcpy(outBuf, &g_eventBuf[5], n); + memcpy(outBuf, &event[5], n); + FinishCommand(OP_INTEL_READ_VERSION); return n; } } @@ -1247,12 +1652,14 @@ namespace Drivers::USB::Bluetooth::Hci { << (uint64_t)(g_intInCompletions.load(std::memory_order_relaxed) - completionsBefore) << " int-in completions during wait)"; DumpFwTrace(); + FinishCommand(OP_INTEL_READ_VERSION); } return -1; } void ClearSecureSendResult() { g_secureResultValid = false; + g_aclTxTransportError.store(false, std::memory_order_release); g_ssBytesSent = 0; g_ssFragsSent = 0; } @@ -1275,13 +1682,22 @@ namespace Drivers::USB::Bluetooth::Hci { bool expected = false; if (!s_active.compare_exchange_strong(expected, true, std::memory_order_acquire)) return; - while (g_pendingTail != g_pendingHead) { - PendingHciCmd c = g_pending[g_pendingTail]; - g_pendingTail = (uint8_t)((g_pendingTail + 1) & 15); + for (;;) { + uint8_t tail = g_pendingTail.load(std::memory_order_relaxed); + if (tail == g_pendingHead.load(std::memory_order_acquire)) break; + PendingHciCmd c = g_pending[tail]; bool sent = SendCommand(c.opcode, c.params, c.len); - // Set Connection Encryption returns Command Status (not Complete), - // so wait on that instead of burning a 1s timeout that delays A2DP. - if (c.opcode == OP_SET_CONN_ENCRYPT) { + if (!sent) { + KernelLogStream(WARNING, "BT-HCI") << "queued cmd send failed: " + << base::hex << (uint64_t)c.opcode << base::dec; + break; + } + // These link-control commands return Command Status, not Complete. + if (c.opcode == OP_SET_CONN_ENCRYPT + || c.opcode == OP_ACCEPT_CONN_REQ + || c.opcode == OP_REJECT_CONN_REQ + || c.opcode == OP_AUTH_REQUESTED + || c.opcode == OP_DISCONNECT) { WaitCommandStatus(c.opcode, 1000); } else { // Log delivery + the controller's verdict: security replies @@ -1296,6 +1712,8 @@ namespace Drivers::USB::Bluetooth::Hci { << " cc=" << (uint64_t)(ok ? 1 : 0) << " status=" << base::hex << (uint64_t)st[0] << base::dec; } + g_pendingTail.store((uint8_t)((tail + 1) & 15), + std::memory_order_release); } s_active.store(false, std::memory_order_release); } @@ -1304,6 +1722,7 @@ namespace Drivers::USB::Bluetooth::Hci { uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); + DrainEvents(); if (g_secureResultValid) { if (outResult) *outResult = g_secureResult; if (outStatus) *outStatus = g_secureStatus; @@ -1338,6 +1757,7 @@ namespace Drivers::USB::Bluetooth::Hci { return false; } Xhci::PollEvents(); + DrainEvents(); g_ssBytesSent += frag; g_ssFragsSent++; @@ -1358,6 +1778,7 @@ namespace Drivers::USB::Bluetooth::Hci { // touched: these are commands, not ACL data). bool IntelSecureSendBulk(uint8_t fragmentType, const uint8_t* data, uint32_t len) { if (!g_initialized || !g_aclTxRing[0]) return false; + if (g_aclTxTransportError.load(std::memory_order_acquire)) return false; uint32_t off = 0; while (len > 0) { @@ -1369,6 +1790,13 @@ namespace Drivers::USB::Bluetooth::Hci { uint64_t start = Timekeeping::GetMilliseconds(); while (AclTxInFlight() >= (uint32_t)(ACL_TX_SLOTS - 1)) { Xhci::PollEvents(); + DrainEvents(); + if (g_aclTxTransportError.load(std::memory_order_acquire)) { + KernelLogStream(ERROR, "BT-HCI") + << "Bulk secure-send transport error at frag #" + << (uint64_t)g_ssFragsSent << " byte " << g_ssBytesSent; + return false; + } if (Timekeeping::GetMilliseconds() - start > 2000) { KernelLogStream(ERROR, "BT-HCI") << "Bulk secure-send stalled at frag #" << (uint64_t)g_ssFragsSent @@ -1409,10 +1837,12 @@ namespace Drivers::USB::Bluetooth::Hci { uint64_t start = Timekeeping::GetMilliseconds(); while (AclTxInFlight() > 0) { Xhci::PollEvents(); + DrainEvents(); + if (g_aclTxTransportError.load(std::memory_order_acquire)) return false; if (Timekeeping::GetMilliseconds() - start > timeoutMs) return false; asm volatile("pause" ::: "memory"); } - return true; + return !g_aclTxTransportError.load(std::memory_order_acquire); } bool IntelBootFirmware(uint32_t bootAddr, uint32_t timeoutMs) { @@ -1436,6 +1866,7 @@ namespace Drivers::USB::Bluetooth::Hci { uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); + DrainEvents(); if (g_intelBootup) return true; for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } @@ -1488,6 +1919,12 @@ namespace Drivers::USB::Bluetooth::Hci { return WaitCommandComplete(OP_WRITE_SSP_MODE); } + bool WriteInquiryMode(uint8_t mode) { + if (mode > 2) return false; + if (!SendCommand(OP_WRITE_INQUIRY_MODE, &mode, 1)) return false; + return WaitCommandComplete(OP_WRITE_INQUIRY_MODE); + } + bool AcceptConnection(const uint8_t* bdAddr, uint8_t role) { uint8_t params[7]; memcpy(params, bdAddr, 6); @@ -1544,8 +1981,8 @@ namespace Drivers::USB::Bluetooth::Hci { // ========================================================================= bool StartInquiry(uint8_t durationUnits) { - g_inquiryResultCount = 0; - g_inquiryActive = true; + g_inquiryResultCount.store(0, std::memory_order_release); + g_inquiryActive.store(true, std::memory_order_release); // HCI Inquiry: LAP(3) + InquiryLength(1) + NumResponses(1) // GIAC LAP = 0x9E8B33 @@ -1556,13 +1993,13 @@ namespace Drivers::USB::Bluetooth::Hci { }; if (!SendCommand(OP_INQUIRY, params, 5)) { - g_inquiryActive = false; + g_inquiryActive.store(false, std::memory_order_release); return false; } // Inquiry uses Command Status (not Command Complete) if (!WaitCommandStatus(OP_INQUIRY)) { - g_inquiryActive = false; + g_inquiryActive.store(false, std::memory_order_release); return false; } @@ -1570,15 +2007,16 @@ namespace Drivers::USB::Bluetooth::Hci { } bool CancelInquiry() { - if (!g_inquiryActive) return true; + if (!g_inquiryActive.load(std::memory_order_acquire)) return true; if (!SendCommand(OP_INQUIRY_CANCEL, nullptr, 0)) return false; - WaitCommandComplete(OP_INQUIRY_CANCEL, nullptr, 0, 2000); - g_inquiryActive = false; - return true; + bool cancelled = WaitCommandComplete(OP_INQUIRY_CANCEL, nullptr, 0, 2000); + if (cancelled) + g_inquiryActive.store(false, std::memory_order_release); + return cancelled; } int GetInquiryResults(InquiryDevice* buf, int maxCount) { - int count = g_inquiryResultCount; + int count = g_inquiryResultCount.load(std::memory_order_acquire); if (count > maxCount) count = maxCount; if (buf && count > 0) { memcpy(buf, g_inquiryResults, count * sizeof(InquiryDevice)); @@ -1587,11 +2025,11 @@ namespace Drivers::USB::Bluetooth::Hci { } void ClearInquiryResults() { - g_inquiryResultCount = 0; + g_inquiryResultCount.store(0, std::memory_order_release); } bool IsInquiryActive() { - return g_inquiryActive; + return g_inquiryActive.load(std::memory_order_acquire); } // ========================================================================= @@ -1611,16 +2049,61 @@ namespace Drivers::USB::Bluetooth::Hci { if (!s_draining.compare_exchange_strong(expected, true, std::memory_order_acquire)) return; - // Discard any unconsumed Command Complete/Status events that weren't - // picked up by WaitCommandComplete/WaitCommandStatus. - if (g_eventReady) { - g_eventReady = false; + // Endpoint errors stop their xHCI rings. The callback cannot issue the + // reset commands while nested under PollEvents, so recover here before + // consuming protocol traffic. The reset helpers restore the dequeue + // pointer; explicitly re-arm bulk IN (interrupt reset re-arms itself). + if (g_eventPipeNeedsRecovery.exchange(false, std::memory_order_acq_rel)) { + KernelLogStream(WARNING, "BT-HCI") << "Recovering HCI event pipe"; + Xhci::ResetInterruptEndpoint(g_slotId); + g_eventAssemblyLen = 0; + g_eventAssemblyExpected = 0; + } + if (g_aclDataEnabled.load(std::memory_order_acquire) + && g_aclPipeNeedsRecovery.exchange(false, std::memory_order_acq_rel)) { + KernelLogStream(WARNING, "BT-HCI") << "Recovering HCI ACL receive pipe"; + Xhci::ResetBulkInEndpoint(g_slotId); + auto* dev = Xhci::GetDevice(g_slotId); + if (dev && dev->BulkInEpNum) + Xhci::QueueBulkInTransfer(g_slotId, nullptr, 0, dev->BulkInMaxPacket); + g_aclUsbAssemblyLen = 0; + g_aclUsbAssemblyExpected = 0; + } + if (g_aclTxPipeNeedsRecovery.exchange(false, std::memory_order_acq_rel)) { + // Reset Endpoint + Set TR Dequeue discards the errored transfer and + // all TRBs queued behind it. Free those DMA slots and remove only + // those never-delivered packets from the controller-credit count; + // earlier successful OUT transfers still await their NOCP events. + uint32_t discarded = AclTxInFlight(); + KernelLogStream(WARNING, "BT-HCI") + << "Recovering HCI ACL transmit pipe; discarding " + << (uint64_t)discarded << " queued packet(s)"; + Xhci::ResetBulkOutEndpoint(g_slotId); + g_aclTxDoneCount.store(g_aclTxCount.load(std::memory_order_acquire), + std::memory_order_release); + int32_t remaining = g_aclPendingCount.fetch_sub((int32_t)discarded, + std::memory_order_acq_rel) + - (int32_t)discarded; + if (remaining < 0) + g_aclPendingCount.store(0, std::memory_order_release); + } + + // Asynchronous HCI events are processed at top level. Besides avoiding + // logging/command submission in the xHCI callback, this guarantees only + // complete reassembled events reach ProcessEvent. + for (;;) { + uint8_t tail = g_asyncEventTail.load(std::memory_order_relaxed); + if (tail == g_asyncEventHead.load(std::memory_order_acquire)) break; + ProcessEvent(g_asyncEventRing[tail], g_asyncEventLens[tail]); + g_asyncEventTail.store((uint8_t)((tail + 1) % ASYNC_EVENT_SLOTS), + std::memory_order_release); } // Drain all queued ACL packets (process the whole ring, not just one, // so a burst is never left unhandled). - while (g_aclRxTail != g_aclRxHead) { - uint8_t slot = g_aclRxTail; + for (;;) { + uint8_t slot = g_aclRxTail.load(std::memory_order_relaxed); + if (slot == g_aclRxHead.load(std::memory_order_acquire)) break; uint16_t pl = g_aclRxLens[slot]; // Diagnostic (top-level, safe to log -- not the IRQ path): trace the @@ -1642,7 +2125,8 @@ namespace Drivers::USB::Bluetooth::Hci { } ProcessAcl(g_aclRxRing[slot], pl); - g_aclRxTail = (uint8_t)((g_aclRxTail + 1) % ACL_RX_SLOTS); + g_aclRxTail.store((uint8_t)((slot + 1) % ACL_RX_SLOTS), + std::memory_order_release); } s_draining.store(false, std::memory_order_release); diff --git a/kernel/src/Drivers/USB/Bluetooth/Hci.hpp b/kernel/src/Drivers/USB/Bluetooth/Hci.hpp index 4803abd..b89f45d 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Hci.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/Hci.hpp @@ -181,6 +181,11 @@ namespace Drivers::USB::Bluetooth::Hci { // and keeps the event pipe alive -- see the definition). void StartEventPipe(); + // Enable operational ACL receive framing after firmware/HCI setup. Bulk IN + // is armed earlier to protect Intel firmware loading; pre-operational runts + // are intentionally ignored and any endpoint error is recovered here. + void EnableAclDataReception(); + // Send an HCI command via USB control transfer (EP0) bool SendCommand(uint16_t opcode, const uint8_t* params, uint8_t paramLen); @@ -308,6 +313,9 @@ namespace Drivers::USB::Bluetooth::Hci { // Write Simple Secure Pairing mode bool WriteSSPMode(uint8_t mode); + // Select inquiry result format: 0=standard, 1=RSSI, 2=extended/EIR. + bool WriteInquiryMode(uint8_t mode); + // Accept an incoming connection bool AcceptConnection(const uint8_t* bdAddr, uint8_t role); diff --git a/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp b/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp index b3e0f73..e3a5fb7 100644 --- a/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/L2cap.cpp @@ -26,11 +26,6 @@ namespace Drivers::USB::Bluetooth::L2cap { static bool g_initialized = false; static uint8_t g_sigIdentifier = 1; - // Diagnostic: result of the most recent L2CAP Connection Response we got for - // an outgoing connection (0=success, 1=pending, 2=PSM-unsupported, - // 3=security-block, 4=no-resources). 0xFFFF = none received yet. - static volatile uint16_t g_lastConnRspResult = 0xFFFF; - // Channel table static ChannelInfo g_channels[MAX_CHANNELS] = {}; static uint16_t g_nextCid = CID_DYNAMIC_START; @@ -39,8 +34,29 @@ namespace Drivers::USB::Bluetooth::L2cap { // Helpers // ========================================================================= + static uint8_t NextIdentifier() { + uint8_t id = g_sigIdentifier++; + if (id == 0) id = g_sigIdentifier++; + if (g_sigIdentifier == 0) g_sigIdentifier = 1; + return id; + } + static uint16_t AllocCid() { - return g_nextCid++; + // Dynamic CIDs are 0x0040..0xFFFF and must not collide with an active + // channel after wraparound. + for (uint32_t tries = 0; tries <= 0xFFC0; tries++) { + if (g_nextCid < CID_DYNAMIC_START) g_nextCid = CID_DYNAMIC_START; + uint16_t candidate = g_nextCid++; + bool used = false; + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].LocalCid == candidate) { + used = true; + break; + } + } + if (!used) return candidate; + } + return 0; } static ChannelInfo* AllocChannel(uint16_t psm) { @@ -48,12 +64,18 @@ namespace Drivers::USB::Bluetooth::L2cap { if (!g_channels[i].Active) { g_channels[i].Active = true; g_channels[i].LocalCid = AllocCid(); + if (g_channels[i].LocalCid == 0) { + g_channels[i].Active = false; + return nullptr; + } g_channels[i].RemoteCid = 0; g_channels[i].Psm = psm; g_channels[i].RemoteMtu = 672; // Default L2CAP MTU g_channels[i].Configured = false; g_channels[i].LocalConfigDone = false; g_channels[i].RemoteConfigDone = false; + g_channels[i].ConnRspResult = 0xFFFF; + g_channels[i].ConnRspStatus = 0; return &g_channels[i]; } } @@ -61,13 +83,14 @@ namespace Drivers::USB::Bluetooth::L2cap { } // Send L2CAP signaling command - static void SendSignal(uint8_t code, uint8_t identifier, + static bool SendSignal(uint8_t code, uint8_t identifier, const uint8_t* payload, uint16_t payloadLen) { // L2CAP header + Signal header + payload uint16_t sigLen = sizeof(SignalHeader) + payloadLen; uint16_t totalPayload = sizeof(L2capHeader) + sigLen; uint8_t buf[128] = {}; + if (totalPayload > sizeof(buf)) return false; auto* l2hdr = (L2capHeader*)buf; l2hdr->Length = sigLen; l2hdr->ChannelId = CID_SIGNALING; @@ -81,8 +104,8 @@ namespace Drivers::USB::Bluetooth::L2cap { memcpy(buf + sizeof(L2capHeader) + sizeof(SignalHeader), payload, payloadLen); } - Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_FLUSH, - buf, totalPayload); + return Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_NON_FLUSH, + buf, totalPayload); } // ========================================================================= @@ -96,17 +119,31 @@ namespace Drivers::USB::Bluetooth::L2cap { g_nextCid = CID_DYNAMIC_START; for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active) + Avrcp::OnChannelClosed(g_channels[i].LocalCid); g_channels[i].Active = false; } KernelLogStream(OK, "BT-L2CAP") << "Initialized for ACL handle " << (uint64_t)aclHandle; } + void OnDisconnected(uint16_t aclHandle) { + if (!g_initialized || aclHandle != g_aclHandle) return; + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active) + Avrcp::OnChannelClosed(g_channels[i].LocalCid); + g_channels[i].Active = false; + } + g_aclHandle = 0; + g_initialized = false; + } + // ========================================================================= // ProcessPacket // ========================================================================= void ProcessPacket(uint16_t aclHandle, const uint8_t* data, uint16_t len) { + if (!g_initialized || aclHandle != g_aclHandle) return; if (len < sizeof(L2capHeader)) return; auto* l2hdr = (const L2capHeader*)data; @@ -145,8 +182,41 @@ namespace Drivers::USB::Bluetooth::L2cap { // Accept connections for AVDTP, SDP, and AVRCP control if (psm == PSM_AVDTP || psm == PSM_SDP || psm == PSM_AVCTP) { - auto* ch = AllocChannel(psm); - if (ch) { + // Detect a duplicate source CID instead of creating + // two local channels that address the same endpoint. + // Same-PSM duplicates are normal retransmissions; + // a different PSM reusing the CID is invalid. + ChannelInfo* existing = nullptr; + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active + && g_channels[i].RemoteCid == srcCid) { + existing = &g_channels[i]; + break; + } + } + auto* ch = existing ? nullptr : AllocChannel(psm); + if (existing && existing->Psm == psm) { + // Retransmission of a request whose response was + // lost: repeat SUCCESS for the original channel, + // and repeat our config request as well. + uint8_t rsp[8] = { + (uint8_t)existing->LocalCid, + (uint8_t)(existing->LocalCid >> 8), + (uint8_t)srcCid, (uint8_t)(srcCid >> 8), + 0, 0, 0, 0 + }; + SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8); + uint8_t cfgReq[4] = { + (uint8_t)srcCid, (uint8_t)(srcCid >> 8), 0, 0 + }; + SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4); + } else if (existing) { + uint8_t rsp[8] = { + 0, 0, (uint8_t)srcCid, (uint8_t)(srcCid >> 8), + 0x07, 0x00, 0, 0 // source CID already allocated + }; + SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8); + } else if (ch) { ch->RemoteCid = srcCid; // Send Connection Response (success) @@ -168,7 +238,13 @@ namespace Drivers::USB::Bluetooth::L2cap { cfgReq[0] = (uint8_t)(srcCid & 0xFF); cfgReq[1] = (uint8_t)(srcCid >> 8); cfgReq[2] = 0; cfgReq[3] = 0; // Flags - SendSignal(SIG_CONFIG_REQ, g_sigIdentifier++, cfgReq, 4); + SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4); + } else { + uint8_t rsp[8] = { + 0, 0, (uint8_t)srcCid, (uint8_t)(srcCid >> 8), + 0x04, 0x00, 0, 0 // no resources + }; + SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8); } } else { // Reject: PSM not supported @@ -193,8 +269,6 @@ namespace Drivers::USB::Bluetooth::L2cap { // info, 1=authentication pending, 2=authorization pending. uint16_t status = (sigPayloadLen >= 8) ? ((uint16_t)sigPayload[6] | ((uint16_t)sigPayload[7] << 8)) : 0; - g_lastConnRspResult = result; - KernelLogStream(INFO, "BT-L2CAP") << "Connection Response: dstCID=" << base::hex << (uint64_t)dstCid << " result=" << (uint64_t)result << " status=" << (uint64_t)status << base::dec; @@ -203,6 +277,8 @@ namespace Drivers::USB::Bluetooth::L2cap { // Find our channel by srcCid (which is our local CID) for (int i = 0; i < MAX_CHANNELS; i++) { if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) { + g_channels[i].ConnRspResult = result; + g_channels[i].ConnRspStatus = status; g_channels[i].RemoteCid = dstCid; // Send Configuration Request @@ -210,7 +286,7 @@ namespace Drivers::USB::Bluetooth::L2cap { cfgReq[0] = (uint8_t)(dstCid & 0xFF); cfgReq[1] = (uint8_t)(dstCid >> 8); cfgReq[2] = 0; cfgReq[3] = 0; // Flags - SendSignal(SIG_CONFIG_REQ, g_sigIdentifier++, cfgReq, 4); + SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4); break; } } @@ -223,10 +299,21 @@ namespace Drivers::USB::Bluetooth::L2cap { // a Disconnect for the half-open remote endpoint. for (int i = 0; i < MAX_CHANNELS; i++) { if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) { + g_channels[i].ConnRspResult = result; + g_channels[i].ConnRspStatus = status; g_channels[i].RemoteCid = dstCid; break; } } + } else { + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active + && g_channels[i].LocalCid == srcCid) { + g_channels[i].ConnRspResult = result; + g_channels[i].ConnRspStatus = status; + break; + } + } } } break; @@ -333,7 +420,10 @@ namespace Drivers::USB::Bluetooth::L2cap { for (int i = 0; i < MAX_CHANNELS; i++) { if (g_channels[i].Active && g_channels[i].LocalCid == dstCid) { + uint16_t closedCid = g_channels[i].LocalCid; g_channels[i].Active = false; + A2dp::OnChannelClosed(closedCid); + Avrcp::OnChannelClosed(closedCid); break; } } @@ -378,6 +468,13 @@ namespace Drivers::USB::Bluetooth::L2cap { // SDP client channel after a query); nothing left to do. break; + case SIG_COMMAND_REJECT: + case SIG_ECHO_RSP: + case SIG_INFO_RSP: + // Terminal responses to requests sent by us. No response + // is required; in particular, never reject a response. + break; + case SIG_ECHO_REQ: { // Echo Request must be answered (echo the payload back) or // a peer probing the link times out on us. @@ -392,6 +489,10 @@ namespace Drivers::USB::Bluetooth::L2cap { KernelLogStream(INFO, "BT-L2CAP") << "Unhandled signaling code=" << base::hex << (uint64_t)sig->Code << base::dec << " len=" << (uint64_t)sigPayloadLen; + // Command Reject, reason 0x0000 = command not understood. + // Silence leaves the peer waiting for its signaling timer. + uint8_t reject[2] = {0x00, 0x00}; + SendSignal(SIG_COMMAND_REJECT, sig->Identifier, reject, 2); break; } } @@ -402,7 +503,7 @@ namespace Drivers::USB::Bluetooth::L2cap { for (int i = 0; i < MAX_CHANNELS; i++) { if (g_channels[i].Active && g_channels[i].LocalCid == cid) { if (g_channels[i].Psm == PSM_AVDTP) { - A2dp::ProcessAvdtp(payload, l2len); + A2dp::ProcessAvdtp(cid, payload, l2len); } else if (g_channels[i].Psm == PSM_SDP) { A2dp::ProcessSdp(cid, payload, l2len); } else if (g_channels[i].Psm == PSM_AVCTP) { @@ -421,8 +522,6 @@ namespace Drivers::USB::Bluetooth::L2cap { uint16_t Connect(uint16_t psm) { if (!g_initialized) return 0; - g_lastConnRspResult = 0xFFFF; // reset diagnostic for this attempt - auto* ch = AllocChannel(psm); if (!ch) return 0; @@ -432,7 +531,10 @@ namespace Drivers::USB::Bluetooth::L2cap { req[1] = (uint8_t)(psm >> 8); req[2] = (uint8_t)(ch->LocalCid & 0xFF); req[3] = (uint8_t)(ch->LocalCid >> 8); - SendSignal(SIG_CONN_REQ, g_sigIdentifier++, req, 4); + if (!SendSignal(SIG_CONN_REQ, NextIdentifier(), req, 4)) { + ch->Active = false; + return 0; + } return ch->LocalCid; } @@ -518,9 +620,11 @@ namespace Drivers::USB::Bluetooth::L2cap { req[1] = (uint8_t)(g_channels[i].RemoteCid >> 8); req[2] = (uint8_t)(g_channels[i].LocalCid & 0xFF); req[3] = (uint8_t)(g_channels[i].LocalCid >> 8); - SendSignal(SIG_DISCONN_REQ, g_sigIdentifier++, req, 4); + SendSignal(SIG_DISCONN_REQ, NextIdentifier(), req, 4); } g_channels[i].Active = false; + A2dp::OnChannelClosed(localCid); + Avrcp::OnChannelClosed(localCid); return true; } } @@ -547,12 +651,28 @@ namespace Drivers::USB::Bluetooth::L2cap { return 0; } + uint16_t FindAvdtpChannelExcept(uint16_t exceptCid) { + for (int i = 0; i < MAX_CHANNELS; i++) { + if (g_channels[i].Active && g_channels[i].Psm == PSM_AVDTP + && g_channels[i].LocalCid != exceptCid) { + return g_channels[i].LocalCid; + } + } + return 0; + } + uint16_t GetAclHandle() { return g_aclHandle; } - uint16_t LastConnRspResult() { - return g_lastConnRspResult; + uint16_t ConnRspResult(uint16_t localCid) { + auto* ch = GetChannel(localCid); + return ch ? ch->ConnRspResult : 0xFFFF; + } + + uint16_t ConnRspStatus(uint16_t localCid) { + auto* ch = GetChannel(localCid); + return ch ? ch->ConnRspStatus : 0; } } diff --git a/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp b/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp index 09c7494..4448dfe 100644 --- a/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp +++ b/kernel/src/Drivers/USB/Bluetooth/L2cap.hpp @@ -78,6 +78,8 @@ namespace Drivers::USB::Bluetooth::L2cap { bool Configured; // Both sides configured bool LocalConfigDone; bool RemoteConfigDone; + uint16_t ConnRspResult; // 0xFFFF until this outgoing dial is answered + uint16_t ConnRspStatus; // meaningful while result == CONN_PENDING }; constexpr int MAX_CHANNELS = 8; @@ -89,6 +91,11 @@ namespace Drivers::USB::Bluetooth::L2cap { // Initialize L2CAP for a new HCI connection void Initialize(uint16_t aclHandle); + // Invalidate every dynamic channel when its ACL link disappears. Without + // this, AVRCP or media code can send to stale CIDs until the next connection + // happens to reinitialize the table. + void OnDisconnected(uint16_t aclHandle); + // Process an L2CAP packet (called from HCI ACL processing) void ProcessPacket(uint16_t aclHandle, const uint8_t* data, uint16_t len); @@ -114,6 +121,11 @@ namespace Drivers::USB::Bluetooth::L2cap { // it (we dial it after AVDTP_OPEN, but some sinks open it inbound instead). uint16_t FindConfiguredAvdtpChannelExcept(uint16_t exceptCid); + // Find any active AVDTP channel except the signaling channel, including a + // still-pending outgoing media dial. Manual retries reuse that channel + // instead of leaking another slot/CID while authorization is in progress. + uint16_t FindAvdtpChannelExcept(uint16_t exceptCid); + // Reclaim a dialed channel during A2DP bring-up retries so a failed connect // doesn't leak the fixed-size channel table. If the peer had acknowledged // the channel (RemoteCid != 0) a Disconnect Request is sent first; an @@ -125,10 +137,9 @@ namespace Drivers::USB::Bluetooth::L2cap { // Get the ACL handle uint16_t GetAclHandle(); - // Result code of the most recent outgoing L2CAP Connection Response - // (0xFFFF if none yet). 0=success, 1=pending, 2=PSM-unsupported, - // 3=security-block, 4=no-resources. StartSource() retries only when the - // remote ignored the dial entirely (0xFFFF). - uint16_t LastConnRspResult(); + // Per-channel connection response state. A global "last result" is unsafe: + // an unrelated SDP, AVDTP, or AVRCP response can arrive concurrently. + uint16_t ConnRspResult(uint16_t localCid); + uint16_t ConnRspStatus(uint16_t localCid); } diff --git a/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp b/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp index a4ae238..da12b5e 100644 --- a/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp +++ b/kernel/src/Drivers/USB/Bluetooth/Sbc.cpp @@ -115,6 +115,7 @@ namespace Drivers::USB::Bluetooth::Sbc { headerBits += enc->Subbands; // join bits } uint32_t dataBits = enc->Blocks * enc->Bitpool; + if (enc->ChannelMode == MODE_DUAL_CHANNEL) dataBits *= enc->Channels; enc->FrameSize = (headerBits + dataBits + 7) / 8; } @@ -143,6 +144,7 @@ namespace Drivers::USB::Bluetooth::Sbc { uint32_t headerBits = 32 + (4 * enc->Subbands * enc->Channels); if (enc->ChannelMode == MODE_JOINT_STEREO) headerBits += enc->Subbands; uint32_t dataBits = enc->Blocks * enc->Bitpool; + if (enc->ChannelMode == MODE_DUAL_CHANNEL) dataBits *= enc->Channels; enc->FrameSize = (headerBits + dataBits + 7) / 8; } diff --git a/kernel/src/Drivers/USB/Xhci.cpp b/kernel/src/Drivers/USB/Xhci.cpp index 892baa9..647bdc6 100644 --- a/kernel/src/Drivers/USB/Xhci.cpp +++ b/kernel/src/Drivers/USB/Xhci.cpp @@ -988,6 +988,38 @@ namespace Drivers::USB::Xhci { SendCommand(deqTrb); } + // ------------------------------------------------------------------------- + // ResetBulkOutEndpoint - clear a halted bulk OUT endpoint + // ------------------------------------------------------------------------- + + void ResetBulkOutEndpoint(uint8_t slotId) { + if (slotId == 0 || slotId > MAX_SLOTS || !g_devices[slotId].Active) return; + UsbDeviceInfo& dev = g_devices[slotId]; + if (dev.BulkOutEpNum == 0 || !dev.BulkOutRing) return; + + uint8_t dci = dev.BulkOutEpNum * 2; + + TRB resetTrb = {}; + resetTrb.Control = (TRB_RESET_ENDPOINT << TRB_TYPE_SHIFT) + | ((uint32_t)slotId << 24) + | ((uint32_t)dci << 16); + SendCommand(resetTrb); + + // Skip the errored TRB and everything queued behind it. The class + // driver releases the corresponding DMA slots before it sends again. + uint64_t newDeq = dev.BulkOutRingPhys + + (uint64_t)dev.BulkOutRingEnqueue * sizeof(TRB); + if (dev.BulkOutRingCCS) newDeq |= 1; // DCS bit + + TRB deqTrb = {}; + deqTrb.Parameter0 = (uint32_t)(newDeq & 0xFFFFFFFF); + deqTrb.Parameter1 = (uint32_t)(newDeq >> 32); + deqTrb.Control = (TRB_SET_TR_DEQUEUE << TRB_TYPE_SHIFT) + | ((uint32_t)slotId << 24) + | ((uint32_t)dci << 16); + SendCommand(deqTrb); + } + // ------------------------------------------------------------------------- // QueueBulkInTransfer // ------------------------------------------------------------------------- diff --git a/kernel/src/Drivers/USB/Xhci.hpp b/kernel/src/Drivers/USB/Xhci.hpp index dbf41de..6ab2bba 100644 --- a/kernel/src/Drivers/USB/Xhci.hpp +++ b/kernel/src/Drivers/USB/Xhci.hpp @@ -326,6 +326,11 @@ namespace Drivers::USB::Xhci { // Used for SDR stream stall recovery (RTL2832 bulk IN can STALL on start). void ResetBulkInEndpoint(uint8_t slotId); + // Clear a halted bulk OUT endpoint and discard the errored/queued TRBs. + // Must be called from process context; the class driver reconciles its + // outstanding-transfer accounting before submitting fresh work. + void ResetBulkOutEndpoint(uint8_t slotId); + // True while PollEvents() is draining the event ring. A command submitter // reached from inside an event callback must fire-and-forget (not wait), // since a nested PollEvents() is a no-op. diff --git a/programs/include/Api/Syscall.hpp b/programs/include/Api/Syscall.hpp index 20b5aa0..8382aa4 100644 --- a/programs/include/Api/Syscall.hpp +++ b/programs/include/Api/Syscall.hpp @@ -258,7 +258,7 @@ namespace montauk::abi { static constexpr int AUDIO_CTL_PAUSE = 3; static constexpr int AUDIO_CTL_GET_OUTPUT = 4; // 0=HDA, 1=Bluetooth static constexpr int AUDIO_CTL_SET_OUTPUT = 5; // Switch audio output - static constexpr int AUDIO_CTL_BT_STATUS = 6; // Get Bluetooth status + static constexpr int AUDIO_CTL_BT_STATUS = 6; // 0=unavailable, 1=setup, 2=ready static constexpr int AUDIO_CTL_SET_MASTER_VOLUME = 7; // 0-100 static constexpr int AUDIO_CTL_GET_MASTER_VOLUME = 8; static constexpr int AUDIO_CTL_SET_MUTE = 9; // 0/1, per-stream diff --git a/programs/man/syscalls.2 b/programs/man/syscalls.2 index b30a06b..3ed3316 100644 --- a/programs/man/syscalls.2 +++ b/programs/man/syscalls.2 @@ -550,7 +550,7 @@ audio_get_output AUDIO_CTL_GET_OUTPUT (4): 0=HDA, 1=Bluetooth audio_set_output AUDIO_CTL_SET_OUTPUT (5): switch all streams (SET_OUTPUT, 5) switch a stream's output route - audio_bt_status AUDIO_CTL_BT_STATUS (6) + audio_bt_status AUDIO_CTL_BT_STATUS (6): 0=unavailable, 1=setup, 2=ready audio_set_master_volume, _get_ AUDIO_CTL_{SET,GET}_MASTER_VOLUME (7/8), 0-100 audio_set_mute, audio_get_mute AUDIO_CTL_{SET,GET}_MUTE (9/10), per-stream audio_set_master_mute, _get_ AUDIO_CTL_{SET,GET}_MASTER_MUTE (11/12) diff --git a/programs/src/bluetooth/main.cpp b/programs/src/bluetooth/main.cpp index 2f78401..8ee7b5e 100644 --- a/programs/src/bluetooth/main.cpp +++ b/programs/src/bluetooth/main.cpp @@ -325,6 +325,7 @@ static void finish_scan() { static void do_connect(const uint8_t* addr) { int r = montauk::bt_connect(addr); if (r >= 0) set_status("Connected"); + else if (r == -2) set_status("Connected; audio setup failed - use Retry"); else set_status("Connection failed"); refresh_devices(); clamp_scroll(); @@ -354,6 +355,10 @@ static bool is_connected(const uint8_t* addr) { return false; } +static bool is_audio_ready() { + return montauk::audio_bt_status(-1) >= 2; +} + // ============================================================================ // Format helpers // ============================================================================ @@ -565,8 +570,14 @@ static bool handle_click(int mx, int my) { if (!row.contains(mx, my)) continue; Rect action = row_action_button_rect(row); + Rect retry = row_forget_button_rect(row); + bool connected = is_connected(g_scan[i].bdAddr); + if (connected && !is_audio_ready() && retry.contains(mx, my)) { + do_connect(g_scan[i].bdAddr); + return true; + } if (action.contains(mx, my)) { - if (is_connected(g_scan[i].bdAddr)) + if (connected) do_disconnect(g_scan[i].bdAddr); else do_connect(g_scan[i].bdAddr); @@ -580,6 +591,11 @@ static bool handle_click(int mx, int my) { Rect action = row_action_button_rect(row); if (g_devices[i].connected) { + Rect retry = row_forget_button_rect(row); + if (!is_audio_ready() && retry.contains(mx, my)) { + do_connect(g_devices[i].bdAddr); + return true; + } if (action.contains(mx, my)) { do_disconnect(g_devices[i].bdAddr); return true; @@ -672,6 +688,7 @@ static bool handle_adapter_key(const montauk::abi::KeyEvent& key) { static void render_devices_tab(Canvas& canvas, const mtk::Theme& theme) { Rect list = {0, devices_list_top(), g_win_w, gui_max(devices_list_bottom() - devices_list_top(), 0)}; int fh = system_font_height(); + bool bt_audio_ready = is_audio_ready(); if (g_device_count == 0) { draw_centered_text(canvas, list.y, list.h, "No paired devices", theme.text_subtle); @@ -688,15 +705,17 @@ static void render_devices_tab(Canvas& canvas, const mtk::Theme& theme) { mtk::draw_list_row(canvas, row, false, (i & 1) != 0, theme); bool connected = g_devices[i].connected; + bool audio_ready = !connected || bt_audio_ready; Color dot_color = connected ? GREEN : theme.text_subtle; fill_circle(canvas, PAD + 6, row.y + ROW_H / 2, 5, dot_color); Rect action = row_action_button_rect(row); Rect forget = row_forget_button_rect(row); int text_x = PAD + 20; - // Connected rows have one button (Disconnect); paired rows have two - // (Connect + Forget), so the text must stop before the leftmost button. - int text_w_max = (connected ? action.x : forget.x) - text_x - 10; + // A connected row whose media path failed exposes Retry + Disconnect; + // otherwise connected rows need only Disconnect. + int text_w_max = (connected && audio_ready ? action.x : forget.x) + - text_x - 10; draw_text_fit(canvas, text_x, row.y + 8, g_device_names[i], text_w_max, theme.text); char addr_str[24]; @@ -704,6 +723,10 @@ static void render_devices_tab(Canvas& canvas, const mtk::Theme& theme) { draw_text_fit(canvas, text_x, row.y + 8 + fh + 2, addr_str, text_w_max, theme.text_subtle); if (connected) { + if (!audio_ready) { + mtk::draw_button(canvas, forget, "Retry", mtk::BUTTON_PRIMARY, + button_state(forget, true), theme); + } mtk::draw_button(canvas, action, "Disconnect", mtk::BUTTON_DANGER, button_state(action, true), theme); } else { @@ -721,6 +744,7 @@ static void render_scan_tab(Canvas& canvas, const mtk::Theme& theme) { Rect scan_btn = scan_button_rect(); Rect list = {0, scan_list_top(), g_win_w, gui_max(scan_list_bottom() - scan_list_top(), 0)}; int fh = system_font_height(); + bool bt_audio_ready = is_audio_ready(); if (g_scanning) { mtk::draw_button(canvas, scan_btn, "Scanning...", mtk::BUTTON_SECONDARY, @@ -747,8 +771,12 @@ static void render_scan_tab(Canvas& canvas, const mtk::Theme& theme) { fill_circle(canvas, PAD + 6, row.y + ROW_H / 2, 5, theme.accent); Rect action = row_action_button_rect(row); + Rect retry = row_forget_button_rect(row); int text_x = PAD + 20; - int text_w_max = action.x - text_x - 10; + bool conn = is_connected(g_scan[i].bdAddr); + bool audio_ready = !conn || bt_audio_ready; + int text_w_max = (conn && !audio_ready ? retry.x : action.x) + - text_x - 10; const char* display_name = g_scan[i].name[0] ? g_scan[i].name : "Unknown Device"; draw_text_fit(canvas, text_x, row.y + 4, display_name, text_w_max, theme.text); @@ -760,7 +788,10 @@ static void render_scan_tab(Canvas& canvas, const mtk::Theme& theme) { device_class_str(g_scan[i].classOfDevice), addr_str, rssi_bar(g_scan[i].rssi)); draw_text_fit(canvas, text_x, row.y + 4 + fh + 2, detail, text_w_max, theme.text_subtle); - bool conn = is_connected(g_scan[i].bdAddr); + if (conn && !audio_ready) { + mtk::draw_button(canvas, retry, "Retry", mtk::BUTTON_PRIMARY, + button_state(retry, true), theme); + } mtk::draw_button(canvas, action, conn ? "Disconnect" : "Connect", conn ? mtk::BUTTON_DANGER : mtk::BUTTON_PRIMARY, button_state(action, true), theme);