daniel/MontaukOS
1
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: Intel BT firmware download, A2dp & Bluetooth audio progress

Browse Source
This commit is contained in:
Daniel Hammer
2026-06-03 18:05:17 +02:00
parent 52b01a7d73
commit c119a70d5b
22 changed files with 6452 additions and 273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
kernel/src/Drivers/USB/Bluetooth/A2dp.cpp
+484 -50
View File
@@ -70,10 +70,41 @@ namespace Drivers::USB::Bluetooth::A2dp {
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;
@@ -86,6 +117,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
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
// =========================================================================
@@ -94,7 +131,8 @@ namespace Drivers::USB::Bluetooth::A2dp {
uint8_t buf[128] = {};
// AVDTP single packet header
buf[0] = (g_txLabel << 4) | (PKT_SINGLE << 2) | MSG_COMMAND;
uint8_t lbl = g_txLabel;
buf[0] = (lbl << 4) | (PKT_SINGLE << 2) | MSG_COMMAND;
buf[1] = signalId;
g_txLabel = (g_txLabel + 1) & 0x0F;
@@ -102,6 +140,13 @@ namespace Drivers::USB::Bluetooth::A2dp {
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);
}
@@ -129,6 +174,7 @@ namespace Drivers::USB::Bluetooth::A2dp {
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");
@@ -137,6 +183,14 @@ namespace Drivers::USB::Bluetooth::A2dp {
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
// =========================================================================
@@ -148,67 +202,185 @@ namespace Drivers::USB::Bluetooth::A2dp {
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover timeout";
return false;
}
if (!AvdtpAccepted()) {
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover rejected";
return false;
}
// Parse discover response to find audio sink SEID
// Response format: each SEP is 2 bytes:
// Byte 0: SEID(6) | InUse(1) | Rsvd(1)
// Byte 1: MediaType(4) | SEPType(4) (SEPType: 0=Source, 1=Sink)
// 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] & 0x0F;
uint8_t sepType = (g_avdtpResponseBuf[i + 1] >> 3) & 0x01; // TSEP: 1=Sink
if (mediaType == MEDIA_AUDIO && sepType == 0x01 && !inUse) {
g_remoteSeid = seid;
KernelLogStream(INFO, "BT-A2DP") << "Found audio sink SEID="
<< (uint64_t)seid;
return true;
if (g_numSinkSeids < (sizeof(g_sinkSeids) / sizeof(g_sinkSeids[0]))) {
g_sinkSeids[g_numSinkSeids++] = seid;
}
}
}
}
KernelLogStream(WARNING, "BT-A2DP") << "No audio sink SEP found";
return false;
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;
}
static bool AvdtpGetCapabilities() {
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
// 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);
return WaitAvdtpResponse();
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() {
// Set Configuration payload:
// ACP SEID (1 byte) | INT SEID (1 byte) | Service Capabilities...
uint8_t payload[12] = {};
payload[0] = (g_remoteSeid << 2); // ACP SEID
payload[1] = (g_localSeid << 2); // INT SEID
// 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;
// Media Transport capability (no data)
payload[2] = CAT_MEDIA_TRANSPORT; // Category
payload[3] = 0; // Length
// 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; }
// Media Codec capability (SBC)
payload[4] = CAT_MEDIA_CODEC; // Category
payload[5] = 6; // Length
payload[6] = (MEDIA_AUDIO << 4); // Media Type
payload[7] = CODEC_SBC; // Codec Type
// SBC codec info (4 bytes)
// Sampling: 44.1kHz (bit 5), Channel mode: Joint Stereo (bit 0)
payload[8] = 0x21; // 44.1kHz | Joint Stereo
// Block length: 16 (bit 7), Subbands: 8 (bit 1), Alloc: Loudness (bit 0)
payload[9] = 0x83; // 16 blocks | 8 subbands | Loudness
payload[10] = 2; // Min bitpool
payload[11] = 53; // Max bitpool
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, 12);
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";
@@ -223,6 +395,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
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";
@@ -237,6 +415,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
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";
@@ -248,26 +432,248 @@ namespace Drivers::USB::Bluetooth::A2dp {
// =========================================================================
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) {
// First AVDTP channel is signaling
g_sigCid = l2capCid;
KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready: CID="
<< (uint64_t)l2capCid;
// Auto-discover remote SEPs
g_state = State::Discovering;
if (AvdtpDiscover()) {
AvdtpGetCapabilities();
AvdtpSetConfiguration();
}
} else if (g_mediaCid == 0) {
// Second AVDTP channel is media transport
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
// =========================================================================
@@ -281,10 +687,15 @@ namespace Drivers::USB::Bluetooth::A2dp {
uint8_t signalId = data[1] & 0x3F;
if (msgType == MSG_RESPONSE_ACCEPT || msgType == MSG_RESPONSE_REJECT) {
// This is a response to our command
memcpy(g_avdtpResponseBuf, data, len > sizeof(g_avdtpResponseBuf) ? sizeof(g_avdtpResponseBuf) : len);
g_avdtpResponseLen = len;
g_avdtpResponseReady = true;
// 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;
}
@@ -310,7 +721,7 @@ namespace Drivers::USB::Bluetooth::A2dp {
rsp[4] = (MEDIA_AUDIO << 4);
rsp[5] = CODEC_SBC;
rsp[6] = 0x21; // 44.1kHz | Joint Stereo
rsp[7] = 0x83; // 16 blocks | 8 subbands | Loudness
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);
@@ -375,6 +786,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
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;
@@ -429,6 +846,8 @@ namespace Drivers::USB::Bluetooth::A2dp {
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
@@ -464,8 +883,23 @@ namespace Drivers::USB::Bluetooth::A2dp {
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;