daniel
d9d32deb3e
Four fixes, each a root cause verified on hardware (AX211 + Bose QC Ultra):
1. Link Key Request Reply TRUNCATED: the pending-command queue's params
buffer was 16 bytes; the reply is 22 (addr 6 + key 16). The controller
got 10 key bytes -> every stored-key reconnection failed authentication
(status 5) since 2026-06-03 (c119a70). Fresh pairings never touch this
path, which kept the bug perfectly disguised as a headset quirk.
2. Secure Connections host support (0x0C7A) now enabled: bonds are minted
as P-256 (Type=7), interoperable with BlueZ's, and SC-bonded peers can
actually authenticate us.
3. Never write the BD_ADDR override (0xFC31) with the factory address:
it desyncs the firmware's crypto address from the on-air one and ALL
SSP pairing fails with status 5. (The spoofing feature itself was
already known-cosmetic: the baseband answers pages on the factory
address regardless.) import-bluez-bond.sh now removes the override.
4. A2DP channel setup: wait for Encryption Change before dialing L2CAP
(post-SSP sinks ignore unencrypted CONN_REQ), and LISTEN 2.5s first --
on reconnection the sink dials AVDTP itself and ignores our dials while
doing so. Ends the historical connRsp=FFFF retry-then-give-up failures.
Plus: queued security replies now log delivery + controller status.
Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
1603 lines
76 KiB
C++
1603 lines
76 KiB
C++
/*
|
|
* A2dp.cpp
|
|
* Bluetooth A2DP / AVDTP implementation
|
|
* Copyright (c) 2026 Daniel Hammer
|
|
*/
|
|
|
|
#include "A2dp.hpp"
|
|
#include "Sbc.hpp"
|
|
#include "L2cap.hpp"
|
|
#include "Hci.hpp"
|
|
#include <Drivers/USB/Xhci.hpp>
|
|
#include <Terminal/Terminal.hpp>
|
|
#include <CppLib/Stream.hpp>
|
|
#include <Libraries/Memory.hpp>
|
|
#include <Timekeeping/ApicTimer.hpp>
|
|
#include <atomic>
|
|
|
|
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;
|
|
constexpr uint8_t AVDTP_GET_ALL_CAPABILITIES = 0x0C; // AVDTP 1.3
|
|
|
|
// 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;
|
|
|
|
// 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 void ResetMediaClock() {
|
|
g_clockBase = Timekeeping::GetMilliseconds();
|
|
g_sentSamples = 0;
|
|
g_lastSendMs = g_clockBase;
|
|
}
|
|
|
|
// 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)};
|
|
|
|
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;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// =========================================================================
|
|
// 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.
|
|
// =========================================================================
|
|
// 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.
|
|
|
|
// 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).
|
|
// 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(¶ms[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);
|
|
}
|
|
}
|
|
|
|
// 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
|
|
// 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 << ")";
|
|
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));
|
|
|
|
bool answered = false;
|
|
uint64_t start = Timekeeping::GetMilliseconds();
|
|
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
|
Xhci::PollEvents();
|
|
Hci::DrainEvents();
|
|
if (g_sdpRspReady) { answered = true; break; }
|
|
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
|
}
|
|
|
|
// 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
|
|
}
|
|
|
|
// Wait for the ACL link to be ENCRYPTED before dialing any L2CAP channel.
|
|
// Post-SSP, the sink is REQUIRED to ignore unencrypted L2CAP -- dialing
|
|
// SDP/AVDTP before Encryption Change lands is a race we historically lost
|
|
// often (connRsp stays 0xFFFF on every attempt; "toggle the headphones"
|
|
// sometimes won the race by accident). Pump ProcessPendingCommands here:
|
|
// the SET_CONN_ENCRYPT that MAKES encryption happen sits in the pending
|
|
// queue and otherwise only goes out when the idle loop gets around to it.
|
|
static bool WaitEncrypted(uint32_t timeoutMs) {
|
|
uint64_t start = Timekeeping::GetMilliseconds();
|
|
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
|
Xhci::PollEvents();
|
|
Hci::DrainEvents();
|
|
Hci::ProcessPendingCommands();
|
|
auto* c = Hci::GetActiveConnection();
|
|
if (c && c->Encrypted) return true;
|
|
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
|
}
|
|
return false;
|
|
}
|
|
|
|
bool StartSource(uint32_t timeoutMs) {
|
|
constexpr int kMaxAttempts = 4;
|
|
|
|
if (WaitEncrypted(3000)) {
|
|
KernelLogStream(OK, "BT-A2DP") << "Link encrypted; dialing channels";
|
|
} else {
|
|
// Some sinks may not encrypt (legacy); proceed as before, but say so.
|
|
KernelLogStream(WARNING, "BT-A2DP")
|
|
<< "Link not encrypted after 3s; dialing anyway";
|
|
}
|
|
|
|
// Give the sink first move. On RECONNECTION (bonded device) sinks --
|
|
// Bose QC included -- commonly dial the AVDTP channels THEMSELVES
|
|
// right after encryption, and ignore the source's own CONN_REQs while
|
|
// their setup is in flight (observed: connRsp stays 0xFFFF on every
|
|
// dialed attempt, zero inbound traffic... because we never stopped
|
|
// transmitting long enough to receive). Listen briefly before
|
|
// dialing; fresh pairings are source-driven and just spend the wait.
|
|
g_sigCid = 0;
|
|
g_mediaCid = 0;
|
|
g_state = State::Idle;
|
|
g_txLabel = 1;
|
|
g_avdtpResponseReady = false;
|
|
{
|
|
uint64_t lStart = Timekeeping::GetMilliseconds();
|
|
while (Timekeeping::GetMilliseconds() - lStart < 2500) {
|
|
Xhci::PollEvents();
|
|
Hci::DrainEvents();
|
|
Hci::ProcessPendingCommands();
|
|
if (g_sigCid != 0) break;
|
|
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
|
}
|
|
}
|
|
if (g_sigCid != 0) {
|
|
KernelLogStream(OK, "BT-A2DP") << "Sink opened AVDTP to us (cid="
|
|
<< base::hex << (uint64_t)g_sigCid << base::dec << ")";
|
|
}
|
|
|
|
// Connection phase, retried. A sink commonly ignores the very first
|
|
// L2CAP CONN_REQ that lands right after Encryption Change (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.
|
|
// Dial only while no signaling channel exists in either direction;
|
|
// an inbound one (from the listen phase above, or landing between
|
|
// retries) short-circuits straight to negotiation.
|
|
for (int attempt = 0; attempt < kMaxAttempts && g_sigCid == 0; attempt++) {
|
|
g_mediaCid = 0;
|
|
g_state = State::Idle;
|
|
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 << ")";
|
|
|
|
// 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) {
|
|
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";
|
|
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 only churns CIDs and
|
|
// abandons the very connection the sink is authorizing; a single held
|
|
// dial is harmless even when the sink takes seconds to authorize.
|
|
// Also accept an inbound transport channel (some sinks open it).
|
|
// 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;
|
|
|
|
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 << ")";
|
|
return false;
|
|
}
|
|
|
|
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. Dialed before the media
|
|
// channel, the sink answers PENDING and never completes it, leaving a
|
|
// half-open dial parked 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;
|
|
}
|
|
|
|
// =========================================================================
|
|
// 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:
|
|
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;
|
|
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, signalId, 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;
|
|
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: {
|
|
// 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;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// =========================================================================
|
|
// 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;
|
|
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 "
|
|
<< (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;
|
|
}
|
|
if (!AvdtpStart()) return false;
|
|
ResetMediaClock();
|
|
return true;
|
|
}
|
|
return (g_state == State::Streaming);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
// =========================================================================
|
|
// 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.
|
|
|
|
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;
|
|
int16_t framePcm[512];
|
|
|
|
// Bundle as many SBC frames as fit in the media channel's MTU into
|
|
// each RTP packet. One frame per packet means 375 packets/s at
|
|
// 48 kHz -- each costing a 2-DH3 + ACK (~2.5 ms) on air, ~94% of the
|
|
// radio's airtime. With zero headroom, any retransmission burst,
|
|
// WiFi coexistence window (combo chip), or multipoint service to the
|
|
// phone delays frames past the sink's deadline -> concealment static
|
|
// and dropouts. Bundling to the MTU (5 frames at bitpool 53) cuts
|
|
// this to ~75 packets/s, ~28% airtime, like every stock stack does.
|
|
uint16_t maxPayload = 672; // default L2CAP MTU; never exceed buffer
|
|
if (auto* mch = L2cap::GetChannel(g_mediaCid)) {
|
|
if (mch->RemoteMtu >= 48 && mch->RemoteMtu < maxPayload) {
|
|
maxPayload = mch->RemoteMtu;
|
|
}
|
|
}
|
|
|
|
while (bytesPerFrame <= sizeof(framePcm) && g_pcmRate != 0) {
|
|
if (g_state != State::Streaming) break; // torn down mid-pump
|
|
|
|
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;
|
|
|
|
if (fill < bytesPerFrame) break; // ring dry, nothing to send
|
|
if (audioMs >= elapsed + LEAD_MS) break; // sink lead is full
|
|
if (!Hci::AclTxReady()) {
|
|
// Normal credit pacing most of the time. A credit pool stuck
|
|
// for 250+ ms with the USB side fully drained means NOCP
|
|
// events were lost -- reset and carry on.
|
|
if (now - g_lastSendMs > 250 && Hci::AclTxInFlight() == 0) {
|
|
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) + optional SCMS-T
|
|
// content-protection header (1 byte) + SBC payload header (1 byte)
|
|
// + up to 15 SBC frames, MTU permitting
|
|
uint8_t mediaPkt[768] = {};
|
|
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);
|
|
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
|
|
|
|
// 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;
|
|
uint32_t sbcHdrPos = hdr++; // SBC payload header: frame count
|
|
|
|
// Pull frames from the ring (may wrap), apply volume, encode.
|
|
// Frame size is constant for a fixed config; the size of the
|
|
// first encode bounds whether the next one still fits.
|
|
uint32_t off = hdr;
|
|
uint32_t nFrames = 0;
|
|
uint32_t frameLen = 0;
|
|
while (nFrames < 15) {
|
|
if (frameLen != 0 && off + frameLen > maxPayload) break;
|
|
uint32_t avail = g_ringHead.load(std::memory_order_acquire)
|
|
- g_ringTail.load(std::memory_order_relaxed);
|
|
if (avail < bytesPerFrame) break;
|
|
|
|
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);
|
|
|
|
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);
|
|
}
|
|
|
|
frameLen = Sbc::Encode(&g_sbcEncoder, framePcm, &mediaPkt[off]);
|
|
off += frameLen;
|
|
nFrames++;
|
|
}
|
|
if (nFrames == 0) break;
|
|
mediaPkt[sbcHdrPos] = (uint8_t)nFrames;
|
|
|
|
L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)off);
|
|
|
|
g_seqNum++;
|
|
g_timestamp += samplesPerFrame * nFrames;
|
|
g_sentSamples += samplesPerFrame * nFrames;
|
|
g_lastSendMs = now;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
// =========================================================================
|
|
// 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;
|
|
}
|
|
|
|
}
|