feat: Intel BT firmware download, A2dp & Bluetooth audio progress
This commit is contained in:
@@ -70,10 +70,41 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
static uint8_t g_remoteSeid = 0; // Remote stream endpoint ID
|
||||
static uint8_t g_localSeid = 1; // Our local SEID
|
||||
|
||||
// Audio sink endpoints advertised by the remote (AVDTP Discover). A modern
|
||||
// headset typically exposes several SEPs -- one per codec (SBC, AAC, aptX,
|
||||
// ...). Each SEP carries exactly ONE media codec, so codec choice is
|
||||
// endpoint choice. We must probe each with GetCapabilities and configure the
|
||||
// one that offers SBC (the codec our encoder produces). The old code grabbed
|
||||
// the FIRST audio sink and committed to it; on Bose that is the AAC endpoint,
|
||||
// so the subsequent SBC SetConfiguration was rejected (cat=1 err=0x29,
|
||||
// UNSUPPORTED_CONFIGURATION) and music never played.
|
||||
static uint8_t g_sinkSeids[16] = {};
|
||||
static uint32_t g_numSinkSeids = 0;
|
||||
|
||||
// The transaction label + signal id of the command we are currently waiting
|
||||
// on a response for. ProcessAvdtp only accepts a response that matches both,
|
||||
// so the headset's own AVDTP traffic (it opens channels and issues its own
|
||||
// commands) cannot be mistaken for our reply and desync the handshake.
|
||||
static uint8_t g_expectLabel = 0xFF;
|
||||
static uint8_t g_expectSignal = 0xFF;
|
||||
|
||||
// SBC encoder
|
||||
static Sbc::SbcEncoder g_sbcEncoder = {};
|
||||
static bool g_sbcInitialized = false;
|
||||
|
||||
// SBC capability negotiation. An A2DP source must SetConfiguration with a
|
||||
// subset of what the sink advertised in GetCapabilities -- asserting a fixed
|
||||
// config the sink didn't offer gets it rejected with UNSUPPORTED_CONFIGURATION
|
||||
// (0x29), the observed Bose behaviour. We parse the sink's SBC capability and
|
||||
// pick a supported config; g_cfgSbc is what we actually configured (used to
|
||||
// set up the encoder so the frame headers match).
|
||||
static uint8_t g_sinkSbcCaps[4] = {}; // advertised: [freq|mode][blk|sub|alloc][minBP][maxBP]
|
||||
static bool g_haveSinkSbcCaps = false;
|
||||
static bool g_sinkDelayReporting = false; // sink advertised Delay Reporting (cat 0x08)
|
||||
static bool g_sinkContentProtection = false; // sink advertised Content Protection (cat 0x04)
|
||||
static uint8_t g_sinkCpType[2] = {}; // advertised CP_TYPE (LSB,MSB); SCMS-T = {0x02,0x00}
|
||||
static uint8_t g_cfgSbc[4] = {}; // the SBC octets we configured
|
||||
|
||||
// Media packet state
|
||||
static uint16_t g_seqNum = 0;
|
||||
static uint32_t g_timestamp = 0;
|
||||
@@ -86,6 +117,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
static uint8_t g_avdtpResponseBuf[128] = {};
|
||||
static uint32_t g_avdtpResponseLen = 0;
|
||||
|
||||
// SDP (service discovery) state. Many A2DP sinks refuse to engage AVDTP
|
||||
// until the source has queried their service record, so we do a minimal SDP
|
||||
// ServiceSearchAttribute query for the AudioSink service first.
|
||||
static uint16_t g_sdpCid = 0;
|
||||
static volatile bool g_sdpRspReady = false;
|
||||
|
||||
// =========================================================================
|
||||
// AVDTP signaling helpers
|
||||
// =========================================================================
|
||||
@@ -94,7 +131,8 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
uint8_t buf[128] = {};
|
||||
|
||||
// AVDTP single packet header
|
||||
buf[0] = (g_txLabel << 4) | (PKT_SINGLE << 2) | MSG_COMMAND;
|
||||
uint8_t lbl = g_txLabel;
|
||||
buf[0] = (lbl << 4) | (PKT_SINGLE << 2) | MSG_COMMAND;
|
||||
buf[1] = signalId;
|
||||
g_txLabel = (g_txLabel + 1) & 0x0F;
|
||||
|
||||
@@ -102,6 +140,13 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
memcpy(&buf[2], payload, len);
|
||||
}
|
||||
|
||||
// Record what we're waiting for and discard any stale response, so only
|
||||
// the matching reply satisfies WaitAvdtpResponse (see g_expectLabel).
|
||||
g_expectLabel = lbl;
|
||||
g_expectSignal = signalId;
|
||||
g_avdtpResponseReady = false;
|
||||
g_avdtpResponseLen = 0;
|
||||
|
||||
L2cap::SendData(g_sigCid, buf, 2 + len);
|
||||
}
|
||||
|
||||
@@ -129,6 +174,7 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents(); // AVDTP responses arrive as ACL data
|
||||
if (g_avdtpResponseReady) return true;
|
||||
for (int j = 0; j < 100; j++) {
|
||||
asm volatile("" ::: "memory");
|
||||
@@ -137,6 +183,14 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
return false;
|
||||
}
|
||||
|
||||
// WaitAvdtpResponse() returns true for ACCEPT *and* REJECT (the msgType is in
|
||||
// the low 2 bits of byte 0). This distinguishes them so a rejected
|
||||
// configuration is surfaced instead of silently treated as success.
|
||||
static bool AvdtpAccepted() {
|
||||
return g_avdtpResponseLen >= 1 &&
|
||||
(g_avdtpResponseBuf[0] & 0x03) == MSG_RESPONSE_ACCEPT;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// AVDTP signaling procedures
|
||||
// =========================================================================
|
||||
@@ -148,67 +202,185 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover timeout";
|
||||
return false;
|
||||
}
|
||||
if (!AvdtpAccepted()) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover rejected";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Parse discover response to find audio sink SEID
|
||||
// Response format: each SEP is 2 bytes:
|
||||
// Byte 0: SEID(6) | InUse(1) | Rsvd(1)
|
||||
// Byte 1: MediaType(4) | SEPType(4) (SEPType: 0=Source, 1=Sink)
|
||||
// Parse discover response to enumerate audio sink SEIDs. Each SEP is 2
|
||||
// bytes:
|
||||
// Byte 0: ACP SEID (bits 7-2) | In Use (bit 1) | RFA (bit 0)
|
||||
// Byte 1: Media Type (bits 7-4) | TSEP (bit 3) | RFA (bits 2-0)
|
||||
// TSEP (the Stream End Point type) is BIT 3: 0=Source (SRC), 1=Sink (SNK)
|
||||
// -- it is NOT the low nibble. The old `& 0x0F` read RFA bits too, so an
|
||||
// audio sink (byte1 = 0x08: Audio<<4 | SNK<<3) computed 0x08 != 0x01 and
|
||||
// was rejected -> "No audio sink SEP found", an earlier HW symptom.
|
||||
//
|
||||
// Collect EVERY usable audio sink (the remote exposes one per codec);
|
||||
// StartSource then probes each for SBC rather than committing to the
|
||||
// first, which on Bose is the AAC endpoint.
|
||||
g_numSinkSeids = 0;
|
||||
if (g_avdtpResponseLen >= 4) {
|
||||
for (uint32_t i = 2; i + 1 < g_avdtpResponseLen; i += 2) {
|
||||
uint8_t seid = (g_avdtpResponseBuf[i] >> 2) & 0x3F;
|
||||
bool inUse = (g_avdtpResponseBuf[i] >> 1) & 1;
|
||||
uint8_t mediaType = (g_avdtpResponseBuf[i + 1] >> 4) & 0x0F;
|
||||
uint8_t sepType = g_avdtpResponseBuf[i + 1] & 0x0F;
|
||||
uint8_t sepType = (g_avdtpResponseBuf[i + 1] >> 3) & 0x01; // TSEP: 1=Sink
|
||||
|
||||
if (mediaType == MEDIA_AUDIO && sepType == 0x01 && !inUse) {
|
||||
g_remoteSeid = seid;
|
||||
KernelLogStream(INFO, "BT-A2DP") << "Found audio sink SEID="
|
||||
<< (uint64_t)seid;
|
||||
return true;
|
||||
if (g_numSinkSeids < (sizeof(g_sinkSeids) / sizeof(g_sinkSeids[0]))) {
|
||||
g_sinkSeids[g_numSinkSeids++] = seid;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "No audio sink SEP found";
|
||||
return false;
|
||||
if (g_numSinkSeids == 0) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "No audio sink SEP found";
|
||||
return false;
|
||||
}
|
||||
|
||||
KernelLogStream cl(INFO, "BT-A2DP");
|
||||
cl << "Found " << (uint64_t)g_numSinkSeids << " audio sink SEID(s):";
|
||||
for (uint32_t i = 0; i < g_numSinkSeids; i++) cl << " " << (uint64_t)g_sinkSeids[i];
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool AvdtpGetCapabilities() {
|
||||
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
||||
// Pick a single capability bit: the first preference (MSB of the list first)
|
||||
// that the sink advertises in `avail`; fall back to the lowest set bit.
|
||||
static uint8_t PickBit(uint8_t avail, const uint8_t* pref, int n) {
|
||||
for (int i = 0; i < n; i++) if (avail & pref[i]) return pref[i];
|
||||
for (int b = 0; b < 8; b++) if (avail & (1u << b)) return (uint8_t)(1u << b);
|
||||
return 0;
|
||||
}
|
||||
|
||||
static bool AvdtpGetCapabilities(uint8_t seid) {
|
||||
// Clear the per-endpoint capability state up front, BEFORE any early
|
||||
// return, so a probe that times out or is rejected leaves no stale SBC
|
||||
// caps behind for the next endpoint in the probe loop or a later
|
||||
// StartSource() reconnect/retry to misread.
|
||||
g_haveSinkSbcCaps = false;
|
||||
g_sinkDelayReporting = false;
|
||||
g_sinkContentProtection = false;
|
||||
g_sinkCpType[0] = g_sinkCpType[1] = 0;
|
||||
g_sinkSbcCaps[0] = g_sinkSbcCaps[1] = g_sinkSbcCaps[2] = g_sinkSbcCaps[3] = 0;
|
||||
|
||||
uint8_t payload[1] = {(uint8_t)(seid << 2)};
|
||||
SendAvdtpCommand(AVDTP_GET_CAPABILITIES, payload, 1);
|
||||
return WaitAvdtpResponse();
|
||||
if (!WaitAvdtpResponse()) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP GetCapabilities timeout (SEID="
|
||||
<< (uint64_t)seid << ")";
|
||||
return false;
|
||||
}
|
||||
if (!AvdtpAccepted()) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP GetCapabilities rejected (SEID="
|
||||
<< (uint64_t)seid << ")";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Parse the advertised service capabilities so SetConfiguration offers
|
||||
// only a subset the sink actually supports. Capabilities follow the
|
||||
// AVDTP header (bytes 0-1): [category][LOSC][content...] repeated.
|
||||
// (Capability state was already cleared at function entry.)
|
||||
KernelLogStream cl(INFO, "BT-A2DP");
|
||||
cl << "GetCap SEID=" << (uint64_t)seid << " cats:" << base::hex;
|
||||
uint32_t off = 2;
|
||||
while (off + 2 <= g_avdtpResponseLen) {
|
||||
uint8_t cat = g_avdtpResponseBuf[off];
|
||||
uint8_t losc = g_avdtpResponseBuf[off + 1];
|
||||
cl << " " << (uint64_t)cat << "/" << (uint64_t)losc;
|
||||
if (off + 2 + (uint32_t)losc > g_avdtpResponseLen) break;
|
||||
const uint8_t* content = &g_avdtpResponseBuf[off + 2];
|
||||
if (cat == 0x08) g_sinkDelayReporting = true; // Delay Reporting
|
||||
if (cat == 0x04 && losc >= 2) { // Content Protection (e.g. SCMS-T)
|
||||
g_sinkContentProtection = true;
|
||||
g_sinkCpType[0] = content[0];
|
||||
g_sinkCpType[1] = content[1];
|
||||
cl << " [CP " << (uint64_t)content[0] << " " << (uint64_t)content[1] << "]";
|
||||
}
|
||||
if (cat == CAT_MEDIA_CODEC && losc >= 6 && content[1] == CODEC_SBC) {
|
||||
g_sinkSbcCaps[0] = content[2];
|
||||
g_sinkSbcCaps[1] = content[3];
|
||||
g_sinkSbcCaps[2] = content[4];
|
||||
g_sinkSbcCaps[3] = content[5];
|
||||
g_haveSinkSbcCaps = true;
|
||||
cl << " [SBC " << (uint64_t)content[2] << " " << (uint64_t)content[3]
|
||||
<< " " << (uint64_t)content[4] << " " << (uint64_t)content[5] << "]";
|
||||
}
|
||||
off += 2 + losc;
|
||||
}
|
||||
cl << base::dec;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool AvdtpSetConfiguration() {
|
||||
// Set Configuration payload:
|
||||
// ACP SEID (1 byte) | INT SEID (1 byte) | Service Capabilities...
|
||||
uint8_t payload[12] = {};
|
||||
payload[0] = (g_remoteSeid << 2); // ACP SEID
|
||||
payload[1] = (g_localSeid << 2); // INT SEID
|
||||
// Choose an SBC configuration that is a SUBSET of the sink's advertised
|
||||
// capabilities (each field exactly one bit). Asserting unsupported
|
||||
// values gets UNSUPPORTED_CONFIGURATION (0x29).
|
||||
uint8_t oct0, oct1, minBP, maxBP;
|
||||
if (g_haveSinkSbcCaps) {
|
||||
static const uint8_t freqPref[4] = {0x10, 0x20, 0x40, 0x80}; // 48,44.1,32,16 kHz
|
||||
static const uint8_t modePref[4] = {0x01, 0x02, 0x04, 0x08}; // Joint,Stereo,Dual,Mono
|
||||
static const uint8_t blkPref[4] = {0x10, 0x20, 0x40, 0x80}; // 16,12,8,4 blocks
|
||||
static const uint8_t subPref[2] = {0x04, 0x08}; // 8,4 subbands
|
||||
static const uint8_t allocPref[2] = {0x01, 0x02}; // Loudness,SNR
|
||||
oct0 = PickBit(g_sinkSbcCaps[0] & 0xF0, freqPref, 4)
|
||||
| PickBit(g_sinkSbcCaps[0] & 0x0F, modePref, 4);
|
||||
oct1 = PickBit(g_sinkSbcCaps[1] & 0xF0, blkPref, 4)
|
||||
| PickBit(g_sinkSbcCaps[1] & 0x0C, subPref, 2)
|
||||
| PickBit(g_sinkSbcCaps[1] & 0x03, allocPref, 2);
|
||||
minBP = g_sinkSbcCaps[2] < 2 ? 2 : g_sinkSbcCaps[2];
|
||||
maxBP = g_sinkSbcCaps[3] > 53 ? 53 : g_sinkSbcCaps[3]; // cap to our quality target
|
||||
if (maxBP < minBP) maxBP = minBP;
|
||||
} else {
|
||||
oct0 = 0x11; oct1 = 0x15; minBP = 2; maxBP = 53; // mandatory SBC baseline
|
||||
}
|
||||
g_cfgSbc[0] = oct0; g_cfgSbc[1] = oct1; g_cfgSbc[2] = minBP; g_cfgSbc[3] = maxBP;
|
||||
|
||||
// Media Transport capability (no data)
|
||||
payload[2] = CAT_MEDIA_TRANSPORT; // Category
|
||||
payload[3] = 0; // Length
|
||||
// Set Configuration: ACP SEID | INT SEID | Service Capabilities.
|
||||
uint8_t payload[24] = {};
|
||||
int n = 0;
|
||||
payload[n++] = (g_remoteSeid << 2); // ACP SEID
|
||||
payload[n++] = (g_localSeid << 2); // INT SEID
|
||||
payload[n++] = CAT_MEDIA_TRANSPORT; payload[n++] = 0;
|
||||
payload[n++] = CAT_MEDIA_CODEC; payload[n++] = 6;
|
||||
payload[n++] = (MEDIA_AUDIO << 4); // Media Type (audio)
|
||||
payload[n++] = CODEC_SBC; // Codec Type (SBC)
|
||||
payload[n++] = oct0; payload[n++] = oct1; payload[n++] = minBP; payload[n++] = maxBP;
|
||||
// If the sink advertised Content Protection (cat 0x04), configure SCMS-T
|
||||
// (CP_TYPE 0x0002). Bose QC sinks advertise it and gate the media
|
||||
// transport channel on it: SetConfiguration is accepted without it, but
|
||||
// the sink then never authorizes the transport L2CAP channel (it answers
|
||||
// the transport CONN_REQ with a perpetual PENDING). CP_TYPE is 2 bytes,
|
||||
// LSB first; SCMS-T carries no extra CP-type-specific data (LOSC=2).
|
||||
if (g_sinkContentProtection) {
|
||||
payload[n++] = 0x04; payload[n++] = 2; // Content Protection, LOSC=2
|
||||
payload[n++] = g_sinkCpType[0]; // CP_TYPE LSB (SCMS-T = 0x02)
|
||||
payload[n++] = g_sinkCpType[1]; // CP_TYPE MSB (SCMS-T = 0x00)
|
||||
}
|
||||
// If the sink advertised Delay Reporting, configure it too -- some sinks
|
||||
// reject SetConfiguration that omits a category they require.
|
||||
if (g_sinkDelayReporting) { payload[n++] = 0x08; payload[n++] = 0; }
|
||||
|
||||
// Media Codec capability (SBC)
|
||||
payload[4] = CAT_MEDIA_CODEC; // Category
|
||||
payload[5] = 6; // Length
|
||||
payload[6] = (MEDIA_AUDIO << 4); // Media Type
|
||||
payload[7] = CODEC_SBC; // Codec Type
|
||||
// SBC codec info (4 bytes)
|
||||
// Sampling: 44.1kHz (bit 5), Channel mode: Joint Stereo (bit 0)
|
||||
payload[8] = 0x21; // 44.1kHz | Joint Stereo
|
||||
// Block length: 16 (bit 7), Subbands: 8 (bit 1), Alloc: Loudness (bit 0)
|
||||
payload[9] = 0x83; // 16 blocks | 8 subbands | Loudness
|
||||
payload[10] = 2; // Min bitpool
|
||||
payload[11] = 53; // Max bitpool
|
||||
KernelLogStream(INFO, "BT-A2DP") << "SetConfig SBC oct0=" << base::hex << (uint64_t)oct0
|
||||
<< " oct1=" << (uint64_t)oct1 << base::dec << " bp=" << (uint64_t)minBP << ".."
|
||||
<< (uint64_t)maxBP << (g_sinkContentProtection ? " +scms-t" : "")
|
||||
<< (g_sinkDelayReporting ? " +delay" : "");
|
||||
|
||||
SendAvdtpCommand(AVDTP_SET_CONFIGURATION, payload, 12);
|
||||
SendAvdtpCommand(AVDTP_SET_CONFIGURATION, payload, (uint16_t)n);
|
||||
|
||||
if (!WaitAvdtpResponse()) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP SetConfiguration timeout";
|
||||
return false;
|
||||
}
|
||||
if (!AvdtpAccepted()) {
|
||||
// Reject payload: [failing service category][error code]
|
||||
uint8_t cat = (g_avdtpResponseLen > 2) ? g_avdtpResponseBuf[2] : 0;
|
||||
uint8_t err = (g_avdtpResponseLen > 3) ? g_avdtpResponseBuf[3] : 0;
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP SetConfiguration rejected (cat="
|
||||
<< base::hex << (uint64_t)cat << " err=" << (uint64_t)err << base::dec << ")";
|
||||
return false;
|
||||
}
|
||||
|
||||
g_state = State::Configured;
|
||||
KernelLogStream(OK, "BT-A2DP") << "Stream configured";
|
||||
@@ -223,6 +395,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Open timeout";
|
||||
return false;
|
||||
}
|
||||
if (!AvdtpAccepted()) {
|
||||
uint8_t err = (g_avdtpResponseLen > 2) ? g_avdtpResponseBuf[2] : 0;
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Open rejected (err="
|
||||
<< base::hex << (uint64_t)err << base::dec << ")";
|
||||
return false;
|
||||
}
|
||||
|
||||
g_state = State::Open;
|
||||
KernelLogStream(OK, "BT-A2DP") << "Stream opened";
|
||||
@@ -237,6 +415,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start timeout";
|
||||
return false;
|
||||
}
|
||||
if (!AvdtpAccepted()) {
|
||||
uint8_t err = (g_avdtpResponseLen > 2) ? g_avdtpResponseBuf[2] : 0;
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start rejected (err="
|
||||
<< base::hex << (uint64_t)err << base::dec << ")";
|
||||
return false;
|
||||
}
|
||||
|
||||
g_state = State::Streaming;
|
||||
KernelLogStream(OK, "BT-A2DP") << "Streaming started";
|
||||
@@ -248,26 +432,248 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
// =========================================================================
|
||||
|
||||
void OnChannelReady(uint16_t l2capCid) {
|
||||
// Invoked from L2CAP inside the ACL receive path -- i.e. NESTED under
|
||||
// Xhci::PollEvents. PollEvents is non-reentrant (re-entry guard), so we
|
||||
// must NOT run the blocking AVDTP signaling chain here; a nested poll
|
||||
// would stall and every WaitAvdtpResponse() would time out. Just record
|
||||
// the channel; StartSource() drives the handshakes from top-level
|
||||
// (process) context where polling is free to run.
|
||||
if (g_sigCid == 0) {
|
||||
// First AVDTP channel is signaling
|
||||
g_sigCid = l2capCid;
|
||||
KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready: CID="
|
||||
<< (uint64_t)l2capCid;
|
||||
|
||||
// Auto-discover remote SEPs
|
||||
g_state = State::Discovering;
|
||||
if (AvdtpDiscover()) {
|
||||
AvdtpGetCapabilities();
|
||||
AvdtpSetConfiguration();
|
||||
}
|
||||
} else if (g_mediaCid == 0) {
|
||||
// Second AVDTP channel is media transport
|
||||
g_mediaCid = l2capCid;
|
||||
KernelLogStream(OK, "BT-A2DP") << "AVDTP media channel ready: CID="
|
||||
<< (uint64_t)l2capCid;
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// StartSource — drive the A2DP source role after the ACL link is up
|
||||
// =========================================================================
|
||||
// Runs at top-level (process) context, NOT nested under PollEvents, so the
|
||||
// blocking AVDTP handshakes are safe. Opens the AVDTP signaling channel,
|
||||
// negotiates an SBC stream (Discover -> GetCapabilities -> SetConfiguration
|
||||
// -> Open), then opens the media transport channel. Leaves the stream in
|
||||
// the Open state; the first audio write (StartStream) issues AVDTP_START.
|
||||
// Without this the headset has no media stream and terminates the link
|
||||
// (HCI disconnect reason 0x13), which is the connect/disconnect flapping.
|
||||
// Called by L2CAP when data arrives on the SDP channel (the query response).
|
||||
void ProcessSdp(const uint8_t* data, uint16_t len) {
|
||||
(void)data;
|
||||
if (len > 0) g_sdpRspReady = true;
|
||||
}
|
||||
|
||||
// Minimal SDP: open PSM 0x0001, send a ServiceSearchAttributeRequest for the
|
||||
// AudioSink service (UUID 0x110B), wait briefly for the response. Best
|
||||
// effort -- the goal is to satisfy sinks that gate AVDTP on a prior SDP
|
||||
// query. Returns true if the SDP channel configured (i.e. ACL data flows).
|
||||
static bool DoSdpQuery(uint32_t timeoutMs) {
|
||||
g_sdpCid = 0;
|
||||
g_sdpRspReady = false;
|
||||
|
||||
uint16_t cid = L2cap::Connect(L2cap::PSM_SDP);
|
||||
if (!cid || !L2cap::WaitConfigured(cid, timeoutMs)) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "SDP channel setup failed (connRsp="
|
||||
<< base::hex << (uint64_t)L2cap::LastConnRspResult() << base::dec << ")";
|
||||
Hci::DumpAclStats(); // did our CONN_REQ even go out / any reply arrive?
|
||||
return false;
|
||||
}
|
||||
g_sdpCid = cid;
|
||||
|
||||
// ServiceSearchAttributeRequest for AudioSink (0x110B), all attributes.
|
||||
uint8_t pdu[20] = {
|
||||
0x06, // PDU ID: ServiceSearchAttributeRequest
|
||||
0x00, 0x01, // Transaction ID
|
||||
0x00, 0x0F, // Parameter length = 15
|
||||
0x35, 0x03, // ServiceSearchPattern: DES, 3 bytes
|
||||
0x19, 0x11, 0x0B, // UUID16 0x110B (AudioSink)
|
||||
0xFF, 0xFF, // MaximumAttributeByteCount
|
||||
0x35, 0x05, // AttributeIDList: DES, 5 bytes
|
||||
0x0A, 0x00, 0x00, 0xFF, 0xFF, // UINT32 range 0x0000-0xFFFF
|
||||
0x00 // ContinuationState (none)
|
||||
};
|
||||
L2cap::SendData(cid, pdu, sizeof(pdu));
|
||||
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
if (g_sdpRspReady) {
|
||||
KernelLogStream(OK, "BT-A2DP") << "SDP query answered";
|
||||
return true;
|
||||
}
|
||||
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
||||
}
|
||||
KernelLogStream(INFO, "BT-A2DP") << "SDP query sent, no response (continuing)";
|
||||
return true; // channel configured + query sent; proceed to AVDTP
|
||||
}
|
||||
|
||||
bool StartSource(uint32_t timeoutMs) {
|
||||
constexpr int kMaxAttempts = 4;
|
||||
|
||||
// Connection phase, retried. A sink commonly ignores the very first
|
||||
// L2CAP CONN_REQ that lands right after Encryption Change (the build-39
|
||||
// HW symptom: connRsp stays 0xFFFF, the headset never answers). Re-dial
|
||||
// up to kMaxAttempts. We retry ONLY when the remote ignored us
|
||||
// (connRsp==0xFFFF -> our dialed channel has RemoteCid==0, so freeing it
|
||||
// owes the peer no Disconnect); if it answered but config stalled
|
||||
// (connRsp!=0xFFFF) we do NOT loop -- that's the config phase, surfaced
|
||||
// by its own logs. We never reset the CID allocator, so each retry uses
|
||||
// a fresh local CID and a late response for an old one cannot cross-wire.
|
||||
for (int attempt = 0; attempt < kMaxAttempts; attempt++) {
|
||||
g_sigCid = 0;
|
||||
g_mediaCid = 0;
|
||||
g_state = State::Idle;
|
||||
g_txLabel = 1;
|
||||
g_avdtpResponseReady = false;
|
||||
|
||||
// SDP service query: best effort, FIRST attempt only -- some sinks
|
||||
// gate AVDTP on a prior SDP query; repeating it on retries only burns
|
||||
// a channel slot and 2s with no benefit.
|
||||
if (attempt == 0) DoSdpQuery(2000);
|
||||
|
||||
// AVDTP signaling channel (PSM 0x0019). Dial out, then wait for a
|
||||
// channel to become ready in EITHER direction (OnChannelReady sets
|
||||
// g_sigCid for ours, or one the headset opened to us).
|
||||
uint16_t sig = L2cap::Connect(L2cap::PSM_AVDTP);
|
||||
KernelLogStream(INFO, "BT-A2DP") << "AVDTP signaling: attempt "
|
||||
<< (uint64_t)(attempt + 1) << "/" << (uint64_t)kMaxAttempts
|
||||
<< " dialed cid=" << base::hex << (uint64_t)sig << base::dec
|
||||
<< " (acl=" << (uint64_t)L2cap::GetAclHandle() << "), waiting...";
|
||||
|
||||
uint64_t sigStart = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - sigStart < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
if (g_sigCid != 0) break;
|
||||
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
||||
}
|
||||
if (g_sigCid != 0) break; // signaling channel up -> proceed
|
||||
|
||||
auto* ch = sig ? L2cap::GetChannel(sig) : nullptr;
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP signaling attempt "
|
||||
<< (uint64_t)(attempt + 1) << " TIMEOUT (remoteCid="
|
||||
<< base::hex << (uint64_t)(ch ? ch->RemoteCid : 0) << base::dec
|
||||
<< " localCfg=" << (uint64_t)(ch ? ch->LocalConfigDone : 0)
|
||||
<< " remoteCfg=" << (uint64_t)(ch ? ch->RemoteConfigDone : 0)
|
||||
<< " connRsp=" << base::hex << (uint64_t)L2cap::LastConnRspResult()
|
||||
<< base::dec << " incomingReqs="
|
||||
<< (uint64_t)L2cap::IncomingAvdtpReqCount() << ")";
|
||||
|
||||
// Retry only if the remote IGNORED our CONN_REQ entirely. If it
|
||||
// answered (connRsp != 0xFFFF) the stall is in the config exchange,
|
||||
// not the dial -- don't loop; let the config-phase logs speak.
|
||||
if (L2cap::LastConnRspResult() != 0xFFFF) {
|
||||
Hci::DumpAclStats();
|
||||
return false;
|
||||
}
|
||||
// Free our unanswered dialed channel so repeated retries don't leak
|
||||
// the fixed channel table, then settle (draining events, so the RX
|
||||
// ring keeps being serviced) before re-dialing.
|
||||
if (sig) L2cap::FreeChannel(sig);
|
||||
uint64_t st = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - st < 400) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
for (int j = 0; j < 200; j++) asm volatile("pause" ::: "memory");
|
||||
}
|
||||
}
|
||||
|
||||
if (g_sigCid == 0) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP signaling setup gave up after retries";
|
||||
Hci::DumpAclStats();
|
||||
return false;
|
||||
}
|
||||
KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready, cid="
|
||||
<< base::hex << (uint64_t)g_sigCid << base::dec;
|
||||
|
||||
// 2. Negotiate the SBC stream (top-level: polling is free to run).
|
||||
g_state = State::Discovering;
|
||||
if (!AvdtpDiscover()) return false;
|
||||
|
||||
// Probe each advertised audio sink and configure the first that offers
|
||||
// SBC. Each SEP carries a single codec, so we cannot assume the first
|
||||
// sink is SBC -- on Bose it is AAC, and configuring SBC against it is
|
||||
// rejected (UNSUPPORTED_CONFIGURATION). SBC is mandatory for any A2DP
|
||||
// sink, so a usable endpoint is always present once we look past the
|
||||
// first. AvdtpGetCapabilities sets g_haveSinkSbcCaps when the probed
|
||||
// SEID advertises SBC; we keep that endpoint's caps for SetConfiguration.
|
||||
bool pickedSbc = false;
|
||||
for (uint32_t i = 0; i < g_numSinkSeids; i++) {
|
||||
if (!AvdtpGetCapabilities(g_sinkSeids[i])) continue; // skip endpoints that error
|
||||
if (g_haveSinkSbcCaps) {
|
||||
g_remoteSeid = g_sinkSeids[i];
|
||||
pickedSbc = true;
|
||||
KernelLogStream(OK, "BT-A2DP") << "Selected SBC sink SEID="
|
||||
<< (uint64_t)g_remoteSeid;
|
||||
break;
|
||||
}
|
||||
KernelLogStream(INFO, "BT-A2DP") << "SEID=" << (uint64_t)g_sinkSeids[i]
|
||||
<< " is not SBC, trying next";
|
||||
}
|
||||
if (!pickedSbc) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "No SBC-capable sink endpoint found";
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!AvdtpSetConfiguration()) return false; // -> Configured
|
||||
|
||||
// 3. Open the stream endpoint.
|
||||
if (!AvdtpOpen()) return false; // -> Open
|
||||
|
||||
// 4. Media transport channel: a SECOND PSM 0x0019 L2CAP channel the AVDTP
|
||||
// initiator opens after AVDTP_OPEN. Dial ONCE, immediately (inside the
|
||||
// sink's open-acceptor window), and HOLD that same channel while
|
||||
// polling. A CONN_RSP result=1 (PENDING) is NON-FINAL: a conformant
|
||||
// sink follows it with SUCCESS/REFUSED on the SAME channel, so we must
|
||||
// NOT tear it down and re-dial -- doing so (build 47) only churned CIDs
|
||||
// and abandoned the very connection the sink was authorizing, while a
|
||||
// single held dial (build 44) already proved holding alone is harmless.
|
||||
// Also accept an inbound transport channel (some sinks open it).
|
||||
// NOTE: the real gate on Bose is Content Protection -- see
|
||||
// AvdtpSetConfiguration's SCMS-T handling; without it the sink pends
|
||||
// this channel forever (connRsp=1, remoteCid=0).
|
||||
g_mediaCid = 0;
|
||||
constexpr uint32_t kMediaWaitMs = 8000;
|
||||
|
||||
uint16_t media = L2cap::Connect(L2cap::PSM_AVDTP);
|
||||
KernelLogStream(INFO, "BT-A2DP") << "AVDTP media: dialed cid="
|
||||
<< base::hex << (uint64_t)media << base::dec << ", holding channel...";
|
||||
|
||||
uint64_t mStart = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - mStart < kMediaWaitMs) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
auto* ch = media ? L2cap::GetChannel(media) : nullptr;
|
||||
if (ch && ch->Configured) { g_mediaCid = media; break; } // PENDING->SUCCESS->configured
|
||||
uint16_t other = L2cap::FindConfiguredAvdtpChannelExcept(g_sigCid);
|
||||
if (other) { g_mediaCid = other; break; } // sink opened it inbound
|
||||
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
||||
}
|
||||
|
||||
if (g_mediaCid == 0) {
|
||||
// Do NOT FreeChannel here -- a late SUCCESS would then match no
|
||||
// active channel. Leave it; the next connection's Initialize resets
|
||||
// the table. Log the held channel's state for diagnosis.
|
||||
auto* ch = media ? L2cap::GetChannel(media) : nullptr;
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP media channel setup failed (remoteCid="
|
||||
<< base::hex << (uint64_t)(ch ? ch->RemoteCid : 0) << base::dec
|
||||
<< " localCfg=" << (uint64_t)(ch ? ch->LocalConfigDone : 0)
|
||||
<< " remoteCfg=" << (uint64_t)(ch ? ch->RemoteConfigDone : 0)
|
||||
<< " connRsp=" << base::hex << (uint64_t)L2cap::LastConnRspResult()
|
||||
<< base::dec << " incomingReqs="
|
||||
<< (uint64_t)L2cap::IncomingAvdtpReqCount() << ")";
|
||||
Hci::DumpAclStats();
|
||||
return false;
|
||||
}
|
||||
|
||||
KernelLogStream(OK, "BT-A2DP") << "A2DP source ready (signaling + media), cid="
|
||||
<< base::hex << (uint64_t)g_mediaCid << base::dec << " state=Open";
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ProcessAvdtp — handle AVDTP signaling packets
|
||||
// =========================================================================
|
||||
@@ -281,10 +687,15 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
uint8_t signalId = data[1] & 0x3F;
|
||||
|
||||
if (msgType == MSG_RESPONSE_ACCEPT || msgType == MSG_RESPONSE_REJECT) {
|
||||
// This is a response to our command
|
||||
memcpy(g_avdtpResponseBuf, data, len > sizeof(g_avdtpResponseBuf) ? sizeof(g_avdtpResponseBuf) : len);
|
||||
g_avdtpResponseLen = len;
|
||||
g_avdtpResponseReady = true;
|
||||
// Only accept the response to the command we are actually waiting on
|
||||
// (matching transaction label AND signal id). Otherwise the headset's
|
||||
// own responses/duplicates could be read as ours and desync the chain.
|
||||
if (txLabel == g_expectLabel && signalId == g_expectSignal) {
|
||||
uint32_t cp = (len > sizeof(g_avdtpResponseBuf)) ? sizeof(g_avdtpResponseBuf) : len;
|
||||
memcpy(g_avdtpResponseBuf, data, cp);
|
||||
g_avdtpResponseLen = len;
|
||||
g_avdtpResponseReady = true;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -310,7 +721,7 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
rsp[4] = (MEDIA_AUDIO << 4);
|
||||
rsp[5] = CODEC_SBC;
|
||||
rsp[6] = 0x21; // 44.1kHz | Joint Stereo
|
||||
rsp[7] = 0x83; // 16 blocks | 8 subbands | Loudness
|
||||
rsp[7] = 0x15; // 16 blocks (b4) | 8 subbands (b2) | Loudness (b0)
|
||||
rsp[8] = 2; // Min bitpool
|
||||
rsp[9] = 53; // Max bitpool
|
||||
SendAvdtpResponse(txLabel, AVDTP_GET_CAPABILITIES, rsp, 10);
|
||||
@@ -375,6 +786,12 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
|
||||
bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample) {
|
||||
Sbc::Init(&g_sbcEncoder, sampleRate, channels, bitsPerSample);
|
||||
// Override with the SBC parameters actually negotiated in
|
||||
// SetConfiguration so the encoded frame headers match what the sink
|
||||
// agreed to (Init's defaults may differ from the negotiated subset).
|
||||
if (g_cfgSbc[0] || g_cfgSbc[1]) {
|
||||
Sbc::Configure(&g_sbcEncoder, g_cfgSbc[0], g_cfgSbc[1], g_cfgSbc[3]);
|
||||
}
|
||||
g_sbcInitialized = true;
|
||||
g_seqNum = 0;
|
||||
g_timestamp = 0;
|
||||
@@ -429,6 +846,8 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
if (bytesPerFrame > sizeof(scaledPcm)) return -1;
|
||||
|
||||
uint32_t consumed = 0;
|
||||
uint16_t maxOut = Hci::AclMaxPackets();
|
||||
if (maxOut == 0) maxOut = 4; // controller ACL buffer credits
|
||||
|
||||
while (consumed + bytesPerFrame <= pcmLen) {
|
||||
// Copy and scale by volume
|
||||
@@ -464,8 +883,23 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
|
||||
uint32_t totalLen = 13 + encodedSize;
|
||||
|
||||
// Flow control + event pump. WriteAudio runs in syscall context,
|
||||
// which otherwise never services the xHCI event ring -- so without
|
||||
// this the controller's ACL credits (Number-Of-Completed-Packets)
|
||||
// and the RX ring are never processed and the TX ring stalls/overruns
|
||||
// (-> no audio). Wait for a controller buffer credit, send, then pump
|
||||
// so the completion + credit are reaped before the next frame.
|
||||
uint64_t t0 = Timekeeping::GetMilliseconds();
|
||||
while (Hci::AclPendingCount() >= maxOut
|
||||
&& Timekeeping::GetMilliseconds() - t0 < 100) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
}
|
||||
|
||||
// Send via L2CAP on media channel
|
||||
L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)totalLen);
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
|
||||
g_seqNum++;
|
||||
g_timestamp += samplesPerFrame;
|
||||
|
||||
@@ -26,12 +26,21 @@ namespace Drivers::USB::Bluetooth::A2dp {
|
||||
// Public API
|
||||
// =========================================================================
|
||||
|
||||
// Drive the A2DP source role after an ACL link is up: open the AVDTP
|
||||
// signaling channel, negotiate an SBC stream, and open the media transport
|
||||
// channel. Must be called from process context (it blocks on the AVDTP
|
||||
// handshakes), NOT from an event/interrupt path. Leaves the stream Open.
|
||||
bool StartSource(uint32_t timeoutMs = 5000);
|
||||
|
||||
// Called by L2CAP when an AVDTP channel becomes ready
|
||||
void OnChannelReady(uint16_t l2capCid);
|
||||
|
||||
// Process an AVDTP signaling packet
|
||||
void ProcessAvdtp(const uint8_t* data, uint16_t len);
|
||||
|
||||
// Process an SDP response packet (on the SDP L2CAP channel)
|
||||
void ProcessSdp(const uint8_t* data, uint16_t len);
|
||||
|
||||
// Configure a stream for the given PCM parameters
|
||||
bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample);
|
||||
|
||||
|
||||
@@ -7,8 +7,10 @@
|
||||
#include "Bluetooth.hpp"
|
||||
#include "Hci.hpp"
|
||||
#include "A2dp.hpp"
|
||||
#include "IntelFirmware.hpp"
|
||||
#include <Drivers/USB/Xhci.hpp>
|
||||
#include <Drivers/USB/UsbDevice.hpp>
|
||||
#include <Fs/Vfs.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
@@ -26,6 +28,15 @@ namespace Drivers::USB::Bluetooth {
|
||||
static uint8_t g_slotId = 0;
|
||||
static uint8_t g_bdAddr[6] = {};
|
||||
|
||||
// True when the USB transport is up but the firmware-dependent HCI init is
|
||||
// still waiting for the ramdisk (drive 0) to be mounted. Set when an
|
||||
// adapter enumerates during the boot port scan, which runs before the boot
|
||||
// filesystems are mounted; cleared by ServiceDeferredInit() once VFS is up.
|
||||
static bool g_initPending = false;
|
||||
|
||||
// Forward declaration: firmware-dependent HCI bring-up, run once VFS is up.
|
||||
static void CompleteInit();
|
||||
|
||||
// Intel Bluetooth device IDs
|
||||
static bool IsIntelBt(uint16_t vid, uint16_t pid) {
|
||||
if (vid != 0x8087) return false;
|
||||
@@ -76,7 +87,9 @@ namespace Drivers::USB::Bluetooth {
|
||||
<< " subver=" << (uint64_t)lver.LmpSubversion << base::dec;
|
||||
}
|
||||
|
||||
// Read Intel version to check firmware state
|
||||
// Read legacy Intel version for diagnostics (TLV parts return this in
|
||||
// a different layout; the authoritative state check happens inside the
|
||||
// firmware download path below via the TLV version).
|
||||
Hci::IntelVersion ver = {};
|
||||
if (!Hci::ReadIntelVersion(&ver)) {
|
||||
KernelLogStream(WARNING, "BT") << "Failed to read Intel BT version";
|
||||
@@ -85,23 +98,24 @@ namespace Drivers::USB::Bluetooth {
|
||||
<< " FW variant=" << base::hex << (uint64_t)ver.FwVariant
|
||||
<< " FW rev=" << (uint64_t)ver.FwRevision << "."
|
||||
<< (uint64_t)ver.FwBuildNum << base::dec;
|
||||
|
||||
if (ver.FwVariant == 0x23) {
|
||||
KernelLogStream(OK, "BT") << "Intel BT firmware already loaded (operational mode)";
|
||||
} else if (ver.FwVariant == 0x06) {
|
||||
KernelLogStream(WARNING, "BT") << "Intel BT in bootloader mode, firmware not loaded";
|
||||
KernelLogStream(WARNING, "BT") << "Bluetooth will have limited functionality without firmware";
|
||||
} else if (!hciVersionOk) {
|
||||
// Standard HCI commands failed AND Intel version is zeros/unknown
|
||||
// -> controller is in bootloader mode, needs firmware download
|
||||
KernelLogStream(WARNING, "BT") << "Intel BT in bootloader mode (FW not loaded by UEFI)";
|
||||
KernelLogStream(WARNING, "BT") << "Bluetooth requires firmware download for full functionality";
|
||||
} else {
|
||||
KernelLogStream(INFO, "BT") << "Intel BT firmware variant: "
|
||||
<< base::hex << (uint64_t)ver.FwVariant;
|
||||
}
|
||||
}
|
||||
|
||||
// Run the firmware download path. This reads the TLV version, and if
|
||||
// the controller is in bootloader mode, loads the matching .sfi image
|
||||
// from the ramdisk, secure-sends it, boots the operational firmware
|
||||
// and applies DDC parameters. Returns true if the controller ends up
|
||||
// operational (either already loaded, or freshly downloaded).
|
||||
if (!DownloadIntelFirmware()) {
|
||||
KernelLogStream(WARNING, "BT")
|
||||
<< "Intel BT firmware not loaded; limited functionality";
|
||||
// Standard init already issued an HCI Reset above.
|
||||
return true;
|
||||
}
|
||||
|
||||
// The operational firmware just (re)booted. Give it a clean reset and
|
||||
// re-enable the Intel vendor event mask before the generic HCI setup.
|
||||
Hci::Reset();
|
||||
Hci::IntelSetEventMask();
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -137,10 +151,31 @@ namespace Drivers::USB::Bluetooth {
|
||||
// so it must be queued to receive them.
|
||||
Hci::StartEventPipe();
|
||||
|
||||
// Intel-specific initialization (includes HCI Reset)
|
||||
// The firmware download path reads the .sfi/.ddc images from the
|
||||
// ramdisk (drive 0). Adapters present at boot enumerate during the
|
||||
// xHCI port scan, which runs before the boot filesystems are mounted,
|
||||
// so defer the firmware-dependent bring-up until VFS is available.
|
||||
if (!Fs::Vfs::IsDriveRegistered(0)) {
|
||||
g_initPending = true;
|
||||
KernelLogStream(INFO, "BT") << "Transport up; deferring init until ramdisk is mounted";
|
||||
return;
|
||||
}
|
||||
|
||||
CompleteInit();
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// CompleteInit — firmware-dependent HCI bring-up (needs VFS/ramdisk)
|
||||
// =========================================================================
|
||||
|
||||
static void CompleteInit() {
|
||||
auto* dev = Xhci::GetDevice(g_slotId);
|
||||
if (!dev) return;
|
||||
|
||||
// Intel-specific initialization (firmware download + HCI Reset)
|
||||
bool didReset = false;
|
||||
if (IsIntelBt(dev->VendorId, dev->ProductId)) {
|
||||
if (InitIntelBluetooth(slotId)) {
|
||||
if (InitIntelBluetooth(g_slotId)) {
|
||||
didReset = true; // InitIntelBluetooth already sent HCI Reset
|
||||
} else {
|
||||
KernelLogStream(WARNING, "BT") << "Intel BT init failed, continuing with basic HCI";
|
||||
@@ -164,6 +199,13 @@ namespace Drivers::USB::Bluetooth {
|
||||
<< (uint64_t)g_bdAddr[1] << ":" << (uint64_t)g_bdAddr[0] << base::dec;
|
||||
}
|
||||
|
||||
// NOTE: an earlier build overrode the BD_ADDR via 0xFC31 to dodge a
|
||||
// (since disproven) stale-bond theory. Removed: the BD_ADDR is an input
|
||||
// to the SSP authentication confirmation, and if the override only
|
||||
// changes the advertised address but not the address the firmware uses
|
||||
// in the crypto, the two sides compute different confirmations and
|
||||
// pairing fails (Simple Pairing Complete = 0x05). Use the real address.
|
||||
|
||||
// Read buffer size
|
||||
uint16_t aclLen = 0, aclNum = 0;
|
||||
uint8_t scoLen = 0;
|
||||
@@ -185,18 +227,40 @@ namespace Drivers::USB::Bluetooth {
|
||||
// Enable Simple Secure Pairing
|
||||
Hci::WriteSSPMode(1);
|
||||
|
||||
// Set event mask to receive relevant events
|
||||
uint8_t eventMask[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F, 0x00, 0x20};
|
||||
// Set event mask to receive relevant events. Octet 6 (events 0x31-0x38)
|
||||
// MUST be enabled for Secure Simple Pairing: IO Capability Request
|
||||
// (0x31, bit 48), IO Capability Response (0x32), User Confirmation
|
||||
// Request (0x33), Simple Pairing Complete (0x36) all live there. It was
|
||||
// 0x00 -> the controller started SSP but the IO-Capability Request event
|
||||
// never reached us, so pairing always timed out with auth failure 0x05.
|
||||
uint8_t eventMask[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F, 0xFF, 0x20};
|
||||
Hci::SendCommand(Hci::OP_SET_EVENT_MASK, eventMask, 8);
|
||||
Hci::WaitCommandComplete(Hci::OP_SET_EVENT_MASK);
|
||||
|
||||
// Enable inquiry + page scan (discoverable and connectable)
|
||||
Hci::WriteScanEnable(0x03);
|
||||
|
||||
// Load persisted bonds so previously-paired devices reconnect without
|
||||
// re-pairing (VFS is up by the time CompleteInit runs).
|
||||
Hci::LoadLinkKeys();
|
||||
|
||||
g_initialized = true;
|
||||
KernelLogStream(OK, "BT") << "Bluetooth adapter initialized successfully";
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ServiceDeferredInit — run boot-deferred bring-up once VFS is ready
|
||||
// =========================================================================
|
||||
|
||||
void ServiceDeferredInit() {
|
||||
if (!g_initPending || g_initialized) return;
|
||||
if (!Fs::Vfs::IsDriveRegistered(0)) return; // ramdisk still not mounted
|
||||
|
||||
g_initPending = false;
|
||||
KernelLogStream(INFO, "BT") << "Ramdisk mounted; completing Bluetooth init";
|
||||
CompleteInit();
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Public queries
|
||||
// =========================================================================
|
||||
@@ -255,6 +319,8 @@ namespace Drivers::USB::Bluetooth {
|
||||
int Connect(const uint8_t* bdAddr, uint32_t timeoutMs) {
|
||||
if (!g_initialized || !bdAddr) return -1;
|
||||
|
||||
Hci::ResetEventTrace(); // capture the pairing/SSP event sequence
|
||||
|
||||
if (!Hci::CreateConnection(bdAddr)) return -1;
|
||||
|
||||
// Wait for Connection Complete event
|
||||
@@ -271,7 +337,61 @@ namespace Drivers::USB::Bluetooth {
|
||||
for (int j = 0; j < 6; j++) {
|
||||
if (conn->BdAddr[j] != bdAddr[j]) { match = false; break; }
|
||||
}
|
||||
if (match) return 0;
|
||||
if (match) {
|
||||
// The headset (in pairing mode) drives Secure Simple
|
||||
// Pairing itself right after the ACL link comes up. Let
|
||||
// authentication + encryption finish BEFORE opening any
|
||||
// L2CAP/AVDTP channels: doing A2DP on a not-yet-
|
||||
// authenticated link races with the pairing handshake and
|
||||
// the headset drops us (reason 0x05). Drain events here
|
||||
// so the IO-capability / user-confirm / link-key / encrypt
|
||||
// events all get serviced.
|
||||
//
|
||||
// We are the initiator: request authentication so the
|
||||
// controller starts Secure Simple Pairing (Link Key
|
||||
// Request -> our negative reply -> IO Capability Request
|
||||
// -> ... ). The headset does not start this on its own.
|
||||
// NB this only works now that octet 6 of the event mask
|
||||
// is enabled so the IO-Capability Request event actually
|
||||
// reaches us; before that this produced 03 17 06 05.
|
||||
Hci::AuthenticateLink(conn->Handle);
|
||||
|
||||
uint64_t t0 = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - t0 < 5000) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
// Send queued pairing replies reliably (top-level,
|
||||
// not nested under PollEvents).
|
||||
Hci::ProcessPendingCommands();
|
||||
if (!conn->Active) break; // link dropped during pairing
|
||||
if (conn->Encrypted) break; // authenticated + encrypted -> ready
|
||||
for (int k = 0; k < 200; k++) asm volatile("pause" ::: "memory");
|
||||
}
|
||||
|
||||
// Bring up the A2DP source stream (signaling + media
|
||||
// channels, SBC negotiation) only once the link is
|
||||
// secured. Without a media stream the headset also drops
|
||||
// the link (reason 0x13), so this keeps it engaged too.
|
||||
if (conn->Active) {
|
||||
// Let the link settle after Encryption Change before
|
||||
// dialing L2CAP: some sinks ignore a CONN_REQ that
|
||||
// arrives the instant encryption completes. Drain
|
||||
// (don't blind-sleep) so the ACL RX ring stays live.
|
||||
uint64_t st = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - st < 300) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
for (int k = 0; k < 200; k++) asm volatile("pause" ::: "memory");
|
||||
}
|
||||
A2dp::StartSource();
|
||||
}
|
||||
// Persist any new link key now (process context), even if
|
||||
// the link later dropped, so the disk write never stalls
|
||||
// the nested pairing event handler.
|
||||
Hci::FlushLinkKeys();
|
||||
Hci::DumpEventTrace(); // show the pairing/SSP sequence
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -280,6 +400,7 @@ namespace Drivers::USB::Bluetooth {
|
||||
}
|
||||
}
|
||||
|
||||
Hci::DumpEventTrace(); // show whatever events did arrive
|
||||
return -1; // Timeout
|
||||
}
|
||||
|
||||
|
||||
@@ -13,6 +13,12 @@ namespace Drivers::USB::Bluetooth {
|
||||
// Called by USB enumeration when a Bluetooth adapter is detected
|
||||
void RegisterAdapter(uint8_t slotId);
|
||||
|
||||
// Complete any firmware-dependent bring-up that was deferred because the
|
||||
// adapter enumerated during the boot port scan, before the ramdisk (which
|
||||
// holds the firmware images) was mounted. Safe to call unconditionally
|
||||
// once the boot filesystems are up; a no-op if nothing is pending.
|
||||
void ServiceDeferredInit();
|
||||
|
||||
// Query adapter state
|
||||
bool IsInitialized();
|
||||
uint8_t GetSlotId();
|
||||
|
||||
@@ -6,6 +6,7 @@
|
||||
|
||||
#include "Hci.hpp"
|
||||
#include "L2cap.hpp"
|
||||
#include <Fs/Vfs.hpp>
|
||||
#include <Drivers/USB/Xhci.hpp>
|
||||
#include <Drivers/USB/UsbDevice.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
@@ -31,14 +32,27 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
static volatile uint32_t g_eventLen = 0;
|
||||
static volatile bool g_eventReady = false;
|
||||
|
||||
// ACL receive buffer
|
||||
static uint8_t g_aclRxBuf[1024] = {};
|
||||
static volatile uint32_t g_aclRxLen = 0;
|
||||
static volatile bool g_aclRxReady = false;
|
||||
// ACL receive ring buffer. The bulk-IN callback (nested under PollEvents)
|
||||
// only copies an incoming packet into a slot; DrainEvents() processes them
|
||||
// at top level. A ring (not a single buffer) is required because the headset
|
||||
// bursts many ACL packets at once -- the single buffer was overwriting and
|
||||
// dropping the L2CAP Config Response, leaving our channel half-configured.
|
||||
static constexpr int ACL_RX_SLOTS = 32;
|
||||
static constexpr int ACL_RX_SLOT_SIZE = 1024;
|
||||
static uint8_t g_aclRxRing[ACL_RX_SLOTS][ACL_RX_SLOT_SIZE] = {};
|
||||
static volatile uint16_t g_aclRxLens[ACL_RX_SLOTS] = {};
|
||||
static volatile uint8_t g_aclRxHead = 0;
|
||||
static volatile uint8_t g_aclRxTail = 0;
|
||||
|
||||
// ACL transmit DMA buffer
|
||||
static uint8_t* g_aclTxBuf = nullptr;
|
||||
static uint64_t g_aclTxBufPhys = 0;
|
||||
// ACL transmit DMA buffers (a ring, not one): SendAcl queues an async bulk
|
||||
// OUT transfer, so two sends in quick succession (e.g. our Config Request
|
||||
// then a Config Response, both fired while DrainEvents processes a burst)
|
||||
// would have the second overwrite the first buffer before it is DMA'd to the
|
||||
// wire -- corrupting the first packet. Rotate buffers to avoid that.
|
||||
static constexpr int ACL_TX_SLOTS = 8;
|
||||
static uint8_t* g_aclTxRing[ACL_TX_SLOTS] = {};
|
||||
static uint64_t g_aclTxRingPhys[ACL_TX_SLOTS] = {};
|
||||
static uint8_t g_aclTxSlot = 0;
|
||||
|
||||
// HCI command DMA buffer (separate from ACL to avoid conflicts)
|
||||
static uint8_t* g_cmdDmaBuf = nullptr;
|
||||
@@ -52,11 +66,179 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
static uint16_t g_aclMaxNum = 0;
|
||||
static volatile uint16_t g_aclPendingCount = 0;
|
||||
|
||||
// Diagnostic ACL data-path counters: TX submitted, TX completed (bulk OUT
|
||||
// completion), RX received (bulk IN). Used to tell whether L2CAP signaling
|
||||
// actually flows over ACL after encryption is enabled.
|
||||
static volatile uint32_t g_aclTxCount = 0;
|
||||
static volatile uint32_t g_aclTxDoneCount = 0;
|
||||
static volatile uint32_t g_aclRxCount = 0;
|
||||
// Incremented when an incoming ACL packet is dropped because the RX ring was
|
||||
// full -- a nonzero value means a burst overran the ring (and could have
|
||||
// dropped an L2CAP Config Response). Surfaced by DumpAclStats().
|
||||
static volatile uint32_t g_aclRxDropCount = 0;
|
||||
|
||||
// Inquiry results
|
||||
static InquiryDevice g_inquiryResults[MAX_INQUIRY_RESULTS] = {};
|
||||
static volatile int g_inquiryResultCount = 0;
|
||||
static volatile bool g_inquiryActive = false;
|
||||
|
||||
// Set when the Intel "bootup" vendor event arrives after a firmware boot.
|
||||
// Written from the USB transfer callback (ProcessEvent), polled by
|
||||
// IntelBootFirmware, hence volatile.
|
||||
static volatile bool g_intelBootup = false;
|
||||
|
||||
// Latest Intel "secure send result" vendor event (0xFF sub-opcode 0x06).
|
||||
// The bootloader uses this, not Command Complete, to report the outcome of
|
||||
// secure-send (0xFC09) firmware download. Written from ProcessEvent.
|
||||
static volatile bool g_secureResultValid = false;
|
||||
static volatile uint8_t g_secureResult = 0; // 0 = success
|
||||
static volatile uint8_t g_secureStatus = 0; // 0 = success
|
||||
|
||||
// Firmware-download diagnostics. g_ssBytesSent / g_ssFragsSent accumulate
|
||||
// the bytes / 0xFC09 fragments handed to the controller since the last
|
||||
// ClearSecureSendResult(), so an async 0xFF/0x06 result or a fragment error
|
||||
// can be pinned to an exact upload position. g_lastControlCC is the xHCI
|
||||
// completion code of the most recent SendCommand() control transfer.
|
||||
static volatile uint64_t g_ssBytesSent = 0;
|
||||
static volatile uint32_t g_ssFragsSent = 0;
|
||||
static volatile uint32_t g_lastControlCC = 0;
|
||||
|
||||
// Lockless HCI-event trace for diagnosing the pairing/SSP sequence.
|
||||
// ProcessEvent (which may run from the xHCI IRQ) only does an array write
|
||||
// here -- no lock, no terminal I/O, so it cannot deadlock against g_termLock.
|
||||
// DumpEventTrace() prints it from top-level (process) context.
|
||||
static constexpr int EVT_TRACE_MAX = 48;
|
||||
static volatile uint8_t g_evtTrace[EVT_TRACE_MAX] = {};
|
||||
static volatile uint8_t g_evtTraceCount = 0;
|
||||
// Status byte of the last Simple Pairing Complete (0x36) / Auth Complete
|
||||
// (0x06) / Disconnection (0x05) -- captured to find WHY pairing fails.
|
||||
static volatile uint8_t g_lastSppStatus = 0xEE;
|
||||
static volatile uint8_t g_lastAuthStatus = 0xEE;
|
||||
static volatile uint8_t g_lastDiscReason = 0xEE;
|
||||
|
||||
// Pending-command queue. Pairing replies (IO-cap / user-confirm / link-key)
|
||||
// are triggered from event handlers running NESTED under PollEvents, where a
|
||||
// command can only be fire-and-forget (the reentrancy guard makes a nested
|
||||
// wait a no-op). Fire-and-forget proved unreliable for SSP -- the IO-cap
|
||||
// value feeds the authentication confirmation, so a late/garbled reply makes
|
||||
// the DHKey check fail (Simple Pairing Complete status 0x05). Instead the
|
||||
// handlers ENQUEUE the reply here; ProcessPendingCommands() sends it later
|
||||
// from top-level (process) context with a real, confirmed transfer.
|
||||
struct PendingHciCmd { uint16_t opcode; uint8_t len; uint8_t params[16]; };
|
||||
static PendingHciCmd g_pending[16] = {};
|
||||
static volatile uint8_t g_pendingHead = 0;
|
||||
static volatile uint8_t g_pendingTail = 0;
|
||||
|
||||
static void EnqueueHciCmd(uint16_t opcode, const uint8_t* params, uint8_t len) {
|
||||
uint8_t next = (uint8_t)((g_pendingHead + 1) & 15);
|
||||
if (next == g_pendingTail) return; // full -> drop (should never happen)
|
||||
if (len > 16) len = 16;
|
||||
g_pending[g_pendingHead].opcode = opcode;
|
||||
g_pending[g_pendingHead].len = len;
|
||||
for (uint8_t i = 0; i < len; i++) g_pending[g_pendingHead].params[i] = params[i];
|
||||
g_pendingHead = next;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Bonded-device link key store
|
||||
// =========================================================================
|
||||
// Persisted to disk so a pairing survives reboots. Without it a once-paired
|
||||
// device challenges us for the link key on reconnect, we have nothing to
|
||||
// answer with, and authentication fails -> the remote drops the link with
|
||||
// disconnect reason 0x05. On EVT_LINK_KEY_NOTIFICATION we cache the new key
|
||||
// (RAM, marked dirty); FlushLinkKeys() writes it from process context so the
|
||||
// disk I/O never blocks mid-pairing while nested under PollEvents.
|
||||
static constexpr int MAX_BONDS = 8;
|
||||
static constexpr uint32_t LINK_KEY_MAGIC = 0x314B5442; // 'BTK1'
|
||||
|
||||
struct StoredLinkKey {
|
||||
uint8_t addr[6];
|
||||
uint8_t key[16];
|
||||
bool valid;
|
||||
};
|
||||
static StoredLinkKey g_bonds[MAX_BONDS] = {};
|
||||
static bool g_bondsDirty = false;
|
||||
|
||||
static bool AddrEq(const uint8_t* a, const uint8_t* b) {
|
||||
for (int i = 0; i < 6; i++) if (a[i] != b[i]) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
static int FindBondIndex(const uint8_t* addr) {
|
||||
for (int i = 0; i < MAX_BONDS; i++) {
|
||||
if (g_bonds[i].valid && AddrEq(g_bonds[i].addr, addr)) return i;
|
||||
}
|
||||
return -1;
|
||||
}
|
||||
|
||||
static void StoreLinkKey(const uint8_t* addr, const uint8_t* key) {
|
||||
int idx = FindBondIndex(addr);
|
||||
if (idx < 0) {
|
||||
for (int i = 0; i < MAX_BONDS; i++) {
|
||||
if (!g_bonds[i].valid) { idx = i; break; }
|
||||
}
|
||||
if (idx < 0) idx = 0; // table full: recycle the first slot
|
||||
}
|
||||
memcpy(g_bonds[idx].addr, addr, 6);
|
||||
memcpy(g_bonds[idx].key, key, 16);
|
||||
g_bonds[idx].valid = true;
|
||||
g_bondsDirty = true; // FlushLinkKeys() persists from safe context
|
||||
}
|
||||
|
||||
// On-disk layout: [magic u32][MAX_BONDS x { addr[6], key[16], valid[1] }].
|
||||
static constexpr uint64_t LINK_KEY_BLOB_SIZE = 4 + MAX_BONDS * 23;
|
||||
|
||||
void LoadLinkKeys() {
|
||||
Fs::Vfs::BackendFile f;
|
||||
if (Fs::Vfs::OpenBackendFile("0:/os/btkeys.bin", f) < 0) return; // none stored yet
|
||||
uint64_t size = Fs::Vfs::GetBackendFileSize(f);
|
||||
if (size < LINK_KEY_BLOB_SIZE) { Fs::Vfs::CloseBackendFile(f); return; }
|
||||
|
||||
uint8_t blob[LINK_KEY_BLOB_SIZE];
|
||||
Fs::Vfs::ReadBackendFile(f, blob, 0, LINK_KEY_BLOB_SIZE);
|
||||
Fs::Vfs::CloseBackendFile(f);
|
||||
|
||||
uint32_t magic = (uint32_t)blob[0] | ((uint32_t)blob[1] << 8)
|
||||
| ((uint32_t)blob[2] << 16) | ((uint32_t)blob[3] << 24);
|
||||
if (magic != LINK_KEY_MAGIC) return;
|
||||
|
||||
int off = 4, n = 0;
|
||||
for (int i = 0; i < MAX_BONDS; i++) {
|
||||
memcpy(g_bonds[i].addr, &blob[off], 6); off += 6;
|
||||
memcpy(g_bonds[i].key, &blob[off], 16); off += 16;
|
||||
g_bonds[i].valid = blob[off++] != 0;
|
||||
if (g_bonds[i].valid) n++;
|
||||
}
|
||||
g_bondsDirty = false;
|
||||
KernelLogStream(INFO, "BT-HCI") << "Loaded " << (uint64_t)n << " bonded device key(s)";
|
||||
}
|
||||
|
||||
void FlushLinkKeys() {
|
||||
if (!g_bondsDirty) return;
|
||||
|
||||
uint8_t blob[LINK_KEY_BLOB_SIZE] = {};
|
||||
blob[0] = (uint8_t)(LINK_KEY_MAGIC);
|
||||
blob[1] = (uint8_t)(LINK_KEY_MAGIC >> 8);
|
||||
blob[2] = (uint8_t)(LINK_KEY_MAGIC >> 16);
|
||||
blob[3] = (uint8_t)(LINK_KEY_MAGIC >> 24);
|
||||
int off = 4;
|
||||
for (int i = 0; i < MAX_BONDS; i++) {
|
||||
memcpy(&blob[off], g_bonds[i].addr, 6); off += 6;
|
||||
memcpy(&blob[off], g_bonds[i].key, 16); off += 16;
|
||||
blob[off++] = g_bonds[i].valid ? 1 : 0;
|
||||
}
|
||||
|
||||
Fs::Vfs::BackendFile f;
|
||||
if (Fs::Vfs::CreateBackendFile("0:/os/btkeys.bin", f) < 0) {
|
||||
KernelLogStream(WARNING, "BT-HCI") << "Could not open link key store for writing";
|
||||
return;
|
||||
}
|
||||
Fs::Vfs::WriteBackendFile(f, blob, 0, LINK_KEY_BLOB_SIZE);
|
||||
Fs::Vfs::CloseBackendFile(f);
|
||||
g_bondsDirty = false;
|
||||
KernelLogStream(OK, "BT-HCI") << "Link key store persisted";
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// USB transfer callback
|
||||
// =========================================================================
|
||||
@@ -93,18 +275,38 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
|
||||
// Re-queue interrupt transfer for next event
|
||||
Xhci::QueueInterruptTransfer(slotId);
|
||||
} else if (epDci == bulkInDci && data && length > 0) {
|
||||
// ACL data received on bulk IN
|
||||
uint32_t copyLen = length;
|
||||
if (copyLen > sizeof(g_aclRxBuf)) copyLen = sizeof(g_aclRxBuf);
|
||||
memcpy(g_aclRxBuf, data, copyLen);
|
||||
g_aclRxLen = copyLen;
|
||||
g_aclRxReady = true;
|
||||
} else if (epDci == bulkInDci) {
|
||||
// ACL data received on bulk IN -> copy into the ring; processed by
|
||||
// DrainEvents() at top level (do NOT process here, nested).
|
||||
// Only enqueue a packet big enough to carry an L2CAP header (ACL
|
||||
// header + L2CAP header = 8 bytes). Smaller completions are ZLP /
|
||||
// re-arm artifacts and the firmware-phase bulk-IN runts (4-7 bytes,
|
||||
// the old "rx flood") -- ProcessPacket would reject them anyway, and
|
||||
// dropping them here keeps the ring + rx stats clean. We STILL
|
||||
// re-arm on every completion below (the bulk IN must keep cycling to
|
||||
// absorb the device's ~635 KB cc=4 glitch during firmware download).
|
||||
if (data && length >= sizeof(AclHeader) + 4) {
|
||||
g_aclRxCount++;
|
||||
uint8_t next = (uint8_t)((g_aclRxHead + 1) % ACL_RX_SLOTS);
|
||||
if (next != g_aclRxTail) { // ring not full
|
||||
uint32_t copyLen = length;
|
||||
if (copyLen > ACL_RX_SLOT_SIZE) copyLen = ACL_RX_SLOT_SIZE;
|
||||
memcpy(g_aclRxRing[g_aclRxHead], data, copyLen);
|
||||
g_aclRxLens[g_aclRxHead] = (uint16_t)copyLen;
|
||||
g_aclRxHead = next;
|
||||
} else {
|
||||
g_aclRxDropCount++; // ring overran -> a packet was lost
|
||||
}
|
||||
}
|
||||
|
||||
// Re-queue bulk IN transfer
|
||||
Xhci::QueueBulkInTransfer(slotId, nullptr, 0, dev->BulkInMaxPacket);
|
||||
// Re-queue bulk IN transfer (only on a real success/short completion;
|
||||
// the error path passes data==nullptr and is handled elsewhere).
|
||||
if (data) {
|
||||
Xhci::QueueBulkInTransfer(slotId, nullptr, 0, dev->BulkInMaxPacket);
|
||||
}
|
||||
} else if (epDci == (dev->BulkOutEpNum ? (uint8_t)(dev->BulkOutEpNum * 2) : (uint8_t)0)) {
|
||||
// Bulk OUT completion — decrement pending count
|
||||
g_aclTxDoneCount++;
|
||||
if (g_aclPendingCount > 0) g_aclPendingCount--;
|
||||
}
|
||||
}
|
||||
@@ -145,8 +347,10 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
g_cmdDmaBuf = (uint8_t*)Memory::g_pfa->AllocateZeroed();
|
||||
g_cmdDmaBufPhys = Memory::SubHHDM(g_cmdDmaBuf);
|
||||
|
||||
g_aclTxBuf = (uint8_t*)Memory::g_pfa->AllocateZeroed();
|
||||
g_aclTxBufPhys = Memory::SubHHDM(g_aclTxBuf);
|
||||
for (int i = 0; i < ACL_TX_SLOTS; i++) {
|
||||
g_aclTxRing[i] = (uint8_t*)Memory::g_pfa->AllocateZeroed();
|
||||
g_aclTxRingPhys[i] = Memory::SubHHDM(g_aclTxRing[i]);
|
||||
}
|
||||
|
||||
// NOTE: Do NOT queue interrupt IN or bulk IN transfers here.
|
||||
// The BT controller is not yet HCI-initialized and may misbehave.
|
||||
@@ -156,7 +360,16 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
KernelLogStream(OK, "BT-HCI") << "HCI transport initialized on slot " << (uint64_t)slotId;
|
||||
}
|
||||
|
||||
// Start receiving HCI events and ACL data — call after HCI init sequence
|
||||
// Start receiving HCI events and ACL data — call after HCI init sequence.
|
||||
// Arms BOTH the interrupt IN (events) and the bulk IN (ACL). The bulk IN
|
||||
// MUST stay armed across the firmware download: on this controller the device
|
||||
// glitches a USB transaction error (cc=4) near ~635 KB of the upload, and an
|
||||
// armed bulk IN ABSORBS it (a benign cc=4 on the bulk endpoint) so the
|
||||
// interrupt-IN event pipe survives and the download completes. Deferring the
|
||||
// bulk-IN arm (build 40) moved that cc=4 onto the interrupt IN and wedged the
|
||||
// download at 635 KB -- reverted. The harmless firmware-phase bulk-IN runts
|
||||
// (4-7 byte completions) are filtered at enqueue in TransferCallback so they
|
||||
// never pollute the RX ring.
|
||||
void StartEventPipe() {
|
||||
if (!g_initialized) return;
|
||||
|
||||
@@ -207,6 +420,8 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
g_cmdDmaBuf,
|
||||
false); // dirIn = false (host to device)
|
||||
|
||||
g_lastControlCC = cc;
|
||||
|
||||
if (cc != Xhci::CC_SUCCESS) {
|
||||
KernelLogStream(WARNING, "BT-HCI") << "SendCommand failed, opcode="
|
||||
<< base::hex << (uint64_t)opcode << " cc=" << base::dec << (uint64_t)cc;
|
||||
@@ -222,6 +437,12 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
|
||||
bool WaitCommandComplete(uint16_t opcode, uint8_t* outParams,
|
||||
uint8_t maxLen, uint32_t timeoutMs) {
|
||||
// Nested inside PollEvents (an event handler issued this command): we
|
||||
// cannot wait -- a nested PollEvents is a no-op, so the Command Complete
|
||||
// is reaped by the active PollEvents after we return. The command was
|
||||
// already submitted (fire-and-forget); report success.
|
||||
if (Xhci::InPollContext()) return true;
|
||||
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
@@ -275,6 +496,9 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
// =========================================================================
|
||||
|
||||
bool WaitCommandStatus(uint16_t opcode, uint32_t timeoutMs) {
|
||||
// See WaitCommandComplete: cannot wait when nested under PollEvents.
|
||||
if (Xhci::InPollContext()) return true;
|
||||
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
@@ -311,25 +535,44 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
// =========================================================================
|
||||
|
||||
bool SendAcl(uint16_t handle, uint16_t pbFlag, const uint8_t* data, uint16_t len) {
|
||||
if (!g_initialized || !g_aclTxBuf) return false;
|
||||
if (!g_initialized || !g_aclTxRing[0]) return false;
|
||||
if (len + sizeof(AclHeader) > 4096) return false; // Single page DMA buffer
|
||||
|
||||
// Use the next TX ring slot so a rapid second send can't overwrite this
|
||||
// packet before its bulk OUT transfer DMAs it to the wire.
|
||||
uint8_t* txBuf = g_aclTxRing[g_aclTxSlot];
|
||||
uint64_t txPhys = g_aclTxRingPhys[g_aclTxSlot];
|
||||
g_aclTxSlot = (uint8_t)((g_aclTxSlot + 1) % ACL_TX_SLOTS);
|
||||
|
||||
// Build ACL packet in DMA buffer
|
||||
auto* hdr = (AclHeader*)g_aclTxBuf;
|
||||
auto* hdr = (AclHeader*)txBuf;
|
||||
hdr->HandleFlags = (handle & 0x0FFF) | pbFlag;
|
||||
hdr->DataLength = len;
|
||||
if (data && len > 0) {
|
||||
memcpy(g_aclTxBuf + sizeof(AclHeader), data, len);
|
||||
memcpy(txBuf + sizeof(AclHeader), data, len);
|
||||
}
|
||||
|
||||
uint32_t totalLen = sizeof(AclHeader) + len;
|
||||
|
||||
g_aclPendingCount++;
|
||||
Xhci::QueueBulkOutTransfer(g_slotId, g_aclTxBuf, g_aclTxBufPhys, totalLen);
|
||||
g_aclTxCount++;
|
||||
Xhci::QueueBulkOutTransfer(g_slotId, txBuf, txPhys, totalLen);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
uint16_t AclPendingCount() { return g_aclPendingCount; }
|
||||
uint16_t AclMaxPackets() { return g_aclMaxNum; }
|
||||
|
||||
void DumpAclStats() {
|
||||
KernelLogStream(INFO, "BT-HCI") << "ACL stats: tx=" << (uint64_t)g_aclTxCount
|
||||
<< " txDone=" << (uint64_t)g_aclTxDoneCount
|
||||
<< " rx=" << (uint64_t)g_aclRxCount
|
||||
<< " rxDrop=" << (uint64_t)g_aclRxDropCount
|
||||
<< " pending=" << (uint64_t)g_aclPendingCount
|
||||
<< " bufNum=" << (uint64_t)g_aclMaxNum;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ProcessEvent — handle HCI events
|
||||
// =========================================================================
|
||||
@@ -341,6 +584,17 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
uint8_t evtParamLen = data[1];
|
||||
const uint8_t* params = data + 2;
|
||||
|
||||
// Record into the lockless trace (safe from the IRQ path: array write
|
||||
// only, no lock / no terminal I/O). DumpEventTrace() prints it later
|
||||
// from top-level context.
|
||||
if (evtCode != EVT_NUM_COMPLETED_PACKETS && g_evtTraceCount < EVT_TRACE_MAX) {
|
||||
g_evtTrace[g_evtTraceCount++] = evtCode;
|
||||
}
|
||||
// status byte = params[0] for these (Disconnection: status,handle,reason)
|
||||
if (evtCode == EVT_SIMPLE_PAIRING_COMPLETE && evtParamLen >= 1) g_lastSppStatus = params[0];
|
||||
if (evtCode == EVT_AUTH_COMPLETE && evtParamLen >= 1) g_lastAuthStatus = params[0];
|
||||
if (evtCode == EVT_DISCONNECTION_COMPLETE && evtParamLen >= 4) g_lastDiscReason = params[3];
|
||||
|
||||
switch (evtCode) {
|
||||
case EVT_CONNECTION_COMPLETE: {
|
||||
if (evtParamLen >= 11) {
|
||||
@@ -430,18 +684,68 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
memcpy(reply, ¶ms[0], 6); // BD_ADDR
|
||||
reply[6] = 0x03; // IO Capability: NoInputNoOutput
|
||||
reply[7] = 0x00; // OOB data not present
|
||||
reply[8] = 0x00; // Authentication requirements: MITM not required
|
||||
SendCommand(OP_IO_CAPABILITY_REPLY, reply, 9);
|
||||
WaitCommandComplete(OP_IO_CAPABILITY_REPLY, nullptr, 0, 1000);
|
||||
reply[8] = 0x04; // Auth req: MITM not required, General Bonding
|
||||
// (0x04, not 0x00 "No Bonding") so a
|
||||
// persistent link key is created -> the bond
|
||||
// survives reboots via the link-key store.
|
||||
// Queue for reliable top-level delivery (see EnqueueHciCmd):
|
||||
// the IO-cap value feeds the SSP confirmation, so it must be
|
||||
// sent intact, not fire-and-forget.
|
||||
EnqueueHciCmd(OP_IO_CAPABILITY_REPLY, reply, 9);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_USER_CONFIRM_REQUEST: {
|
||||
if (evtParamLen >= 6) {
|
||||
// Auto-confirm
|
||||
SendCommand(OP_USER_CONFIRM_REPLY, ¶ms[0], 6);
|
||||
WaitCommandComplete(OP_USER_CONFIRM_REPLY, nullptr, 0, 1000);
|
||||
// Auto-confirm (Just Works). Queue for reliable top-level
|
||||
// delivery -- a late confirm makes pairing fail (spp=0x05).
|
||||
EnqueueHciCmd(OP_USER_CONFIRM_REPLY, ¶ms[0], 6);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_LINK_KEY_REQUEST: {
|
||||
if (evtParamLen >= 6) {
|
||||
int idx = FindBondIndex(¶ms[0]);
|
||||
if (idx >= 0) {
|
||||
// We remember this device: hand back the stored key so
|
||||
// authentication succeeds without re-pairing.
|
||||
uint8_t reply[22];
|
||||
memcpy(reply, ¶ms[0], 6);
|
||||
memcpy(&reply[6], g_bonds[idx].key, 16);
|
||||
EnqueueHciCmd(OP_LINK_KEY_REQ_REPLY, reply, 22);
|
||||
} else {
|
||||
// Unknown device: tell the controller we have no key so
|
||||
// the remote falls back to fresh Secure Simple Pairing
|
||||
// (Just Works) instead of failing auth (reason 0x05).
|
||||
EnqueueHciCmd(OP_LINK_KEY_REQ_NEG_REPLY, ¶ms[0], 6);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_LINK_KEY_NOTIFICATION: {
|
||||
// Pairing produced a new link key: BD_ADDR[6] + key[16] + type[1].
|
||||
// Cache it now (fast); FlushLinkKeys() persists it to disk from
|
||||
// process context so the disk write never stalls this nested
|
||||
// event handler mid-pairing.
|
||||
if (evtParamLen >= 22) {
|
||||
StoreLinkKey(¶ms[0], ¶ms[6]);
|
||||
KernelLogStream(INFO, "BT-HCI") << "Link key notification (new bond cached)";
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_AUTH_COMPLETE: {
|
||||
// Authentication succeeded -> turn on encryption. The link must
|
||||
// be encrypted before A2DP; without this the headset finishes
|
||||
// pairing, waits for encryption that never comes, and drops us
|
||||
// (reason 0x05). Set Connection Encryption = handle(2) + 0x01.
|
||||
// Queue for reliable top-level delivery.
|
||||
if (evtParamLen >= 3 && params[0] == 0) {
|
||||
uint8_t enc[3] = { params[1], params[2], 0x01 };
|
||||
EnqueueHciCmd(OP_SET_CONN_ENCRYPT, enc, 3);
|
||||
}
|
||||
break;
|
||||
}
|
||||
@@ -542,6 +846,31 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_VENDOR_SPECIFIC: {
|
||||
// Intel vendor events carry a sub-opcode in the first byte.
|
||||
// 0x02 = "bootup" (operational firmware booted after 0xFC01)
|
||||
// 0x06 = "secure send result": result(1) opcode(2) status(1)
|
||||
uint8_t sub = (evtParamLen >= 1) ? params[0] : 0xFF;
|
||||
if (sub == 0x02) {
|
||||
g_intelBootup = true;
|
||||
} else if (sub == 0x06 && evtParamLen >= 5) {
|
||||
g_secureResult = params[1];
|
||||
g_secureStatus = params[4];
|
||||
g_secureResultValid = true;
|
||||
// A healthy bootloader stays silent until the final
|
||||
// fragment, so the byte/frag position here pins exactly
|
||||
// where it reacted -- and a non-zero result/status mid
|
||||
// upload is the signature of an active rejection.
|
||||
bool err = (params[1] != 0) || (params[4] != 0);
|
||||
KernelLogStream(err ? ERROR : INFO, "BT-HCI") << "secure-send result="
|
||||
<< base::hex << (uint64_t)params[1] << " status="
|
||||
<< (uint64_t)params[4] << base::dec
|
||||
<< " @ byte " << g_ssBytesSent
|
||||
<< " frag " << (uint64_t)g_ssFragsSent;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
@@ -658,6 +987,195 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Intel firmware download primitives
|
||||
// =========================================================================
|
||||
|
||||
int ReadIntelVersionTlv(uint8_t* outBuf, int maxLen) {
|
||||
if (!outBuf || maxLen <= 0) return -1;
|
||||
|
||||
uint8_t param = 0xFF;
|
||||
if (!SendCommand(OP_INTEL_READ_VERSION, ¶m, 1)) return -1;
|
||||
|
||||
// Wait for the matching Command Complete and copy the *actually
|
||||
// received* return parameters. The TLV version response can exceed a
|
||||
// single interrupt packet; WaitCommandComplete trusts the event's
|
||||
// declared length, so we read g_eventBuf/g_eventLen directly to avoid
|
||||
// copying past what the controller delivered.
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - start < 2000) {
|
||||
Xhci::PollEvents();
|
||||
|
||||
if (g_eventReady) {
|
||||
g_eventReady = false;
|
||||
if (g_eventLen >= 5 && g_eventBuf[0] == EVT_COMMAND_COMPLETE) {
|
||||
uint16_t op = (uint16_t)g_eventBuf[3] | ((uint16_t)g_eventBuf[4] << 8);
|
||||
if (op == OP_INTEL_READ_VERSION) {
|
||||
// Return params begin at byte 5 (status, then TLVs).
|
||||
int avail = (int)g_eventLen - 5;
|
||||
if (avail < 0) avail = 0;
|
||||
int n = (avail < maxLen) ? avail : maxLen;
|
||||
memcpy(outBuf, &g_eventBuf[5], n);
|
||||
return n;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
||||
}
|
||||
|
||||
KernelLogStream(WARNING, "BT-HCI") << "ReadIntelVersionTlv timeout";
|
||||
return -1;
|
||||
}
|
||||
|
||||
void ClearSecureSendResult() {
|
||||
g_secureResultValid = false;
|
||||
g_ssBytesSent = 0;
|
||||
g_ssFragsSent = 0;
|
||||
}
|
||||
|
||||
bool PeekSecureSendResult(uint8_t* outResult, uint8_t* outStatus) {
|
||||
if (!g_secureResultValid) return false;
|
||||
if (outResult) *outResult = g_secureResult;
|
||||
if (outStatus) *outStatus = g_secureStatus;
|
||||
return true;
|
||||
}
|
||||
|
||||
void ResetEventTrace() {
|
||||
g_evtTraceCount = 0;
|
||||
g_lastSppStatus = 0xEE;
|
||||
g_lastAuthStatus = 0xEE;
|
||||
g_lastDiscReason = 0xEE;
|
||||
}
|
||||
|
||||
void DumpEventTrace() {
|
||||
// Top-level only (acquires g_termLock). Shows the HCI-event sequence so
|
||||
// a stalled pairing/SSP flow is visible, e.g. whether an IO Capability
|
||||
// Request (0x31) / User Confirm (0x33) arrive after a link-key reply, or
|
||||
// the link just goes 0x03 (connect) -> 0x17 (link-key req) -> 0x05
|
||||
// (disconnect/auth-fail) with no pairing in between.
|
||||
KernelLogStream s(INFO, "BT-HCI");
|
||||
s << "event trace:";
|
||||
uint8_t n = g_evtTraceCount;
|
||||
if (n > EVT_TRACE_MAX) n = EVT_TRACE_MAX;
|
||||
for (uint8_t i = 0; i < n; i++) {
|
||||
s << " " << base::hex << (uint64_t)g_evtTrace[i] << base::dec;
|
||||
}
|
||||
s << " | spp=" << base::hex << (uint64_t)g_lastSppStatus
|
||||
<< " auth=" << (uint64_t)g_lastAuthStatus
|
||||
<< " disc=" << (uint64_t)g_lastDiscReason << base::dec;
|
||||
}
|
||||
|
||||
void ProcessPendingCommands() {
|
||||
// Top-level only: drains queued pairing replies with real, confirmed
|
||||
// control transfers. Must NOT be called from inside PollEvents.
|
||||
if (Xhci::InPollContext()) return;
|
||||
while (g_pendingTail != g_pendingHead) {
|
||||
PendingHciCmd c = g_pending[g_pendingTail];
|
||||
g_pendingTail = (uint8_t)((g_pendingTail + 1) & 15);
|
||||
SendCommand(c.opcode, c.params, c.len);
|
||||
// Set Connection Encryption returns Command Status (not Complete),
|
||||
// so wait on that instead of burning a 1s timeout that delays A2DP.
|
||||
if (c.opcode == OP_SET_CONN_ENCRYPT) {
|
||||
WaitCommandStatus(c.opcode, 1000);
|
||||
} else {
|
||||
WaitCommandComplete(c.opcode, nullptr, 0, 1000);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool WaitSecureSendResult(uint32_t timeoutMs, uint8_t* outResult, uint8_t* outStatus) {
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
if (g_secureResultValid) {
|
||||
if (outResult) *outResult = g_secureResult;
|
||||
if (outStatus) *outStatus = g_secureStatus;
|
||||
return true;
|
||||
}
|
||||
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool IntelSecureSend(uint8_t fragmentType, const uint8_t* data, uint32_t len) {
|
||||
uint32_t off = 0;
|
||||
while (len > 0) {
|
||||
uint8_t frag = (len > 252) ? 252 : (uint8_t)len;
|
||||
|
||||
// Fragment: [type][up to 252 data bytes]
|
||||
uint8_t buf[253];
|
||||
buf[0] = fragmentType;
|
||||
memcpy(&buf[1], data + off, frag);
|
||||
|
||||
// The Intel bootloader does NOT return a Command Complete for
|
||||
// 0xFC09 (Linux's btusb injects a fake one). Pacing comes from the
|
||||
// synchronous USB transfer in SendCommand; the download outcome is
|
||||
// reported asynchronously via the 0xFF/0x06 secure-send result
|
||||
// event. So: send, drain events, and move on -- do not block per
|
||||
// fragment waiting for a reply that never arrives.
|
||||
if (!SendCommand(OP_INTEL_SECURE_SEND, buf, (uint8_t)(frag + 1))) {
|
||||
KernelLogStream(ERROR, "BT-HCI") << "Secure send transport error: type="
|
||||
<< base::hex << (uint64_t)fragmentType << " cc=" << (uint64_t)g_lastControlCC
|
||||
<< base::dec << " at frag #" << (uint64_t)g_ssFragsSent
|
||||
<< " byte " << g_ssBytesSent;
|
||||
return false;
|
||||
}
|
||||
Xhci::PollEvents();
|
||||
|
||||
g_ssBytesSent += frag;
|
||||
g_ssFragsSent++;
|
||||
|
||||
len -= frag;
|
||||
off += frag;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool IntelBootFirmware(uint32_t bootAddr, uint32_t timeoutMs) {
|
||||
g_intelBootup = false;
|
||||
|
||||
// intel_reset: reset_type=0x00, patch_enable=0x01, ddc_reload=0x00,
|
||||
// boot_option=0x01 (boot at specified address), boot_param (LE32).
|
||||
uint8_t params[8] = {
|
||||
0x00, 0x01, 0x00, 0x01,
|
||||
(uint8_t)(bootAddr & 0xFF),
|
||||
(uint8_t)((bootAddr >> 8) & 0xFF),
|
||||
(uint8_t)((bootAddr >> 16) & 0xFF),
|
||||
(uint8_t)((bootAddr >> 24) & 0xFF),
|
||||
};
|
||||
|
||||
// Fire and forget: the controller reboots into operational firmware
|
||||
// and signals readiness via the Intel bootup vendor event rather than
|
||||
// a Command Complete for 0xFC01.
|
||||
if (!SendCommand(OP_INTEL_RESET, params, sizeof(params))) return false;
|
||||
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
if (g_intelBootup) return true;
|
||||
for (int j = 0; j < 100; j++) asm volatile("" ::: "memory");
|
||||
}
|
||||
|
||||
KernelLogStream(ERROR, "BT-HCI") << "Timed out waiting for Intel bootup event";
|
||||
return false;
|
||||
}
|
||||
|
||||
bool IntelWriteDdcRecord(const uint8_t* record, uint8_t recordLen) {
|
||||
if (!record || recordLen == 0) return false;
|
||||
if (!SendCommand(OP_INTEL_DDC_CONFIG_WRITE, record, recordLen)) return false;
|
||||
uint8_t st[4] = {};
|
||||
if (!WaitCommandComplete(OP_INTEL_DDC_CONFIG_WRITE, st, sizeof(st), 2000)) return false;
|
||||
return st[0] == 0;
|
||||
}
|
||||
|
||||
bool IntelSetEventMask() {
|
||||
// Enables the Intel vendor events used during/after firmware load.
|
||||
uint8_t mask[8] = { 0x87, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
|
||||
if (!SendCommand(OP_INTEL_SET_EVENT_MASK, mask, sizeof(mask))) return false;
|
||||
return WaitCommandComplete(OP_INTEL_SET_EVENT_MASK, nullptr, 0, 2000);
|
||||
}
|
||||
|
||||
bool WriteLocalName(const char* name) {
|
||||
uint8_t params[248] = {};
|
||||
int i = 0;
|
||||
@@ -695,6 +1213,24 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
return WaitCommandStatus(OP_ACCEPT_CONN_REQ);
|
||||
}
|
||||
|
||||
bool AuthenticateLink(uint16_t handle) {
|
||||
// Request authentication on an established ACL link. As the connection
|
||||
// initiator we must drive this: a bonded headset will not start pairing
|
||||
// on its own for a device it believes it already knows, so the link just
|
||||
// sits unauthenticated until it drops (reason 0x05). This kicks off the
|
||||
// flow -- the controller raises a Link Key Request (we answer with a
|
||||
// stored key, or negatively to force fresh Secure Simple Pairing).
|
||||
uint8_t params[2] = { (uint8_t)(handle & 0xFF), (uint8_t)(handle >> 8) };
|
||||
if (!SendCommand(OP_AUTH_REQUESTED, params, 2)) return false;
|
||||
return WaitCommandStatus(OP_AUTH_REQUESTED, 2000);
|
||||
}
|
||||
|
||||
bool SetBdAddr(const uint8_t* addr) {
|
||||
// Intel 0xFC31: 6-byte BD_ADDR, little-endian (same order as ReadBdAddr).
|
||||
if (!SendCommand(OP_INTEL_WRITE_BD_ADDR, addr, 6)) return false;
|
||||
return WaitCommandComplete(OP_INTEL_WRITE_BD_ADDR, nullptr, 0, 2000);
|
||||
}
|
||||
|
||||
bool Disconnect(uint16_t handle, uint8_t reason) {
|
||||
uint8_t params[3] = {
|
||||
(uint8_t)(handle & 0xFF),
|
||||
@@ -786,12 +1322,32 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
g_eventReady = false;
|
||||
}
|
||||
|
||||
// Drain ACL data
|
||||
if (g_aclRxReady) {
|
||||
g_aclRxReady = false;
|
||||
if (g_aclRxLen > 0) {
|
||||
ProcessAcl(g_aclRxBuf, g_aclRxLen);
|
||||
// Drain all queued ACL packets (process the whole ring, not just one,
|
||||
// so a burst is never left unhandled).
|
||||
while (g_aclRxTail != g_aclRxHead) {
|
||||
uint8_t slot = g_aclRxTail;
|
||||
uint16_t pl = g_aclRxLens[slot];
|
||||
|
||||
// Diagnostic (top-level, safe to log -- not the IRQ path): trace the
|
||||
// first few ACL packets so the rx flood / missing Config Response is
|
||||
// identifiable in ONE boot. Layout: ACL header(4) + L2CAP header(4),
|
||||
// so the L2CAP CID is at bytes 6-7; on the signaling channel (CID 1)
|
||||
// byte 8 is the command code (0x03 CONN_RSP, 0x04 CONFIG_REQ,
|
||||
// 0x05 CONFIG_RSP). A flood of len==maxpacket junk CIDs vs real
|
||||
// cid=0001 code=05 packets tells the two failure modes apart.
|
||||
static uint32_t s_rxTraced = 0;
|
||||
if (s_rxTraced < 24 && pl >= 8) {
|
||||
const uint8_t* d = g_aclRxRing[slot];
|
||||
uint16_t cid = (uint16_t)d[6] | ((uint16_t)d[7] << 8);
|
||||
uint8_t code = (pl >= 9) ? d[8] : 0;
|
||||
KernelLogStream(INFO, "BT-HCI") << "rx[" << (uint64_t)s_rxTraced
|
||||
<< "] len=" << (uint64_t)pl << " cid=" << base::hex << (uint64_t)cid
|
||||
<< " code=" << (uint64_t)code << base::dec;
|
||||
s_rxTraced++;
|
||||
}
|
||||
|
||||
ProcessAcl(g_aclRxRing[slot], pl);
|
||||
g_aclRxTail = (uint8_t)((g_aclRxTail + 1) % ACL_RX_SLOTS);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -30,6 +30,8 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
constexpr uint16_t OP_DISCONNECT = 0x0406;
|
||||
constexpr uint16_t OP_ACCEPT_CONN_REQ = 0x0409;
|
||||
constexpr uint16_t OP_REJECT_CONN_REQ = 0x040A;
|
||||
constexpr uint16_t OP_LINK_KEY_REQ_REPLY = 0x040B;
|
||||
constexpr uint16_t OP_LINK_KEY_REQ_NEG_REPLY = 0x040C;
|
||||
constexpr uint16_t OP_AUTH_REQUESTED = 0x0411;
|
||||
constexpr uint16_t OP_SET_CONN_ENCRYPT = 0x0413;
|
||||
constexpr uint16_t OP_IO_CAPABILITY_REPLY = 0x042B;
|
||||
@@ -63,6 +65,8 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
constexpr uint16_t OP_INTEL_RESET = 0xFC01;
|
||||
constexpr uint16_t OP_INTEL_SET_EVENT_MASK = 0xFC52;
|
||||
constexpr uint16_t OP_INTEL_DDC_CONFIG_WRITE = 0xFC8B;
|
||||
constexpr uint16_t OP_INTEL_SECURE_SEND = 0xFC09;
|
||||
constexpr uint16_t OP_INTEL_WRITE_BD_ADDR = 0xFC31; // set adapter BD_ADDR
|
||||
|
||||
// =========================================================================
|
||||
// HCI event codes
|
||||
@@ -75,6 +79,8 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
constexpr uint8_t EVT_DISCONNECTION_COMPLETE = 0x05;
|
||||
constexpr uint8_t EVT_AUTH_COMPLETE = 0x06;
|
||||
constexpr uint8_t EVT_ENCRYPT_CHANGE = 0x08;
|
||||
constexpr uint8_t EVT_LINK_KEY_REQUEST = 0x17;
|
||||
constexpr uint8_t EVT_LINK_KEY_NOTIFICATION = 0x18;
|
||||
constexpr uint8_t EVT_COMMAND_COMPLETE = 0x0E;
|
||||
constexpr uint8_t EVT_COMMAND_STATUS = 0x0F;
|
||||
constexpr uint8_t EVT_NUM_COMPLETED_PACKETS = 0x13;
|
||||
@@ -163,7 +169,10 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
// Initialize HCI transport over USB for the given slot
|
||||
void Initialize(uint8_t slotId);
|
||||
|
||||
// Start receiving HCI events and ACL data (call after HCI init sequence)
|
||||
// Start receiving HCI events and ACL data (call after HCI init sequence).
|
||||
// Arms both the interrupt IN and the bulk IN; the bulk IN must stay armed
|
||||
// through the firmware download (it absorbs the device's ~635 KB cc=4 glitch
|
||||
// and keeps the event pipe alive -- see the definition).
|
||||
void StartEventPipe();
|
||||
|
||||
// Send an HCI command via USB control transfer (EP0)
|
||||
@@ -212,6 +221,53 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
// Read Intel-specific version info
|
||||
bool ReadIntelVersion(IntelVersion* ver);
|
||||
|
||||
// =========================================================================
|
||||
// Intel firmware download primitives (bootloader mode)
|
||||
// =========================================================================
|
||||
|
||||
// Read the Intel version response in TLV format (0xFC05 with parameter
|
||||
// 0xFF). Copies the raw return parameters (byte 0 = status, followed by
|
||||
// the TLV stream) into outBuf, bounded by the number of bytes actually
|
||||
// received from the controller. Returns that length, or -1 on failure.
|
||||
int ReadIntelVersionTlv(uint8_t* outBuf, int maxLen);
|
||||
|
||||
// Intel "Secure Send" (0xFC09): pushes one logical fragment to the
|
||||
// bootloader, split into <=252-byte chunks each prefixed with the
|
||||
// fragment type (0x00 CSS init, 0x01 firmware data, 0x02 signature,
|
||||
// 0x03 public key). The bootloader does not Command-Complete these; pacing
|
||||
// is by USB transfer completion and the result arrives asynchronously as a
|
||||
// 0xFF/0x06 secure-send result event (see WaitSecureSendResult).
|
||||
bool IntelSecureSend(uint8_t fragmentType, const uint8_t* data, uint32_t len);
|
||||
|
||||
// Reset / await the Intel "secure send result" vendor event (0xFF/0x06).
|
||||
// Call ClearSecureSendResult() before a download phase, then
|
||||
// WaitSecureSendResult() to read the outcome (result/status, 0 = success).
|
||||
void ClearSecureSendResult();
|
||||
bool WaitSecureSendResult(uint32_t timeoutMs, uint8_t* outResult, uint8_t* outStatus);
|
||||
|
||||
// Bonded-device link key persistence (so pairings survive reboots).
|
||||
// LoadLinkKeys(): read the on-disk store once VFS is up.
|
||||
// FlushLinkKeys(): write the store to disk if it changed -- call from
|
||||
// process context (NOT an event handler), since it does blocking disk I/O.
|
||||
void LoadLinkKeys();
|
||||
void FlushLinkKeys();
|
||||
|
||||
// Non-blocking peek at the most recent 0xFF/0x06 secure-send result without
|
||||
// consuming it. Returns true if one has arrived since the last
|
||||
// ClearSecureSendResult(). The payload loop uses this to catch a mid-stream
|
||||
// rejection -- a healthy bootloader stays silent until the final fragment.
|
||||
bool PeekSecureSendResult(uint8_t* outResult, uint8_t* outStatus);
|
||||
|
||||
// Reset the controller into operational firmware at bootAddr (0xFC01) and
|
||||
// wait for the Intel "bootup" vendor event. Returns true once booted.
|
||||
bool IntelBootFirmware(uint32_t bootAddr, uint32_t timeoutMs = 5000);
|
||||
|
||||
// Apply one DDC parameter record (record[0] = payload length) via 0xFC8B.
|
||||
bool IntelWriteDdcRecord(const uint8_t* record, uint8_t recordLen);
|
||||
|
||||
// Configure the Intel vendor event mask (0xFC52).
|
||||
bool IntelSetEventMask();
|
||||
|
||||
// Set local name
|
||||
bool WriteLocalName(const char* name);
|
||||
|
||||
@@ -227,6 +283,33 @@ namespace Drivers::USB::Bluetooth::Hci {
|
||||
// Accept an incoming connection
|
||||
bool AcceptConnection(const uint8_t* bdAddr, uint8_t role);
|
||||
|
||||
// Request authentication on an ACL link (we are the connection initiator).
|
||||
// Drives Link Key Request -> pairing; needed for bonded-device reconnects.
|
||||
bool AuthenticateLink(uint16_t handle);
|
||||
|
||||
// Set the adapter's BD_ADDR (Intel vendor command 0xFC31). Used to dodge a
|
||||
// remote that holds a stale, un-clearable bond to our real address.
|
||||
bool SetBdAddr(const uint8_t* addr);
|
||||
|
||||
// Lockless HCI-event trace (diagnostic). Reset before a connection attempt,
|
||||
// dump afterwards from top-level to see the pairing/SSP event sequence.
|
||||
void ResetEventTrace();
|
||||
void DumpEventTrace();
|
||||
|
||||
// Send any queued pairing replies (IO-cap / user-confirm / link-key) with
|
||||
// real confirmed transfers. Call from top-level (e.g. the connect loop),
|
||||
// NOT from an event handler -- event handlers only enqueue.
|
||||
void ProcessPendingCommands();
|
||||
|
||||
// Diagnostic: print ACL data-path counters (tx / txDone / rx / pending).
|
||||
void DumpAclStats();
|
||||
|
||||
// ACL TX flow control: outstanding (un-acked) ACL packets, and the
|
||||
// controller's ACL buffer count (Number-Of-Completed-Packets credits). The
|
||||
// media writer throttles on these so it never overruns the controller.
|
||||
uint16_t AclPendingCount();
|
||||
uint16_t AclMaxPackets();
|
||||
|
||||
// Disconnect a connection
|
||||
bool Disconnect(uint16_t handle, uint8_t reason);
|
||||
|
||||
|
||||
@@ -0,0 +1,470 @@
|
||||
/*
|
||||
* IntelFirmware.cpp
|
||||
* Intel Bluetooth bootloader firmware download path
|
||||
*
|
||||
* Ports the btintel "TLV" bootloader sequence: read the version, derive the
|
||||
* ibt-<cnvi>-<cnvr>.sfi / .ddc file names, secure-send the signed RSA/ECDSA
|
||||
* header and command-buffer payload, boot the operational image and apply
|
||||
* the DDC parameters. The .sfi/.ddc files are staged on the ramdisk at
|
||||
* 0:/os/firmware/intel/ by the userspace build.
|
||||
*
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "IntelFirmware.hpp"
|
||||
#include "Hci.hpp"
|
||||
#include <Fs/Vfs.hpp>
|
||||
#include <Memory/Heap.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
#include <Timekeeping/ApicTimer.hpp>
|
||||
|
||||
using namespace Kt;
|
||||
|
||||
namespace Drivers::USB::Bluetooth {
|
||||
|
||||
// =========================================================================
|
||||
// .sfi / TLV format constants (from drivers/bluetooth/btintel.{c,h})
|
||||
// =========================================================================
|
||||
|
||||
// CSS secure-boot header layout.
|
||||
static constexpr uint32_t RSA_HEADER_LEN = 644; // RSA header + key + sig
|
||||
static constexpr uint32_t ECDSA_HEADER_LEN = 320; // follows the RSA header
|
||||
static constexpr uint32_t ECDSA_OFFSET = 644;
|
||||
static constexpr uint32_t CSS_HEADER_OFFSET = 8; // version field offset
|
||||
|
||||
static constexpr uint32_t RSA_HEADER_VER = 0x00010000;
|
||||
static constexpr uint32_t HYBRID_HEADER_VER = 0x00069700;
|
||||
static constexpr uint32_t ECDSA_HEADER_VER = 0x00020000;
|
||||
|
||||
// The operational image embeds its reset/boot address in a final
|
||||
// CMD_WRITE_BOOT_PARAMS HCI command.
|
||||
static constexpr uint16_t CMD_WRITE_BOOT_PARAMS = 0xFC0E;
|
||||
|
||||
// Image type (INTEL_TLV_IMAGE_TYPE value): 0x01 bootloader, 0x02
|
||||
// intermediate loader, 0x03 operational.
|
||||
static constexpr uint8_t IMG_BOOTLOADER = 0x01;
|
||||
static constexpr uint8_t IMG_OPERATIONAL = 0x03;
|
||||
|
||||
// Version TLV type ids (enum INTEL_TLV_* starts at 0x10).
|
||||
static constexpr uint8_t TLV_CNVI_TOP = 0x10;
|
||||
static constexpr uint8_t TLV_CNVR_TOP = 0x11;
|
||||
static constexpr uint8_t TLV_CNVI_BT = 0x12;
|
||||
static constexpr uint8_t TLV_IMAGE_TYPE = 0x1C;
|
||||
static constexpr uint8_t TLV_MIN_FW = 0x2D;
|
||||
static constexpr uint8_t TLV_SBE_TYPE = 0x2F;
|
||||
|
||||
// =========================================================================
|
||||
// Helpers
|
||||
// =========================================================================
|
||||
|
||||
static uint32_t Rd32(const uint8_t* p) {
|
||||
return (uint32_t)p[0] | ((uint32_t)p[1] << 8)
|
||||
| ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
|
||||
}
|
||||
|
||||
struct IntelTlvVersion {
|
||||
uint32_t CnviTop = 0;
|
||||
uint32_t CnvrTop = 0;
|
||||
uint32_t CnviBt = 0;
|
||||
uint8_t ImgType = 0;
|
||||
uint8_t SbeType = 0xFF; // 0x00 RSA, 0x01 ECDSA, 0xFF unknown
|
||||
bool SbeValid = false;
|
||||
bool Valid = false;
|
||||
};
|
||||
|
||||
static void ParseTlv(const uint8_t* buf, int len, IntelTlvVersion* v) {
|
||||
int i = 0;
|
||||
while (i + 2 <= len) {
|
||||
uint8_t type = buf[i];
|
||||
uint8_t l = buf[i + 1];
|
||||
if (i + 2 + l > len) break;
|
||||
const uint8_t* val = &buf[i + 2];
|
||||
|
||||
switch (type) {
|
||||
case TLV_CNVI_TOP: if (l >= 4) v->CnviTop = Rd32(val); break;
|
||||
case TLV_CNVR_TOP: if (l >= 4) v->CnvrTop = Rd32(val); break;
|
||||
case TLV_CNVI_BT: if (l >= 4) v->CnviBt = Rd32(val); break;
|
||||
case TLV_IMAGE_TYPE: if (l >= 1) v->ImgType = val[0]; break;
|
||||
case TLV_SBE_TYPE: if (l >= 1) { v->SbeType = val[0]; v->SbeValid = true; } break;
|
||||
default: break;
|
||||
}
|
||||
i += 2 + l;
|
||||
}
|
||||
// CNVI top is always present on a real TLV response.
|
||||
v->Valid = (v->CnviTop != 0);
|
||||
}
|
||||
|
||||
// ibt-<cnvi>-<cnvr> packing: __swab16(((top & 0xfff) << 4) | ((top >> 24) & 0xf))
|
||||
static uint16_t CnvxPack(uint32_t top) {
|
||||
uint16_t t = (uint16_t)(top & 0x0FFF);
|
||||
uint8_t s = (uint8_t)((top >> 24) & 0x0F);
|
||||
uint16_t packed = (uint16_t)((t << 4) | s);
|
||||
return (uint16_t)((packed >> 8) | (packed << 8)); // __swab16
|
||||
}
|
||||
|
||||
static uint8_t HwVariant(uint32_t cnviBt) {
|
||||
return (uint8_t)((cnviBt >> 16) & 0x3F);
|
||||
}
|
||||
|
||||
static char HexDigit(uint8_t n) {
|
||||
return (char)(n < 10 ? ('0' + n) : ('a' + n - 10));
|
||||
}
|
||||
|
||||
// Build "0:/os/firmware/intel/ibt-<cnvi>-<cnvr>.<ext>" into out (>= 48 bytes).
|
||||
static void BuildFwPath(char* out, uint16_t cnvi, uint16_t cnvr, const char* ext) {
|
||||
const char* prefix = "0:/os/firmware/intel/ibt-";
|
||||
char* p = out;
|
||||
while (*prefix) *p++ = *prefix++;
|
||||
for (int sh = 12; sh >= 0; sh -= 4) *p++ = HexDigit((cnvi >> sh) & 0xF);
|
||||
*p++ = '-';
|
||||
for (int sh = 12; sh >= 0; sh -= 4) *p++ = HexDigit((cnvr >> sh) & 0xF);
|
||||
*p++ = '.';
|
||||
while (*ext) *p++ = *ext++;
|
||||
*p = '\0';
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Secure-boot header transmission
|
||||
// =========================================================================
|
||||
|
||||
// Try one secure-boot header type. Layout:
|
||||
// RSA : CSS 128 @0, pkey 256 @128, sig 256 @388
|
||||
// ECDSA: CSS 128 @644, pkey 96 @772, sig 96 @868 (after the RSA hdr)
|
||||
//
|
||||
// The CSS init fragment is sent and checked FIRST: a non-zero secure-send
|
||||
// result (0xFF/0x06) means the controller's secure-boot engine rejected
|
||||
// this header type, so we bail before blasting the ~200-512 byte key and
|
||||
// signature -- sending those after a rejected CSS wedges EP0 and (since
|
||||
// this runs on the boot path) freezes the kernel. Returns true only if the
|
||||
// full header is accepted (or the controller stays silent, in which case
|
||||
// the payload + bootup event remain the final gate).
|
||||
static bool TryHeader(const uint8_t* fw, bool ecdsa) {
|
||||
const uint32_t cssOff = ecdsa ? 644 : 0;
|
||||
const uint32_t pkeyOff = ecdsa ? 644 + 128 : 128;
|
||||
const uint32_t pkeyLen = ecdsa ? 96 : 256;
|
||||
const uint32_t sigOff = ecdsa ? 644 + 224 : 388;
|
||||
const uint32_t sigLen = ecdsa ? 96 : 256;
|
||||
const char* name = ecdsa ? "ECDSA" : "RSA";
|
||||
|
||||
KernelLogStream(INFO, "BT-FW") << "Trying " << name << " secure-boot header";
|
||||
|
||||
uint8_t result = 0, status = 0;
|
||||
|
||||
// 1. CSS header init -- check the result before committing key/sig.
|
||||
Hci::ClearSecureSendResult();
|
||||
if (!Hci::IntelSecureSend(0x00, fw + cssOff, 128)) return false;
|
||||
if (Hci::WaitSecureSendResult(1500, &result, &status) && (result || status)) {
|
||||
KernelLogStream(WARNING, "BT-FW") << name << " CSS rejected (result="
|
||||
<< base::hex << (uint64_t)result << " status=" << (uint64_t)status
|
||||
<< base::dec << ")";
|
||||
return false;
|
||||
}
|
||||
|
||||
// 2. Public key + signature.
|
||||
Hci::ClearSecureSendResult();
|
||||
if (!Hci::IntelSecureSend(0x03, fw + pkeyOff, pkeyLen)) return false;
|
||||
if (!Hci::IntelSecureSend(0x02, fw + sigOff, sigLen)) return false;
|
||||
if (Hci::WaitSecureSendResult(2000, &result, &status) && (result || status)) {
|
||||
KernelLogStream(WARNING, "BT-FW") << name << " header rejected (result="
|
||||
<< base::hex << (uint64_t)result << " status=" << (uint64_t)status
|
||||
<< base::dec << ")";
|
||||
return false;
|
||||
}
|
||||
|
||||
KernelLogStream(OK, "BT-FW") << name << " secure-boot header accepted";
|
||||
return true;
|
||||
}
|
||||
|
||||
// Push the command-buffer payload starting at `offset`. Fragments are cut
|
||||
// on HCI command boundaries and only flushed when 4-byte aligned, matching
|
||||
// btintel_download_firmware_payload().
|
||||
static bool SendPayload(const uint8_t* fw, uint64_t size, uint64_t offset) {
|
||||
uint64_t fwPtr = offset;
|
||||
uint32_t fragLen = 0;
|
||||
uint32_t sends = 0;
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
|
||||
while (fwPtr < size) {
|
||||
// Safety net: never let a stalled upload freeze the boot path.
|
||||
if (Timekeeping::GetMilliseconds() - start > 30000) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Payload upload stalled at " << fwPtr - offset
|
||||
<< "/" << (size - offset) << " bytes; aborting";
|
||||
return false;
|
||||
}
|
||||
uint64_t cmd = fwPtr + fragLen;
|
||||
if (cmd + 3 > size) break; // truncated header
|
||||
uint8_t plen = fw[cmd + 2];
|
||||
fragLen += 3 + plen;
|
||||
if (fwPtr + fragLen > size) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Firmware payload command overruns file";
|
||||
return false;
|
||||
}
|
||||
|
||||
if ((fragLen % 4) == 0) {
|
||||
if (!Hci::IntelSecureSend(0x01, fw + fwPtr, fragLen)) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Payload secure-send failed at offset "
|
||||
<< fwPtr;
|
||||
return false;
|
||||
}
|
||||
fwPtr += fragLen;
|
||||
fragLen = 0;
|
||||
|
||||
// A healthy bootloader only emits an 0xFF/0x06 result on the
|
||||
// final fragment. One arriving mid-stream with a non-zero
|
||||
// result/status means it rejected what we just sent -- surface
|
||||
// the exact offset instead of blasting the rest of the image
|
||||
// into a wedged device (which then NAKs to a cc=4 on EP0).
|
||||
uint8_t r = 0, s = 0;
|
||||
if (Hci::PeekSecureSendResult(&r, &s) && (r || s)) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Bootloader rejected payload at byte "
|
||||
<< (fwPtr - offset) << "/" << (size - offset) << " (result="
|
||||
<< base::hex << (uint64_t)r << " status=" << (uint64_t)s
|
||||
<< base::dec << ")";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Coarse progress so a slow/stalled upload is distinguishable
|
||||
// from steady progress (one line per ~512 fragments).
|
||||
if ((++sends % 512) == 0) {
|
||||
KernelLogStream(INFO, "BT-FW") << " payload " << fwPtr - offset
|
||||
<< "/" << (size - offset) << " bytes (" << (uint64_t)sends << " frags)";
|
||||
}
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Walk the command stream from the start of the image to recover the boot
|
||||
// address embedded in the CMD_WRITE_BOOT_PARAMS command.
|
||||
static bool FindBootAddress(const uint8_t* fw, uint64_t size, uint32_t* outAddr) {
|
||||
uint64_t p = 0;
|
||||
while (p + 3 <= size) {
|
||||
uint16_t op = (uint16_t)fw[p] | ((uint16_t)fw[p + 1] << 8);
|
||||
uint8_t plen = fw[p + 2];
|
||||
if (op == CMD_WRITE_BOOT_PARAMS) {
|
||||
if (p + 3 + 4 > size) return false;
|
||||
*outAddr = Rd32(&fw[p + 3]);
|
||||
KernelLogStream(INFO, "BT-FW") << "Boot address: " << base::hex
|
||||
<< (uint64_t)*outAddr << base::dec
|
||||
<< " (fw build " << (uint64_t)fw[p + 7] << "-"
|
||||
<< (uint64_t)fw[p + 8] << "." << (uint64_t)fw[p + 9] << ")";
|
||||
return true;
|
||||
}
|
||||
p += 3 + plen;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// DDC parameter application (best effort)
|
||||
// =========================================================================
|
||||
|
||||
static void ApplyDdc(uint16_t cnvi, uint16_t cnvr) {
|
||||
char path[48];
|
||||
BuildFwPath(path, cnvi, cnvr, "ddc");
|
||||
|
||||
Fs::Vfs::BackendFile file;
|
||||
if (Fs::Vfs::OpenBackendFile(path, file) < 0) {
|
||||
KernelLogStream(INFO, "BT-FW") << "No DDC parameters (" << path << ")";
|
||||
return;
|
||||
}
|
||||
|
||||
uint64_t size = Fs::Vfs::GetBackendFileSize(file);
|
||||
if (size == 0 || size > 4096) {
|
||||
Fs::Vfs::CloseBackendFile(file);
|
||||
return;
|
||||
}
|
||||
|
||||
uint8_t* ddc = (uint8_t*)Memory::g_heap->Request(size);
|
||||
if (!ddc) {
|
||||
Fs::Vfs::CloseBackendFile(file);
|
||||
return;
|
||||
}
|
||||
Fs::Vfs::ReadBackendFile(file, ddc, 0, size);
|
||||
Fs::Vfs::CloseBackendFile(file);
|
||||
|
||||
// Each record is [len][len bytes of parameters]; the whole record is
|
||||
// sent as the 0xFC8B payload.
|
||||
uint64_t p = 0;
|
||||
int applied = 0;
|
||||
while (p < size) {
|
||||
uint8_t total = (uint8_t)(ddc[p] + 1);
|
||||
if (p + total > size) break;
|
||||
if (Hci::IntelWriteDdcRecord(&ddc[p], total)) applied++;
|
||||
p += total;
|
||||
}
|
||||
|
||||
Memory::g_heap->Free(ddc);
|
||||
KernelLogStream(OK, "BT-FW") << "Applied " << (uint64_t)applied << " DDC record(s)";
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Download orchestration
|
||||
// =========================================================================
|
||||
|
||||
static bool DownloadImage(const uint8_t* fw, uint64_t size,
|
||||
const IntelTlvVersion& ver,
|
||||
uint16_t cnvi, uint16_t cnvr) {
|
||||
if (size < RSA_HEADER_LEN + 16) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Firmware image too small (" << size << " bytes)";
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t cssVer = Rd32(&fw[CSS_HEADER_OFFSET]);
|
||||
if (cssVer != RSA_HEADER_VER && cssVer != HYBRID_HEADER_VER) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Invalid CSS header version: "
|
||||
<< base::hex << (uint64_t)cssVer << base::dec;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t bootAddr = 0;
|
||||
if (!FindBootAddress(fw, size, &bootAddr)) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Could not locate boot address in firmware";
|
||||
return false;
|
||||
}
|
||||
|
||||
uint8_t hw = HwVariant(ver.CnviBt);
|
||||
uint64_t payloadOffset;
|
||||
bool headerOk = false;
|
||||
|
||||
if (hw <= 0x14) {
|
||||
// RSA-only parts: RSA header then command buffer.
|
||||
headerOk = TryHeader(fw, false);
|
||||
payloadOffset = RSA_HEADER_LEN;
|
||||
} else {
|
||||
// 0x17+ parts ship RSA(644) + ECDSA(320) headers; the command
|
||||
// buffer follows both. sbe_type selects which header the part's
|
||||
// secure-boot engine expects.
|
||||
if (size < RSA_HEADER_LEN + ECDSA_HEADER_LEN || fw[ECDSA_OFFSET] != 0x06) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Missing ECDSA header for hw variant "
|
||||
<< base::hex << (uint64_t)hw << base::dec;
|
||||
return false;
|
||||
}
|
||||
payloadOffset = RSA_HEADER_LEN + ECDSA_HEADER_LEN;
|
||||
|
||||
// Honor sbe_type if the version reply included it; otherwise (the
|
||||
// common case here -- the reply is truncated to one USB packet so
|
||||
// sbe_type is missing) try ECDSA first. These newer parts
|
||||
// (AX210/AX211, hw_variant >= 0x17) use the ECDSA secure-boot
|
||||
// engine, and TryHeader checks the CSS result before committing the
|
||||
// key/signature, so a wrong guess fails cleanly instead of wedging.
|
||||
bool order[2];
|
||||
int n;
|
||||
if (ver.SbeValid && ver.SbeType == 0x00) { order[0] = false; n = 1; } // RSA
|
||||
else if (ver.SbeValid && ver.SbeType == 0x01) { order[0] = true; n = 1; } // ECDSA
|
||||
else { order[0] = true; order[1] = false; n = 2; } // ECDSA, then RSA
|
||||
|
||||
for (int k = 0; k < n && !headerOk; k++) {
|
||||
headerOk = TryHeader(fw, order[k]);
|
||||
}
|
||||
}
|
||||
|
||||
if (!headerOk) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Secure-boot header rejected";
|
||||
return false;
|
||||
}
|
||||
|
||||
KernelLogStream(INFO, "BT-FW") << "Downloading firmware payload ("
|
||||
<< (size - payloadOffset) << " bytes)";
|
||||
Hci::ClearSecureSendResult(); // discard header-era result
|
||||
if (!SendPayload(fw, size, payloadOffset)) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Firmware payload download failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
// The bootloader signals download completion with a final 0xFF/0x06
|
||||
// secure-send result. Wait for it (and check it) before booting.
|
||||
uint8_t r = 0, s = 0;
|
||||
if (Hci::WaitSecureSendResult(5000, &r, &s)) {
|
||||
if (r != 0 || s != 0) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Firmware download failed (result="
|
||||
<< base::hex << (uint64_t)r << " status=" << (uint64_t)s
|
||||
<< base::dec << ")";
|
||||
return false;
|
||||
}
|
||||
KernelLogStream(OK, "BT-FW") << "Firmware download complete";
|
||||
} else {
|
||||
KernelLogStream(INFO, "BT-FW") << "No download-complete event; booting anyway";
|
||||
}
|
||||
|
||||
KernelLogStream(INFO, "BT-FW") << "Booting operational firmware";
|
||||
if (!Hci::IntelBootFirmware(bootAddr)) return false;
|
||||
|
||||
KernelLogStream(OK, "BT-FW") << "Operational firmware booted";
|
||||
|
||||
// DDC parameters are applied against the now-running firmware.
|
||||
ApplyDdc(cnvi, cnvr);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DownloadIntelFirmware() {
|
||||
// 1. Read the TLV version to learn the SKU and current image type.
|
||||
uint8_t raw[256];
|
||||
int rawLen = Hci::ReadIntelVersionTlv(raw, sizeof(raw));
|
||||
if (rawLen < 1 || raw[0] != 0) {
|
||||
KernelLogStream(WARNING, "BT-FW") << "Could not read Intel TLV version";
|
||||
return false;
|
||||
}
|
||||
|
||||
IntelTlvVersion ver;
|
||||
ParseTlv(raw + 1, rawLen - 1, &ver);
|
||||
if (!ver.Valid) {
|
||||
KernelLogStream(WARNING, "BT-FW") << "Unrecognized Intel version response";
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t cnvi = CnvxPack(ver.CnviTop);
|
||||
uint16_t cnvr = CnvxPack(ver.CnvrTop);
|
||||
|
||||
KernelLogStream(INFO, "BT-FW") << "Intel CNVi=" << base::hex << (uint64_t)cnvi
|
||||
<< " CNVr=" << (uint64_t)cnvr << " hw_variant=" << (uint64_t)HwVariant(ver.CnviBt)
|
||||
<< " img_type=" << (uint64_t)ver.ImgType << base::dec;
|
||||
|
||||
// Diagnostic: how much of the TLV version response we actually received
|
||||
// (a value near 60 means the controller's reply was truncated to one
|
||||
// interrupt packet), and whether sbe_type made it through.
|
||||
KernelLogStream(INFO, "BT-FW") << "TLV bytes=" << (uint64_t)rawLen
|
||||
<< " sbe_valid=" << (uint64_t)(ver.SbeValid ? 1 : 0)
|
||||
<< " sbe_type=" << base::hex << (uint64_t)ver.SbeType << base::dec;
|
||||
|
||||
// 2. If the operational image is already running, nothing to do.
|
||||
if (ver.ImgType == IMG_OPERATIONAL) {
|
||||
KernelLogStream(OK, "BT-FW") << "Operational firmware already loaded";
|
||||
return true;
|
||||
}
|
||||
if (ver.ImgType != IMG_BOOTLOADER) {
|
||||
KernelLogStream(WARNING, "BT-FW") << "Unexpected image type, attempting download anyway";
|
||||
}
|
||||
|
||||
// 3. Load the .sfi image from the ramdisk.
|
||||
char sfiPath[48];
|
||||
BuildFwPath(sfiPath, cnvi, cnvr, "sfi");
|
||||
KernelLogStream(INFO, "BT-FW") << "Loading firmware: " << sfiPath;
|
||||
|
||||
Fs::Vfs::BackendFile file;
|
||||
if (Fs::Vfs::OpenBackendFile(sfiPath, file) < 0) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Firmware not found: " << sfiPath;
|
||||
KernelLogStream(WARNING, "BT-FW") << "Bluetooth stays in bootloader mode (no firmware)";
|
||||
return false;
|
||||
}
|
||||
|
||||
uint64_t size = Fs::Vfs::GetBackendFileSize(file);
|
||||
uint8_t* fw = (uint8_t*)Memory::g_heap->Request(size);
|
||||
if (!fw) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Failed to allocate " << size << " bytes for firmware";
|
||||
Fs::Vfs::CloseBackendFile(file);
|
||||
return false;
|
||||
}
|
||||
Fs::Vfs::ReadBackendFile(file, fw, 0, size);
|
||||
Fs::Vfs::CloseBackendFile(file);
|
||||
asm volatile("" ::: "memory");
|
||||
|
||||
// 4. Run the secure download + boot sequence.
|
||||
bool ok = DownloadImage(fw, size, ver, cnvi, cnvr);
|
||||
Memory::g_heap->Free(fw);
|
||||
return ok;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,27 @@
|
||||
/*
|
||||
* IntelFirmware.hpp
|
||||
* Intel Bluetooth bootloader firmware download path
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
|
||||
namespace Drivers::USB::Bluetooth {
|
||||
|
||||
// Drive the Intel bootloader firmware download sequence for the currently
|
||||
// registered adapter:
|
||||
// 1. Read the TLV version and decide whether a download is required.
|
||||
// 2. Derive the firmware/DDC file names (ibt-<cnvi>-<cnvr>.{sfi,ddc})
|
||||
// and load them from 0:/os/firmware/intel/.
|
||||
// 3. Secure-send the signed header + command-buffer payload (0xFC09).
|
||||
// 4. Boot the operational firmware (0xFC01) and wait for the bootup
|
||||
// event, then apply DDC parameters (0xFC8B).
|
||||
//
|
||||
// Returns true if the controller is operational afterwards (either it was
|
||||
// already running firmware, or the download completed). Returns false if
|
||||
// the controller is in bootloader mode and the download could not be
|
||||
// completed (e.g. firmware file missing).
|
||||
bool DownloadIntelFirmware();
|
||||
|
||||
}
|
||||
@@ -25,6 +25,15 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
static bool g_initialized = false;
|
||||
static uint8_t g_sigIdentifier = 1;
|
||||
|
||||
// Diagnostic: result of the most recent L2CAP Connection Response we got for
|
||||
// an outgoing connection (0=success, 1=pending, 2=PSM-unsupported,
|
||||
// 3=security-block, 4=no-resources). 0xFFFF = none received yet.
|
||||
static volatile uint16_t g_lastConnRspResult = 0xFFFF;
|
||||
|
||||
// Diagnostic: how many incoming AVDTP connection requests the remote has
|
||||
// made (tells us whether the headset wants to initiate the audio channel).
|
||||
static volatile uint16_t g_incomingAvdtpReqs = 0;
|
||||
|
||||
// Channel table
|
||||
static ChannelInfo g_channels[MAX_CHANNELS] = {};
|
||||
static uint16_t g_nextCid = CID_DYNAMIC_START;
|
||||
@@ -116,14 +125,24 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
if (l2len + sizeof(L2capHeader) > len) return;
|
||||
|
||||
if (cid == CID_SIGNALING) {
|
||||
// L2CAP signaling channel
|
||||
if (l2len < sizeof(SignalHeader)) return;
|
||||
// L2CAP signaling channel. One packet may carry MULTIPLE commands
|
||||
// back to back -- a headset commonly bundles its Config Response
|
||||
// (to our request) with its own Config Request in a single packet.
|
||||
// Loop over all of them, or we'd handle one and silently drop the
|
||||
// rest (which left our channel half-configured: localCfg=0).
|
||||
uint16_t sOff = 0;
|
||||
while (sOff + sizeof(SignalHeader) <= l2len) {
|
||||
auto* sig = (const SignalHeader*)(payload + sOff);
|
||||
const uint8_t* sigPayload = payload + sOff + sizeof(SignalHeader);
|
||||
uint16_t sigPayloadLen = sig->Length;
|
||||
// Clamp (don't bail) so a length that doesn't exactly match the
|
||||
// packet never skips this command -- the cases bounds-check their
|
||||
// own reads. Advancing by the clamped length ends the loop after
|
||||
// the last real command.
|
||||
uint16_t avail = (uint16_t)(l2len - sOff - sizeof(SignalHeader));
|
||||
if (sigPayloadLen > avail) sigPayloadLen = avail;
|
||||
|
||||
auto* sig = (const SignalHeader*)payload;
|
||||
const uint8_t* sigPayload = payload + sizeof(SignalHeader);
|
||||
uint16_t sigPayloadLen = sig->Length;
|
||||
|
||||
switch (sig->Code) {
|
||||
switch (sig->Code) {
|
||||
case SIG_CONN_REQ: {
|
||||
if (sigPayloadLen >= 4) {
|
||||
uint16_t psm = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
||||
@@ -134,6 +153,7 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
|
||||
// Accept connections for AVDTP
|
||||
if (psm == PSM_AVDTP || psm == PSM_SDP) {
|
||||
if (psm == PSM_AVDTP) g_incomingAvdtpReqs++;
|
||||
auto* ch = AllocChannel(psm);
|
||||
if (ch) {
|
||||
ch->RemoteCid = srcCid;
|
||||
@@ -147,6 +167,17 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
rsp[4] = 0; rsp[5] = 0; // Result: success
|
||||
rsp[6] = 0; rsp[7] = 0; // Status: no info
|
||||
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
||||
|
||||
// As the acceptor we must ALSO send our own
|
||||
// Configuration Request -- otherwise our side
|
||||
// never reaches LocalConfigDone and the channel
|
||||
// never becomes Configured (OnChannelReady never
|
||||
// fires). Address the remote's CID.
|
||||
uint8_t cfgReq[4] = {};
|
||||
cfgReq[0] = (uint8_t)(srcCid & 0xFF);
|
||||
cfgReq[1] = (uint8_t)(srcCid >> 8);
|
||||
cfgReq[2] = 0; cfgReq[3] = 0; // Flags
|
||||
SendSignal(SIG_CONFIG_REQ, g_sigIdentifier++, cfgReq, 4);
|
||||
}
|
||||
} else {
|
||||
// Reject: PSM not supported
|
||||
@@ -167,9 +198,15 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
||||
uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8);
|
||||
uint16_t result = (uint16_t)sigPayload[4] | ((uint16_t)sigPayload[5] << 8);
|
||||
// Status (bytes 6-7) explains a PENDING (result=1): 0=no
|
||||
// info, 1=authentication pending, 2=authorization pending.
|
||||
uint16_t status = (sigPayloadLen >= 8)
|
||||
? ((uint16_t)sigPayload[6] | ((uint16_t)sigPayload[7] << 8)) : 0;
|
||||
g_lastConnRspResult = result;
|
||||
|
||||
KernelLogStream(INFO, "BT-L2CAP") << "Connection Response: dstCID="
|
||||
<< base::hex << (uint64_t)dstCid << " result=" << (uint64_t)result;
|
||||
<< base::hex << (uint64_t)dstCid << " result=" << (uint64_t)result
|
||||
<< " status=" << (uint64_t)status << base::dec;
|
||||
|
||||
if (result == CONN_SUCCESS) {
|
||||
// Find our channel by srcCid (which is our local CID)
|
||||
@@ -239,12 +276,29 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
|
||||
case SIG_CONFIG_RSP: {
|
||||
if (sigPayloadLen >= 6) {
|
||||
// The Config Response's first field is the "Source CID".
|
||||
// In a RECEIVED Config Response it echoes back OUR local
|
||||
// channel endpoint (the device that sent the Config
|
||||
// Request -- us), NOT the remote's CID -- exactly like the
|
||||
// SIG_CONN_RSP handler above matches srcCid to LocalCid.
|
||||
// Matching RemoteCid here was THE bug that pinned every
|
||||
// AVDTP channel at localCfg=0: a success Config Response
|
||||
// arrived but matched no channel, so LocalConfigDone never
|
||||
// got set and the channel never became Configured.
|
||||
uint16_t srcCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
||||
uint16_t result = (uint16_t)sigPayload[4] | ((uint16_t)sigPayload[5] << 8);
|
||||
|
||||
if (result != CFG_SUCCESS) {
|
||||
KernelLogStream(WARNING, "BT-L2CAP") << "Config Response result="
|
||||
<< base::hex << (uint64_t)result << base::dec
|
||||
<< " (our config rejected)";
|
||||
}
|
||||
|
||||
if (result == CFG_SUCCESS) {
|
||||
bool matched = false;
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].RemoteCid == srcCid) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) {
|
||||
matched = true;
|
||||
g_channels[i].LocalConfigDone = true;
|
||||
if (g_channels[i].LocalConfigDone && g_channels[i].RemoteConfigDone) {
|
||||
g_channels[i].Configured = true;
|
||||
@@ -258,6 +312,11 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!matched) {
|
||||
KernelLogStream(WARNING, "BT-L2CAP")
|
||||
<< "Config Response (success) for unknown CID="
|
||||
<< base::hex << (uint64_t)srcCid << base::dec;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
@@ -312,6 +371,8 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
sOff += sizeof(SignalHeader) + sigPayloadLen;
|
||||
}
|
||||
} else {
|
||||
// Data on a dynamic channel
|
||||
@@ -319,6 +380,8 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == cid) {
|
||||
if (g_channels[i].Psm == PSM_AVDTP) {
|
||||
A2dp::ProcessAvdtp(payload, l2len);
|
||||
} else if (g_channels[i].Psm == PSM_SDP) {
|
||||
A2dp::ProcessSdp(payload, l2len);
|
||||
}
|
||||
break;
|
||||
}
|
||||
@@ -333,6 +396,8 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
uint16_t Connect(uint16_t psm) {
|
||||
if (!g_initialized) return 0;
|
||||
|
||||
g_lastConnRspResult = 0xFFFF; // reset diagnostic for this attempt
|
||||
|
||||
auto* ch = AllocChannel(psm);
|
||||
if (!ch) return 0;
|
||||
|
||||
@@ -356,6 +421,7 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents(); // process incoming ACL (the Connection Response!)
|
||||
|
||||
auto* ch = GetChannel(localCid);
|
||||
if (ch && ch->Configured) return true;
|
||||
@@ -406,6 +472,36 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
bool FreeChannel(uint16_t localCid) {
|
||||
// Reclaim a channel slot from a failed/abandoned dial during A2DP
|
||||
// bring-up retries. Two cases:
|
||||
// - RemoteCid == 0: the remote never acknowledged our CONN_REQ (ignored,
|
||||
// or answered only with a stuck PENDING that never completed), so its
|
||||
// side was never created and no Disconnect is owed -- just free it.
|
||||
// - RemoteCid != 0: a CONN_RSP success arrived (the peer allocated its
|
||||
// CID) but the channel never finished configuring, so the peer holds a
|
||||
// half-open channel. Send it a Disconnect Request before freeing, or
|
||||
// BOTH our slot leaks (the old RemoteCid==0-only guard left it Active)
|
||||
// AND the peer keeps a dangling channel.
|
||||
// We deliberately do NOT reset g_nextCid, so a late response for this
|
||||
// (now freed) CID matches nothing rather than cross-wiring the retry.
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == localCid) {
|
||||
if (g_channels[i].RemoteCid != 0) {
|
||||
uint8_t req[4] = {};
|
||||
req[0] = (uint8_t)(g_channels[i].RemoteCid & 0xFF);
|
||||
req[1] = (uint8_t)(g_channels[i].RemoteCid >> 8);
|
||||
req[2] = (uint8_t)(g_channels[i].LocalCid & 0xFF);
|
||||
req[3] = (uint8_t)(g_channels[i].LocalCid >> 8);
|
||||
SendSignal(SIG_DISCONN_REQ, g_sigIdentifier++, req, 4);
|
||||
}
|
||||
g_channels[i].Active = false;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
ChannelInfo* FindChannelByPsm(uint16_t psm) {
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].Psm == psm) {
|
||||
@@ -415,8 +511,27 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
uint16_t FindConfiguredAvdtpChannelExcept(uint16_t exceptCid) {
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].Configured
|
||||
&& g_channels[i].Psm == PSM_AVDTP
|
||||
&& g_channels[i].LocalCid != exceptCid) {
|
||||
return g_channels[i].LocalCid;
|
||||
}
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
uint16_t GetAclHandle() {
|
||||
return g_aclHandle;
|
||||
}
|
||||
|
||||
uint16_t LastConnRspResult() {
|
||||
return g_lastConnRspResult;
|
||||
}
|
||||
|
||||
uint16_t IncomingAvdtpReqCount() {
|
||||
return g_incomingAvdtpReqs;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -105,7 +105,30 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
// Find channel by PSM
|
||||
ChannelInfo* FindChannelByPsm(uint16_t psm);
|
||||
|
||||
// Find a CONFIGURED AVDTP channel whose LocalCid != exceptCid, returning its
|
||||
// LocalCid (0 if none). Used to locate the AVDTP media transport channel --
|
||||
// the second configured PSM-0x19 channel -- regardless of which side opened
|
||||
// it (we dial it after AVDTP_OPEN, but some sinks open it inbound instead).
|
||||
uint16_t FindConfiguredAvdtpChannelExcept(uint16_t exceptCid);
|
||||
|
||||
// Reclaim a dialed channel during A2DP bring-up retries so a failed connect
|
||||
// doesn't leak the fixed-size channel table. If the peer had acknowledged
|
||||
// the channel (RemoteCid != 0) a Disconnect Request is sent first; an
|
||||
// unanswered dial (RemoteCid == 0) owes nothing. Does NOT reset the CID
|
||||
// allocator (a late response for the freed CID then matches nothing).
|
||||
// Returns true if a matching active channel was found and freed.
|
||||
bool FreeChannel(uint16_t localCid);
|
||||
|
||||
// Get the ACL handle
|
||||
uint16_t GetAclHandle();
|
||||
|
||||
// Diagnostic: result code of the most recent outgoing L2CAP Connection
|
||||
// Response (0xFFFF if none yet). 0=success, 1=pending, 2=PSM-unsupported,
|
||||
// 3=security-block, 4=no-resources.
|
||||
uint16_t LastConnRspResult();
|
||||
|
||||
// Diagnostic: number of incoming AVDTP connection requests seen (whether the
|
||||
// remote wants to initiate the audio channel itself).
|
||||
uint16_t IncomingAvdtpReqCount();
|
||||
|
||||
}
|
||||
|
||||
@@ -106,6 +106,34 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
||||
enc->FrameSize = (headerBits + dataBits + 7) / 8;
|
||||
}
|
||||
|
||||
void Configure(SbcEncoder* enc, uint8_t octet0, uint8_t octet1, uint8_t bitpool) {
|
||||
// octet0: SamplingFreq[7:4] | ChannelMode[3:0]
|
||||
if (octet0 & 0x80) enc->Frequency = FREQ_16000;
|
||||
else if (octet0 & 0x40) enc->Frequency = FREQ_32000;
|
||||
else if (octet0 & 0x20) enc->Frequency = FREQ_44100;
|
||||
else enc->Frequency = FREQ_48000; // b4 (48k) or default
|
||||
|
||||
if (octet0 & 0x08) { enc->ChannelMode = MODE_MONO; enc->Channels = 1; }
|
||||
else if (octet0 & 0x04) { enc->ChannelMode = MODE_DUAL_CHANNEL; enc->Channels = 2; }
|
||||
else if (octet0 & 0x02) { enc->ChannelMode = MODE_STEREO; enc->Channels = 2; }
|
||||
else { enc->ChannelMode = MODE_JOINT_STEREO; enc->Channels = 2; } // b0
|
||||
|
||||
// octet1: BlockLength[7:4] | Subbands[3:2] | Alloc[1:0]
|
||||
if (octet1 & 0x80) enc->Blocks = 4;
|
||||
else if (octet1 & 0x40) enc->Blocks = 8;
|
||||
else if (octet1 & 0x20) enc->Blocks = 12;
|
||||
else enc->Blocks = 16; // b4 (16) or default
|
||||
enc->Subbands = (octet1 & 0x08) ? 4 : 8; // b3=4, else 8
|
||||
enc->AllocMethod = (octet1 & 0x02) ? ALLOC_SNR : ALLOC_LOUDNESS; // b1=SNR, else Loudness
|
||||
if (bitpool) enc->Bitpool = bitpool;
|
||||
|
||||
enc->SamplesPerFrame = enc->Blocks * enc->Subbands;
|
||||
uint32_t headerBits = 32 + (4 * enc->Subbands * enc->Channels);
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) headerBits += enc->Subbands;
|
||||
uint32_t dataBits = enc->Blocks * enc->Bitpool;
|
||||
enc->FrameSize = (headerBits + dataBits + 7) / 8;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Analysis filter bank (8 subbands)
|
||||
// =========================================================================
|
||||
@@ -149,12 +177,13 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
||||
// Bit allocation (Loudness method)
|
||||
// =========================================================================
|
||||
|
||||
static void BitAllocation(SbcEncoder* enc,
|
||||
int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS],
|
||||
int ch,
|
||||
int32_t scale_factors[SBC_SUBBANDS],
|
||||
uint8_t bits[SBC_SUBBANDS]) {
|
||||
// Compute scale factors
|
||||
// Compute the 4-bit scale factor for each subband of one channel: the number
|
||||
// of bits needed to represent the largest-magnitude subband sample. Must be
|
||||
// run on the FINAL sb_samples (after any joint-stereo transform), because the
|
||||
// decoder re-derives the bit allocation from these transmitted scale factors.
|
||||
static void ComputeScaleFactors(SbcEncoder* enc,
|
||||
int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS],
|
||||
int32_t scale_factors[SBC_SUBBANDS]) {
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
int32_t maxVal = 0;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
@@ -162,95 +191,120 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
||||
if (val < 0) val = -val;
|
||||
if (val > maxVal) maxVal = val;
|
||||
}
|
||||
|
||||
// Find scale factor (highest bit position)
|
||||
scale_factors[sb] = 0;
|
||||
int32_t tmp = maxVal;
|
||||
while (tmp > 0) {
|
||||
scale_factors[sb]++;
|
||||
tmp >>= 1;
|
||||
}
|
||||
int32_t sf = 0, tmp = maxVal;
|
||||
while (tmp > 0) { sf++; tmp >>= 1; }
|
||||
scale_factors[sb] = sf;
|
||||
}
|
||||
}
|
||||
|
||||
// Loudness offset table for 8 subbands (from SBC spec)
|
||||
static const int8_t loudness_offset_8[4][8] = {
|
||||
{-2, 0, 0, 0, 0, 0, 0, 1}, // 16kHz
|
||||
{-3, 0, 0, 0, 0, 0, 1, 2}, // 32kHz
|
||||
{-4, 0, 0, 0, 0, 0, 1, 2}, // 44.1kHz
|
||||
{-4, 0, 0, 0, 0, 0, 1, 2}, // 48kHz
|
||||
};
|
||||
// Loudness offset table for 8 subbands (SBC spec), indexed by sampling freq.
|
||||
static const int8_t g_loudnessOffset8[4][8] = {
|
||||
{-2, 0, 0, 0, 0, 0, 0, 1}, // 16kHz
|
||||
{-3, 0, 0, 0, 0, 0, 1, 2}, // 32kHz
|
||||
{-4, 0, 0, 0, 0, 0, 1, 2}, // 44.1kHz
|
||||
{-4, 0, 0, 0, 0, 0, 1, 2}, // 48kHz
|
||||
};
|
||||
|
||||
// Compute bitneed
|
||||
int32_t bitneed[SBC_SUBBANDS];
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (enc->AllocMethod == ALLOC_LOUDNESS) {
|
||||
bitneed[sb] = scale_factors[sb] - loudness_offset_8[enc->Frequency][sb];
|
||||
} else {
|
||||
bitneed[sb] = scale_factors[sb];
|
||||
}
|
||||
}
|
||||
// =========================================================================
|
||||
// Bit allocation -- canonical A2DP / BlueZ algorithm
|
||||
// =========================================================================
|
||||
// The per-subband bit widths are NOT transmitted; the decoder re-derives
|
||||
// them from the scale factors + bitpool + allocation method using exactly
|
||||
// this algorithm, so the encoder must reproduce it bit-for-bit or the
|
||||
// sample bitstream desyncs into noise. For STEREO/JOINT the two channels
|
||||
// share one bitpool (scope = both channels); for MONO each channel is
|
||||
// independent. (The old hand-rolled version over-allocated and overran the
|
||||
// frame buffer.)
|
||||
static void AllocateScope(SbcEncoder* enc,
|
||||
int32_t bitneed[SBC_CHANNELS][SBC_SUBBANDS],
|
||||
uint8_t bits[SBC_CHANNELS][SBC_SUBBANDS],
|
||||
int c0, int c1) {
|
||||
int sub = enc->Subbands;
|
||||
|
||||
// Bit allocation loop
|
||||
int32_t bitcount = 0;
|
||||
int32_t slicecount = 0;
|
||||
int32_t bitslice = (int32_t)(scale_factors[0] > 0 ? scale_factors[0] : 1);
|
||||
int maxBitneed = 0;
|
||||
for (int c = c0; c < c1; c++)
|
||||
for (int sb = 0; sb < sub; sb++)
|
||||
if (bitneed[c][sb] > maxBitneed) maxBitneed = bitneed[c][sb];
|
||||
|
||||
// Find max bitneed
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (bitneed[sb] > bitslice) bitslice = bitneed[sb];
|
||||
}
|
||||
bitslice++;
|
||||
|
||||
// Iterative allocation
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) bits[sb] = 0;
|
||||
|
||||
while (true) {
|
||||
int bitcount = 0, slicecount = 0, bitslice = maxBitneed + 1;
|
||||
do {
|
||||
bitslice--;
|
||||
bitcount = 0;
|
||||
bitcount += slicecount;
|
||||
slicecount = 0;
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (bitneed[sb] >= bitslice + 1 && bitneed[sb] < bitslice + 16) {
|
||||
if (bitneed[sb] == bitslice + 1) {
|
||||
bitcount += 2;
|
||||
for (int c = c0; c < c1; c++) {
|
||||
for (int sb = 0; sb < sub; sb++) {
|
||||
if (bitneed[c][sb] > bitslice + 1 && bitneed[c][sb] < bitslice + 16)
|
||||
slicecount++;
|
||||
else if (bitneed[c][sb] == bitslice + 1)
|
||||
slicecount += 2;
|
||||
}
|
||||
}
|
||||
} while (bitcount + slicecount < enc->Bitpool);
|
||||
|
||||
if (bitcount + slicecount == enc->Bitpool) {
|
||||
bitcount += slicecount;
|
||||
bitslice--;
|
||||
}
|
||||
|
||||
for (int c = c0; c < c1; c++) {
|
||||
for (int sb = 0; sb < sub; sb++) {
|
||||
if (bitneed[c][sb] < bitslice + 2) {
|
||||
bits[c][sb] = 0;
|
||||
} else {
|
||||
int b = bitneed[c][sb] - bitslice;
|
||||
bits[c][sb] = (uint8_t)(b > 16 ? 16 : b);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Distribute leftover bits in spec order (subband-major, channel-minor).
|
||||
int twoCh = (c1 - c0 == 2) ? 1 : 0;
|
||||
int c = c0, sb = 0;
|
||||
while (bitcount < enc->Bitpool && sb < sub) {
|
||||
if (bits[c][sb] >= 2 && bits[c][sb] < 16) {
|
||||
bits[c][sb]++;
|
||||
bitcount++;
|
||||
} else if (bitneed[c][sb] == bitslice + 1 && enc->Bitpool > bitcount + 1) {
|
||||
bits[c][sb] = 2;
|
||||
bitcount += 2;
|
||||
}
|
||||
if (twoCh) { if (c == c0) c = c0 + 1; else { c = c0; sb++; } }
|
||||
else sb++;
|
||||
}
|
||||
c = c0; sb = 0;
|
||||
while (bitcount < enc->Bitpool && sb < sub) {
|
||||
if (bits[c][sb] < 16) { bits[c][sb]++; bitcount++; }
|
||||
if (twoCh) { if (c == c0) c = c0 + 1; else { c = c0; sb++; } }
|
||||
else sb++;
|
||||
}
|
||||
}
|
||||
|
||||
static void BitAllocation(SbcEncoder* enc,
|
||||
int32_t scale_factors[SBC_CHANNELS][SBC_SUBBANDS],
|
||||
uint8_t bits[SBC_CHANNELS][SBC_SUBBANDS]) {
|
||||
int sub = enc->Subbands;
|
||||
int32_t bitneed[SBC_CHANNELS][SBC_SUBBANDS];
|
||||
|
||||
for (int c = 0; c < enc->Channels; c++) {
|
||||
for (int sb = 0; sb < sub; sb++) {
|
||||
if (enc->AllocMethod == ALLOC_LOUDNESS) {
|
||||
if (scale_factors[c][sb] == 0) {
|
||||
bitneed[c][sb] = -5;
|
||||
} else {
|
||||
bitcount++;
|
||||
slicecount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (bitcount + slicecount >= enc->Bitpool) break;
|
||||
if (bitslice <= -16) break;
|
||||
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (bitneed[sb] >= bitslice + 1 && bitneed[sb] < bitslice + 16) {
|
||||
if (bitneed[sb] == bitslice + 1) {
|
||||
bits[sb] = 2;
|
||||
} else if (bits[sb] < 16) {
|
||||
bits[sb]++;
|
||||
int loud = scale_factors[c][sb] - g_loudnessOffset8[enc->Frequency][sb];
|
||||
bitneed[c][sb] = (loud > 0) ? (loud / 2) : loud;
|
||||
}
|
||||
} else {
|
||||
bitneed[c][sb] = scale_factors[c][sb];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Distribute remaining bits
|
||||
int32_t remaining = enc->Bitpool - bitcount;
|
||||
for (int sb = 0; sb < enc->Subbands && remaining > 0; sb++) {
|
||||
if (bits[sb] >= 2 && bits[sb] < 16) {
|
||||
bits[sb]++;
|
||||
remaining--;
|
||||
} else if (bitneed[sb] == bitslice && bits[sb] == 0) {
|
||||
bits[sb] = 2;
|
||||
remaining -= 2;
|
||||
if (remaining < 0) { bits[sb] = 0; break; }
|
||||
}
|
||||
}
|
||||
|
||||
for (int sb = 0; sb < enc->Subbands && remaining > 0; sb++) {
|
||||
if (bits[sb] < 16) {
|
||||
bits[sb]++;
|
||||
remaining--;
|
||||
}
|
||||
bool shared = (enc->ChannelMode == MODE_STEREO || enc->ChannelMode == MODE_JOINT_STEREO);
|
||||
if (shared && enc->Channels == 2) {
|
||||
AllocateScope(enc, bitneed, bits, 0, 2);
|
||||
} else {
|
||||
for (int c = 0; c < enc->Channels; c++) AllocateScope(enc, bitneed, bits, c, c + 1);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -280,38 +334,34 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
||||
|
||||
uint32_t Encode(SbcEncoder* enc, const int16_t* pcm, uint8_t* out) {
|
||||
int32_t sb_samples[SBC_CHANNELS][SBC_BLOCKS][SBC_SUBBANDS];
|
||||
int32_t scale_factors[SBC_CHANNELS][SBC_SUBBANDS];
|
||||
uint8_t bits[SBC_CHANNELS][SBC_SUBBANDS];
|
||||
int32_t scale_factors[SBC_CHANNELS][SBC_SUBBANDS] = {};
|
||||
uint8_t bits[SBC_CHANNELS][SBC_SUBBANDS] = {};
|
||||
|
||||
// Clear output
|
||||
memset(out, 0, enc->FrameSize);
|
||||
|
||||
// Analysis filter for each channel
|
||||
// 1. Analysis filterbank per channel.
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
AnalysisFilter(enc, pcm, ch, sb_samples[ch]);
|
||||
BitAllocation(enc, sb_samples[ch], ch, scale_factors[ch], bits[ch]);
|
||||
}
|
||||
|
||||
// Joint stereo processing
|
||||
// 2. Joint-stereo decision + mid/side transform, applied BEFORE scale
|
||||
// factors and bit allocation: the decoder re-derives the bit widths
|
||||
// from the transmitted scale factors, which must reflect the FINAL
|
||||
// (post-transform) samples, or the sample bitstream desyncs.
|
||||
uint8_t joint = 0;
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
||||
for (int sb = 0; sb < enc->Subbands - 1; sb++) {
|
||||
// Simple heuristic: use joint coding if it saves bits
|
||||
int32_t maxMid = 0, maxSide = 0;
|
||||
int32_t maxMid = 0, maxSide = 0, maxOrig = 0;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
int32_t mid = (sb_samples[0][blk][sb] + sb_samples[1][blk][sb]) / 2;
|
||||
int32_t side = (sb_samples[0][blk][sb] - sb_samples[1][blk][sb]) / 2;
|
||||
int32_t l = sb_samples[0][blk][sb], r = sb_samples[1][blk][sb];
|
||||
int32_t mid = (l + r) / 2, side = (l - r) / 2;
|
||||
if (mid < 0) mid = -mid;
|
||||
if (side < 0) side = -side;
|
||||
int32_t al = l < 0 ? -l : l, ar = r < 0 ? -r : r;
|
||||
if (mid > maxMid) maxMid = mid;
|
||||
if (side > maxSide) maxSide = side;
|
||||
}
|
||||
int32_t maxOrig = 0;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
int32_t v0 = sb_samples[0][blk][sb]; if (v0 < 0) v0 = -v0;
|
||||
int32_t v1 = sb_samples[1][blk][sb]; if (v1 < 0) v1 = -v1;
|
||||
if (v0 > maxOrig) maxOrig = v0;
|
||||
if (v1 > maxOrig) maxOrig = v1;
|
||||
if (al > maxOrig) maxOrig = al;
|
||||
if (ar > maxOrig) maxOrig = ar;
|
||||
}
|
||||
if (maxMid + maxSide < maxOrig) {
|
||||
joint |= (1 << (enc->Subbands - 1 - sb));
|
||||
@@ -321,84 +371,85 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
||||
sb_samples[0][blk][sb] = (l + r) / 2;
|
||||
sb_samples[1][blk][sb] = (l - r) / 2;
|
||||
}
|
||||
// Recalculate scale factors for joint channels
|
||||
for (int ch = 0; ch < 2; ch++) {
|
||||
int32_t maxVal = 0;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
int32_t val = sb_samples[ch][blk][sb];
|
||||
if (val < 0) val = -val;
|
||||
if (val > maxVal) maxVal = val;
|
||||
}
|
||||
scale_factors[ch][sb] = 0;
|
||||
int32_t tmp = maxVal;
|
||||
while (tmp > 0) { scale_factors[ch][sb]++; tmp >>= 1; }
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Pack SBC frame header
|
||||
out[0] = 0x9C; // Sync word
|
||||
out[1] = (enc->Frequency << 6) | ((enc->Blocks == 4 ? 0 : enc->Blocks == 8 ? 1 : enc->Blocks == 12 ? 2 : 3) << 4)
|
||||
| (enc->ChannelMode << 2) | (enc->AllocMethod << 1) | (enc->Subbands == 8 ? 1 : 0);
|
||||
// 3. Scale factors from the final subband samples.
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
ComputeScaleFactors(enc, sb_samples[ch], scale_factors[ch]);
|
||||
}
|
||||
|
||||
// 4. Bit allocation (canonical; decoder re-derives the same widths).
|
||||
BitAllocation(enc, scale_factors, bits);
|
||||
|
||||
// 5a. Frame header. In the FRAME HEADER allocation_method is 0=Loudness,
|
||||
// 1=SNR -- the OPPOSITE polarity of the codec-capability bit. Since
|
||||
// ALLOC_LOUDNESS==1 internally, emit (Loudness ? 0 : 1).
|
||||
out[0] = 0x9C;
|
||||
uint8_t blkCode = (enc->Blocks == 4 ? 0 : enc->Blocks == 8 ? 1 : enc->Blocks == 12 ? 2 : 3);
|
||||
out[1] = (uint8_t)((enc->Frequency << 6) | (blkCode << 4)
|
||||
| (enc->ChannelMode << 2)
|
||||
| ((enc->AllocMethod == ALLOC_LOUDNESS ? 0u : 1u) << 1)
|
||||
| (enc->Subbands == 8 ? 1u : 0u));
|
||||
out[2] = enc->Bitpool;
|
||||
// out[3] = CRC, filled after the join + scale-factor bits are packed.
|
||||
|
||||
// CRC (computed over header bytes 1-2 and scale factors)
|
||||
// Will be filled after scale factors are packed
|
||||
BitWriter bw = {out, 32}; // packing starts at byte 4 (after 4-byte header)
|
||||
|
||||
BitWriter bw = {out, 32}; // Start after 4-byte header
|
||||
|
||||
// Joint stereo flags
|
||||
// 5b. Join flags (joint stereo only).
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
||||
WriteBits(&bw, joint, enc->Subbands);
|
||||
}
|
||||
|
||||
// Pack scale factors (4 bits each)
|
||||
// 5c. Scale factors, 4 bits each (ch0 all subbands, then ch1).
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
uint32_t sf = scale_factors[ch][sb];
|
||||
uint32_t sf = (uint32_t)scale_factors[ch][sb];
|
||||
if (sf > 15) sf = 15;
|
||||
WriteBits(&bw, sf, 4);
|
||||
}
|
||||
}
|
||||
|
||||
// Compute CRC (over bytes 1, 2, and scale factor bits)
|
||||
uint32_t crcBits = 16 + (enc->Channels * enc->Subbands * 4);
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) crcBits += enc->Subbands;
|
||||
out[3] = SbcCrc8(&out[1], (crcBits + 7) / 8, crcBits % 8 ? crcBits % 8 : 8);
|
||||
// 5d. CRC-8 over the LOGICAL stream: byte1, byte2, then the join +
|
||||
// scale-factor bits. Those bits physically start at out[4] (out[3]
|
||||
// is the CRC slot itself), so build a contiguous buffer that SKIPS
|
||||
// out[3]. Computing over &out[1] as one span (the old bug) folds the
|
||||
// zero CRC byte into the CRC and a compliant sink drops every frame.
|
||||
uint32_t sfBits = (enc->ChannelMode == MODE_JOINT_STEREO ? (uint32_t)enc->Subbands : 0u)
|
||||
+ (uint32_t)enc->Channels * enc->Subbands * 4;
|
||||
uint32_t sfBytes = (sfBits + 7) / 8;
|
||||
uint8_t crcbuf[2 + (SBC_CHANNELS * SBC_SUBBANDS * 4 + SBC_SUBBANDS + 7) / 8];
|
||||
crcbuf[0] = out[1];
|
||||
crcbuf[1] = out[2];
|
||||
for (uint32_t i = 0; i < sfBytes; i++) crcbuf[2 + i] = out[4 + i];
|
||||
uint32_t crcTotalBits = 16 + sfBits;
|
||||
out[3] = SbcCrc8(crcbuf, 2 + sfBytes, (crcTotalBits % 8) ? (crcTotalBits % 8) : 8);
|
||||
|
||||
// Pack audio samples
|
||||
// 5e. Quantize + pack audio samples (block-major, then channel, subband).
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (bits[ch][sb] == 0) continue;
|
||||
|
||||
int32_t sf = scale_factors[ch][sb];
|
||||
int32_t sample = sb_samples[ch][blk][sb];
|
||||
|
||||
// Quantize: levels = (1 << bits) - 1
|
||||
uint32_t levels = (1u << bits[ch][sb]) - 1;
|
||||
int32_t quantized;
|
||||
|
||||
if (sf > 0) {
|
||||
// Normalize and quantize
|
||||
int32_t maxRange = (1 << sf);
|
||||
quantized = (int32_t)(((int64_t)(sample + maxRange) * levels) / (2 * maxRange));
|
||||
} else {
|
||||
quantized = levels / 2;
|
||||
}
|
||||
|
||||
if (quantized < 0) quantized = 0;
|
||||
if (quantized > (int32_t)levels) quantized = (int32_t)levels;
|
||||
|
||||
WriteBits(&bw, (uint32_t)quantized, bits[ch][sb]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Pad to byte boundary
|
||||
uint32_t totalBytes = (bw.BitPos + 7) / 8;
|
||||
return totalBytes > enc->FrameSize ? enc->FrameSize : totalBytes;
|
||||
// SBC frames are fixed-size for a given configuration.
|
||||
return enc->FrameSize;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
|
||||
@@ -75,6 +75,13 @@ namespace Drivers::USB::Bluetooth::Sbc {
|
||||
// Initialize encoder with given parameters
|
||||
void Init(SbcEncoder* enc, uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample);
|
||||
|
||||
// Override the encoder's SBC parameters from the negotiated AVDTP capability
|
||||
// octets (octet0 = SamplingFreq[7:4]|ChannelMode[3:0], octet1 =
|
||||
// BlockLength[7:4]|Subbands[3:2]|Alloc[1:0]) + the chosen bitpool, so the
|
||||
// emitted frame headers match exactly what the sink agreed to. Call after
|
||||
// Init.
|
||||
void Configure(SbcEncoder* enc, uint8_t octet0, uint8_t octet1, uint8_t bitpool);
|
||||
|
||||
// Encode one SBC frame from PCM data
|
||||
// pcm: interleaved 16-bit signed PCM, length = blocks * subbands * channels
|
||||
// out: output buffer for SBC frame
|
||||
|
||||
@@ -107,6 +107,13 @@ namespace Drivers::USB::Xhci {
|
||||
// Per-device info
|
||||
static UsbDeviceInfo g_devices[MAX_SLOTS + 1] = {};
|
||||
|
||||
// True while PollEvents() is draining the event ring. Submitters that are
|
||||
// reached from inside an event callback (e.g. an HCI reply sent from a
|
||||
// Bluetooth event handler) see this and must NOT wait for completion --
|
||||
// PollEvents is non-reentrant, so the wait would never observe the
|
||||
// completion. Used by InPollContext() / ControlTransfer().
|
||||
static volatile bool g_pollActive = false;
|
||||
|
||||
// Interrupt transfer data buffers (per slot)
|
||||
static uint8_t* g_interruptDataBuf[MAX_SLOTS + 1] = {};
|
||||
static uint64_t g_interruptDataBufPhys[MAX_SLOTS + 1] = {};
|
||||
@@ -366,12 +373,40 @@ namespace Drivers::USB::Xhci {
|
||||
return true;
|
||||
}
|
||||
|
||||
// True when the caller is running inside PollEvents() (e.g. an HCI reply
|
||||
// sent from a Bluetooth event handler). Such callers must fire-and-forget,
|
||||
// not wait, since a nested PollEvents is a no-op.
|
||||
bool InPollContext() {
|
||||
return g_pollActive;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// PollEvents - process event ring
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
void PollEvents() {
|
||||
while (true) {
|
||||
// PollEvents runs both from the synchronous poll loops (ControlTransfer,
|
||||
// SendCommand, firmware download) and from the xHCI MSI handler. On a
|
||||
// single core the IRQ can preempt a poll loop mid-drain; if both advance
|
||||
// g_evtRingDequeue / g_evtRingCCS the ring tracking desyncs and the
|
||||
// cycle-bit check can start matching stale entries forever -> the boot
|
||||
// freezes (observed wedging the Bluetooth firmware download at ~635 KB,
|
||||
// where the dying device floods the event ring). Guard against re-entry:
|
||||
// the interrupt is already acked (IMAN.IP cleared in HandleInterrupt) and
|
||||
// the active poll loop drains these events itself. g_pollActive is also
|
||||
// read by InPollContext() so command submitters (ControlTransfer) can
|
||||
// tell they are nested and must fire-and-forget instead of waiting.
|
||||
if (g_pollActive) return;
|
||||
g_pollActive = true;
|
||||
|
||||
// Bound the work per call so a flooding/wedged device can never spin
|
||||
// here forever; the outer wall-clock timeouts then fire instead of
|
||||
// freezing. 4x the ring size is far above any legitimate burst.
|
||||
constexpr uint32_t MAX_EVENTS_PER_CALL = EVT_RING_SIZE * 4;
|
||||
uint32_t processed = 0, portEvts = 0, xferEvts = 0;
|
||||
uint32_t lastType = 0, lastSlot = 0, lastEp = 0, lastCC = 0;
|
||||
|
||||
while (processed < MAX_EVENTS_PER_CALL) {
|
||||
TRB& evt = g_evtRing[g_evtRingDequeue];
|
||||
|
||||
// Check if the cycle bit matches our expected cycle state
|
||||
@@ -381,6 +416,7 @@ namespace Drivers::USB::Xhci {
|
||||
}
|
||||
|
||||
uint32_t trbType = (evt.Control & TRB_TYPE_MASK) >> TRB_TYPE_SHIFT;
|
||||
lastType = trbType;
|
||||
|
||||
switch (trbType) {
|
||||
case TRB_COMMAND_COMPLETION: {
|
||||
@@ -393,6 +429,7 @@ namespace Drivers::USB::Xhci {
|
||||
}
|
||||
|
||||
case TRB_PORT_STATUS_CHANGE: {
|
||||
portEvts++;
|
||||
uint32_t portId = (evt.Parameter0 >> 24) & 0xFF;
|
||||
uint32_t portsc = ReadOp(OP_PORTSC_BASE + (portId - 1) * OP_PORTSC_STRIDE);
|
||||
// Clear change bits (write-1-to-clear)
|
||||
@@ -412,6 +449,8 @@ namespace Drivers::USB::Xhci {
|
||||
uint32_t slotId = (evt.Control >> 24) & 0xFF;
|
||||
uint32_t epDci = (evt.Control >> 16) & 0x1F;
|
||||
uint32_t residual = evt.Status & 0x00FFFFFF;
|
||||
xferEvts++;
|
||||
lastSlot = slotId; lastEp = epDci; lastCC = completionCode;
|
||||
|
||||
if (epDci == 1) {
|
||||
// EP0 (DCI 1) - control transfer completion
|
||||
@@ -437,9 +476,18 @@ namespace Drivers::USB::Xhci {
|
||||
uint8_t intDci = dev.InterruptEpNum ? (dev.InterruptEpNum * 2 + 1) : 0;
|
||||
|
||||
if (epDci == bulkInDci && g_transferCallbacks[slotId]) {
|
||||
// Bulk IN — dispatch via registered callback
|
||||
uint16_t len = dev.BulkInMaxPacket;
|
||||
if (residual < len) len = dev.BulkInMaxPacket - (uint16_t)residual;
|
||||
// Bulk IN — dispatch via registered callback.
|
||||
// len = actually-transferred bytes (requested -
|
||||
// residual). A 0-byte / ZLP completion has
|
||||
// residual == requested, so len MUST be 0: the old
|
||||
// code left len at the full max-packet and handed
|
||||
// the callback a slice of STALE DMA buffer, which
|
||||
// the BT driver then counted as a bogus ACL packet
|
||||
// (the constant rx flood) and pushed into its RX
|
||||
// ring, crowding out the real Config Response.
|
||||
uint32_t reqLen = dev.BulkInMaxPacket;
|
||||
uint16_t len = (residual < reqLen)
|
||||
? (uint16_t)(reqLen - residual) : 0;
|
||||
g_transferCallbacks[slotId](slotId, epDci,
|
||||
g_bulkInDataBuf[slotId], len, completionCode);
|
||||
} else if (epDci == bulkOutDci && g_transferCallbacks[slotId]) {
|
||||
@@ -495,6 +543,7 @@ namespace Drivers::USB::Xhci {
|
||||
g_evtRingDequeue = 0;
|
||||
g_evtRingCCS = !g_evtRingCCS;
|
||||
}
|
||||
processed++;
|
||||
}
|
||||
|
||||
// Update ERDP to tell the controller we have processed events
|
||||
@@ -504,6 +553,20 @@ namespace Drivers::USB::Xhci {
|
||||
WriteRt(IR0_ERDP, (uint32_t)(erdp & 0xFFFFFFFF));
|
||||
WriteRt(IR0_ERDP + 4, (uint32_t)(erdp >> 32));
|
||||
|
||||
// A full batch means the ring is being flooded -- surface the dominant
|
||||
// event source (rate-limited) so a wedge is diagnosable, not silent.
|
||||
if (processed >= MAX_EVENTS_PER_CALL) {
|
||||
static uint32_t stormLogs = 0;
|
||||
if (stormLogs < 8) {
|
||||
stormLogs++;
|
||||
KernelLogStream(WARNING, "xHCI") << "Event storm: " << (uint64_t)processed
|
||||
<< "/call (port=" << (uint64_t)portEvts << " xfer=" << (uint64_t)xferEvts
|
||||
<< " lastType=" << (uint64_t)lastType << " slot=" << (uint64_t)lastSlot
|
||||
<< " ep=" << (uint64_t)lastEp << " cc=" << (uint64_t)lastCC << ")";
|
||||
}
|
||||
}
|
||||
|
||||
g_pollActive = false;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
@@ -559,8 +622,11 @@ namespace Drivers::USB::Xhci {
|
||||
g_cmdCompleted = false;
|
||||
WriteDoorbell(0, 0);
|
||||
|
||||
// Poll until command completes (with timeout)
|
||||
for (uint32_t i = 0; i < 100000; i++) {
|
||||
// Poll until command completes. Wall-clock bounded so a storm/wedge
|
||||
// can't stretch this into a multi-second freeze; normal commands
|
||||
// complete in well under a millisecond.
|
||||
uint64_t cmdStart = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - cmdStart < 2000) {
|
||||
PollEvents();
|
||||
if (g_cmdCompleted) {
|
||||
return g_cmdCompletionCode;
|
||||
@@ -654,8 +720,22 @@ namespace Drivers::USB::Xhci {
|
||||
g_xferCompleted = false;
|
||||
WriteDoorbell(slotId, 1);
|
||||
|
||||
// Poll until transfer completes
|
||||
for (uint32_t i = 0; i < 100000; i++) {
|
||||
// If we are nested inside PollEvents (e.g. an HCI reply sent from a
|
||||
// Bluetooth event handler), we cannot wait for completion here: the
|
||||
// reentrancy guard makes a nested PollEvents a no-op, so the completion
|
||||
// would never be observed and the timeout+recovery path would corrupt
|
||||
// the EP0 ring. The transfer is submitted (doorbell rung); let the
|
||||
// active PollEvents reap its completion. Fire-and-forget.
|
||||
if (g_pollActive) {
|
||||
return CC_SUCCESS;
|
||||
}
|
||||
|
||||
// Poll until transfer completes. Wall-clock bounded (not iteration
|
||||
// bounded) so a wedged device fails in ~2s and reports its cc, instead
|
||||
// of the per-iteration cost ballooning under an event storm into a
|
||||
// multi-second freeze with no output.
|
||||
uint64_t xferStart = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - xferStart < 2000) {
|
||||
PollEvents();
|
||||
if (g_xferCompleted) {
|
||||
return g_xferCompletionCode;
|
||||
@@ -730,6 +810,43 @@ namespace Drivers::USB::Xhci {
|
||||
WriteDoorbell(slotId, target);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// ResetInterruptEndpoint - clear a halted interrupt IN endpoint and re-arm
|
||||
// -------------------------------------------------------------------------
|
||||
// A USB transaction error (cc=4) halts the endpoint; the host must issue
|
||||
// Reset Endpoint + Set TR Dequeue before it will accept transfers again.
|
||||
// Used by the Bluetooth firmware-download path, where a glitch on the event
|
||||
// pipe near the end of a large upload otherwise kills event reception for
|
||||
// good (no Command Complete / bootup events).
|
||||
void ResetInterruptEndpoint(uint8_t slotId) {
|
||||
if (slotId == 0 || slotId > MAX_SLOTS || !g_devices[slotId].Active) return;
|
||||
UsbDeviceInfo& dev = g_devices[slotId];
|
||||
if (dev.InterruptEpNum == 0 || !dev.InterruptRing) return;
|
||||
|
||||
uint8_t dci = dev.InterruptEpNum * 2 + 1;
|
||||
|
||||
TRB resetTrb = {};
|
||||
resetTrb.Control = (TRB_RESET_ENDPOINT << TRB_TYPE_SHIFT)
|
||||
| ((uint32_t)slotId << 24)
|
||||
| ((uint32_t)dci << 16);
|
||||
SendCommand(resetTrb);
|
||||
|
||||
uint64_t newDeq = dev.InterruptRingPhys
|
||||
+ (uint64_t)dev.InterruptRingEnqueue * sizeof(TRB);
|
||||
if (dev.InterruptRingCCS) newDeq |= 1; // DCS bit
|
||||
|
||||
TRB deqTrb = {};
|
||||
deqTrb.Parameter0 = (uint32_t)(newDeq & 0xFFFFFFFF);
|
||||
deqTrb.Parameter1 = (uint32_t)(newDeq >> 32);
|
||||
deqTrb.Control = (TRB_SET_TR_DEQUEUE << TRB_TYPE_SHIFT)
|
||||
| ((uint32_t)slotId << 24)
|
||||
| ((uint32_t)dci << 16);
|
||||
SendCommand(deqTrb);
|
||||
|
||||
// Re-arm reception.
|
||||
QueueInterruptTransfer(slotId);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// QueueBulkInTransfer
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
@@ -315,6 +315,16 @@ namespace Drivers::USB::Xhci {
|
||||
// Queue an interrupt IN transfer on a device's interrupt endpoint
|
||||
void QueueInterruptTransfer(uint8_t slotId);
|
||||
|
||||
// Clear a halted interrupt IN endpoint (Reset Endpoint + Set TR Dequeue)
|
||||
// and re-arm reception. Needed after a USB transaction error on the event
|
||||
// pipe (e.g. during Bluetooth firmware download).
|
||||
void ResetInterruptEndpoint(uint8_t slotId);
|
||||
|
||||
// True while PollEvents() is draining the event ring. A command submitter
|
||||
// reached from inside an event callback must fire-and-forget (not wait),
|
||||
// since a nested PollEvents() is a no-op.
|
||||
bool InPollContext();
|
||||
|
||||
// Queue a bulk transfer on a device's bulk IN or OUT endpoint
|
||||
void QueueBulkInTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length);
|
||||
void QueueBulkOutTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length);
|
||||
|
||||
Reference in New Issue
Block a user