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

feat: A2DP bluetooth audio working

Browse Source
This commit is contained in:
Daniel Hammer
2026-06-10 16:00:42 +02:00
parent dff5a4a4b1
commit ca79678823
18 changed files with 1312 additions and 4340 deletions
Download Patch File Download Diff File Expand all files Collapse all files
kernel/src/Drivers/USB/Bluetooth/A2dp.cpp
+710 -83
View File
@@ -13,6 +13,7 @@
#include <CppLib/Stream.hpp>
#include <Libraries/Memory.hpp>
#include <Timekeeping/ApicTimer.hpp>
#include <atomic>
using namespace Kt;
@@ -33,6 +34,7 @@ namespace Drivers::USB::Bluetooth::A2dp {
constexpr uint8_t AVDTP_CLOSE = 0x08;
constexpr uint8_t AVDTP_SUSPEND = 0x09;
constexpr uint8_t AVDTP_ABORT = 0x0A;
constexpr uint8_t AVDTP_GET_ALL_CAPABILITIES = 0x0C; // AVDTP 1.3
// AVDTP message types
constexpr uint8_t MSG_COMMAND = 0x00;
@@ -109,6 +111,29 @@ namespace Drivers::USB::Bluetooth::A2dp {
static uint16_t g_seqNum = 0;
static uint32_t g_timestamp = 0;
// PCM ring between WriteAudio (producer, syscall context) and PumpMedia
// (consumer, idle-loop/syscall context). Absolute byte counters wrapping
// mod 2^32: fill = head - tail, buffer index = counter & (SIZE - 1).
constexpr uint32_t PCM_RING_SIZE = 128 * 1024; // ~0.68 s at 48 kHz stereo
constexpr uint64_t LEAD_MS = 150; // sink jitter-buffer target
static uint8_t g_pcmRing[PCM_RING_SIZE];
static std::atomic<uint32_t> g_ringHead{0}; // producer: WriteAudio
static std::atomic<uint32_t> g_ringTail{0}; // consumer: PumpMedia
static std::atomic<bool> g_pumpActive{false}; // single pumper at a time
static uint32_t g_pcmRate = 48000;
static uint64_t g_clockBase = 0; // ms timestamp of the media clock zero
static uint64_t g_sentSamples = 0; // per-channel samples sent since reset
static uint64_t g_lastSendMs = 0;
static uint32_t g_underruns = 0; // ring ran dry while behind schedule
static bool g_inUnderrun = false;
static void ResetMediaClock() {
g_clockBase = Timekeeping::GetMilliseconds();
g_sentSamples = 0;
g_lastSendMs = g_clockBase;
g_inUnderrun = false;
}
// Volume
static int g_volume = 80;
@@ -409,22 +434,40 @@ namespace Drivers::USB::Bluetooth::A2dp {
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;
for (int attempt = 0; attempt < 2; attempt++) {
SendAvdtpCommand(AVDTP_START, payload, 1);
if (!WaitAvdtpResponse()) {
// A lost response is recoverable: re-issue once. AVDTP START
// is idempotent enough for this (a sink that DID start answers
// the retry with BAD_STATE, handled below).
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start timeout"
<< (attempt == 0 ? " (retrying)" : "");
continue;
}
if (AvdtpAccepted()) {
g_state = State::Streaming;
KernelLogStream(OK, "BT-A2DP") << "Streaming started";
return true;
}
// Reject payload for START: [first failing ACP SEID][error code].
uint8_t err = (g_avdtpResponseLen >= 4) ? g_avdtpResponseBuf[3]
: (g_avdtpResponseLen >= 3) ? g_avdtpResponseBuf[2] : 0;
if (err == 0x31) {
// BAD_STATE: the sink's stream is ALREADY streaming -- a state
// desync (our earlier SUSPEND was lost, or a START retry after
// the sink accepted the first one). Adopt its view.
g_state = State::Streaming;
KernelLogStream(INFO, "BT-A2DP")
<< "AVDTP Start: sink already streaming (BAD_STATE), continuing";
return true;
}
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;
return false;
}
// =========================================================================
@@ -459,10 +502,435 @@ namespace Drivers::USB::Bluetooth::A2dp {
// 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;
// =========================================================================
// SDP server
// =========================================================================
// The headset runs its own SDP query against US right after the signaling
// channel comes up (the Bose QC does: it connects PSM 1 inbound and sends a
// ServiceSearchAttributeRequest), looking for the A2DP AudioSource service
// record the A2DP spec requires a source to expose. If that query goes
// unanswered, the sink never service-authorizes the AVDTP media transport
// channel: it answers our transport CONN_REQ with PENDING (status=2,
// "authorization pending") and the final SUCCESS never arrives -- the
// remoteCid=0/connRsp=1 media-setup failure seen on HW even with SCMS-T
// configured. So we serve one record: A2DP AudioSource.
// SDP-HOST-TEST-BEGIN: the host harness (/tmp/sdptest) extracts the region
// between these markers verbatim and unit-tests it with libc, so keep it
// free of kernel-only dependencies (KernelLogStream and L2cap::SendData
// are stubbed there).
// AudioSource service record: attribute id (uint16 DE) + value, sorted by
// id, all SDP data elements big-endian.
static const uint8_t kSourceRecord[] = {
// 0x0000 ServiceRecordHandle: uint32 0x00010000
0x09, 0x00, 0x00, 0x0A, 0x00, 0x01, 0x00, 0x00,
// 0x0001 ServiceClassIDList: DES { UUID16 AudioSource (0x110A) }
0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x11, 0x0A,
// 0x0004 ProtocolDescriptorList:
// DES { DES { UUID16 L2CAP (0x0100), uint16 PSM 0x0019 },
// DES { UUID16 AVDTP (0x0019), uint16 version 0x0103 } }
0x09, 0x00, 0x04, 0x35, 0x10,
0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x19,
0x35, 0x06, 0x19, 0x00, 0x19, 0x09, 0x01, 0x03,
// 0x0005 BrowseGroupList: DES { UUID16 PublicBrowseRoot (0x1002) }
0x09, 0x00, 0x05, 0x35, 0x03, 0x19, 0x10, 0x02,
// 0x0009 BluetoothProfileDescriptorList:
// DES { DES { UUID16 AdvancedAudioDistribution (0x110D), uint16 0x0103 } }
0x09, 0x00, 0x09, 0x35, 0x08,
0x35, 0x06, 0x19, 0x11, 0x0D, 0x09, 0x01, 0x03,
// 0x0311 SupportedFeatures: uint16 0x0001 (Player)
0x09, 0x03, 0x11, 0x09, 0x00, 0x01,
};
constexpr uint32_t kSourceRecordHandle = 0x00010000;
// AVRCP Target service record. Bose (CSR/Qualcomm-stack) sinks couple the
// audio path to remote control: the headset acts as AVRCP Controller for
// absolute volume and may gate/delay the media path when the source has no
// Target. Category 2 (amplifier) + AVRCP 1.4 = absolute volume capable.
static const uint8_t kAvrcpRecord[] = {
// 0x0000 ServiceRecordHandle: uint32 0x00010001
0x09, 0x00, 0x00, 0x0A, 0x00, 0x01, 0x00, 0x01,
// 0x0001 ServiceClassIDList: DES { UUID16 A/V RemoteControlTarget (0x110C) }
0x09, 0x00, 0x01, 0x35, 0x03, 0x19, 0x11, 0x0C,
// 0x0004 ProtocolDescriptorList:
// DES { DES { UUID16 L2CAP (0x0100), uint16 PSM 0x0017 },
// DES { UUID16 AVCTP (0x0017), uint16 version 0x0103 } }
0x09, 0x00, 0x04, 0x35, 0x10,
0x35, 0x06, 0x19, 0x01, 0x00, 0x09, 0x00, 0x17,
0x35, 0x06, 0x19, 0x00, 0x17, 0x09, 0x01, 0x03,
// 0x0005 BrowseGroupList: DES { UUID16 PublicBrowseRoot (0x1002) }
0x09, 0x00, 0x05, 0x35, 0x03, 0x19, 0x10, 0x02,
// 0x0009 BluetoothProfileDescriptorList:
// DES { DES { UUID16 A/V RemoteControl (0x110E), uint16 0x0104 } }
0x09, 0x00, 0x09, 0x35, 0x08,
0x35, 0x06, 0x19, 0x11, 0x0E, 0x09, 0x01, 0x04,
// 0x0311 SupportedFeatures: uint16 0x0002 (category 2: amplifier)
0x09, 0x03, 0x11, 0x09, 0x00, 0x02,
};
constexpr uint32_t kAvrcpRecordHandle = 0x00010001;
struct SdpRecordDef {
uint32_t Handle;
const uint8_t* Rec;
uint8_t RecLen;
const uint16_t* Uuids; // UUIDs the record "contains" for pattern match
uint8_t NumUuids;
};
static const uint16_t kSourceUuids[] = {0x110A, 0x110D, 0x0019, 0x0100, 0x1002};
static const uint16_t kAvrcpUuids[] = {0x110C, 0x110E, 0x0017, 0x0100, 0x1002};
static const SdpRecordDef kSdpRecords[] = {
{kSourceRecordHandle, kSourceRecord, (uint8_t)sizeof(kSourceRecord), kSourceUuids, 5},
{kAvrcpRecordHandle, kAvrcpRecord, (uint8_t)sizeof(kAvrcpRecord), kAvrcpUuids, 5},
};
constexpr int kNumSdpRecords = 2;
// Parse one SDP data element header at `off`; on success sets the value's
// offset and length (bounds-checked against `len`).
static bool SdpDeHeader(const uint8_t* d, uint32_t len, uint32_t off,
uint32_t* valOff, uint32_t* valLen) {
if (off >= len) return false;
uint32_t vo = off + 1, vl = 0;
switch (d[off] & 0x07) { // size index
case 0: vl = 1; break;
case 1: vl = 2; break;
case 2: vl = 4; break;
case 3: vl = 8; break;
case 4: vl = 16; break;
case 5: if (vo >= len) return false;
vl = d[vo]; vo += 1; break;
case 6: if (vo + 1 >= len) return false;
vl = ((uint32_t)d[vo] << 8) | d[vo + 1]; vo += 2; break;
case 7: if (vo + 3 >= len) return false;
vl = ((uint32_t)d[vo] << 24) | ((uint32_t)d[vo + 1] << 16)
| ((uint32_t)d[vo + 2] << 8) | d[vo + 3]; vo += 4; break;
}
if (vo + vl > len) return false;
*valOff = vo; *valLen = vl;
return true;
}
// Collect the UUIDs named in a ServiceSearchPattern (a DES of UUIDs at
// `off`). UUID32/UUID128-on-base values above 16 bits become 0xFFFF
// (match nothing). Returns the number collected.
static uint32_t SdpPatternUuids(const uint8_t* d, uint32_t len, uint32_t off,
uint16_t* out, uint32_t maxOut) {
static const uint8_t kBaseUuid[12] = // Bluetooth base UUID, bytes 4-15
{0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB};
uint32_t po, pl;
if (!SdpDeHeader(d, len, off, &po, &pl)) return 0;
uint32_t end = po + pl;
uint32_t i = po, n = 0;
while (i < end && n < maxOut) {
uint32_t vo, vl;
if (!SdpDeHeader(d, end, i, &vo, &vl)) break;
uint32_t uuid = 0xFFFFFFFF;
if (d[i] == 0x19 && vl == 2) { // UUID16
uuid = ((uint32_t)d[vo] << 8) | d[vo + 1];
} else if (d[i] == 0x1A && vl == 4) { // UUID32
uuid = ((uint32_t)d[vo] << 24) | ((uint32_t)d[vo + 1] << 16)
| ((uint32_t)d[vo + 2] << 8) | d[vo + 3];
} else if (d[i] == 0x1C && vl == 16
&& memcmp(&d[vo + 4], kBaseUuid, 12) == 0) { // UUID128 on base
uuid = ((uint32_t)d[vo] << 24) | ((uint32_t)d[vo + 1] << 16)
| ((uint32_t)d[vo + 2] << 8) | d[vo + 3];
}
out[n++] = (uuid <= 0xFFFF) ? (uint16_t)uuid : (uint16_t)0xFFFF;
i = vo + vl;
}
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.
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;
}
// Attribute-id ranges requested in an AttributeIDList data element.
struct AttrRange { uint16_t Start; uint16_t End; };
// Parse the AttributeIDList DES at `off` into inclusive (start,end) ranges:
// a uint16 element is a single attribute id, a uint32 element packs a
// start/end range. Returns the count; 0 if absent or malformed, which
// callers treat as "all attributes" (lenient beats wrongly rejecting an
// odd but well-meaning query).
static uint32_t SdpAttrRanges(const uint8_t* d, uint32_t len, uint32_t off,
AttrRange* out, uint32_t maxOut) {
if (off >= len || (d[off] & 0xF8) != 0x30) return 0; // not a DES
uint32_t po, pl;
if (!SdpDeHeader(d, len, off, &po, &pl)) return 0;
uint32_t end = po + pl, i = po, n = 0;
while (i < end && n < maxOut) {
uint32_t vo, vl;
if (!SdpDeHeader(d, end, i, &vo, &vl)) break;
if (d[i] == 0x09 && vl == 2) { // uint16: one attribute id
uint16_t id = ((uint16_t)d[vo] << 8) | d[vo + 1];
out[n].Start = id; out[n].End = id; n++;
} else if (d[i] == 0x0A && vl == 4) { // uint32: id range
out[n].Start = (uint16_t)(((uint16_t)d[vo] << 8) | d[vo + 1]);
out[n].End = (uint16_t)(((uint16_t)d[vo + 2] << 8) | d[vo + 3]);
n++;
}
i = vo + vl;
}
return n;
}
static bool SdpAttrWanted(uint16_t id, const AttrRange* r, uint32_t n) {
if (n == 0) return true;
for (uint32_t i = 0; i < n; i++)
if (id >= r[i].Start && id <= r[i].End) return true;
return false;
}
// Copy the (attribute id, value) pairs of `rec` that the request asked for
// into `out`; returns bytes written. Returning ONLY the requested
// attributes matters: the SDP spec requires it, and an embedded peer's
// parser may walk the response expecting exactly what it asked.
static uint16_t SdpFilterRecord(const uint8_t* rec, uint16_t recLen,
const AttrRange* r, uint32_t nr,
uint8_t* out, uint16_t outMax) {
uint16_t n = 0;
uint32_t i = 0;
while (i + 3 <= recLen) {
if (rec[i] != 0x09) break; // attribute id is always uint16
uint16_t id = (uint16_t)(((uint16_t)rec[i + 1] << 8) | rec[i + 2]);
uint32_t vo, vl;
if (!SdpDeHeader(rec, recLen, i + 3, &vo, &vl)) break;
uint32_t pairLen = (vo - i) + vl; // id element + value element
if (SdpAttrWanted(id, r, nr)) {
if (n + pairLen > outMax) break;
memcpy(&out[n], &rec[i], pairLen);
n = (uint16_t)(n + pairLen);
}
i += pairLen;
}
return n;
}
// The in-flight attribute response body, served in chunks no larger than
// the request's MaximumAttributeByteCount. The continuation state we hand
// out is {len=2, resume offset} into this buffer. One transaction at a
// time is plenty for a headset peer.
static uint8_t g_sdpSrvBody[384];
static uint16_t g_sdpSrvBodyLen = 0;
static void SdpServerSend(uint16_t cid, uint8_t pduId, uint16_t tid,
const uint8_t* params, uint16_t paramLen) {
uint8_t buf[224] = {};
if (5u + paramLen > sizeof(buf)) return;
buf[0] = pduId;
buf[1] = (uint8_t)(tid >> 8); // SDP is big-endian throughout
buf[2] = (uint8_t)(tid & 0xFF);
buf[3] = (uint8_t)(paramLen >> 8);
buf[4] = (uint8_t)(paramLen & 0xFF);
memcpy(&buf[5], params, paramLen);
L2cap::SendData(cid, buf, (uint16_t)(5 + paramLen));
}
// Answer one SDP request PDU. Runs nested under PollEvents (the ACL rx
// path), which is fine: sending is safe there, only blocking waits are not.
static void SdpHandleRequest(uint16_t cid, const uint8_t* d, uint16_t len) {
if (len < 5) return;
uint8_t pdu = d[0];
uint16_t tid = ((uint16_t)d[1] << 8) | d[2];
uint8_t params[200] = {};
uint16_t n = 0;
// Pattern match (search PDUs only) against every record we serve.
uint16_t pat[8] = {};
uint32_t numPat = (pdu == 0x02 || pdu == 0x06)
? SdpPatternUuids(d, len, 5, pat, 8) : 0;
bool match[kNumSdpRecords] = {};
uint32_t numMatch = 0;
for (int r = 0; r < kNumSdpRecords; r++) {
match[r] = SdpRecordMatches(kSdpRecords[r], pat, numPat);
if (match[r]) numMatch++;
}
if (pdu == 0x06 || pdu == 0x04) {
// ServiceSearchAttributeRequest -> 0x07 / ServiceAttributeRequest
// -> 0x05. Both carry a MaximumAttributeByteCount, an
// AttributeIDList, and a continuation state, and both MUST be
// honored: return ONLY the requested attributes and never more
// bytes per response than the peer allowed, continuing across
// requests otherwise. The old server ignored all three and dumped
// every attribute of every record in one oversized response -- a
// spec violation an embedded sink's SDP client may choke on
// silently (its device interrogation then never completes, and a
// stack that service-authorizes channels against that
// interrogation parks them at "authorization pending" forever).
{ // Raw request dump: ONE decisive run beats guessing what the
// headset asked for. Queries are once-per-connection rare.
KernelLogStream cl(INFO, "BT-A2DP");
cl << "SDP req:" << base::hex;
for (uint16_t i = 0; i < len && i < 28; i++)
cl << " " << (uint64_t)d[i];
cl << base::dec;
}
// Field offsets differ: 0x06 has the search pattern at 5, 0x04 a
// 4-byte record handle at 5. Both follow with max byte count,
// AttributeIDList, continuation state.
uint32_t at = 5;
const SdpRecordDef* attrRec = nullptr; // 0x04's addressed record
if (pdu == 0x06) {
uint32_t po, pl;
if (!SdpDeHeader(d, len, 5, &po, &pl)) return;
at = po + pl;
} else {
uint32_t handle = (len >= 9)
? (((uint32_t)d[5] << 24) | ((uint32_t)d[6] << 16)
| ((uint32_t)d[7] << 8) | d[8]) : 0;
for (int r = 0; r < kNumSdpRecords; r++)
if (kSdpRecords[r].Handle == handle) attrRec = &kSdpRecords[r];
if (!attrRec) {
params[n++] = 0x00; params[n++] = 0x02; // invalid record handle
SdpServerSend(cid, 0x01, tid, params, n);
return;
}
at = 9;
}
uint16_t maxBytes = 0xFFFF;
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 (maxBytes < 7) maxBytes = 7; // spec minimum
if (!isCont) {
// Fresh request: build the full response body once, then chunk.
// 0x07's body is an outer DES of per-record attribute-list
// DESes; 0x05's body is the single record's attribute list DES.
uint8_t filtered[192];
g_sdpSrvBodyLen = 0;
if (pdu == 0x06) {
uint16_t inner = 0;
uint16_t flen[kNumSdpRecords] = {};
uint8_t fbuf[kNumSdpRecords][192];
for (int r = 0; r < kNumSdpRecords; r++) {
if (!match[r]) continue;
flen[r] = SdpFilterRecord(kSdpRecords[r].Rec,
kSdpRecords[r].RecLen,
ranges, numRanges,
fbuf[r], sizeof(fbuf[r]));
inner = (uint16_t)(inner + 2 + flen[r]);
}
g_sdpSrvBody[g_sdpSrvBodyLen++] = 0x35;
g_sdpSrvBody[g_sdpSrvBodyLen++] = (uint8_t)inner;
for (int r = 0; r < kNumSdpRecords; r++) {
if (!match[r]) continue;
g_sdpSrvBody[g_sdpSrvBodyLen++] = 0x35;
g_sdpSrvBody[g_sdpSrvBodyLen++] = (uint8_t)flen[r];
memcpy(&g_sdpSrvBody[g_sdpSrvBodyLen], fbuf[r], flen[r]);
g_sdpSrvBodyLen = (uint16_t)(g_sdpSrvBodyLen + flen[r]);
}
} else {
uint16_t flen = SdpFilterRecord(attrRec->Rec, attrRec->RecLen,
ranges, numRanges,
filtered, sizeof(filtered));
g_sdpSrvBody[g_sdpSrvBodyLen++] = 0x35;
g_sdpSrvBody[g_sdpSrvBodyLen++] = (uint8_t)flen;
memcpy(&g_sdpSrvBody[g_sdpSrvBodyLen], filtered, flen);
g_sdpSrvBodyLen = (uint16_t)(g_sdpSrvBodyLen + flen);
}
} else if (g_sdpSrvBodyLen == 0 || resumeOff >= g_sdpSrvBodyLen) {
params[n++] = 0x00; params[n++] = 0x05; // invalid continuation
SdpServerSend(cid, 0x01, tid, params, n);
return;
}
uint16_t chunk = (uint16_t)(g_sdpSrvBodyLen - resumeOff);
if (chunk > maxBytes) chunk = maxBytes;
if (chunk > 180) chunk = 180; // stay inside our buffers
bool more = (uint16_t)(resumeOff + chunk) < g_sdpSrvBodyLen;
{ // Log WHICH services + attributes the headset wants -- decisive
// for diagnosing a sink that gates audio on something we lack.
KernelLogStream cl(OK, "BT-A2DP");
cl << "SDP server: " << (pdu == 0x06 ? "search [" : "attr [")
<< base::hex;
if (pdu == 0x06)
for (uint32_t i = 0; i < numPat; i++)
cl << (i ? " " : "") << (uint64_t)pat[i];
else
cl << (uint64_t)attrRec->Handle;
cl << "] attrs=";
for (uint32_t i = 0; i < numRanges; i++)
cl << (i ? "," : "") << (uint64_t)ranges[i].Start
<< "-" << (uint64_t)ranges[i].End;
cl << base::dec << " max=" << (uint64_t)maxBytes
<< " -> " << (uint64_t)(pdu == 0x06 ? numMatch : 1)
<< " record(s), " << (uint64_t)chunk << "/"
<< (uint64_t)g_sdpSrvBodyLen << " B"
<< (isCont ? " (cont)" : "") << (more ? " (+more)" : "");
}
params[n++] = (uint8_t)(chunk >> 8); // AttributeList(s)ByteCount
params[n++] = (uint8_t)(chunk & 0xFF);
memcpy(&params[n], &g_sdpSrvBody[resumeOff], chunk);
n = (uint16_t)(n + chunk);
if (more) {
uint16_t next = (uint16_t)(resumeOff + chunk);
params[n++] = 0x02; // continuation: 2 bytes
params[n++] = (uint8_t)(next >> 8);
params[n++] = (uint8_t)(next & 0xFF);
} else {
params[n++] = 0x00; // no continuation state
}
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++) {
if (!match[r]) 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);
}
params[n++] = 0x00;
SdpServerSend(cid, 0x03, tid, params, n);
} else {
params[n++] = 0x00; params[n++] = 0x03; // invalid request syntax
SdpServerSend(cid, 0x01, tid, params, n);
}
}
// SDP-HOST-TEST-END
// Called by L2CAP when data arrives on ANY SDP channel -- ours (the client
// query response) or one the headset opened to us (a request to serve).
// Dispatch by PDU id, not by channel: requests are even (0x02/0x04/0x06),
// responses odd -- so a stale g_sdpCid colliding with a fresh inbound
// channel after reconnect can never swallow a request.
void ProcessSdp(uint16_t localCid, const uint8_t* data, uint16_t len) {
if (len < 1) return;
uint8_t pdu = data[0];
if (pdu == 0x02 || pdu == 0x04 || pdu == 0x06) {
SdpHandleRequest(localCid, data, len);
return;
}
if (localCid == g_sdpCid) g_sdpRspReady = true;
}
// Minimal SDP: open PSM 0x0001, send a ServiceSearchAttributeRequest for the
@@ -496,17 +964,28 @@ namespace Drivers::USB::Bluetooth::A2dp {
};
L2cap::SendData(cid, pdu, sizeof(pdu));
bool answered = false;
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;
}
if (g_sdpRspReady) { answered = true; break; }
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
}
KernelLogStream(INFO, "BT-A2DP") << "SDP query sent, no response (continuing)";
// Transaction over: CLOSE the client channel (FreeChannel sends the
// L2CAP Disconnect since the peer acked it). SDP links are
// per-transaction; every stock stack closes them when done, and a
// channel left dangling for the whole session is exactly the kind of
// oddity an embedded peer's connection manager can trip over.
L2cap::FreeChannel(cid);
g_sdpCid = 0;
if (answered) {
KernelLogStream(OK, "BT-A2DP") << "SDP query answered (channel closed)";
} else {
KernelLogStream(INFO, "BT-A2DP") << "SDP query sent, no response (continuing)";
}
return true; // channel configured + query sent; proceed to AVDTP
}
@@ -632,9 +1111,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
// 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).
// NOTE: Bose service-gates this channel TWICE: SetConfiguration must
// include Content Protection (SCMS-T, see AvdtpSetConfiguration), AND
// the headset's own inbound SDP query for our AudioSource record must
// 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;
@@ -671,6 +1153,25 @@ namespace Drivers::USB::Bluetooth::A2dp {
KernelLogStream(OK, "BT-A2DP") << "A2DP source ready (signaling + media), cid="
<< base::hex << (uint64_t)g_mediaCid << base::dec << " state=Open";
// 5. AVRCP control channel (PSM 0x17), best effort, AFTER the stream
// exists -- the order every phone uses. An earlier build dialed it
// right after the signaling channel; the sink answered PENDING and
// never completed it, so the half-open dial just sat in its
// authorization queue ahead of the media channel. Dialing here
// keeps the bring-up clean and still gives the headset its absolute
// volume path; an inbound AVCTP connect is accepted at any time.
{
uint16_t avrcp = L2cap::Connect(L2cap::PSM_AVCTP);
if (avrcp && L2cap::WaitConfigured(avrcp, 1500)) {
KernelLogStream(OK, "BT-A2DP") << "AVRCP control channel ready, cid="
<< 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
<< ", continuing)";
}
}
return true;
}
@@ -711,8 +1212,11 @@ namespace Drivers::USB::Bluetooth::A2dp {
break;
}
case AVDTP_GET_CAPABILITIES: {
// Respond with our SBC capabilities
case AVDTP_GET_CAPABILITIES:
case AVDTP_GET_ALL_CAPABILITIES: {
// 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.
uint8_t rsp[10] = {};
rsp[0] = CAT_MEDIA_TRANSPORT;
rsp[1] = 0;
@@ -724,7 +1228,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, AVDTP_GET_CAPABILITIES, rsp, 10);
SendAvdtpResponse(txLabel, signalId, rsp, 10);
break;
}
@@ -751,31 +1255,49 @@ namespace Drivers::USB::Bluetooth::A2dp {
case AVDTP_START: {
g_state = State::Streaming;
ResetMediaClock();
SendAvdtpResponse(txLabel, AVDTP_START, nullptr, 0);
KernelLogStream(OK, "BT-A2DP") << "Remote started streaming";
break;
}
// The sink tearing the stream down MUST be visible in the log:
// each of these silently flips g_state, after which every
// WriteAudio returns -1 (app symptom: track frozen at 0:00
// with no kernel log output at all).
case AVDTP_CLOSE: {
g_state = State::Idle;
SendAvdtpResponse(txLabel, AVDTP_CLOSE, nullptr, 0);
KernelLogStream(WARNING, "BT-A2DP") << "Remote CLOSED stream";
break;
}
case AVDTP_SUSPEND: {
g_state = State::Open;
SendAvdtpResponse(txLabel, AVDTP_SUSPEND, nullptr, 0);
KernelLogStream(WARNING, "BT-A2DP") << "Remote SUSPENDED stream";
break;
}
case AVDTP_ABORT: {
g_state = State::Idle;
SendAvdtpResponse(txLabel, AVDTP_ABORT, nullptr, 0);
KernelLogStream(WARNING, "BT-A2DP") << "Remote ABORTED stream";
break;
}
default:
default: {
// AVDTP General Reject -- silence to an unknown command can
// stall the peer's signaling state machine.
KernelLogStream(INFO, "BT-A2DP") << "Rejecting unhandled AVDTP cmd 0x"
<< base::hex << (uint64_t)signalId << base::dec;
uint8_t rej[2] = {
(uint8_t)((txLabel << 4) | (PKT_SINGLE << 2) | MSG_GENERAL_REJECT),
signalId
};
L2cap::SendData(g_sigCid, rej, 2);
break;
}
}
}
}
@@ -795,6 +1317,11 @@ namespace Drivers::USB::Bluetooth::A2dp {
g_sbcInitialized = true;
g_seqNum = 0;
g_timestamp = 0;
g_pcmRate = sampleRate ? sampleRate : 48000;
// Fresh stream: drop any queued PCM from the previous one.
g_ringTail.store(g_ringHead.load(std::memory_order_relaxed),
std::memory_order_release);
ResetMediaClock();
KernelLogStream(OK, "BT-A2DP") << "SBC encoder initialized: "
<< (uint64_t)sampleRate << "Hz " << (uint64_t)bitsPerSample << "-bit "
@@ -812,63 +1339,114 @@ namespace Drivers::USB::Bluetooth::A2dp {
if (g_state == State::Configured) {
if (!AvdtpOpen()) return false;
}
return AvdtpStart();
if (!AvdtpStart()) return false;
ResetMediaClock();
return true;
}
return (g_state == State::Streaming);
}
bool StopStream() {
bool StopStream(bool flushQueued) {
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;
}
if (flushQueued) {
// Closing the stream (track change / app exit): drop the queued
// tail. A pause keeps it so resume continues gaplessly. (A pump
// on another core may send one final stale frame -- harmless.)
g_ringTail.store(g_ringHead.load(std::memory_order_relaxed),
std::memory_order_release);
}
return true;
}
// =========================================================================
// WriteAudio — encode PCM to SBC and stream over Bluetooth
// Media pipeline: PCM ring buffer + paced feeder
// =========================================================================
// WriteAudio only copies the app's PCM into a ring and returns at once;
// PumpMedia() encodes and sends frames from the ring, paced to the audio
// clock with LEAD_MS of sink-side jitter buffer, gated on ACL TX
// readiness. It runs from the idle-loop event pump and from WriteAudio.
// Before this ring, the only buffering between the app and the air was
// the controller's handful of ACL buffers (~20 ms of audio) -- any stall
// longer than that (RF retransmission burst, app render/decode hiccup,
// scheduler delay) drained it and audibly dropped out.
int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen) {
if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) {
return -1;
void PumpMedia() {
if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) return;
bool expected = false;
if (!g_pumpActive.compare_exchange_strong(expected, true,
std::memory_order_acquire)) {
return; // someone else is pumping
}
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);
uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2;
int16_t framePcm[512];
// 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;
while (bytesPerFrame <= sizeof(framePcm) && g_pcmRate != 0) {
if (g_state != State::Streaming) break; // torn down mid-pump
uint32_t consumed = 0;
uint16_t maxOut = Hci::AclMaxPackets();
if (maxOut == 0) maxOut = 4; // controller ACL buffer credits
uint32_t fill = g_ringHead.load(std::memory_order_acquire)
- g_ringTail.load(std::memory_order_relaxed);
uint64_t now = Timekeeping::GetMilliseconds();
uint64_t audioMs = g_sentSamples * 1000 / g_pcmRate;
uint64_t elapsed = now - g_clockBase;
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);
if (fill < bytesPerFrame) {
// Ring dry. If the sink's lead is exhausted too, this is an
// audible dropout caused by the app not feeding in time.
if (!g_inUnderrun && g_sentSamples != 0 && elapsed > audioMs) {
g_inUnderrun = true;
g_underruns++;
}
break;
}
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) {
KernelLogStream(WARNING, "BT-A2DP")
<< "media credit stall (seq=" << (uint64_t)g_seqNum
<< "); resetting credits";
Hci::AclResetCredits();
continue;
}
break;
}
// Way behind schedule (app went silent without SUSPEND): rebase
// the clock instead of bursting the whole backlog at the sink.
if (elapsed > audioMs + 1000) {
g_clockBase = now - audioMs;
elapsed = audioMs;
}
// Build media packet: RTP-like header (12 bytes) + SBC payload header (1 byte) + SBC frames
uint8_t mediaPkt[256] = {};
// Pull one frame from the ring (may wrap) and apply volume.
uint32_t tail = g_ringTail.load(std::memory_order_relaxed);
uint32_t idx = tail & (PCM_RING_SIZE - 1);
uint32_t firstPart = PCM_RING_SIZE - idx;
if (firstPart > bytesPerFrame) firstPart = bytesPerFrame;
memcpy(framePcm, &g_pcmRing[idx], firstPart);
memcpy((uint8_t*)framePcm + firstPart, &g_pcmRing[0],
bytesPerFrame - firstPart);
g_ringTail.store(tail + bytesPerFrame, std::memory_order_release);
// 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;
uint32_t numSamples = samplesPerFrame * g_sbcEncoder.Channels;
for (uint32_t i = 0; i < numSamples; i++) {
framePcm[i] = (int16_t)(((int32_t)framePcm[i] * g_volume) / 100);
}
// Build media packet: RTP-like header (12 bytes) + optional SCMS-T
// content-protection header (1 byte) + SBC payload header (1 byte)
// + SBC frame
uint8_t mediaPkt[256] = {};
mediaPkt[0] = 0x80; // V=2, P=0, X=0, CC=0
mediaPkt[1] = 0x60; // M=0, PT=96
mediaPkt[2] = (uint8_t)(g_seqNum >> 8);
mediaPkt[3] = (uint8_t)(g_seqNum & 0xFF);
mediaPkt[4] = (uint8_t)(g_timestamp >> 24);
@@ -876,37 +1454,86 @@ namespace Drivers::USB::Bluetooth::A2dp {
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]);
// When SCMS-T was configured (SetConfiguration cat 0x04), every
// media packet carries a 1-byte CP header BETWEEN the RTP header
// and the SBC payload header (0x00 = copy permitted, as BlueZ).
uint32_t hdr = 12;
if (g_sinkContentProtection) mediaPkt[hdr++] = 0x00;
mediaPkt[hdr++] = 1; // SBC payload header: number of SBC frames
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();
uint32_t encodedSize = Sbc::Encode(&g_sbcEncoder, framePcm, &mediaPkt[hdr]);
L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)(hdr + encodedSize));
g_seqNum++;
g_timestamp += samplesPerFrame;
consumed += bytesPerFrame;
g_sentSamples += samplesPerFrame;
g_lastSendMs = now;
g_inUnderrun = false;
// Bring-up diagnostic: heartbeat every 2048 frames (~5.5 s of
// audio) proving the pipeline is healthy.
if ((g_seqNum & 0x7FF) == 0) {
uint32_t ringMs = (g_pcmRate && g_sbcEncoder.Channels)
? (uint32_t)((uint64_t)(fill - bytesPerFrame) * 1000
/ ((uint64_t)g_pcmRate * g_sbcEncoder.Channels * 2))
: 0;
KernelLogStream(INFO, "BT-A2DP") << "media: seq="
<< (uint64_t)g_seqNum
<< " ring=" << (uint64_t)ringMs << "ms"
<< " lead=" << (uint64_t)(audioMs > elapsed ? audioMs - elapsed : 0) << "ms"
<< " inflight=" << (uint64_t)Hci::AclTxInFlight()
<< " pending=" << (uint64_t)Hci::AclPendingCount()
<< " underruns=" << (uint64_t)g_underruns;
}
}
return (int)consumed;
g_pumpActive.store(false, std::memory_order_release);
}
// =========================================================================
// WriteAudio — accept PCM into the ring (never blocks)
// =========================================================================
int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen) {
if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) {
// The app retries every few ms on -1, so rate-limit hard; but the
// FIRST rejection must be visible -- a silently flipped state here
// is otherwise indistinguishable from the app never writing.
static uint32_t rejCount = 0;
rejCount++;
if (rejCount <= 2 || (rejCount & 0x3FF) == 0) {
KernelLogStream(WARNING, "BT-A2DP") << "WriteAudio rejected #"
<< (uint64_t)rejCount << ": sbc=" << (uint64_t)(g_sbcInitialized ? 1 : 0)
<< " state=" << (uint64_t)(int)g_state
<< " mediaCid=" << base::hex << (uint64_t)g_mediaCid << base::dec;
}
return -1;
}
// Copy into the ring, clamped to free space and aligned to whole
// stereo sample pairs. Returns the bytes accepted; 0 = ring full,
// the app retries on its next loop pass.
uint32_t head = g_ringHead.load(std::memory_order_relaxed);
uint32_t tail = g_ringTail.load(std::memory_order_acquire);
uint32_t freeBytes = PCM_RING_SIZE - (head - tail);
uint32_t n = (pcmLen < freeBytes ? pcmLen : freeBytes) & ~3u;
uint32_t idx = head & (PCM_RING_SIZE - 1);
uint32_t firstPart = PCM_RING_SIZE - idx;
if (firstPart > n) firstPart = n;
memcpy(&g_pcmRing[idx], pcmData, firstPart);
memcpy(&g_pcmRing[0], pcmData + firstPart, n - firstPart);
g_ringHead.store(head + n, std::memory_order_release);
// Reap events (NOCP credits, inbound traffic) and feed the link from
// syscall context too, so streaming keeps moving even when no core
// is idle.
Xhci::PollEvents();
Hci::DrainEvents();
PumpMedia();
return (int)n;
}
// =========================================================================