/* * A2dp.cpp * Bluetooth A2DP / AVDTP implementation * Copyright (c) 2026 Daniel Hammer */ #include "A2dp.hpp" #include "Sbc.hpp" #include "L2cap.hpp" #include "Hci.hpp" #include #include #include #include #include using namespace Kt; namespace Drivers::USB::Bluetooth::A2dp { // ========================================================================= // AVDTP constants // ========================================================================= // AVDTP signal IDs constexpr uint8_t AVDTP_DISCOVER = 0x01; constexpr uint8_t AVDTP_GET_CAPABILITIES = 0x02; constexpr uint8_t AVDTP_SET_CONFIGURATION = 0x03; constexpr uint8_t AVDTP_GET_CONFIGURATION = 0x04; constexpr uint8_t AVDTP_RECONFIGURE = 0x05; constexpr uint8_t AVDTP_OPEN = 0x06; constexpr uint8_t AVDTP_START = 0x07; constexpr uint8_t AVDTP_CLOSE = 0x08; constexpr uint8_t AVDTP_SUSPEND = 0x09; constexpr uint8_t AVDTP_ABORT = 0x0A; // AVDTP message types constexpr uint8_t MSG_COMMAND = 0x00; constexpr uint8_t MSG_GENERAL_REJECT = 0x01; constexpr uint8_t MSG_RESPONSE_ACCEPT = 0x02; constexpr uint8_t MSG_RESPONSE_REJECT = 0x03; // AVDTP packet types constexpr uint8_t PKT_SINGLE = 0x00; // Service category IDs constexpr uint8_t CAT_MEDIA_TRANSPORT = 0x01; constexpr uint8_t CAT_MEDIA_CODEC = 0x07; // Media type constexpr uint8_t MEDIA_AUDIO = 0x00; // Codec type constexpr uint8_t CODEC_SBC = 0x00; // SBC capability octets // Octet 0: Sampling Frequency (bits 7-4) | Channel Mode (bits 3-0) // Octet 1: Block Length (bits 7-4) | Subbands (bits 3-2) | Alloc Method (bits 1-0) // Octet 2: Min Bitpool // Octet 3: Max Bitpool // ========================================================================= // State // ========================================================================= static State g_state = State::Idle; static uint16_t g_sigCid = 0; // L2CAP CID for AVDTP signaling static uint16_t g_mediaCid = 0; // L2CAP CID for AVDTP media transport 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 // Audio sink endpoints advertised by the remote (AVDTP Discover). A modern // headset typically exposes several SEPs -- one per codec (SBC, AAC, aptX, // ...). Each SEP carries exactly ONE media codec, so codec choice is // endpoint choice. We must probe each with GetCapabilities and configure the // one that offers SBC (the codec our encoder produces). The old code grabbed // the FIRST audio sink and committed to it; on Bose that is the AAC endpoint, // so the subsequent SBC SetConfiguration was rejected (cat=1 err=0x29, // UNSUPPORTED_CONFIGURATION) and music never played. static uint8_t g_sinkSeids[16] = {}; static uint32_t g_numSinkSeids = 0; // The transaction label + signal id of the command we are currently waiting // 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; // SBC encoder static Sbc::SbcEncoder g_sbcEncoder = {}; static bool g_sbcInitialized = false; // SBC capability negotiation. An A2DP source must SetConfiguration with a // subset of what the sink advertised in GetCapabilities -- asserting a fixed // config the sink didn't offer gets it rejected with UNSUPPORTED_CONFIGURATION // (0x29), the observed Bose behaviour. We parse the sink's SBC capability and // pick a supported config; g_cfgSbc is what we actually configured (used to // set up the encoder so the frame headers match). static uint8_t g_sinkSbcCaps[4] = {}; // advertised: [freq|mode][blk|sub|alloc][minBP][maxBP] static bool g_haveSinkSbcCaps = false; static bool g_sinkDelayReporting = false; // sink advertised Delay Reporting (cat 0x08) static bool g_sinkContentProtection = false; // sink advertised Content Protection (cat 0x04) static uint8_t g_sinkCpType[2] = {}; // advertised CP_TYPE (LSB,MSB); SCMS-T = {0x02,0x00} static uint8_t g_cfgSbc[4] = {}; // the SBC octets we configured // Media packet state static uint16_t g_seqNum = 0; static uint32_t g_timestamp = 0; // Volume static int g_volume = 80; // AVDTP response tracking static volatile bool g_avdtpResponseReady = false; static uint8_t g_avdtpResponseBuf[128] = {}; 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; // ========================================================================= // AVDTP signaling helpers // ========================================================================= static void SendAvdtpCommand(uint8_t signalId, const uint8_t* payload, uint16_t len) { uint8_t buf[128] = {}; // AVDTP single packet header uint8_t lbl = g_txLabel; buf[0] = (lbl << 4) | (PKT_SINGLE << 2) | MSG_COMMAND; buf[1] = signalId; g_txLabel = (g_txLabel + 1) & 0x0F; if (payload && len > 0) { memcpy(&buf[2], payload, len); } // 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_avdtpResponseLen = 0; L2cap::SendData(g_sigCid, buf, 2 + len); } static void SendAvdtpResponse(uint8_t txLabel, uint8_t signalId, const uint8_t* payload, uint16_t len) { uint8_t buf[128] = {}; buf[0] = (txLabel << 4) | (PKT_SINGLE << 2) | MSG_RESPONSE_ACCEPT; buf[1] = signalId; if (payload && len > 0) { memcpy(&buf[2], payload, len); } L2cap::SendData(g_sigCid, buf, 2 + len); } // ========================================================================= // WaitAvdtpResponse // ========================================================================= static bool WaitAvdtpResponse(uint32_t timeoutMs = 3000) { g_avdtpResponseReady = false; 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; for (int j = 0; j < 100; j++) { asm volatile("" ::: "memory"); } } return false; } // WaitAvdtpResponse() returns true for ACCEPT *and* REJECT (the msgType is in // the low 2 bits of byte 0). This distinguishes them so a rejected // configuration is surfaced instead of silently treated as success. static bool AvdtpAccepted() { return g_avdtpResponseLen >= 1 && (g_avdtpResponseBuf[0] & 0x03) == MSG_RESPONSE_ACCEPT; } // ========================================================================= // AVDTP signaling procedures // ========================================================================= static bool AvdtpDiscover() { SendAvdtpCommand(AVDTP_DISCOVER, nullptr, 0); if (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover timeout"; return false; } if (!AvdtpAccepted()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover rejected"; return false; } // Parse discover response to enumerate audio sink SEIDs. Each SEP is 2 // bytes: // Byte 0: ACP SEID (bits 7-2) | In Use (bit 1) | RFA (bit 0) // Byte 1: Media Type (bits 7-4) | TSEP (bit 3) | RFA (bits 2-0) // TSEP (the Stream End Point type) is BIT 3: 0=Source (SRC), 1=Sink (SNK) // -- it is NOT the low nibble. The old `& 0x0F` read RFA bits too, so an // audio sink (byte1 = 0x08: Audio<<4 | SNK<<3) computed 0x08 != 0x01 and // was rejected -> "No audio sink SEP found", an earlier HW symptom. // // Collect EVERY usable audio sink (the remote exposes one per codec); // StartSource then probes each for SBC rather than committing to the // first, which on Bose is the AAC endpoint. g_numSinkSeids = 0; if (g_avdtpResponseLen >= 4) { for (uint32_t i = 2; i + 1 < g_avdtpResponseLen; i += 2) { uint8_t seid = (g_avdtpResponseBuf[i] >> 2) & 0x3F; bool inUse = (g_avdtpResponseBuf[i] >> 1) & 1; uint8_t mediaType = (g_avdtpResponseBuf[i + 1] >> 4) & 0x0F; uint8_t sepType = (g_avdtpResponseBuf[i + 1] >> 3) & 0x01; // TSEP: 1=Sink if (mediaType == MEDIA_AUDIO && sepType == 0x01 && !inUse) { if (g_numSinkSeids < (sizeof(g_sinkSeids) / sizeof(g_sinkSeids[0]))) { g_sinkSeids[g_numSinkSeids++] = seid; } } } } if (g_numSinkSeids == 0) { KernelLogStream(WARNING, "BT-A2DP") << "No audio sink SEP found"; return false; } KernelLogStream cl(INFO, "BT-A2DP"); cl << "Found " << (uint64_t)g_numSinkSeids << " audio sink SEID(s):"; for (uint32_t i = 0; i < g_numSinkSeids; i++) cl << " " << (uint64_t)g_sinkSeids[i]; return true; } // Pick a single capability bit: the first preference (MSB of the list first) // that the sink advertises in `avail`; fall back to the lowest set bit. static uint8_t PickBit(uint8_t avail, const uint8_t* pref, int n) { for (int i = 0; i < n; i++) if (avail & pref[i]) return pref[i]; for (int b = 0; b < 8; b++) if (avail & (1u << b)) return (uint8_t)(1u << b); return 0; } static bool AvdtpGetCapabilities(uint8_t seid) { // Clear the per-endpoint capability state up front, BEFORE any early // return, so a probe that times out or is rejected leaves no stale SBC // caps behind for the next endpoint in the probe loop or a later // StartSource() reconnect/retry to misread. g_haveSinkSbcCaps = false; g_sinkDelayReporting = false; g_sinkContentProtection = false; g_sinkCpType[0] = g_sinkCpType[1] = 0; 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 (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP GetCapabilities timeout (SEID=" << (uint64_t)seid << ")"; return false; } if (!AvdtpAccepted()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP GetCapabilities rejected (SEID=" << (uint64_t)seid << ")"; return false; } // Parse the advertised service capabilities so SetConfiguration offers // only a subset the sink actually supports. Capabilities follow the // AVDTP header (bytes 0-1): [category][LOSC][content...] repeated. // (Capability state was already cleared at function entry.) KernelLogStream cl(INFO, "BT-A2DP"); cl << "GetCap SEID=" << (uint64_t)seid << " cats:" << base::hex; uint32_t off = 2; while (off + 2 <= g_avdtpResponseLen) { uint8_t cat = g_avdtpResponseBuf[off]; uint8_t losc = g_avdtpResponseBuf[off + 1]; cl << " " << (uint64_t)cat << "/" << (uint64_t)losc; 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) 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) { g_sinkSbcCaps[0] = content[2]; g_sinkSbcCaps[1] = content[3]; g_sinkSbcCaps[2] = content[4]; g_sinkSbcCaps[3] = content[5]; g_haveSinkSbcCaps = true; cl << " [SBC " << (uint64_t)content[2] << " " << (uint64_t)content[3] << " " << (uint64_t)content[4] << " " << (uint64_t)content[5] << "]"; } off += 2 + losc; } cl << base::dec; return true; } static bool AvdtpSetConfiguration() { // Choose an SBC configuration that is a SUBSET of the sink's advertised // capabilities (each field exactly one bit). Asserting unsupported // 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 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); oct1 = PickBit(g_sinkSbcCaps[1] & 0xF0, blkPref, 4) | PickBit(g_sinkSbcCaps[1] & 0x0C, subPref, 2) | 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; } else { oct0 = 0x11; oct1 = 0x15; minBP = 2; maxBP = 53; // mandatory SBC baseline } g_cfgSbc[0] = oct0; g_cfgSbc[1] = oct1; g_cfgSbc[2] = minBP; g_cfgSbc[3] = maxBP; // Set Configuration: ACP SEID | INT SEID | Service Capabilities. uint8_t payload[24] = {}; int n = 0; payload[n++] = (g_remoteSeid << 2); // ACP SEID payload[n++] = (g_localSeid << 2); // INT SEID payload[n++] = CAT_MEDIA_TRANSPORT; payload[n++] = 0; payload[n++] = CAT_MEDIA_CODEC; payload[n++] = 6; payload[n++] = (MEDIA_AUDIO << 4); // Media Type (audio) payload[n++] = CODEC_SBC; // Codec Type (SBC) payload[n++] = oct0; payload[n++] = oct1; payload[n++] = minBP; payload[n++] = maxBP; // If the sink advertised Content Protection (cat 0x04), configure SCMS-T // (CP_TYPE 0x0002). Bose QC sinks advertise it and gate the media // transport channel on it: SetConfiguration is accepted without it, but // the sink then never authorizes the transport L2CAP channel (it answers // the transport CONN_REQ with a perpetual PENDING). CP_TYPE is 2 bytes, // LSB first; SCMS-T carries no extra CP-type-specific data (LOSC=2). if (g_sinkContentProtection) { payload[n++] = 0x04; payload[n++] = 2; // Content Protection, LOSC=2 payload[n++] = g_sinkCpType[0]; // CP_TYPE LSB (SCMS-T = 0x02) payload[n++] = g_sinkCpType[1]; // CP_TYPE MSB (SCMS-T = 0x00) } // If the sink advertised Delay Reporting, configure it too -- some sinks // reject SetConfiguration that omits a category they require. if (g_sinkDelayReporting) { payload[n++] = 0x08; payload[n++] = 0; } KernelLogStream(INFO, "BT-A2DP") << "SetConfig SBC oct0=" << base::hex << (uint64_t)oct0 << " oct1=" << (uint64_t)oct1 << base::dec << " bp=" << (uint64_t)minBP << ".." << (uint64_t)maxBP << (g_sinkContentProtection ? " +scms-t" : "") << (g_sinkDelayReporting ? " +delay" : ""); SendAvdtpCommand(AVDTP_SET_CONFIGURATION, payload, (uint16_t)n); if (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP SetConfiguration timeout"; return false; } if (!AvdtpAccepted()) { // Reject payload: [failing service category][error code] uint8_t cat = (g_avdtpResponseLen > 2) ? g_avdtpResponseBuf[2] : 0; uint8_t err = (g_avdtpResponseLen > 3) ? g_avdtpResponseBuf[3] : 0; KernelLogStream(WARNING, "BT-A2DP") << "AVDTP SetConfiguration rejected (cat=" << base::hex << (uint64_t)cat << " err=" << (uint64_t)err << base::dec << ")"; return false; } g_state = State::Configured; 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 (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Open timeout"; return false; } if (!AvdtpAccepted()) { uint8_t err = (g_avdtpResponseLen > 2) ? g_avdtpResponseBuf[2] : 0; KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Open rejected (err=" << base::hex << (uint64_t)err << base::dec << ")"; return false; } g_state = State::Open; KernelLogStream(OK, "BT-A2DP") << "Stream opened"; return true; } static bool AvdtpStart() { uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; SendAvdtpCommand(AVDTP_START, payload, 1); if (!WaitAvdtpResponse()) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start timeout"; return false; } if (!AvdtpAccepted()) { uint8_t err = (g_avdtpResponseLen > 2) ? g_avdtpResponseBuf[2] : 0; KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start rejected (err=" << base::hex << (uint64_t)err << base::dec << ")"; return false; } g_state = State::Streaming; KernelLogStream(OK, "BT-A2DP") << "Streaming started"; return true; } // ========================================================================= // OnChannelReady — called by L2CAP when an AVDTP channel is configured // ========================================================================= 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 // must NOT run the blocking AVDTP signaling chain here; a nested poll // 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; KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready: CID=" << (uint64_t)l2capCid; } else if (g_mediaCid == 0) { g_mediaCid = l2capCid; KernelLogStream(OK, "BT-A2DP") << "AVDTP media channel ready: CID=" << (uint64_t)l2capCid; } } // ========================================================================= // StartSource — drive the A2DP source role after the ACL link is up // ========================================================================= // Runs at top-level (process) context, NOT nested under PollEvents, so the // blocking AVDTP handshakes are safe. Opens the AVDTP signaling channel, // negotiates an SBC stream (Discover -> GetCapabilities -> SetConfiguration // -> Open), then opens the media transport channel. Leaves the stream in // the Open state; the first audio write (StartStream) issues AVDTP_START. // Without this the headset has no media stream and terminates the link // (HCI disconnect reason 0x13), which is the connect/disconnect flapping. // Called by L2CAP when data arrives on the SDP channel (the query response). void ProcessSdp(const uint8_t* data, uint16_t len) { (void)data; if (len > 0) g_sdpRspReady = true; } // Minimal SDP: open PSM 0x0001, send a ServiceSearchAttributeRequest for the // AudioSink service (UUID 0x110B), wait briefly for the response. Best // effort -- the goal is to satisfy sinks that gate AVDTP on a prior SDP // 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; 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 << ")"; Hci::DumpAclStats(); // did our CONN_REQ even go out / any reply arrive? return false; } g_sdpCid = cid; // ServiceSearchAttributeRequest for AudioSink (0x110B), all attributes. uint8_t pdu[20] = { 0x06, // PDU ID: ServiceSearchAttributeRequest 0x00, 0x01, // Transaction ID 0x00, 0x0F, // Parameter length = 15 0x35, 0x03, // ServiceSearchPattern: DES, 3 bytes 0x19, 0x11, 0x0B, // UUID16 0x110B (AudioSink) 0xFF, 0xFF, // MaximumAttributeByteCount 0x35, 0x05, // AttributeIDList: DES, 5 bytes 0x0A, 0x00, 0x00, 0xFF, 0xFF, // UINT32 range 0x0000-0xFFFF 0x00 // ContinuationState (none) }; L2cap::SendData(cid, pdu, sizeof(pdu)); uint64_t start = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - start < timeoutMs) { Xhci::PollEvents(); Hci::DrainEvents(); if (g_sdpRspReady) { KernelLogStream(OK, "BT-A2DP") << "SDP query answered"; return true; } for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } KernelLogStream(INFO, "BT-A2DP") << "SDP query sent, no response (continuing)"; return true; // channel configured + query sent; proceed to AVDTP } bool StartSource(uint32_t timeoutMs) { constexpr int kMaxAttempts = 4; // Connection phase, retried. A sink commonly ignores the very first // L2CAP CONN_REQ that lands right after Encryption Change (the build-39 // HW symptom: connRsp stays 0xFFFF, the headset never answers). Re-dial // up to kMaxAttempts. We retry ONLY when the remote ignored us // (connRsp==0xFFFF -> our dialed channel has RemoteCid==0, so freeing it // owes the peer no Disconnect); if it answered but config stalled // (connRsp!=0xFFFF) we do NOT loop -- that's the config phase, surfaced // by its own logs. We never reset the CID allocator, so each retry uses // a fresh local CID and a late response for an old one cannot cross-wire. for (int attempt = 0; attempt < kMaxAttempts; attempt++) { g_sigCid = 0; g_mediaCid = 0; g_state = State::Idle; g_txLabel = 1; g_avdtpResponseReady = false; // 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); // AVDTP signaling channel (PSM 0x0019). Dial out, then wait for a // channel to become ready in EITHER direction (OnChannelReady sets // g_sigCid for ours, or one the headset opened to us). uint16_t sig = L2cap::Connect(L2cap::PSM_AVDTP); KernelLogStream(INFO, "BT-A2DP") << "AVDTP signaling: attempt " << (uint64_t)(attempt + 1) << "/" << (uint64_t)kMaxAttempts << " dialed cid=" << base::hex << (uint64_t)sig << base::dec << " (acl=" << (uint64_t)L2cap::GetAclHandle() << "), waiting..."; uint64_t sigStart = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - sigStart < timeoutMs) { Xhci::PollEvents(); Hci::DrainEvents(); if (g_sigCid != 0) break; for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } if (g_sigCid != 0) break; // signaling channel up -> proceed auto* ch = sig ? L2cap::GetChannel(sig) : nullptr; KernelLogStream(WARNING, "BT-A2DP") << "AVDTP signaling attempt " << (uint64_t)(attempt + 1) << " TIMEOUT (remoteCid=" << 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() << base::dec << " incomingReqs=" << (uint64_t)L2cap::IncomingAvdtpReqCount() << ")"; // 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) { Hci::DumpAclStats(); return false; } // Free our unanswered dialed channel so repeated retries don't leak // the fixed channel table, then settle (draining events, so the RX // ring keeps being serviced) before re-dialing. if (sig) L2cap::FreeChannel(sig); uint64_t st = Timekeeping::GetMilliseconds(); while (Timekeeping::GetMilliseconds() - st < 400) { Xhci::PollEvents(); Hci::DrainEvents(); for (int j = 0; j < 200; j++) asm volatile("pause" ::: "memory"); } } if (g_sigCid == 0) { KernelLogStream(WARNING, "BT-A2DP") << "AVDTP signaling setup gave up after retries"; Hci::DumpAclStats(); return false; } KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready, cid=" << base::hex << (uint64_t)g_sigCid << base::dec; // 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 // sink is SBC -- on Bose it is AAC, and configuring SBC against it is // rejected (UNSUPPORTED_CONFIGURATION). SBC is mandatory for any 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; } 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 // 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 // sink's open-acceptor window), and HOLD that same channel while // polling. A CONN_RSP result=1 (PENDING) is NON-FINAL: a conformant // sink follows it with SUCCESS/REFUSED on the SAME channel, so we must // NOT tear it down and re-dial -- doing so (build 47) only churned CIDs // and abandoned the very connection the sink was authorizing, while a // single held dial (build 44) already proved holding alone is harmless. // Also accept an inbound transport channel (some sinks open it). // NOTE: the real gate on Bose is Content Protection -- see // AvdtpSetConfiguration's SCMS-T handling; without it the sink pends // this channel forever (connRsp=1, remoteCid=0). 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..."; uint64_t mStart = Timekeeping::GetMilliseconds(); while (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 for (int j = 0; j < 100; j++) asm volatile("" ::: "memory"); } if (g_mediaCid == 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. auto* ch = media ? L2cap::GetChannel(media) : nullptr; KernelLogStream(WARNING, "BT-A2DP") << "AVDTP media channel setup failed (remoteCid=" << 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() << base::dec << " incomingReqs=" << (uint64_t)L2cap::IncomingAvdtpReqCount() << ")"; Hci::DumpAclStats(); return false; } KernelLogStream(OK, "BT-A2DP") << "A2DP source ready (signaling + media), cid=" << base::hex << (uint64_t)g_mediaCid << base::dec << " state=Open"; return true; } // ========================================================================= // ProcessAvdtp — handle AVDTP signaling packets // ========================================================================= void ProcessAvdtp(const uint8_t* data, uint16_t len) { if (len < 2) 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) { // 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) { uint32_t cp = (len > sizeof(g_avdtpResponseBuf)) ? sizeof(g_avdtpResponseBuf) : len; memcpy(g_avdtpResponseBuf, data, cp); g_avdtpResponseLen = len; g_avdtpResponseReady = true; } return; } // Handle incoming commands if (msgType == MSG_COMMAND) { switch (signalId) { case AVDTP_DISCOVER: { // Respond with our local SEP (audio source) 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); break; } case AVDTP_GET_CAPABILITIES: { // Respond with our SBC capabilities uint8_t rsp[10] = {}; rsp[0] = CAT_MEDIA_TRANSPORT; rsp[1] = 0; rsp[2] = CAT_MEDIA_CODEC; rsp[3] = 6; rsp[4] = (MEDIA_AUDIO << 4); rsp[5] = CODEC_SBC; rsp[6] = 0x21; // 44.1kHz | Joint Stereo rsp[7] = 0x15; // 16 blocks (b4) | 8 subbands (b2) | Loudness (b0) rsp[8] = 2; // Min bitpool rsp[9] = 53; // Max bitpool SendAvdtpResponse(txLabel, AVDTP_GET_CAPABILITIES, rsp, 10); break; } case AVDTP_SET_CONFIGURATION: { // Accept configuration from remote if (len >= 4) { g_remoteSeid = (data[2] >> 2) & 0x3F; g_state = State::Configured; SendAvdtpResponse(txLabel, AVDTP_SET_CONFIGURATION, nullptr, 0); KernelLogStream(OK, "BT-A2DP") << "Remote configured stream, SEID=" << (uint64_t)g_remoteSeid; } break; } case AVDTP_OPEN: { g_state = State::Open; SendAvdtpResponse(txLabel, AVDTP_OPEN, nullptr, 0); KernelLogStream(OK, "BT-A2DP") << "Remote opened stream"; // The media transport channel will be set up via L2CAP after this break; } case AVDTP_START: { g_state = State::Streaming; SendAvdtpResponse(txLabel, AVDTP_START, nullptr, 0); KernelLogStream(OK, "BT-A2DP") << "Remote started streaming"; break; } case AVDTP_CLOSE: { g_state = State::Idle; SendAvdtpResponse(txLabel, AVDTP_CLOSE, nullptr, 0); break; } case AVDTP_SUSPEND: { g_state = State::Open; SendAvdtpResponse(txLabel, AVDTP_SUSPEND, nullptr, 0); break; } case AVDTP_ABORT: { g_state = State::Idle; SendAvdtpResponse(txLabel, AVDTP_ABORT, nullptr, 0); break; } default: break; } } } // ========================================================================= // ConfigureStream // ========================================================================= bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample) { Sbc::Init(&g_sbcEncoder, sampleRate, channels, bitsPerSample); // Override with the SBC parameters actually negotiated in // SetConfiguration so the encoded frame headers match what the sink // agreed to (Init's defaults may differ from the negotiated subset). if (g_cfgSbc[0] || g_cfgSbc[1]) { Sbc::Configure(&g_sbcEncoder, g_cfgSbc[0], g_cfgSbc[1], g_cfgSbc[3]); } g_sbcInitialized = true; g_seqNum = 0; g_timestamp = 0; KernelLogStream(OK, "BT-A2DP") << "SBC encoder initialized: " << (uint64_t)sampleRate << "Hz " << (uint64_t)bitsPerSample << "-bit " << (uint64_t)channels << "ch"; return true; } // ========================================================================= // StartStream / StopStream // ========================================================================= bool StartStream() { if (g_state == State::Open || g_state == State::Configured) { if (g_state == State::Configured) { if (!AvdtpOpen()) return false; } return AvdtpStart(); } return (g_state == State::Streaming); } bool StopStream() { if (g_state == State::Streaming) { uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)}; SendAvdtpCommand(AVDTP_SUSPEND, payload, 1); WaitAvdtpResponse(1000); g_state = State::Open; } return true; } // ========================================================================= // WriteAudio — encode PCM to SBC and stream over Bluetooth // ========================================================================= int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen) { if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) { return -1; } uint32_t samplesPerFrame = Sbc::GetSamplesPerFrame(&g_sbcEncoder); uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2; // 16-bit samples uint32_t sbcFrameSize = Sbc::GetFrameSize(&g_sbcEncoder); // Apply volume scaling to PCM data // We work on a local copy for volume adjustment int16_t scaledPcm[512]; // Max ~128 samples * 2 channels = 256 samples if (bytesPerFrame > sizeof(scaledPcm)) return -1; uint32_t consumed = 0; uint16_t maxOut = Hci::AclMaxPackets(); if (maxOut == 0) maxOut = 4; // controller ACL buffer credits while (consumed + bytesPerFrame <= pcmLen) { // Copy and scale by volume const int16_t* src = (const int16_t*)(pcmData + consumed); uint32_t numSamples = samplesPerFrame * g_sbcEncoder.Channels; for (uint32_t i = 0; i < numSamples; i++) { scaledPcm[i] = (int16_t)(((int32_t)src[i] * g_volume) / 100); } // Build media packet: RTP-like header (12 bytes) + SBC payload header (1 byte) + SBC frames uint8_t mediaPkt[256] = {}; // Simplified media packet header (AVDTP media packet) // Byte 0: V=2, P=0, X=0, CC=0 -> 0x80 // Byte 1: M=0, PT=96 -> 0x60 // Bytes 2-3: Sequence number // Bytes 4-7: Timestamp // Bytes 8-11: SSRC // Byte 12: SBC payload header (number of SBC frames) mediaPkt[0] = 0x80; mediaPkt[1] = 0x60; mediaPkt[2] = (uint8_t)(g_seqNum >> 8); mediaPkt[3] = (uint8_t)(g_seqNum & 0xFF); mediaPkt[4] = (uint8_t)(g_timestamp >> 24); mediaPkt[5] = (uint8_t)(g_timestamp >> 16); mediaPkt[6] = (uint8_t)(g_timestamp >> 8); mediaPkt[7] = (uint8_t)(g_timestamp & 0xFF); mediaPkt[8] = 0; mediaPkt[9] = 0; mediaPkt[10] = 0; mediaPkt[11] = 0x01; // SSRC mediaPkt[12] = 1; // Number of SBC frames in this packet // Encode SBC frame uint32_t encodedSize = Sbc::Encode(&g_sbcEncoder, scaledPcm, &mediaPkt[13]); uint32_t totalLen = 13 + encodedSize; // Flow control + event pump. WriteAudio runs in syscall context, // which otherwise never services the xHCI event ring -- so without // this the controller's ACL credits (Number-Of-Completed-Packets) // and the RX ring are never processed and the TX ring stalls/overruns // (-> no audio). Wait for a controller buffer credit, send, then pump // so the completion + credit are reaped before the next frame. uint64_t t0 = Timekeeping::GetMilliseconds(); while (Hci::AclPendingCount() >= maxOut && Timekeeping::GetMilliseconds() - t0 < 100) { Xhci::PollEvents(); Hci::DrainEvents(); } // Send via L2CAP on media channel L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)totalLen); Xhci::PollEvents(); Hci::DrainEvents(); g_seqNum++; g_timestamp += samplesPerFrame; consumed += bytesPerFrame; } return (int)consumed; } // ========================================================================= // State queries // ========================================================================= State GetState() { return g_state; } bool IsStreaming() { return (g_state == State::Streaming); } int GetVolume() { return g_volume; } void SetVolume(int percent) { if (percent < 0) percent = 0; if (percent > 100) percent = 100; g_volume = percent; } }