feat: Raspberry Pi 4/400 Bluetooth over mini UART (H4, Broadcom patchram, firmware mailbox BT_ON); move BT stack to Drivers/Bluetooth
This commit is contained in:
daniel committed
1 parent
808e1d66fd
commit
ffee9002a6
38 files changed
+2176
-141
No files matched your search
File diff suppressed because it is too large.
Load diff
@@ -1,109 +0,0 @@
|
||||
/*
|
||||
* A2dp.hpp
|
||||
* Bluetooth A2DP (Advanced Audio Distribution Profile)
|
||||
* AVDTP signaling and SBC audio streaming
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
|
||||
namespace Drivers::USB::Bluetooth::A2dp {
|
||||
|
||||
// =========================================================================
|
||||
// A2DP stream states
|
||||
// =========================================================================
|
||||
|
||||
enum class State {
|
||||
Idle, // No A2DP connection
|
||||
Discovering, // AVDTP discover in progress
|
||||
Configured, // Stream endpoint configured
|
||||
Open, // Stream open, ready for audio
|
||||
Streaming // Actively streaming audio
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// 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);
|
||||
|
||||
// Reset all per-link AVDTP state for a newly established ACL connection.
|
||||
// This is deliberately separate from StartSource(): the peer may open its
|
||||
// AVDTP channels during authentication/the post-encryption settle window,
|
||||
// and StartSource must preserve those already-live inbound channels.
|
||||
void OnConnected(uint16_t aclHandle);
|
||||
|
||||
// Called by L2CAP when an AVDTP channel becomes ready
|
||||
void OnChannelReady(uint16_t l2capCid);
|
||||
|
||||
// Process an AVDTP signaling packet
|
||||
void ProcessAvdtp(uint16_t localCid, const uint8_t* data, uint16_t len);
|
||||
|
||||
// L2CAP channel teardown can happen while the ACL link remains alive.
|
||||
// Clear stale signaling/media CIDs so subsequent writes do not target a
|
||||
// dead channel and a manual reconnect can rebuild the A2DP path.
|
||||
void OnChannelClosed(uint16_t localCid);
|
||||
|
||||
// Process an SDP packet (on any SDP L2CAP channel). Dispatches by PDU:
|
||||
// requests (the headset querying OUR services) are answered by the minimal
|
||||
// SDP server; responses on our outgoing client channel complete DoSdpQuery.
|
||||
void ProcessSdp(uint16_t localCid, 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);
|
||||
|
||||
// Start streaming
|
||||
bool StartStream();
|
||||
|
||||
// Stop streaming. flushQueued drops any PCM still queued in the media
|
||||
// ring: pass true when closing the stream (track change / app exit),
|
||||
// false for a pause so resume continues gaplessly.
|
||||
bool StopStream(bool flushQueued = true);
|
||||
|
||||
// Write PCM audio data to the Bluetooth audio stream. Copies into the
|
||||
// media ring and returns immediately (never blocks); returns the number
|
||||
// of bytes accepted (0 = ring full, retry later).
|
||||
int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen);
|
||||
|
||||
// Free bytes in the PCM queue, aligned to complete stereo frames.
|
||||
uint32_t GetWriteSpace();
|
||||
|
||||
// Tear down local media state after the ACL link disappears. The actual
|
||||
// mixer notification is deferred out of the nested HCI receive path.
|
||||
void OnDisconnected(uint16_t aclHandle);
|
||||
bool ConsumeRouteChange();
|
||||
|
||||
// Encode + send queued PCM, paced to the audio clock and gated on ACL TX
|
||||
// readiness. Called from the idle-loop event pump and from WriteAudio;
|
||||
// self-serializing, cheap no-op when not streaming.
|
||||
void PumpMedia();
|
||||
|
||||
// Reap controller events and pump media from process context. Kept
|
||||
// separate from WriteAudio so the mixer never holds its spinlock across
|
||||
// USB event processing or SBC encoding.
|
||||
void ServiceMedia();
|
||||
|
||||
// Get current state
|
||||
State GetState();
|
||||
|
||||
// True only when signaling and media transports are both live and the SEP
|
||||
// has reached a state from which audio can be started.
|
||||
bool IsReady();
|
||||
|
||||
// Check if currently streaming
|
||||
bool IsStreaming();
|
||||
|
||||
// Queue a headset AVRCP absolute-volume request for the system mixer.
|
||||
void RequestMasterVolume(int percent);
|
||||
void SetMuted(bool muted);
|
||||
|
||||
// AVRCP receive runs nested inside the transport event pump. Defer its
|
||||
// master-volume request until the top-level Bluetooth service context.
|
||||
bool ConsumeVolumeRequest(int* percent);
|
||||
|
||||
}
|
||||
@@ -1,264 +0,0 @@
|
||||
/*
|
||||
* Avrcp.cpp
|
||||
* Bluetooth AVRCP -- minimal Target role over AVCTP (PSM 0x0017)
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "Avrcp.hpp"
|
||||
#include "A2dp.hpp"
|
||||
#include "L2cap.hpp"
|
||||
#include <Drivers/Audio/Mixer.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <CppLib/Spinlock.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
|
||||
using namespace Kt;
|
||||
|
||||
namespace Drivers::USB::Bluetooth::Avrcp {
|
||||
|
||||
// =========================================================================
|
||||
// Constants
|
||||
// =========================================================================
|
||||
|
||||
// AV/C command types (commands) / response codes (responses)
|
||||
constexpr uint8_t AVC_CTYPE_CONTROL = 0x0;
|
||||
constexpr uint8_t AVC_CTYPE_STATUS = 0x1;
|
||||
constexpr uint8_t AVC_CTYPE_NOTIFY = 0x3;
|
||||
constexpr uint8_t AVC_RSP_NOT_IMPL = 0x8;
|
||||
constexpr uint8_t AVC_RSP_ACCEPTED = 0x9;
|
||||
constexpr uint8_t AVC_RSP_REJECTED = 0xA;
|
||||
constexpr uint8_t AVC_RSP_STABLE = 0xC;
|
||||
constexpr uint8_t AVC_RSP_CHANGED = 0xD;
|
||||
constexpr uint8_t AVC_RSP_INTERIM = 0xF;
|
||||
|
||||
// AV/C opcodes
|
||||
constexpr uint8_t AVC_OP_VENDOR = 0x00;
|
||||
constexpr uint8_t AVC_OP_UNIT_INFO = 0x30;
|
||||
constexpr uint8_t AVC_OP_SUBUNIT_INFO = 0x31;
|
||||
constexpr uint8_t AVC_OP_PASSTHROUGH = 0x7C;
|
||||
|
||||
// AVRCP vendor-dependent PDUs (BT SIG company id 00 19 58)
|
||||
constexpr uint8_t PDU_GET_CAPABILITIES = 0x10;
|
||||
constexpr uint8_t PDU_GET_PLAY_STATUS = 0x30;
|
||||
constexpr uint8_t PDU_REGISTER_NOTIFY = 0x31;
|
||||
constexpr uint8_t PDU_SET_ABS_VOLUME = 0x50;
|
||||
|
||||
// AVRCP notification events
|
||||
constexpr uint8_t EVT_VOLUME_CHANGED = 0x0D;
|
||||
|
||||
// AVRCP callbacks are dispatched from xHCI's deferred PollEvents path, not
|
||||
// from the hard IRQ. Sending the INTERIM/CHANGED response can descend into
|
||||
// the HCI and xHCI transmit rings, so keep interrupts enabled while this
|
||||
// state is serialized. An IRQ-disabling spinlock here can stall the local
|
||||
// timer behind an unrelated transmit/recovery operation.
|
||||
static kcp::Mutex g_notifyLock;
|
||||
static uint16_t g_volumeNotifyCid = 0;
|
||||
static uint8_t g_volumeNotifyTransaction = 0;
|
||||
|
||||
// =========================================================================
|
||||
// Send helpers
|
||||
// =========================================================================
|
||||
|
||||
// AVCTP single-packet response: byte0 = transaction<<4 | pktType<<2 |
|
||||
// C/R(1=response)<<1 | IPID; bytes 1-2 = PID big-endian.
|
||||
static bool SendAvctp(uint16_t cid, uint8_t transaction, uint16_t pid,
|
||||
bool invalidPid, const uint8_t* avc, uint16_t avcLen) {
|
||||
uint8_t buf[96] = {};
|
||||
if (3u + avcLen > sizeof(buf)) return false;
|
||||
buf[0] = (uint8_t)((transaction << 4) | 0x02 | (invalidPid ? 0x01 : 0));
|
||||
buf[1] = (uint8_t)(pid >> 8);
|
||||
buf[2] = (uint8_t)(pid & 0xFF);
|
||||
if (avc && avcLen) memcpy(&buf[3], avc, avcLen);
|
||||
return L2cap::SendData(cid, buf, (uint16_t)(3 + avcLen));
|
||||
}
|
||||
|
||||
// AVRCP vendor-dependent response frame:
|
||||
// [rsp][subunit 0x48 panel][opcode 0x00][company 00 19 58][pdu][pkt 0][len BE][params]
|
||||
static bool SendVendorRsp(uint16_t cid, uint8_t transaction, uint8_t rspCode,
|
||||
uint8_t pdu, const uint8_t* p, uint16_t n) {
|
||||
uint8_t avc[64] = {};
|
||||
if (10u + n > sizeof(avc)) return false;
|
||||
avc[0] = rspCode;
|
||||
avc[1] = 0x48; // PANEL subunit
|
||||
avc[2] = AVC_OP_VENDOR;
|
||||
avc[3] = 0x00; avc[4] = 0x19; avc[5] = 0x58; // BT SIG company id
|
||||
avc[6] = pdu;
|
||||
avc[7] = 0x00; // packet type: single
|
||||
avc[8] = (uint8_t)(n >> 8);
|
||||
avc[9] = (uint8_t)(n & 0xFF);
|
||||
if (p && n) memcpy(&avc[10], p, n);
|
||||
return SendAvctp(cid, transaction, 0x110E, false,
|
||||
avc, (uint16_t)(10 + n));
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ProcessAvctp
|
||||
// =========================================================================
|
||||
|
||||
void ProcessAvctp(uint16_t localCid, const uint8_t* d, uint16_t len) {
|
||||
if (len < 3) return;
|
||||
|
||||
uint8_t transaction = (d[0] >> 4) & 0x0F;
|
||||
uint8_t pktType = (d[0] >> 2) & 0x03;
|
||||
bool isCommand = ((d[0] >> 1) & 0x01) == 0;
|
||||
uint16_t pid = ((uint16_t)d[1] << 8) | d[2];
|
||||
|
||||
if (!isCommand) return; // a response to something we sent
|
||||
if (pktType != 0) return; // only single-packet AVCTP supported
|
||||
|
||||
if (pid != 0x110E) {
|
||||
// Unsupported PID: respond with the IPID bit set, no message body.
|
||||
SendAvctp(localCid, transaction, pid, true, nullptr, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
const uint8_t* avc = d + 3;
|
||||
uint16_t alen = (uint16_t)(len - 3);
|
||||
if (alen < 3) return;
|
||||
uint8_t ctype = avc[0] & 0x0F;
|
||||
uint8_t opcode = avc[2];
|
||||
|
||||
switch (opcode) {
|
||||
case AVC_OP_UNIT_INFO: {
|
||||
// Fixed-format reply: PANEL subunit, BT SIG company id.
|
||||
uint8_t rsp[8] = {AVC_RSP_STABLE, 0xFF, AVC_OP_UNIT_INFO,
|
||||
0x07, 0x48, 0x00, 0x19, 0x58};
|
||||
SendAvctp(localCid, transaction, pid, false, rsp, sizeof(rsp));
|
||||
break;
|
||||
}
|
||||
|
||||
case AVC_OP_SUBUNIT_INFO: {
|
||||
uint8_t rsp[8] = {AVC_RSP_STABLE, 0xFF, AVC_OP_SUBUNIT_INFO,
|
||||
0x07, 0x48, 0xFF, 0xFF, 0xFF};
|
||||
SendAvctp(localCid, transaction, pid, false, rsp, sizeof(rsp));
|
||||
break;
|
||||
}
|
||||
|
||||
case AVC_OP_PASSTHROUGH: {
|
||||
// No media-session control bridge exists yet. Claiming these
|
||||
// commands were ACCEPTED made headset buttons appear broken and
|
||||
// left the controller believing an action had happened.
|
||||
uint8_t rsp[16] = {};
|
||||
uint16_t cp = (alen > sizeof(rsp)) ? (uint16_t)sizeof(rsp) : alen;
|
||||
memcpy(rsp, avc, cp);
|
||||
rsp[0] = AVC_RSP_NOT_IMPL;
|
||||
SendAvctp(localCid, transaction, pid, false, rsp, cp);
|
||||
if (alen >= 4) {
|
||||
KernelLogStream(INFO, "BT-AVRCP") << "pass-through op="
|
||||
<< base::hex << (uint64_t)(avc[3] & 0x7F)
|
||||
<< ((avc[3] & 0x80) ? " release" : " press") << base::dec;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case AVC_OP_VENDOR: {
|
||||
// [ctype][subunit][00][company 3][pdu][pkt][len BE][params]
|
||||
if (alen < 10) break;
|
||||
if (avc[3] != 0x00 || avc[4] != 0x19 || avc[5] != 0x58) {
|
||||
uint8_t rsp[8] = {};
|
||||
memcpy(rsp, avc, 8);
|
||||
rsp[0] = AVC_RSP_NOT_IMPL;
|
||||
SendAvctp(localCid, transaction, pid, false, rsp, 8);
|
||||
break;
|
||||
}
|
||||
uint8_t pdu = avc[6];
|
||||
uint16_t plen = ((uint16_t)avc[8] << 8) | avc[9];
|
||||
const uint8_t* p = &avc[10];
|
||||
if (10u + plen > alen) {
|
||||
uint8_t err = 0x01; // invalid parameter
|
||||
SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED,
|
||||
pdu, &err, 1);
|
||||
break;
|
||||
}
|
||||
|
||||
if (pdu == PDU_GET_CAPABILITIES && ctype == AVC_CTYPE_STATUS && plen >= 1) {
|
||||
if (p[0] == 0x02) { // CompanyID list
|
||||
uint8_t rp[5] = {0x02, 0x01, 0x00, 0x19, 0x58};
|
||||
SendVendorRsp(localCid, transaction, AVC_RSP_STABLE,
|
||||
pdu, rp, sizeof(rp));
|
||||
} else if (p[0] == 0x03) { // EventsSupported
|
||||
uint8_t rp[3] = {0x03, 0x01, EVT_VOLUME_CHANGED};
|
||||
SendVendorRsp(localCid, transaction, AVC_RSP_STABLE,
|
||||
pdu, rp, sizeof(rp));
|
||||
} else {
|
||||
uint8_t err = 0x01; // invalid parameter
|
||||
SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED,
|
||||
pdu, &err, 1);
|
||||
}
|
||||
} else if (pdu == PDU_GET_PLAY_STATUS && ctype == AVC_CTYPE_STATUS) {
|
||||
// song length / position unknown (0xFFFFFFFF), status by state
|
||||
uint8_t rp[9] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,
|
||||
(uint8_t)(A2dp::IsStreaming() ? 0x01 : 0x00)};
|
||||
SendVendorRsp(localCid, transaction, AVC_RSP_STABLE,
|
||||
pdu, rp, sizeof(rp));
|
||||
} else if (pdu == PDU_REGISTER_NOTIFY && ctype == AVC_CTYPE_NOTIFY && plen >= 1) {
|
||||
// INTERIM response with the current value. (A CHANGED
|
||||
// response is sent by NotifyVolumeChanged; registrations
|
||||
// are one-shot as required by AVRCP.)
|
||||
if (p[0] == EVT_VOLUME_CHANGED) {
|
||||
uint8_t rp[2] = {EVT_VOLUME_CHANGED,
|
||||
(uint8_t)((Drivers::Audio::Mixer::GetMasterVolume() * 127) / 100)};
|
||||
g_notifyLock.Acquire();
|
||||
if (SendVendorRsp(localCid, transaction, AVC_RSP_INTERIM,
|
||||
pdu, rp, sizeof(rp))) {
|
||||
g_volumeNotifyCid = localCid;
|
||||
g_volumeNotifyTransaction = transaction;
|
||||
}
|
||||
g_notifyLock.Release();
|
||||
} else {
|
||||
uint8_t err = 0x01;
|
||||
SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED,
|
||||
pdu, &err, 1);
|
||||
}
|
||||
} else if (pdu == PDU_SET_ABS_VOLUME && ctype == AVC_CTYPE_CONTROL && plen >= 1) {
|
||||
uint8_t vol = p[0] & 0x7F;
|
||||
A2dp::RequestMasterVolume(((int)vol * 100) / 127);
|
||||
SendVendorRsp(localCid, transaction, AVC_RSP_ACCEPTED,
|
||||
pdu, &vol, 1);
|
||||
KernelLogStream(INFO, "BT-AVRCP") << "absolute volume -> "
|
||||
<< (uint64_t)(((int)vol * 100) / 127) << "%";
|
||||
} else {
|
||||
uint8_t err = 0x00; // invalid command
|
||||
SendVendorRsp(localCid, transaction, AVC_RSP_REJECTED,
|
||||
pdu, &err, 1);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
uint8_t rsp[8] = {};
|
||||
uint16_t cp = (alen > sizeof(rsp)) ? (uint16_t)sizeof(rsp) : alen;
|
||||
memcpy(rsp, avc, cp);
|
||||
rsp[0] = AVC_RSP_NOT_IMPL;
|
||||
SendAvctp(localCid, transaction, pid, false, rsp, cp);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void NotifyVolumeChanged(int percent) {
|
||||
if (percent < 0) percent = 0;
|
||||
if (percent > 100) percent = 100;
|
||||
|
||||
g_notifyLock.Acquire();
|
||||
uint16_t cid = g_volumeNotifyCid;
|
||||
uint8_t transaction = g_volumeNotifyTransaction;
|
||||
g_volumeNotifyCid = 0; // one-shot, regardless of transport outcome
|
||||
if (cid != 0) {
|
||||
uint8_t rp[2] = {
|
||||
EVT_VOLUME_CHANGED, (uint8_t)((percent * 127) / 100)
|
||||
};
|
||||
SendVendorRsp(cid, transaction, AVC_RSP_CHANGED,
|
||||
PDU_REGISTER_NOTIFY, rp, sizeof(rp));
|
||||
}
|
||||
g_notifyLock.Release();
|
||||
}
|
||||
|
||||
void OnChannelClosed(uint16_t localCid) {
|
||||
g_notifyLock.Acquire();
|
||||
if (g_volumeNotifyCid == localCid) g_volumeNotifyCid = 0;
|
||||
g_notifyLock.Release();
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,27 +0,0 @@
|
||||
/*
|
||||
* Avrcp.hpp
|
||||
* Bluetooth AVRCP (Audio/Video Remote Control Profile) -- minimal Target role
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
|
||||
namespace Drivers::USB::Bluetooth::Avrcp {
|
||||
|
||||
// Process an AVCTP packet arriving on an AVRCP control channel (PSM 0x0017).
|
||||
// Implements a minimal AVRCP Target: unit/subunit info, honest rejection of
|
||||
// unimplemented pass-through controls, capabilities, one-shot absolute-
|
||||
// volume notification, and absolute-volume control. Runs
|
||||
// from the ACL receive path (DrainEvents, top-level) -- sending is safe
|
||||
// there, blocking waits are not (none are used).
|
||||
void ProcessAvctp(uint16_t localCid, const uint8_t* data, uint16_t len);
|
||||
|
||||
// Complete an outstanding EVENT_VOLUME_CHANGED registration. AVRCP
|
||||
// notifications are one-shot: after CHANGED the controller registers again.
|
||||
void NotifyVolumeChanged(int percent);
|
||||
|
||||
// Drop registrations tied to a channel that L2CAP has torn down.
|
||||
void OnChannelClosed(uint16_t localCid);
|
||||
|
||||
}
|
||||
@@ -1,719 +0,0 @@
|
||||
/*
|
||||
* Bluetooth.cpp
|
||||
* Top-level Bluetooth subsystem — adapter registration and Intel BT initialization
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "Bluetooth.hpp"
|
||||
#include <Arch/Cpu.hpp>
|
||||
#include "Hci.hpp"
|
||||
#include "A2dp.hpp"
|
||||
#include "IntelFirmware.hpp"
|
||||
#include <Drivers/Audio/Mixer.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>
|
||||
#include <Timekeeping/Timer.hpp>
|
||||
#include <Arch/Smp.hpp>
|
||||
#include <atomic>
|
||||
|
||||
using namespace Kt;
|
||||
|
||||
namespace Drivers::USB::Bluetooth {
|
||||
|
||||
// =========================================================================
|
||||
// State
|
||||
// =========================================================================
|
||||
|
||||
static bool g_initialized = false;
|
||||
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; claimed (atomically -- the pickup runs from the
|
||||
// idle loop, concurrently with the rest of the system) by
|
||||
// ServiceDeferredInit() once VFS is up.
|
||||
static std::atomic<bool> g_initPending{false};
|
||||
|
||||
// Forward declaration: firmware-dependent HCI bring-up, run once VFS is up.
|
||||
static void CompleteInit();
|
||||
|
||||
// Path to the Bluetooth config (BD_ADDR override). Written by the
|
||||
// Bluetooth desktop app; read here on boot. See ApplyConfiguredAddress().
|
||||
static constexpr const char* BT_CONFIG_PATH = "0:/config/bluetooth.toml";
|
||||
|
||||
// =========================================================================
|
||||
// bluetooth.toml BD_ADDR override
|
||||
// =========================================================================
|
||||
|
||||
static bool HexNibble(char c, uint8_t& out) {
|
||||
if (c >= '0' && c <= '9') { out = (uint8_t)(c - '0'); return true; }
|
||||
if (c >= 'a' && c <= 'f') { out = (uint8_t)(c - 'a' + 10); return true; }
|
||||
if (c >= 'A' && c <= 'F') { out = (uint8_t)(c - 'A' + 10); return true; }
|
||||
return false;
|
||||
}
|
||||
|
||||
// Parse exactly six ':'/'-'-separated hex octets from a string fragment.
|
||||
// Fills out[0..5] in written order (out[0] is the first printed octet,
|
||||
// matching the desktop app's format_addr); returns false on any deviation.
|
||||
static bool ParseMacStr(const char* s, int len, uint8_t out[6]) {
|
||||
int byteIdx = 0, i = 0;
|
||||
while (byteIdx < 6) {
|
||||
while (i < len && (s[i] == ':' || s[i] == '-' || s[i] == ' ')) i++;
|
||||
uint8_t hi = 0, lo = 0;
|
||||
if (i + 1 >= len || !HexNibble(s[i], hi) || !HexNibble(s[i + 1], lo))
|
||||
return false;
|
||||
out[byteIdx++] = (uint8_t)((hi << 4) | lo);
|
||||
i += 2;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// Read the optional `mac = "XX:XX:XX:XX:XX:XX"` key from bluetooth.toml.
|
||||
// Minimal line scanner (the kernel has no TOML parser, and the file holds a
|
||||
// single value); ignores section headers/comments. Returns false when the
|
||||
// file is absent or the key is missing/malformed.
|
||||
static bool ReadConfiguredMac(uint8_t out[6]) {
|
||||
Fs::Vfs::BackendFile f;
|
||||
if (Fs::Vfs::OpenBackendFile(BT_CONFIG_PATH, f) < 0) return false;
|
||||
uint64_t size = Fs::Vfs::GetBackendFileSize(f);
|
||||
if (size == 0) { Fs::Vfs::CloseBackendFile(f); return false; }
|
||||
|
||||
char buf[512];
|
||||
uint64_t n = size < sizeof(buf) - 1 ? size : sizeof(buf) - 1;
|
||||
Fs::Vfs::ReadBackendFile(f, (uint8_t*)buf, 0, n);
|
||||
Fs::Vfs::CloseBackendFile(f);
|
||||
buf[n] = '\0';
|
||||
|
||||
const char* p = buf;
|
||||
while (*p) {
|
||||
while (*p == ' ' || *p == '\t') p++;
|
||||
// Match a bare "mac" key (next char must end the identifier).
|
||||
if (p[0] == 'm' && p[1] == 'a' && p[2] == 'c' &&
|
||||
(p[3] == ' ' || p[3] == '\t' || p[3] == '=')) {
|
||||
const char* q = p + 3;
|
||||
while (*q == ' ' || *q == '\t') q++;
|
||||
if (*q == '=') {
|
||||
const char* quote = q + 1;
|
||||
while (*quote && *quote != '"' && *quote != '\n') quote++;
|
||||
if (*quote == '"') {
|
||||
const char* end = quote + 1;
|
||||
while (*end && *end != '"' && *end != '\n') end++;
|
||||
if (*end == '"' &&
|
||||
ParseMacStr(quote + 1, (int)(end - quote - 1), out))
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
while (*p && *p != '\n') p++;
|
||||
if (*p == '\n') p++;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// Apply a configured BD_ADDR override (if any) to the freshly-reset
|
||||
// controller. Called from CompleteInit BEFORE ReadBdAddr so the rest of
|
||||
// bring-up uses the overridden address; no HCI Reset must follow (the Intel
|
||||
// 0xFC31 override is volatile and a reset reverts to the factory address).
|
||||
static void ApplyConfiguredAddress() {
|
||||
uint8_t mac[6];
|
||||
if (!ReadConfiguredMac(mac)) return; // no override configured
|
||||
if (Hci::SetBdAddr(mac)) {
|
||||
KernelLogStream(OK, "BT") << "Applied BD_ADDR override from bluetooth.toml";
|
||||
} else {
|
||||
KernelLogStream(WARNING, "BT")
|
||||
<< "BD_ADDR override from bluetooth.toml rejected by controller";
|
||||
}
|
||||
}
|
||||
|
||||
// Intel Bluetooth device IDs
|
||||
static bool IsIntelBt(uint16_t vid, uint16_t pid) {
|
||||
if (vid != 0x8087) return false;
|
||||
// Known Intel Bluetooth USB product IDs
|
||||
switch (pid) {
|
||||
case 0x0032: // AX211 variant
|
||||
case 0x0033: // AX211
|
||||
case 0x0036: // AX211 variant
|
||||
case 0x0038: // AX211 variant
|
||||
case 0x0AAA: // AX200
|
||||
case 0x0026: // AX201
|
||||
case 0x0029: // AX201 variant
|
||||
case 0x0025: // 9560
|
||||
case 0x0A2B: // 8265
|
||||
case 0x0A2A: // 8260
|
||||
case 0x07DC: // 8265 variant
|
||||
case 0x0AA7: // AX200 variant
|
||||
return true;
|
||||
default:
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Intel Bluetooth firmware detection
|
||||
// =========================================================================
|
||||
|
||||
static bool InitIntelBluetooth() {
|
||||
KernelLogStream(INFO, "BT") << "Intel Bluetooth adapter detected";
|
||||
|
||||
// Read the Intel TLV version first. It is the mode probe accepted by
|
||||
// both Intel's bootloader and operational firmware. A standard HCI
|
||||
// Reset must not precede it: the bootloader answers Reset with status
|
||||
// 0x01 (Unknown HCI Command), which is normal rather than a fatal
|
||||
// transport failure.
|
||||
//
|
||||
// If the controller is in bootloader mode, this loads the matching SFI
|
||||
// image from the ramdisk, secure-sends it, boots operational firmware,
|
||||
// and applies DDC parameters. Returns true if the controller ends up
|
||||
// operational (either already loaded, or freshly downloaded).
|
||||
if (!DownloadIntelFirmware()) {
|
||||
// Older Intel parts may lack the TLV command while already running
|
||||
// usable operational firmware. Preserve that compatibility only
|
||||
// when standard HCI proves it is genuinely operational.
|
||||
if (Hci::Reset()) {
|
||||
KernelLogStream(WARNING, "BT")
|
||||
<< "Intel firmware query failed; continuing with operational HCI";
|
||||
return true;
|
||||
}
|
||||
KernelLogStream(ERROR, "BT")
|
||||
<< "Intel controller remains in bootloader mode";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Whether it was already present or was just booted, operational
|
||||
// firmware must now accept standard HCI. Do not mark the adapter ready
|
||||
// if this transition did not actually happen.
|
||||
if (!Hci::Reset()) {
|
||||
KernelLogStream(ERROR, "BT")
|
||||
<< "Operational firmware did not accept HCI Reset";
|
||||
return false;
|
||||
}
|
||||
if (!Hci::IntelSetEventMask()) {
|
||||
KernelLogStream(WARNING, "BT")
|
||||
<< "Intel vendor event mask was not accepted";
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// RegisterAdapter — entry point from USB enumeration
|
||||
// =========================================================================
|
||||
|
||||
void RegisterAdapter(uint8_t slotId) {
|
||||
if (g_initialized) {
|
||||
KernelLogStream(WARNING, "BT") << "Bluetooth adapter already registered";
|
||||
return;
|
||||
}
|
||||
|
||||
g_slotId = slotId;
|
||||
|
||||
// Initialize HCI transport (allocates DMA buffers, registers callback)
|
||||
// NOTE: Does NOT queue receive transfers yet — device isn't ready
|
||||
Hci::Initialize(slotId);
|
||||
|
||||
auto* dev = Xhci::GetDevice(slotId);
|
||||
if (!dev) return;
|
||||
|
||||
// Wait for the USB device to be ready after SET_CONFIGURATION
|
||||
// Intel BT controllers need 200-500ms after config before accepting HCI
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - start < 200) {
|
||||
Xhci::PollEvents();
|
||||
Arch::Pause();
|
||||
}
|
||||
|
||||
// Start the event pipe BEFORE sending any HCI commands.
|
||||
// HCI command responses arrive as events on the interrupt IN endpoint,
|
||||
// so it must be queued to receive them. Deliberately done HERE, at
|
||||
// enumeration time, not in the deferred bring-up: this preserves the
|
||||
// exact transport timing of the original synchronous boot path.
|
||||
Hci::StartEventPipe();
|
||||
|
||||
// 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.
|
||||
// The idle loop (ServiceDeferredInit) picks it up after boot, keeping
|
||||
// the multi-second firmware download off the boot-critical path.
|
||||
if (!Fs::Vfs::IsDriveRegistered(0)) {
|
||||
g_initPending.store(true, std::memory_order_release);
|
||||
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()) {
|
||||
didReset = true; // InitIntelBluetooth already sent HCI Reset
|
||||
} else {
|
||||
// A recognized Intel part that failed its vendor initialization
|
||||
// is normally still a bootloader, where a second standard Reset
|
||||
// only repeats status 0x01. Stop with an accurate failure
|
||||
// instead of pretending a basic-HCI fallback exists.
|
||||
KernelLogStream(ERROR, "BT") << "Intel BT initialization failed";
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Standard HCI Reset (skip if Intel init already did one)
|
||||
if (!didReset) {
|
||||
if (!Hci::Reset()) {
|
||||
KernelLogStream(ERROR, "BT") << "HCI Reset failed";
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// Apply a persisted BD_ADDR override (0:/config/bluetooth.toml) now,
|
||||
// after the last reset and before the address is read back, so the
|
||||
// whole bring-up below uses the overridden address.
|
||||
ApplyConfiguredAddress();
|
||||
|
||||
// Read BD_ADDR
|
||||
if (Hci::ReadBdAddr(g_bdAddr)) {
|
||||
KernelLogStream(OK, "BT") << "BD_ADDR: "
|
||||
<< base::hex
|
||||
<< (uint64_t)g_bdAddr[5] << ":" << (uint64_t)g_bdAddr[4] << ":"
|
||||
<< (uint64_t)g_bdAddr[3] << ":" << (uint64_t)g_bdAddr[2] << ":"
|
||||
<< (uint64_t)g_bdAddr[1] << ":" << (uint64_t)g_bdAddr[0] << base::dec;
|
||||
}
|
||||
|
||||
// NOTE: do NOT override the BD_ADDR via 0xFC31 here. 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;
|
||||
uint16_t scoNum = 0;
|
||||
if (Hci::ReadBufferSize(&aclLen, &scoLen, &aclNum, &scoNum)) {
|
||||
KernelLogStream(INFO, "BT") << "ACL buffer: " << (uint64_t)aclLen
|
||||
<< " bytes x " << (uint64_t)aclNum;
|
||||
}
|
||||
// Bulk IN was armed before Intel firmware loading, but bootloader runts
|
||||
// are not HCI ACL traffic and an absorbed firmware-phase USB error may
|
||||
// have stopped the endpoint. Start framed ACL reception only now.
|
||||
Hci::EnableAclDataReception();
|
||||
|
||||
// Set local name
|
||||
Hci::WriteLocalName("MontaukOS");
|
||||
|
||||
// Class of Device. The A2DP spec MANDATES the Capturing service bit
|
||||
// (0x080000) for a source; Audio (0x200000) is customary. Major/minor
|
||||
// class: Computer/Laptop (0x010C) -- what we actually are. The old
|
||||
// value 0x200408 (Audio/Video major class, minor "hands-free device",
|
||||
// no Capturing bit) presented MontaukOS to the headset as ANOTHER
|
||||
// HEADSET. A sink's connection manager classifies peers by CoD (it
|
||||
// arrives in its Connection Request event and is cached at pairing),
|
||||
// and an A2DP/AVRCP dial from a "hands-free unit" is a credible reason
|
||||
// for it to park those channels at "authorization pending" forever.
|
||||
// NOTE: the headset caches this from pairing -- it must FORGET the
|
||||
// device and re-pair to observe the new class.
|
||||
Hci::WriteClassOfDevice(0x28010C);
|
||||
|
||||
// Enable Simple Secure Pairing
|
||||
Hci::WriteSSPMode(1);
|
||||
|
||||
// Enable Secure Connections host support (P-256 / AES-CCM). Link
|
||||
// keys are procedure-bound: a bond minted over Secure Connections
|
||||
// (BlueZ always negotiates SC -> key Type=7) CANNOT authenticate a
|
||||
// legacy link -- the controller must fail with status 5. Without
|
||||
// this, a key shared with a Linux install (dual boot, see
|
||||
// scripts/import-bluez-bond.sh) is cryptographically fine yet
|
||||
// unusable, and our own pairings mint legacy P-192 keys that Linux
|
||||
// then silently replaces. Must follow Write SSP Mode.
|
||||
uint8_t scOn = 0x01;
|
||||
Hci::SendCommand(Hci::OP_WRITE_SC_HOST_SUPPORT, &scOn, 1);
|
||||
Hci::WaitCommandComplete(Hci::OP_WRITE_SC_HOST_SUPPORT);
|
||||
|
||||
// Allow role switch + sniff on new connections. The controller default
|
||||
// link policy is 0x0000 (deny both). A multipoint headset (Bose QC
|
||||
// Ultra) that also holds a link to a phone requests a role switch to
|
||||
// master on our link to avoid a scatternet; with the switch denied,
|
||||
// some sink firmwares never grant the A2DP media path. Sniff denial
|
||||
// similarly upsets CSR-derived stacks that sniff idle links.
|
||||
uint8_t linkPolicy[2] = {0x05, 0x00}; // bit0 role switch, bit2 sniff
|
||||
Hci::SendCommand(Hci::OP_WRITE_DEFAULT_LP, linkPolicy, 2);
|
||||
Hci::WaitCommandComplete(Hci::OP_WRITE_DEFAULT_LP);
|
||||
|
||||
// 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, 0xFF, 0xFF, 0x20};
|
||||
Hci::SendCommand(Hci::OP_SET_EVENT_MASK, eventMask, 8);
|
||||
Hci::WaitCommandComplete(Hci::OP_SET_EVENT_MASK);
|
||||
|
||||
// Request Extended Inquiry Results so scan entries carry EIR names and
|
||||
// RSSI. Older controllers may support only mode 1 (RSSI); the parser
|
||||
// handles both result layouts and the fallback preserves discovery.
|
||||
if (!Hci::WriteInquiryMode(2)) Hci::WriteInquiryMode(1);
|
||||
|
||||
// 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.load(std::memory_order_relaxed) || g_initialized) return;
|
||||
if (!Fs::Vfs::IsDriveRegistered(0)) return; // ramdisk still not mounted
|
||||
if (Xhci::InPollContext()) return; // never nest under PollEvents
|
||||
|
||||
// Claim the pending init (this runs from the idle loop; make sure only
|
||||
// one pass performs the bring-up). The firmware download inside takes
|
||||
// seconds -- running it here instead of on the boot path is what keeps
|
||||
// boot fast.
|
||||
bool expected = true;
|
||||
if (!g_initPending.compare_exchange_strong(expected, false,
|
||||
std::memory_order_acquire)) return;
|
||||
|
||||
KernelLogStream(INFO, "BT") << "Completing deferred Bluetooth init in background";
|
||||
|
||||
// Reserve this CPU for the duration. The bring-up overlaps desktop
|
||||
// startup, and if the scheduler tick pulls this idle context away
|
||||
// whenever a process is ready, the HCI waits' wall-clock timeouts
|
||||
// expire with almost no polling done. Reserved, the bring-up runs
|
||||
// uninterrupted here while processes use other CPUs; on a single-CPU
|
||||
// system this briefly pauses userspace, matching the old synchronous
|
||||
// behavior minus the boot-path stall.
|
||||
auto* cpu = Smp::GetCurrentCpuData();
|
||||
bool wasReserved = cpu && cpu->reservedForKernelWork;
|
||||
if (cpu) cpu->reservedForKernelWork = true;
|
||||
Hci::SetFwTrace(true); // bounded per-completion event-pipe trace
|
||||
CompleteInit();
|
||||
Hci::DumpFwTrace(); // flush remaining records (process context)
|
||||
Hci::SetFwTrace(false);
|
||||
if (cpu) cpu->reservedForKernelWork = wasReserved;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ServiceEvents — steady-state event pump (idle loop)
|
||||
// =========================================================================
|
||||
|
||||
bool HasDeferredWork() {
|
||||
if (g_initPending.load(std::memory_order_acquire) &&
|
||||
!g_initialized && Fs::Vfs::IsDriveRegistered(0)) {
|
||||
return true;
|
||||
}
|
||||
return g_initialized && Hci::HasPendingCommands();
|
||||
}
|
||||
|
||||
void ServiceEvents() {
|
||||
if (!g_initialized) return;
|
||||
if (Xhci::InPollContext()) return; // never nest under PollEvents
|
||||
A2dp::ServiceMedia(); // reap events and feed queued media
|
||||
// DrainEvents may just have queued an accept/auth/encryption reply.
|
||||
// Send it in this same service pass instead of adding a scheduler-turn
|
||||
// delay to the controller's security timeout.
|
||||
Hci::ProcessPendingCommands();
|
||||
Drivers::Audio::Mixer::OnBluetoothWritable();
|
||||
int requestedVolume;
|
||||
if (A2dp::ConsumeVolumeRequest(&requestedVolume))
|
||||
Drivers::Audio::Mixer::SetMasterVolume(requestedVolume);
|
||||
if (A2dp::ConsumeRouteChange())
|
||||
Drivers::Audio::Mixer::OnBluetoothStateChanged();
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Public queries
|
||||
// =========================================================================
|
||||
|
||||
bool IsInitialized() {
|
||||
return g_initialized;
|
||||
}
|
||||
|
||||
uint8_t GetSlotId() {
|
||||
return g_slotId;
|
||||
}
|
||||
|
||||
const uint8_t* GetBdAddr() {
|
||||
return g_bdAddr;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// SetAddress — live BD_ADDR change
|
||||
// =========================================================================
|
||||
|
||||
bool SetAddress(const uint8_t* addr) {
|
||||
if (!g_initialized || !addr) return false;
|
||||
|
||||
// Reject the obviously-invalid addresses (all-zero, broadcast).
|
||||
bool allZero = true, allOnes = true;
|
||||
for (int i = 0; i < 6; i++) {
|
||||
if (addr[i] != 0x00) allZero = false;
|
||||
if (addr[i] != 0xFF) allOnes = false;
|
||||
}
|
||||
if (allZero || allOnes) return false;
|
||||
|
||||
// Drop any active link first: its pairing/encryption was negotiated
|
||||
// against the old address, so it cannot survive the change.
|
||||
bool droppedLink = false;
|
||||
for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) {
|
||||
auto* conn = Hci::GetConnectionByIndex(i);
|
||||
if (conn && conn->Active) {
|
||||
Hci::Disconnect(conn->Handle, 0x13); // Remote User Terminated
|
||||
droppedLink = true;
|
||||
}
|
||||
}
|
||||
// Let the disconnection(s) complete before reprogramming the address.
|
||||
// A fixed 300 ms pause was merely hopeful and could change the identity
|
||||
// under a still-live encrypted link.
|
||||
if (droppedLink) {
|
||||
uint64_t t0 = Timekeeping::GetMilliseconds();
|
||||
bool anyActive = true;
|
||||
while (anyActive && Timekeeping::GetMilliseconds() - t0 < 2000) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
anyActive = false;
|
||||
for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) {
|
||||
auto* conn = Hci::GetConnectionByIndex(i);
|
||||
if (conn && conn->Active) { anyActive = true; break; }
|
||||
}
|
||||
for (int k = 0; k < 200; k++) Arch::Pause();
|
||||
}
|
||||
if (anyActive) return false;
|
||||
}
|
||||
|
||||
// Program the new address. Do NOT issue an HCI Reset afterwards: the
|
||||
// Intel 0xFC31 override is volatile and a reset reverts it.
|
||||
if (!Hci::SetBdAddr(addr)) return false;
|
||||
|
||||
// Re-read to confirm and refresh the cache; fall back to the requested
|
||||
// bytes if the read fails.
|
||||
uint8_t readback[6] = {};
|
||||
if (Hci::ReadBdAddr(readback)) {
|
||||
memcpy(g_bdAddr, readback, 6);
|
||||
} else {
|
||||
memcpy(g_bdAddr, addr, 6);
|
||||
}
|
||||
|
||||
// Re-assert discoverable + connectable with the new address (no reset
|
||||
// happened, so name/CoD/SSP/event-mask all persist).
|
||||
Hci::WriteScanEnable(0x03);
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
if (g_bdAddr[i] != addr[i]) return false;
|
||||
}
|
||||
KernelLogStream(OK, "BT") << "BD_ADDR changed at runtime";
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Scan — blocking inquiry
|
||||
// =========================================================================
|
||||
|
||||
int Scan(Hci::InquiryDevice* buf, int maxCount, uint32_t timeoutMs) {
|
||||
if (!g_initialized || !buf || maxCount <= 0) return -1;
|
||||
|
||||
Hci::ClearInquiryResults();
|
||||
|
||||
// Convert timeout to 1.28s units (min 1, max 30)
|
||||
uint32_t durationUnits = timeoutMs / 1280;
|
||||
if (durationUnits < 1) durationUnits = 1;
|
||||
if (durationUnits > 30) durationUnits = 30;
|
||||
uint8_t duration = (uint8_t)durationUnits;
|
||||
|
||||
if (!Hci::StartInquiry(duration)) return -1;
|
||||
|
||||
// Poll until inquiry completes or timeout
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Hci::IsInquiryActive() && (Timekeeping::GetMilliseconds() - start < timeoutMs)) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
|
||||
for (int j = 0; j < 200; j++) {
|
||||
Arch::Pause();
|
||||
}
|
||||
}
|
||||
|
||||
// Cancel if still running
|
||||
if (Hci::IsInquiryActive()) {
|
||||
// A failed cancel must not be hidden: Create Connection while the
|
||||
// controller is still in Inquiry is commonly rejected as Command
|
||||
// Disallowed. Give a command that briefly lost HCI ownership one
|
||||
// retry, continuing to service the completion event in between.
|
||||
if (!Hci::CancelInquiry()) {
|
||||
uint64_t cancelStart = Timekeeping::GetMilliseconds();
|
||||
while (Hci::IsInquiryActive()
|
||||
&& Timekeeping::GetMilliseconds() - cancelStart < 250) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
for (int j = 0; j < 100; j++)
|
||||
Arch::Pause();
|
||||
}
|
||||
if (Hci::IsInquiryActive() && !Hci::CancelInquiry()) return -1;
|
||||
}
|
||||
}
|
||||
|
||||
return Hci::GetInquiryResults(buf, maxCount);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Connect — initiate ACL connection
|
||||
// =========================================================================
|
||||
|
||||
static bool SameAddress(const uint8_t* a, const uint8_t* b) {
|
||||
if (!a || !b) return false;
|
||||
for (int i = 0; i < 6; i++) if (a[i] != b[i]) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
static Hci::ConnectionInfo* FindAclConnection(const uint8_t* bdAddr) {
|
||||
for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) {
|
||||
auto* conn = Hci::GetConnectionByIndex(i);
|
||||
if (conn && conn->Active && conn->LinkType == 0x01
|
||||
&& SameAddress(conn->BdAddr, bdAddr)) return conn;
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
int Connect(const uint8_t* bdAddr, uint32_t timeoutMs) {
|
||||
if (!g_initialized || !bdAddr) return -1;
|
||||
|
||||
// A previous attempt can leave a healthy encrypted ACL link with A2DP
|
||||
// incomplete. Treat another click as an A2DP repair attempt on that
|
||||
// link; issuing HCI Create Connection again just returns "connection
|
||||
// already exists" and made manual recovery impossible.
|
||||
Hci::ConnectionInfo* target = FindAclConnection(bdAddr);
|
||||
bool reusedAcl = target != nullptr;
|
||||
|
||||
if (!target && !Hci::CreateConnection(bdAddr)) return -1;
|
||||
|
||||
// Wait for Connection Complete event
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (!target && Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
target = FindAclConnection(bdAddr);
|
||||
for (int j = 0; j < 200; j++) {
|
||||
Arch::Pause();
|
||||
}
|
||||
}
|
||||
if (!target) return -1;
|
||||
|
||||
// Connection Complete queues authentication for both incoming and
|
||||
// outgoing ACLs. Deliver that common request and wait for encryption;
|
||||
// a manual A2DP repair on an encrypted ACL skips this entire wait.
|
||||
if (!target->Encrypted) {
|
||||
// A link that predates this syscall may have exhausted or missed its
|
||||
// earlier security attempt; explicitly restart it. A freshly-created
|
||||
// link already has the request queued by Connection Complete.
|
||||
if (reusedAcl) Hci::AuthenticateLink(target->Handle);
|
||||
uint64_t t0 = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - t0 < 5000) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
Hci::ProcessPendingCommands();
|
||||
if (!target->Active || !SameAddress(target->BdAddr, bdAddr)) break;
|
||||
if (target->Encrypted) break;
|
||||
for (int k = 0; k < 200; k++) Arch::Pause();
|
||||
}
|
||||
}
|
||||
|
||||
bool targetAlive = target->Active
|
||||
&& SameAddress(target->BdAddr, bdAddr);
|
||||
bool a2dpReady = false;
|
||||
if (targetAlive) {
|
||||
a2dpReady = A2dp::StartSource();
|
||||
Drivers::Audio::Mixer::OnBluetoothStateChanged();
|
||||
}
|
||||
|
||||
// Persist any new link key now (process context), even if the link later
|
||||
// dropped, so disk I/O never stalls the nested pairing event handler.
|
||||
Hci::FlushLinkKeys();
|
||||
if (!target->Active || !SameAddress(target->BdAddr, bdAddr)) return -1;
|
||||
if (!a2dpReady) {
|
||||
KernelLogStream(WARNING, "BT")
|
||||
<< "ACL connected but A2DP source setup failed";
|
||||
return -2;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Disconnect — disconnect a device by BD_ADDR
|
||||
// =========================================================================
|
||||
|
||||
int Disconnect(const uint8_t* bdAddr) {
|
||||
if (!g_initialized || !bdAddr) return -1;
|
||||
|
||||
// Find connection with matching BD_ADDR
|
||||
for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) {
|
||||
auto* conn = Hci::GetConnectionByIndex(i);
|
||||
if (conn && conn->Active) {
|
||||
if (SameAddress(conn->BdAddr, bdAddr)) {
|
||||
Hci::Disconnect(conn->Handle, 0x13); // 0x13 = Remote User Terminated
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return -1; // Not found
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ListConnected — list active connections
|
||||
// =========================================================================
|
||||
|
||||
int ListConnected(Hci::ConnectionInfo* buf, int maxCount) {
|
||||
if (!g_initialized || !buf || maxCount <= 0) return 0;
|
||||
|
||||
int count = 0;
|
||||
for (int i = 0; i < Hci::MAX_CONNECTIONS && count < maxCount; i++) {
|
||||
auto* conn = Hci::GetConnectionByIndex(i);
|
||||
if (conn && conn->Active) {
|
||||
buf[count] = *conn;
|
||||
count++;
|
||||
}
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ListBonded / ForgetDevice — paired-device management
|
||||
// =========================================================================
|
||||
|
||||
int ListBonded(Hci::BondInfo* buf, int maxCount) {
|
||||
if (!g_initialized || !buf || maxCount <= 0) return 0;
|
||||
return Hci::ListBonds(buf, maxCount);
|
||||
}
|
||||
|
||||
int ForgetDevice(const uint8_t* bdAddr) {
|
||||
if (!g_initialized || !bdAddr) return -1;
|
||||
// Best-effort: tear down an active link first so we don't keep a live
|
||||
// connection whose key we just discarded.
|
||||
Disconnect(bdAddr);
|
||||
return Hci::ForgetBond(bdAddr) ? 0 : -1;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,77 +0,0 @@
|
||||
/*
|
||||
* Bluetooth.hpp
|
||||
* Top-level Bluetooth subsystem header
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include "Hci.hpp"
|
||||
|
||||
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();
|
||||
|
||||
// Steady-state event service: drains the xHCI event ring + Bluetooth ACL
|
||||
// RX ring and flushes queued HCI replies. Called from the idle loop so
|
||||
// inbound traffic (the headset's SDP/AVRCP queries, AVDTP commands, ACL
|
||||
// credits) is processed even when no syscall happens to be pumping a BT
|
||||
// wait loop. Cheap no-op when the adapter is down; safe from any core
|
||||
// (PollEvents/DrainEvents/ProcessPendingCommands all self-serialize).
|
||||
void ServiceEvents();
|
||||
|
||||
// True when boot-deferred initialization or queued HCI control work needs
|
||||
// an idle-context service pass. USB receive events are signaled separately
|
||||
// by xHCI and cause the dispatcher to service Bluetooth in the same pass.
|
||||
bool HasDeferredWork();
|
||||
|
||||
// Query adapter state
|
||||
bool IsInitialized();
|
||||
uint8_t GetSlotId();
|
||||
|
||||
// Get Bluetooth device address (6 bytes)
|
||||
const uint8_t* GetBdAddr();
|
||||
|
||||
// Change the adapter's BD_ADDR at runtime. Drops any active links (their
|
||||
// pairing/crypto was bound to the old address), issues the Intel vendor
|
||||
// write (0xFC31), re-reads the controller's address into the cache and
|
||||
// re-asserts scan. Returns true only if the controller reports the new
|
||||
// address. Persisting the choice to 0:/config/bluetooth.toml is the
|
||||
// caller's job (userspace); the override is re-applied on boot by
|
||||
// ApplyConfiguredAddress() during init. Bytes are in the same order as
|
||||
// GetBdAddr()/ReadBdAddr (addr[0] = least-significant octet).
|
||||
bool SetAddress(const uint8_t* addr);
|
||||
|
||||
// Scan for nearby devices (blocking, up to timeoutMs)
|
||||
// Returns number of devices found; results written to buf
|
||||
int Scan(Hci::InquiryDevice* buf, int maxCount, uint32_t timeoutMs);
|
||||
|
||||
// Initiate connection to a remote device by BD_ADDR
|
||||
// Returns 0 when ACL + A2DP are ready, -1 when the ACL connection failed,
|
||||
// or -2 when ACL connected but A2DP setup did not complete.
|
||||
int Connect(const uint8_t* bdAddr, uint32_t timeoutMs = 10000);
|
||||
|
||||
// Disconnect a device by BD_ADDR
|
||||
// Returns 0 on success, -1 if not connected
|
||||
int Disconnect(const uint8_t* bdAddr);
|
||||
|
||||
// List connected devices
|
||||
// Returns number of connected devices; info written to buf
|
||||
int ListConnected(Hci::ConnectionInfo* buf, int maxCount);
|
||||
|
||||
// List bonded (paired) devices from the persisted link-key store.
|
||||
// Returns number written to buf (paired devices may be disconnected).
|
||||
int ListBonded(Hci::BondInfo* buf, int maxCount);
|
||||
|
||||
// Forget a paired device: drop the link if connected and remove the bond
|
||||
// (so it must re-pair next time). Returns 0 on success, -1 if not bonded.
|
||||
int ForgetDevice(const uint8_t* bdAddr);
|
||||
|
||||
}
|
||||
File diff suppressed because it is too large.
Load diff
@@ -1,376 +0,0 @@
|
||||
/*
|
||||
* Hci.hpp
|
||||
* Bluetooth HCI (Host Controller Interface) layer
|
||||
* HCI transport over USB bulk/interrupt/control endpoints
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
|
||||
namespace Drivers::USB::Bluetooth::Hci {
|
||||
|
||||
// =========================================================================
|
||||
// HCI packet types (for USB transport)
|
||||
// =========================================================================
|
||||
|
||||
// USB transport uses different endpoints for each packet type:
|
||||
// Commands -> Control EP0 (class request)
|
||||
// ACL data -> Bulk OUT / Bulk IN
|
||||
// Events -> Interrupt IN
|
||||
|
||||
// =========================================================================
|
||||
// HCI command opcodes (OGF << 10 | OCF)
|
||||
// =========================================================================
|
||||
|
||||
// Link Control (OGF 0x01)
|
||||
constexpr uint16_t OP_INQUIRY = 0x0401;
|
||||
constexpr uint16_t OP_INQUIRY_CANCEL = 0x0402;
|
||||
constexpr uint16_t OP_CREATE_CONNECTION = 0x0405;
|
||||
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;
|
||||
constexpr uint16_t OP_USER_CONFIRM_REPLY = 0x042C;
|
||||
|
||||
// Link Policy (OGF 0x02)
|
||||
constexpr uint16_t OP_WRITE_DEFAULT_LP = 0x080F;
|
||||
constexpr uint16_t OP_SNIFF_MODE = 0x0803;
|
||||
|
||||
// Controller & Baseband (OGF 0x03)
|
||||
constexpr uint16_t OP_RESET = 0x0C03;
|
||||
constexpr uint16_t OP_SET_EVENT_FILTER = 0x0C05;
|
||||
constexpr uint16_t OP_WRITE_LOCAL_NAME = 0x0C13;
|
||||
constexpr uint16_t OP_READ_LOCAL_NAME = 0x0C14;
|
||||
constexpr uint16_t OP_WRITE_SCAN_ENABLE = 0x0C1A;
|
||||
constexpr uint16_t OP_WRITE_CLASS_OF_DEVICE = 0x0C24;
|
||||
constexpr uint16_t OP_WRITE_SSP_MODE = 0x0C56;
|
||||
constexpr uint16_t OP_WRITE_SC_HOST_SUPPORT = 0x0C7A;
|
||||
constexpr uint16_t OP_WRITE_INQUIRY_MODE = 0x0C45;
|
||||
constexpr uint16_t OP_WRITE_PAGE_TIMEOUT = 0x0C18;
|
||||
constexpr uint16_t OP_WRITE_AUTH_ENABLE = 0x0C20;
|
||||
constexpr uint16_t OP_SET_EVENT_MASK = 0x0C01;
|
||||
|
||||
// Informational Parameters (OGF 0x04)
|
||||
constexpr uint16_t OP_READ_BD_ADDR = 0x1009;
|
||||
constexpr uint16_t OP_READ_LOCAL_VERSION = 0x1001;
|
||||
constexpr uint16_t OP_READ_LOCAL_FEATURES = 0x1003;
|
||||
constexpr uint16_t OP_READ_BUFFER_SIZE = 0x1005;
|
||||
|
||||
// Intel vendor commands (OGF 0x3F)
|
||||
constexpr uint16_t OP_INTEL_READ_VERSION = 0xFC05;
|
||||
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
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint8_t EVT_INQUIRY_COMPLETE = 0x01;
|
||||
constexpr uint8_t EVT_INQUIRY_RESULT = 0x02;
|
||||
constexpr uint8_t EVT_CONNECTION_COMPLETE = 0x03;
|
||||
constexpr uint8_t EVT_CONNECTION_REQUEST = 0x04;
|
||||
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;
|
||||
constexpr uint8_t EVT_IO_CAPABILITY_REQUEST = 0x31;
|
||||
constexpr uint8_t EVT_IO_CAPABILITY_RESPONSE = 0x32;
|
||||
constexpr uint8_t EVT_USER_CONFIRM_REQUEST = 0x33;
|
||||
constexpr uint8_t EVT_SIMPLE_PAIRING_COMPLETE = 0x36;
|
||||
constexpr uint8_t EVT_INQUIRY_RESULT_RSSI = 0x22;
|
||||
constexpr uint8_t EVT_EXTENDED_INQUIRY_RESULT = 0x2F;
|
||||
constexpr uint8_t EVT_VENDOR_SPECIFIC = 0xFF;
|
||||
|
||||
// =========================================================================
|
||||
// Inquiry result storage
|
||||
// =========================================================================
|
||||
|
||||
struct InquiryDevice {
|
||||
uint8_t BdAddr[6];
|
||||
uint8_t _pad[2];
|
||||
uint32_t ClassOfDevice;
|
||||
int8_t Rssi;
|
||||
uint8_t _pad2[3];
|
||||
char Name[64]; // From Extended Inquiry Result or Remote Name Request
|
||||
};
|
||||
|
||||
constexpr int MAX_INQUIRY_RESULTS = 16;
|
||||
|
||||
// =========================================================================
|
||||
// HCI packet headers
|
||||
// =========================================================================
|
||||
|
||||
struct CommandHeader {
|
||||
uint16_t Opcode;
|
||||
uint8_t ParamLength;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct EventHeader {
|
||||
uint8_t EventCode;
|
||||
uint8_t ParamLength;
|
||||
} __attribute__((packed));
|
||||
|
||||
struct AclHeader {
|
||||
uint16_t HandleFlags; // bits 11:0 = handle, 13:12 = PB flag, 15:14 = BC flag
|
||||
uint16_t DataLength;
|
||||
} __attribute__((packed));
|
||||
|
||||
// ACL PB (Packet Boundary) flag values
|
||||
constexpr uint16_t ACL_PB_FIRST_NON_FLUSH = 0x0000; // First non-auto-flushable
|
||||
constexpr uint16_t ACL_PB_CONTINUING = 0x1000; // Continuing fragment
|
||||
constexpr uint16_t ACL_PB_FIRST_FLUSH = 0x2000; // First auto-flushable
|
||||
|
||||
// =========================================================================
|
||||
// HCI connection info
|
||||
// =========================================================================
|
||||
|
||||
struct ConnectionInfo {
|
||||
bool Active;
|
||||
uint16_t Handle;
|
||||
uint8_t BdAddr[6];
|
||||
uint8_t LinkType; // 0x01 = ACL
|
||||
bool Encrypted;
|
||||
};
|
||||
|
||||
constexpr int MAX_CONNECTIONS = 4;
|
||||
|
||||
// A persisted (bonded/paired) device, as stored in the link-key store.
|
||||
struct BondInfo {
|
||||
uint8_t Addr[6];
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// Intel Bluetooth version info
|
||||
// =========================================================================
|
||||
|
||||
struct IntelVersion {
|
||||
uint8_t Status;
|
||||
uint8_t HwPlatform;
|
||||
uint8_t HwVariant;
|
||||
uint8_t HwRevision;
|
||||
uint8_t FwVariant; // 0x06 = bootloader, 0x23 = operational
|
||||
uint8_t FwRevision;
|
||||
uint8_t FwBuildNum;
|
||||
uint8_t FwBuildWw;
|
||||
uint8_t FwBuildYy;
|
||||
uint8_t FwPatchNum;
|
||||
} __attribute__((packed));
|
||||
|
||||
// =========================================================================
|
||||
// Public API
|
||||
// =========================================================================
|
||||
|
||||
// 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).
|
||||
// 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();
|
||||
|
||||
// Enable operational ACL receive framing after firmware/HCI setup. Bulk IN
|
||||
// is armed earlier to protect Intel firmware loading; pre-operational runts
|
||||
// are intentionally ignored and any endpoint error is recovered here.
|
||||
void EnableAclDataReception();
|
||||
|
||||
// Send an HCI command via USB control transfer (EP0)
|
||||
bool SendCommand(uint16_t opcode, const uint8_t* params, uint8_t paramLen);
|
||||
|
||||
// Wait for a Command Complete event matching the given opcode
|
||||
// Returns true if received within timeout, fills outParams (excluding status byte)
|
||||
bool WaitCommandComplete(uint16_t opcode, uint8_t* outParams = nullptr,
|
||||
uint8_t maxLen = 0, uint32_t timeoutMs = 2000);
|
||||
|
||||
// Wait for a Command Status event matching the given opcode
|
||||
bool WaitCommandStatus(uint16_t opcode, uint32_t timeoutMs = 2000);
|
||||
|
||||
// Send ACL data via USB bulk OUT
|
||||
bool SendAcl(uint16_t handle, uint16_t pbFlag, const uint8_t* data, uint16_t len);
|
||||
|
||||
// Process an HCI event received on the interrupt IN endpoint
|
||||
void ProcessEvent(const uint8_t* data, uint32_t len);
|
||||
|
||||
// Process ACL data received on the bulk IN endpoint
|
||||
void ProcessAcl(const uint8_t* data, uint32_t len);
|
||||
|
||||
// Get connection info
|
||||
ConnectionInfo* GetConnection(uint16_t handle);
|
||||
ConnectionInfo* GetActiveConnection();
|
||||
ConnectionInfo* GetConnectionByIndex(int index); // 0..MAX_CONNECTIONS-1
|
||||
|
||||
// HCI Reset command
|
||||
bool Reset();
|
||||
|
||||
// Read local BD_ADDR
|
||||
bool ReadBdAddr(uint8_t* addr);
|
||||
|
||||
// Read standard HCI local version info
|
||||
struct LocalVersion {
|
||||
uint8_t Status;
|
||||
uint8_t HciVersion;
|
||||
uint16_t HciRevision;
|
||||
uint8_t LmpVersion;
|
||||
uint16_t Manufacturer;
|
||||
uint16_t LmpSubversion;
|
||||
} __attribute__((packed));
|
||||
|
||||
bool ReadLocalVersion(LocalVersion* ver);
|
||||
|
||||
// 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);
|
||||
|
||||
// Pipelined bulk-OUT variant for the firmware payload (btusb bootloader
|
||||
// path: 0xFC09 packets on the bulk endpoint, several in flight). Call
|
||||
// DrainBulkTx() after the last fragment to ensure everything reached the
|
||||
// controller before waiting for the download-complete result.
|
||||
bool IntelSecureSendBulk(uint8_t fragmentType, const uint8_t* data, uint32_t len);
|
||||
bool DrainBulkTx(uint32_t timeoutMs);
|
||||
|
||||
// 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();
|
||||
|
||||
// Enumerate the stored bonds (paired devices) into buf; returns the count
|
||||
// written (<= maxCount). Used to list paired-but-disconnected devices.
|
||||
int ListBonds(BondInfo* buf, int maxCount);
|
||||
|
||||
// Remove a stored bond by BD_ADDR and persist the removal (process context).
|
||||
// Returns true if a matching bond was found and forgotten.
|
||||
bool ForgetBond(const uint8_t* addr);
|
||||
|
||||
// Toggle the bounded firmware-phase interrupt-IN trace (one ring record
|
||||
// per completion, capped). Recording is IRQ-safe (lock-free ring, no
|
||||
// logging); DumpFwTrace() prints pending records and MUST only be called
|
||||
// from process context (terminal lock is a non-IRQ-safe Mutex).
|
||||
void SetFwTrace(bool on);
|
||||
void DumpFwTrace();
|
||||
|
||||
// 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);
|
||||
|
||||
// Set class of device
|
||||
bool WriteClassOfDevice(uint32_t cod);
|
||||
|
||||
// Enable scan (inquiry + page)
|
||||
bool WriteScanEnable(uint8_t mode);
|
||||
|
||||
// Write Simple Secure Pairing mode
|
||||
bool WriteSSPMode(uint8_t mode);
|
||||
|
||||
// Select inquiry result format: 0=standard, 1=RSSI, 2=extended/EIR.
|
||||
bool WriteInquiryMode(uint8_t mode);
|
||||
|
||||
// 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);
|
||||
|
||||
// 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();
|
||||
bool HasPendingCommands();
|
||||
|
||||
// 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();
|
||||
|
||||
// ACL packets handed to the xHCI whose bulk OUT completion has not been
|
||||
// reaped yet (each one still owns a TX DMA ring slot).
|
||||
uint32_t AclTxInFlight();
|
||||
|
||||
// True when SendAcl can be called without overrunning either the
|
||||
// controller's ACL buffers (NOCP credits) or the TX DMA ring. The media
|
||||
// writer polls events until this is true before sending each frame.
|
||||
bool AclTxReady();
|
||||
|
||||
// Lost-NOCP recovery: zero the credit count after a prolonged stall with
|
||||
// the USB side drained (completions presumed lost).
|
||||
void AclResetCredits();
|
||||
|
||||
// Disconnect a connection
|
||||
bool Disconnect(uint16_t handle, uint8_t reason);
|
||||
|
||||
// Read ACL buffer size from controller
|
||||
bool ReadBufferSize(uint16_t* aclLen, uint8_t* scoLen,
|
||||
uint16_t* aclNum, uint16_t* scoNum);
|
||||
|
||||
// Inquiry (device discovery)
|
||||
bool StartInquiry(uint8_t durationUnits); // duration in 1.28s units (e.g., 8 = ~10s)
|
||||
bool CancelInquiry();
|
||||
int GetInquiryResults(InquiryDevice* buf, int maxCount);
|
||||
void ClearInquiryResults();
|
||||
bool IsInquiryActive();
|
||||
|
||||
// Create ACL connection to a remote device
|
||||
bool CreateConnection(const uint8_t* bdAddr);
|
||||
|
||||
// Drain any pending HCI events (call in poll loops that aren't inside
|
||||
// WaitCommandComplete/WaitCommandStatus)
|
||||
void DrainEvents();
|
||||
|
||||
}
|
||||
@@ -1,487 +0,0 @@
|
||||
/*
|
||||
* 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 <Arch/Cpu.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/Timer.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.
|
||||
// A rejection event arrives within milliseconds; on SUCCESS the
|
||||
// controller stays silent until the END of the whole download (traced
|
||||
// on the AX211), so a long timeout here only burns boot time.
|
||||
Hci::ClearSecureSendResult();
|
||||
if (!Hci::IntelSecureSend(0x00, fw + cssOff, 128)) return false;
|
||||
if (Hci::WaitSecureSendResult(250, &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. Same rule: silence means accepted; a
|
||||
// rejected header also fails the payload phase immediately, so the
|
||||
// short window loses nothing.
|
||||
Hci::ClearSecureSendResult();
|
||||
if (!Hci::IntelSecureSend(0x03, fw + pkeyOff, pkeyLen)) return false;
|
||||
if (!Hci::IntelSecureSend(0x02, fw + sigOff, sigLen)) return false;
|
||||
if (Hci::WaitSecureSendResult(250, &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) {
|
||||
// Payload goes over the bulk endpoint, pipelined (the btusb
|
||||
// bootloader path) -- EP0's synchronous 3-stage transfers made
|
||||
// the 720 KB image the dominant boot cost.
|
||||
if (!Hci::IntelSecureSendBulk(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)";
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// The bulk pipeline may still hold up to 7 in-flight fragments; make
|
||||
// sure every byte reached the controller before the caller waits for
|
||||
// the download-complete result.
|
||||
if (!Hci::DrainBulkTx(3000)) {
|
||||
KernelLogStream(ERROR, "BT-FW") << "Bulk TX drain timed out at end of payload";
|
||||
return false;
|
||||
}
|
||||
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);
|
||||
Arch::CompilerBarrier();
|
||||
|
||||
// 4. Run the secure download + boot sequence.
|
||||
bool ok = DownloadImage(fw, size, ver, cnvi, cnvr);
|
||||
Memory::g_heap->Free(fw);
|
||||
return ok;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,27 +0,0 @@
|
||||
/*
|
||||
* 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();
|
||||
|
||||
}
|
||||
@@ -1,679 +0,0 @@
|
||||
/*
|
||||
* L2cap.cpp
|
||||
* Bluetooth L2CAP implementation
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "L2cap.hpp"
|
||||
#include <Arch/Cpu.hpp>
|
||||
#include "Hci.hpp"
|
||||
#include "A2dp.hpp"
|
||||
#include "Avrcp.hpp"
|
||||
#include <Drivers/USB/Xhci.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
#include <Timekeeping/Timer.hpp>
|
||||
|
||||
using namespace Kt;
|
||||
|
||||
namespace Drivers::USB::Bluetooth::L2cap {
|
||||
|
||||
// =========================================================================
|
||||
// State
|
||||
// =========================================================================
|
||||
|
||||
static uint16_t g_aclHandle = 0;
|
||||
static bool g_initialized = false;
|
||||
static uint8_t g_sigIdentifier = 1;
|
||||
|
||||
// Channel table
|
||||
static ChannelInfo g_channels[MAX_CHANNELS] = {};
|
||||
static uint16_t g_nextCid = CID_DYNAMIC_START;
|
||||
|
||||
// =========================================================================
|
||||
// Helpers
|
||||
// =========================================================================
|
||||
|
||||
static uint8_t NextIdentifier() {
|
||||
uint8_t id = g_sigIdentifier++;
|
||||
if (id == 0) id = g_sigIdentifier++;
|
||||
if (g_sigIdentifier == 0) g_sigIdentifier = 1;
|
||||
return id;
|
||||
}
|
||||
|
||||
static uint16_t AllocCid() {
|
||||
// Dynamic CIDs are 0x0040..0xFFFF and must not collide with an active
|
||||
// channel after wraparound.
|
||||
for (uint32_t tries = 0; tries <= 0xFFC0; tries++) {
|
||||
if (g_nextCid < CID_DYNAMIC_START) g_nextCid = CID_DYNAMIC_START;
|
||||
uint16_t candidate = g_nextCid++;
|
||||
bool used = false;
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == candidate) {
|
||||
used = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!used) return candidate;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
static ChannelInfo* AllocChannel(uint16_t psm) {
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (!g_channels[i].Active) {
|
||||
g_channels[i].Active = true;
|
||||
g_channels[i].LocalCid = AllocCid();
|
||||
if (g_channels[i].LocalCid == 0) {
|
||||
g_channels[i].Active = false;
|
||||
return nullptr;
|
||||
}
|
||||
g_channels[i].RemoteCid = 0;
|
||||
g_channels[i].Psm = psm;
|
||||
g_channels[i].RemoteMtu = 672; // Default L2CAP MTU
|
||||
g_channels[i].Configured = false;
|
||||
g_channels[i].LocalConfigDone = false;
|
||||
g_channels[i].RemoteConfigDone = false;
|
||||
g_channels[i].ConnRspResult = 0xFFFF;
|
||||
g_channels[i].ConnRspStatus = 0;
|
||||
return &g_channels[i];
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Send L2CAP signaling command
|
||||
static bool SendSignal(uint8_t code, uint8_t identifier,
|
||||
const uint8_t* payload, uint16_t payloadLen) {
|
||||
// L2CAP header + Signal header + payload
|
||||
uint16_t sigLen = sizeof(SignalHeader) + payloadLen;
|
||||
uint16_t totalPayload = sizeof(L2capHeader) + sigLen;
|
||||
|
||||
uint8_t buf[128] = {};
|
||||
if (totalPayload > sizeof(buf)) return false;
|
||||
auto* l2hdr = (L2capHeader*)buf;
|
||||
l2hdr->Length = sigLen;
|
||||
l2hdr->ChannelId = CID_SIGNALING;
|
||||
|
||||
auto* sig = (SignalHeader*)(buf + sizeof(L2capHeader));
|
||||
sig->Code = code;
|
||||
sig->Identifier = identifier;
|
||||
sig->Length = payloadLen;
|
||||
|
||||
if (payload && payloadLen > 0) {
|
||||
memcpy(buf + sizeof(L2capHeader) + sizeof(SignalHeader), payload, payloadLen);
|
||||
}
|
||||
|
||||
return Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_NON_FLUSH,
|
||||
buf, totalPayload);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Initialize
|
||||
// =========================================================================
|
||||
|
||||
void Initialize(uint16_t aclHandle) {
|
||||
g_aclHandle = aclHandle;
|
||||
g_initialized = true;
|
||||
g_sigIdentifier = 1;
|
||||
g_nextCid = CID_DYNAMIC_START;
|
||||
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active)
|
||||
Avrcp::OnChannelClosed(g_channels[i].LocalCid);
|
||||
g_channels[i].Active = false;
|
||||
}
|
||||
|
||||
KernelLogStream(OK, "BT-L2CAP") << "Initialized for ACL handle " << (uint64_t)aclHandle;
|
||||
}
|
||||
|
||||
void OnDisconnected(uint16_t aclHandle) {
|
||||
if (!g_initialized || aclHandle != g_aclHandle) return;
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active)
|
||||
Avrcp::OnChannelClosed(g_channels[i].LocalCid);
|
||||
g_channels[i].Active = false;
|
||||
}
|
||||
g_aclHandle = 0;
|
||||
g_initialized = false;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ProcessPacket
|
||||
// =========================================================================
|
||||
|
||||
void ProcessPacket(uint16_t aclHandle, const uint8_t* data, uint16_t len) {
|
||||
if (!g_initialized || aclHandle != g_aclHandle) return;
|
||||
if (len < sizeof(L2capHeader)) return;
|
||||
|
||||
auto* l2hdr = (const L2capHeader*)data;
|
||||
uint16_t l2len = l2hdr->Length;
|
||||
uint16_t cid = l2hdr->ChannelId;
|
||||
const uint8_t* payload = data + sizeof(L2capHeader);
|
||||
|
||||
if (l2len + sizeof(L2capHeader) > len) return;
|
||||
|
||||
if (cid == CID_SIGNALING) {
|
||||
// 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;
|
||||
|
||||
switch (sig->Code) {
|
||||
case SIG_CONN_REQ: {
|
||||
if (sigPayloadLen >= 4) {
|
||||
uint16_t psm = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
||||
uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8);
|
||||
|
||||
KernelLogStream(INFO, "BT-L2CAP") << "Connection Request: PSM="
|
||||
<< base::hex << (uint64_t)psm << " srcCID=" << (uint64_t)srcCid;
|
||||
|
||||
// Accept connections for AVDTP, SDP, and AVRCP control
|
||||
if (psm == PSM_AVDTP || psm == PSM_SDP || psm == PSM_AVCTP) {
|
||||
// Detect a duplicate source CID instead of creating
|
||||
// two local channels that address the same endpoint.
|
||||
// Same-PSM duplicates are normal retransmissions;
|
||||
// a different PSM reusing the CID is invalid.
|
||||
ChannelInfo* existing = nullptr;
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active
|
||||
&& g_channels[i].RemoteCid == srcCid) {
|
||||
existing = &g_channels[i];
|
||||
break;
|
||||
}
|
||||
}
|
||||
auto* ch = existing ? nullptr : AllocChannel(psm);
|
||||
if (existing && existing->Psm == psm) {
|
||||
// Retransmission of a request whose response was
|
||||
// lost: repeat SUCCESS for the original channel,
|
||||
// and repeat our config request as well.
|
||||
uint8_t rsp[8] = {
|
||||
(uint8_t)existing->LocalCid,
|
||||
(uint8_t)(existing->LocalCid >> 8),
|
||||
(uint8_t)srcCid, (uint8_t)(srcCid >> 8),
|
||||
0, 0, 0, 0
|
||||
};
|
||||
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
||||
uint8_t cfgReq[4] = {
|
||||
(uint8_t)srcCid, (uint8_t)(srcCid >> 8), 0, 0
|
||||
};
|
||||
SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4);
|
||||
} else if (existing) {
|
||||
uint8_t rsp[8] = {
|
||||
0, 0, (uint8_t)srcCid, (uint8_t)(srcCid >> 8),
|
||||
0x07, 0x00, 0, 0 // source CID already allocated
|
||||
};
|
||||
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
||||
} else if (ch) {
|
||||
ch->RemoteCid = srcCid;
|
||||
|
||||
// Send Connection Response (success)
|
||||
uint8_t rsp[8] = {};
|
||||
rsp[0] = (uint8_t)(ch->LocalCid & 0xFF);
|
||||
rsp[1] = (uint8_t)(ch->LocalCid >> 8);
|
||||
rsp[2] = (uint8_t)(srcCid & 0xFF);
|
||||
rsp[3] = (uint8_t)(srcCid >> 8);
|
||||
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, NextIdentifier(), cfgReq, 4);
|
||||
} else {
|
||||
uint8_t rsp[8] = {
|
||||
0, 0, (uint8_t)srcCid, (uint8_t)(srcCid >> 8),
|
||||
0x04, 0x00, 0, 0 // no resources
|
||||
};
|
||||
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
||||
}
|
||||
} else {
|
||||
// Reject: PSM not supported
|
||||
uint8_t rsp[8] = {};
|
||||
rsp[0] = 0; rsp[1] = 0; // Dest CID = 0
|
||||
rsp[2] = (uint8_t)(srcCid & 0xFF);
|
||||
rsp[3] = (uint8_t)(srcCid >> 8);
|
||||
rsp[4] = 0x02; rsp[5] = 0; // Result: PSM not supported
|
||||
rsp[6] = 0; rsp[7] = 0;
|
||||
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case SIG_CONN_RSP: {
|
||||
if (sigPayloadLen >= 8) {
|
||||
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;
|
||||
KernelLogStream(INFO, "BT-L2CAP") << "Connection Response: dstCID="
|
||||
<< 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)
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) {
|
||||
g_channels[i].ConnRspResult = result;
|
||||
g_channels[i].ConnRspStatus = status;
|
||||
g_channels[i].RemoteCid = dstCid;
|
||||
|
||||
// Send Configuration Request
|
||||
uint8_t cfgReq[4] = {};
|
||||
cfgReq[0] = (uint8_t)(dstCid & 0xFF);
|
||||
cfgReq[1] = (uint8_t)(dstCid >> 8);
|
||||
cfgReq[2] = 0; cfgReq[3] = 0; // Flags
|
||||
SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4);
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else if (result == CONN_PENDING && dstCid != 0) {
|
||||
// A PENDING response may already carry the remote's
|
||||
// allocated CID. Record it (but do NOT configure --
|
||||
// the final SUCCESS on this channel does that): the
|
||||
// diagnostics then show the peer engaged, and if the
|
||||
// dial is later abandoned, FreeChannel knows to send
|
||||
// a Disconnect for the half-open remote endpoint.
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) {
|
||||
g_channels[i].ConnRspResult = result;
|
||||
g_channels[i].ConnRspStatus = status;
|
||||
g_channels[i].RemoteCid = dstCid;
|
||||
break;
|
||||
}
|
||||
}
|
||||
} else {
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active
|
||||
&& g_channels[i].LocalCid == srcCid) {
|
||||
g_channels[i].ConnRspResult = result;
|
||||
g_channels[i].ConnRspStatus = status;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case SIG_CONFIG_REQ: {
|
||||
if (sigPayloadLen >= 4) {
|
||||
uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
||||
|
||||
// Find channel
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == dstCid) {
|
||||
g_channels[i].RemoteConfigDone = true;
|
||||
|
||||
// Parse MTU option if present
|
||||
uint16_t cfgOffset = 4; // Skip dstCid + flags
|
||||
while (cfgOffset + 2 <= sigPayloadLen) {
|
||||
uint8_t optType = sigPayload[cfgOffset];
|
||||
uint8_t optLen = sigPayload[cfgOffset + 1];
|
||||
if (optType == 0x01 && optLen == 2 && cfgOffset + 4 <= sigPayloadLen) {
|
||||
g_channels[i].RemoteMtu = (uint16_t)sigPayload[cfgOffset + 2]
|
||||
| ((uint16_t)sigPayload[cfgOffset + 3] << 8);
|
||||
}
|
||||
cfgOffset += 2 + optLen;
|
||||
}
|
||||
|
||||
// Send Config Response (success)
|
||||
uint8_t rsp[6] = {};
|
||||
rsp[0] = (uint8_t)(g_channels[i].RemoteCid & 0xFF);
|
||||
rsp[1] = (uint8_t)(g_channels[i].RemoteCid >> 8);
|
||||
rsp[2] = 0; rsp[3] = 0; // Flags
|
||||
rsp[4] = 0; rsp[5] = 0; // Result: success
|
||||
SendSignal(SIG_CONFIG_RSP, sig->Identifier, rsp, 6);
|
||||
|
||||
if (g_channels[i].LocalConfigDone && g_channels[i].RemoteConfigDone) {
|
||||
g_channels[i].Configured = true;
|
||||
KernelLogStream(OK, "BT-L2CAP") << "Channel "
|
||||
<< (uint64_t)g_channels[i].LocalCid << " configured";
|
||||
|
||||
// Notify A2DP if this is an AVDTP channel
|
||||
if (g_channels[i].Psm == PSM_AVDTP) {
|
||||
A2dp::OnChannelReady(g_channels[i].LocalCid);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
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].LocalCid == srcCid) {
|
||||
matched = true;
|
||||
g_channels[i].LocalConfigDone = true;
|
||||
if (g_channels[i].LocalConfigDone && g_channels[i].RemoteConfigDone) {
|
||||
g_channels[i].Configured = true;
|
||||
KernelLogStream(OK, "BT-L2CAP") << "Channel "
|
||||
<< (uint64_t)g_channels[i].LocalCid << " configured";
|
||||
|
||||
if (g_channels[i].Psm == PSM_AVDTP) {
|
||||
A2dp::OnChannelReady(g_channels[i].LocalCid);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (!matched) {
|
||||
KernelLogStream(WARNING, "BT-L2CAP")
|
||||
<< "Config Response (success) for unknown CID="
|
||||
<< base::hex << (uint64_t)srcCid << base::dec;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case SIG_DISCONN_REQ: {
|
||||
if (sigPayloadLen >= 4) {
|
||||
uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
||||
uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8);
|
||||
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == dstCid) {
|
||||
uint16_t closedCid = g_channels[i].LocalCid;
|
||||
g_channels[i].Active = false;
|
||||
A2dp::OnChannelClosed(closedCid);
|
||||
Avrcp::OnChannelClosed(closedCid);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Send Disconnect Response
|
||||
uint8_t rsp[4] = {};
|
||||
rsp[0] = (uint8_t)(dstCid & 0xFF);
|
||||
rsp[1] = (uint8_t)(dstCid >> 8);
|
||||
rsp[2] = (uint8_t)(srcCid & 0xFF);
|
||||
rsp[3] = (uint8_t)(srcCid >> 8);
|
||||
SendSignal(SIG_DISCONN_RSP, sig->Identifier, rsp, 4);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case SIG_INFO_REQ: {
|
||||
if (sigPayloadLen >= 2) {
|
||||
uint16_t infoType = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
||||
|
||||
if (infoType == 0x0002) {
|
||||
// Extended features mask
|
||||
uint8_t rsp[8] = {};
|
||||
rsp[0] = 0x02; rsp[1] = 0x00; // InfoType
|
||||
rsp[2] = 0x00; rsp[3] = 0x00; // Result: success
|
||||
rsp[4] = 0x00; rsp[5] = 0x00; // Features: none
|
||||
rsp[6] = 0x00; rsp[7] = 0x00;
|
||||
SendSignal(SIG_INFO_RSP, sig->Identifier, rsp, 8);
|
||||
} else {
|
||||
// Not supported
|
||||
uint8_t rsp[4] = {};
|
||||
rsp[0] = (uint8_t)(infoType & 0xFF);
|
||||
rsp[1] = (uint8_t)(infoType >> 8);
|
||||
rsp[2] = 0x01; rsp[3] = 0x00; // Result: not supported
|
||||
SendSignal(SIG_INFO_RSP, sig->Identifier, rsp, 4);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case SIG_DISCONN_RSP:
|
||||
// Response to a Disconnect Request we sent (e.g. closing the
|
||||
// SDP client channel after a query); nothing left to do.
|
||||
break;
|
||||
|
||||
case SIG_COMMAND_REJECT:
|
||||
case SIG_ECHO_RSP:
|
||||
case SIG_INFO_RSP:
|
||||
// Terminal responses to requests sent by us. No response
|
||||
// is required; in particular, never reject a response.
|
||||
break;
|
||||
|
||||
case SIG_ECHO_REQ: {
|
||||
// Echo Request must be answered (echo the payload back) or
|
||||
// a peer probing the link times out on us.
|
||||
SendSignal(SIG_ECHO_RSP, sig->Identifier, sigPayload, sigPayloadLen);
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
// A signaling command we do not handle. Log it: a peer that
|
||||
// sends something exotic while holding our dials at
|
||||
// "authorization pending" is exactly what we need to see.
|
||||
KernelLogStream(INFO, "BT-L2CAP") << "Unhandled signaling code="
|
||||
<< base::hex << (uint64_t)sig->Code << base::dec
|
||||
<< " len=" << (uint64_t)sigPayloadLen;
|
||||
// Command Reject, reason 0x0000 = command not understood.
|
||||
// Silence leaves the peer waiting for its signaling timer.
|
||||
uint8_t reject[2] = {0x00, 0x00};
|
||||
SendSignal(SIG_COMMAND_REJECT, sig->Identifier, reject, 2);
|
||||
break;
|
||||
}
|
||||
}
|
||||
sOff += sizeof(SignalHeader) + sigPayloadLen;
|
||||
}
|
||||
} else {
|
||||
// Data on a dynamic channel
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == cid) {
|
||||
if (g_channels[i].Psm == PSM_AVDTP) {
|
||||
A2dp::ProcessAvdtp(cid, payload, l2len);
|
||||
} else if (g_channels[i].Psm == PSM_SDP) {
|
||||
A2dp::ProcessSdp(cid, payload, l2len);
|
||||
} else if (g_channels[i].Psm == PSM_AVCTP) {
|
||||
Avrcp::ProcessAvctp(cid, payload, l2len);
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Connect
|
||||
// =========================================================================
|
||||
|
||||
uint16_t Connect(uint16_t psm) {
|
||||
if (!g_initialized) return 0;
|
||||
|
||||
auto* ch = AllocChannel(psm);
|
||||
if (!ch) return 0;
|
||||
|
||||
// Send Connection Request
|
||||
uint8_t req[4] = {};
|
||||
req[0] = (uint8_t)(psm & 0xFF);
|
||||
req[1] = (uint8_t)(psm >> 8);
|
||||
req[2] = (uint8_t)(ch->LocalCid & 0xFF);
|
||||
req[3] = (uint8_t)(ch->LocalCid >> 8);
|
||||
if (!SendSignal(SIG_CONN_REQ, NextIdentifier(), req, 4)) {
|
||||
ch->Active = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
return ch->LocalCid;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// WaitConfigured
|
||||
// =========================================================================
|
||||
|
||||
bool WaitConfigured(uint16_t localCid, uint32_t timeoutMs) {
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
|
||||
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;
|
||||
|
||||
for (int j = 0; j < 100; j++) {
|
||||
Arch::CompilerBarrier();
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// SendData
|
||||
// =========================================================================
|
||||
|
||||
bool SendData(uint16_t localCid, const uint8_t* data, uint16_t len) {
|
||||
if (!g_initialized) return false;
|
||||
|
||||
auto* ch = GetChannel(localCid);
|
||||
if (!ch || !ch->Configured) return false;
|
||||
|
||||
// Build L2CAP packet
|
||||
uint16_t totalLen = sizeof(L2capHeader) + len;
|
||||
uint8_t buf[1024] = {};
|
||||
if (totalLen > sizeof(buf)) return false;
|
||||
|
||||
auto* l2hdr = (L2capHeader*)buf;
|
||||
l2hdr->Length = len;
|
||||
l2hdr->ChannelId = ch->RemoteCid;
|
||||
|
||||
if (data && len > 0) {
|
||||
memcpy(buf + sizeof(L2capHeader), data, len);
|
||||
}
|
||||
|
||||
return Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_FLUSH, buf, totalLen);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Channel queries
|
||||
// =========================================================================
|
||||
|
||||
ChannelInfo* GetChannel(uint16_t localCid) {
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].LocalCid == localCid) {
|
||||
return &g_channels[i];
|
||||
}
|
||||
}
|
||||
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, NextIdentifier(), req, 4);
|
||||
}
|
||||
g_channels[i].Active = false;
|
||||
A2dp::OnChannelClosed(localCid);
|
||||
Avrcp::OnChannelClosed(localCid);
|
||||
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) {
|
||||
return &g_channels[i];
|
||||
}
|
||||
}
|
||||
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 FindAvdtpChannelExcept(uint16_t exceptCid) {
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && 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 ConnRspResult(uint16_t localCid) {
|
||||
auto* ch = GetChannel(localCid);
|
||||
return ch ? ch->ConnRspResult : 0xFFFF;
|
||||
}
|
||||
|
||||
uint16_t ConnRspStatus(uint16_t localCid) {
|
||||
auto* ch = GetChannel(localCid);
|
||||
return ch ? ch->ConnRspStatus : 0;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,145 +0,0 @@
|
||||
/*
|
||||
* L2cap.hpp
|
||||
* Bluetooth L2CAP (Logical Link Control and Adaptation Protocol)
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
|
||||
namespace Drivers::USB::Bluetooth::L2cap {
|
||||
|
||||
// =========================================================================
|
||||
// L2CAP CIDs (Channel Identifiers)
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint16_t CID_SIGNALING = 0x0001; // L2CAP signaling
|
||||
constexpr uint16_t CID_CONNLESS = 0x0002; // Connectionless reception
|
||||
constexpr uint16_t CID_DYNAMIC_START = 0x0040; // First dynamic CID
|
||||
|
||||
// =========================================================================
|
||||
// L2CAP PSMs (Protocol/Service Multiplexers)
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint16_t PSM_SDP = 0x0001; // Service Discovery Protocol
|
||||
constexpr uint16_t PSM_AVCTP = 0x0017; // Audio/Video Control Transport (AVRCP)
|
||||
constexpr uint16_t PSM_AVDTP = 0x0019; // Audio/Video Distribution Transport
|
||||
|
||||
// =========================================================================
|
||||
// L2CAP packet header
|
||||
// =========================================================================
|
||||
|
||||
struct L2capHeader {
|
||||
uint16_t Length;
|
||||
uint16_t ChannelId;
|
||||
} __attribute__((packed));
|
||||
|
||||
// =========================================================================
|
||||
// L2CAP signaling command header
|
||||
// =========================================================================
|
||||
|
||||
struct SignalHeader {
|
||||
uint8_t Code;
|
||||
uint8_t Identifier;
|
||||
uint16_t Length;
|
||||
} __attribute__((packed));
|
||||
|
||||
// Signaling command codes
|
||||
constexpr uint8_t SIG_COMMAND_REJECT = 0x01;
|
||||
constexpr uint8_t SIG_CONN_REQ = 0x02;
|
||||
constexpr uint8_t SIG_CONN_RSP = 0x03;
|
||||
constexpr uint8_t SIG_CONFIG_REQ = 0x04;
|
||||
constexpr uint8_t SIG_CONFIG_RSP = 0x05;
|
||||
constexpr uint8_t SIG_DISCONN_REQ = 0x06;
|
||||
constexpr uint8_t SIG_DISCONN_RSP = 0x07;
|
||||
constexpr uint8_t SIG_ECHO_REQ = 0x08;
|
||||
constexpr uint8_t SIG_ECHO_RSP = 0x09;
|
||||
constexpr uint8_t SIG_INFO_REQ = 0x0A;
|
||||
constexpr uint8_t SIG_INFO_RSP = 0x0B;
|
||||
|
||||
// Connection response results
|
||||
constexpr uint16_t CONN_SUCCESS = 0x0000;
|
||||
constexpr uint16_t CONN_PENDING = 0x0001;
|
||||
constexpr uint16_t CONN_REFUSED_PSM = 0x0002;
|
||||
|
||||
// Configuration response results
|
||||
constexpr uint16_t CFG_SUCCESS = 0x0000;
|
||||
|
||||
// =========================================================================
|
||||
// L2CAP channel info
|
||||
// =========================================================================
|
||||
|
||||
struct ChannelInfo {
|
||||
bool Active;
|
||||
uint16_t LocalCid;
|
||||
uint16_t RemoteCid;
|
||||
uint16_t Psm;
|
||||
uint16_t RemoteMtu;
|
||||
bool Configured; // Both sides configured
|
||||
bool LocalConfigDone;
|
||||
bool RemoteConfigDone;
|
||||
uint16_t ConnRspResult; // 0xFFFF until this outgoing dial is answered
|
||||
uint16_t ConnRspStatus; // meaningful while result == CONN_PENDING
|
||||
};
|
||||
|
||||
constexpr int MAX_CHANNELS = 8;
|
||||
|
||||
// =========================================================================
|
||||
// Public API
|
||||
// =========================================================================
|
||||
|
||||
// Initialize L2CAP for a new HCI connection
|
||||
void Initialize(uint16_t aclHandle);
|
||||
|
||||
// Invalidate every dynamic channel when its ACL link disappears. Without
|
||||
// this, AVRCP or media code can send to stale CIDs until the next connection
|
||||
// happens to reinitialize the table.
|
||||
void OnDisconnected(uint16_t aclHandle);
|
||||
|
||||
// Process an L2CAP packet (called from HCI ACL processing)
|
||||
void ProcessPacket(uint16_t aclHandle, const uint8_t* data, uint16_t len);
|
||||
|
||||
// Connect to a remote PSM (initiate L2CAP connection)
|
||||
// Returns local CID, or 0 on failure
|
||||
uint16_t Connect(uint16_t psm);
|
||||
|
||||
// Wait for connection to be configured
|
||||
bool WaitConfigured(uint16_t localCid, uint32_t timeoutMs = 5000);
|
||||
|
||||
// Send data on an L2CAP channel
|
||||
bool SendData(uint16_t localCid, const uint8_t* data, uint16_t len);
|
||||
|
||||
// Get channel info
|
||||
ChannelInfo* GetChannel(uint16_t localCid);
|
||||
|
||||
// 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);
|
||||
|
||||
// Find any active AVDTP channel except the signaling channel, including a
|
||||
// still-pending outgoing media dial. Manual retries reuse that channel
|
||||
// instead of leaking another slot/CID while authorization is in progress.
|
||||
uint16_t FindAvdtpChannelExcept(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();
|
||||
|
||||
// Per-channel connection response state. A global "last result" is unsafe:
|
||||
// an unrelated SDP, AVDTP, or AVRCP response can arrive concurrently.
|
||||
uint16_t ConnRspResult(uint16_t localCid);
|
||||
uint16_t ConnRspStatus(uint16_t localCid);
|
||||
|
||||
}
|
||||
@@ -1,502 +0,0 @@
|
||||
/*
|
||||
* Sbc.cpp
|
||||
* SBC (Sub-Band Codec) encoder — fixed-point implementation
|
||||
* Based on the Bluetooth SIG specification (A2DP v1.3, Appendix B)
|
||||
* All arithmetic is 32-bit fixed-point (Q15.16 or Q1.30)
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "Sbc.hpp"
|
||||
#include <Libraries/Memory.hpp>
|
||||
|
||||
namespace Drivers::USB::Bluetooth::Sbc {
|
||||
|
||||
// =========================================================================
|
||||
// Analysis filterbank tables
|
||||
// =========================================================================
|
||||
|
||||
// SBC 8-subband prototype window, the 80 coefficients from the A2DP spec
|
||||
// (Annex B), natural order, signs built in, Q31 fixed point. Values match
|
||||
// the spec floats exactly (e.g. [1] = 0x52173/2^31 = 1.56575398E-04).
|
||||
// Verified on host: feeding subband-center sines concentrates energy in the
|
||||
// right band with >63 dB rejection and overall gain 0.9997.
|
||||
static const int32_t g_proto8[80] = {
|
||||
(int32_t)0x00000000, (int32_t)0x00052173, (int32_t)0x000B3F71, (int32_t)0x00122C7D,
|
||||
(int32_t)0x001AFF89, (int32_t)0x00255A62, (int32_t)0x003060F4, (int32_t)0x003A72E7,
|
||||
(int32_t)0x0041EC6A, (int32_t)0x0044EF48, (int32_t)0x00415B75, (int32_t)0x0034F8B6,
|
||||
(int32_t)0x001D8FD2, (int32_t)0xFFFA2413, (int32_t)0xFFC9F10E, (int32_t)0xFF8D6793,
|
||||
(int32_t)0x00B97348, (int32_t)0x01071B96, (int32_t)0x0156B3CA, (int32_t)0x01A1B38B,
|
||||
(int32_t)0x01E0224C, (int32_t)0x0209291F, (int32_t)0x02138653, (int32_t)0x01F5F424,
|
||||
(int32_t)0x01A7ECEF, (int32_t)0x01223EBA, (int32_t)0x005FD0FF, (int32_t)0xFF5EEB73,
|
||||
(int32_t)0xFE20435D, (int32_t)0xFCA86E7E, (int32_t)0xFAFF95FC, (int32_t)0xF9312891,
|
||||
(int32_t)0x08B4307A, (int32_t)0x0A9F3E9A, (int32_t)0x0C7D59B6, (int32_t)0x0E3BB16F,
|
||||
(int32_t)0x0FC721F9, (int32_t)0x110ECEF0, (int32_t)0x120435FA, (int32_t)0x129C226F,
|
||||
(int32_t)0x12CF6C75, (int32_t)0x129C226F, (int32_t)0x120435FA, (int32_t)0x110ECEF0,
|
||||
(int32_t)0x0FC721F9, (int32_t)0x0E3BB16F, (int32_t)0x0C7D59B6, (int32_t)0x0A9F3E9A,
|
||||
(int32_t)0xF74BCF86, (int32_t)0xF9312891, (int32_t)0xFAFF95FC, (int32_t)0xFCA86E7E,
|
||||
(int32_t)0xFE20435D, (int32_t)0xFF5EEB73, (int32_t)0x005FD0FF, (int32_t)0x01223EBA,
|
||||
(int32_t)0x01A7ECEF, (int32_t)0x01F5F424, (int32_t)0x02138653, (int32_t)0x0209291F,
|
||||
(int32_t)0x01E0224C, (int32_t)0x01A1B38B, (int32_t)0x0156B3CA, (int32_t)0x01071B96,
|
||||
(int32_t)0xFF468CB8, (int32_t)0xFF8D6793, (int32_t)0xFFC9F10E, (int32_t)0xFFFA2413,
|
||||
(int32_t)0x001D8FD2, (int32_t)0x0034F8B6, (int32_t)0x00415B75, (int32_t)0x0044EF48,
|
||||
(int32_t)0x0041EC6A, (int32_t)0x003A72E7, (int32_t)0x003060F4, (int32_t)0x00255A62,
|
||||
(int32_t)0x001AFF89, (int32_t)0x00122C7D, (int32_t)0x000B3F71, (int32_t)0x00052173,
|
||||
};
|
||||
|
||||
// Analysis matrixing for 8 subbands (A2DP spec Annex B), Q14:
|
||||
// M[k][i] = cos((i - 4) * (2k + 1) * pi / 16) * 2^14
|
||||
// (The (i-4) phase is the one whose aliasing cancels in the reference
|
||||
// decoder's synthesis; (i+4) passes an energy-concentration test equally
|
||||
// but reconstructs with frequency-dependent distortion.)
|
||||
static const int16_t g_cosMatrix8[8][16] = {
|
||||
{ 11585, 13623, 15137, 16069, 16384, 16069, 15137, 13623, 11585, 9102, 6270, 3196, 0, -3196, -6270, -9102 },
|
||||
{ -11585, -3196, 6270, 13623, 16384, 13623, 6270, -3196, -11585, -16069, -15137, -9102, 0, 9102, 15137, 16069 },
|
||||
{ -11585, -16069, -6270, 9102, 16384, 9102, -6270, -16069, -11585, 3196, 15137, 13623, 0, -13623, -15137, -3196 },
|
||||
{ 11585, -9102, -15137, 3196, 16384, 3196, -15137, -9102, 11585, 13623, -6270, -16069, 0, 16069, 6270, -13623 },
|
||||
{ 11585, 9102, -15137, -3196, 16384, -3196, -15137, 9102, 11585, -13623, -6270, 16069, 0, -16069, 6270, 13623 },
|
||||
{ -11585, 16069, -6270, -9102, 16384, -9102, -6270, 16069, -11585, -3196, 15137, -13623, 0, 13623, -15137, 3196 },
|
||||
{ -11585, 3196, 6270, -13623, 16384, -13623, 6270, 3196, -11585, 16069, -15137, 9102, 0, -9102, 15137, -16069 },
|
||||
{ 11585, -13623, 15137, -16069, 16384, -16069, 15137, -13623, 11585, -9102, 6270, -3196, 0, 3196, -6270, 9102 },
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// CRC-8 table (SBC spec CRC polynomial: x^8 + x^4 + x^3 + x^2 + 1)
|
||||
// =========================================================================
|
||||
|
||||
static uint8_t SbcCrc8(const uint8_t* data, uint32_t len, uint8_t bits_last_byte) {
|
||||
uint8_t crc = 0x0F;
|
||||
for (uint32_t i = 0; i < len; i++) {
|
||||
uint8_t byte = data[i];
|
||||
uint8_t nbits = (i == len - 1) ? bits_last_byte : 8;
|
||||
for (uint8_t bit = 0; bit < nbits; bit++) {
|
||||
uint8_t msb = (crc >> 7) & 1;
|
||||
crc <<= 1;
|
||||
if (((byte >> (7 - bit)) & 1) ^ msb) {
|
||||
crc ^= 0x1D;
|
||||
}
|
||||
}
|
||||
}
|
||||
return crc;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Init
|
||||
// =========================================================================
|
||||
|
||||
void Init(SbcEncoder* enc, uint32_t sampleRate, uint8_t channels, uint8_t /*bitsPerSample*/) {
|
||||
memset(enc, 0, sizeof(SbcEncoder));
|
||||
|
||||
enc->Subbands = SBC_SUBBANDS;
|
||||
enc->Blocks = SBC_BLOCKS;
|
||||
enc->Bitpool = SBC_BITPOOL;
|
||||
enc->AllocMethod = ALLOC_LOUDNESS;
|
||||
|
||||
if (channels >= 2) {
|
||||
enc->Channels = 2;
|
||||
enc->ChannelMode = MODE_JOINT_STEREO;
|
||||
} else {
|
||||
enc->Channels = 1;
|
||||
enc->ChannelMode = MODE_MONO;
|
||||
}
|
||||
|
||||
switch (sampleRate) {
|
||||
case 16000: enc->Frequency = FREQ_16000; break;
|
||||
case 32000: enc->Frequency = FREQ_32000; break;
|
||||
case 44100: enc->Frequency = FREQ_44100; break;
|
||||
default: enc->Frequency = FREQ_48000; break;
|
||||
}
|
||||
|
||||
enc->SamplesPerFrame = enc->Blocks * enc->Subbands;
|
||||
|
||||
// Calculate frame size
|
||||
// For joint stereo: 4 + (4 * subbands * channels) / 8 + ceil(blocks * bitpool / 8) + subbands/8
|
||||
uint32_t headerBits = 32 + (4 * enc->Subbands * enc->Channels);
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
||||
headerBits += enc->Subbands; // join bits
|
||||
}
|
||||
uint32_t dataBits = enc->Blocks * enc->Bitpool;
|
||||
if (enc->ChannelMode == MODE_DUAL_CHANNEL) dataBits *= enc->Channels;
|
||||
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;
|
||||
if (enc->ChannelMode == MODE_DUAL_CHANNEL) dataBits *= enc->Channels;
|
||||
enc->FrameSize = (headerBits + dataBits + 7) / 8;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Analysis filter bank (8 subbands)
|
||||
// =========================================================================
|
||||
|
||||
// Direct implementation of the spec pseudocode (8 subbands):
|
||||
// shift X by 8; insert the 8 new samples NEWEST-FIRST (X[0] = newest)
|
||||
// Y[i] = sum_{j=0..4} X[i+16j] * C[i+16j] (window, 80 taps)
|
||||
// S[k] = sum_{i=0..15} M[k][i] * Y[i] (cosine matrixing)
|
||||
// Fixed point: X holds 16-bit samples, C is Q31, M is Q14. The window
|
||||
// accumulator keeps Y at sample*2^15 (>>16 off Q31), the matrix adds 14
|
||||
// bits, so >>30 would land S at HALF input-sample units -- the 0.5x
|
||||
// analysis gain the SBC synthesis filterbank expects (its synthesis
|
||||
// doubles; verified against the BlueZ reference decoder: 1.0x decodes
|
||||
// 6 dB hot). We shift FRAC_BITS less to carry fractional precision
|
||||
// through quantization: at bitpool 53 the quantizer step is often finer
|
||||
// than one input-sample unit, so integer subband samples would be the
|
||||
// noise floor (costs ~10 dB on sweeps). Scale factors and the quantizer
|
||||
// account for FRAC_BITS; full-scale input still peaks at sf = 14.
|
||||
// Extra fractional bits carried in sb_samples below the half-input-unit
|
||||
// point (subband sample value = signal * 2^FRAC_BITS).
|
||||
constexpr int FRAC_BITS = 12;
|
||||
|
||||
static void AnalysisFilter(SbcEncoder* enc, const int16_t* pcm, int ch,
|
||||
int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS]) {
|
||||
int32_t* X = enc->X[ch];
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
for (int i = 79; i >= 8; i--) X[i] = X[i - 8];
|
||||
const int16_t* in = pcm + blk * 8 * enc->Channels + ch;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
X[i] = (int32_t)in[(7 - i) * enc->Channels];
|
||||
}
|
||||
|
||||
int32_t Y[16];
|
||||
for (int i = 0; i < 16; i++) {
|
||||
int64_t acc = 0;
|
||||
for (int j = 0; j < 5; j++) {
|
||||
acc += (int64_t)X[i + 16 * j] * g_proto8[i + 16 * j];
|
||||
}
|
||||
Y[i] = (int32_t)((acc + (1 << 15)) >> 16);
|
||||
}
|
||||
|
||||
for (int k = 0; k < 8; k++) {
|
||||
int64_t s = 0;
|
||||
for (int i = 0; i < 16; i++) {
|
||||
s += (int64_t)Y[i] * g_cosMatrix8[k][i];
|
||||
}
|
||||
sb_samples[blk][k] =
|
||||
(int32_t)((s + ((int64_t)1 << (29 - FRAC_BITS))) >> (30 - FRAC_BITS));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Bit allocation (Loudness method)
|
||||
// =========================================================================
|
||||
|
||||
// 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++) {
|
||||
int32_t val = sb_samples[blk][sb];
|
||||
if (val < 0) val = -val;
|
||||
if (val > maxVal) maxVal = val;
|
||||
}
|
||||
int32_t sf = 0, tmp = maxVal >> FRAC_BITS;
|
||||
while (tmp > 0) { sf++; tmp >>= 1; }
|
||||
scale_factors[sb] = sf;
|
||||
}
|
||||
}
|
||||
|
||||
// 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
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// 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;
|
||||
|
||||
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];
|
||||
|
||||
int bitcount = 0, slicecount = 0, bitslice = maxBitneed + 1;
|
||||
do {
|
||||
bitslice--;
|
||||
bitcount += slicecount;
|
||||
slicecount = 0;
|
||||
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 {
|
||||
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];
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Bit packing helpers
|
||||
// =========================================================================
|
||||
|
||||
struct BitWriter {
|
||||
uint8_t* Data;
|
||||
uint32_t BitPos;
|
||||
};
|
||||
|
||||
static void WriteBits(BitWriter* bw, uint32_t value, uint8_t nbits) {
|
||||
for (int i = nbits - 1; i >= 0; i--) {
|
||||
uint32_t bytePos = bw->BitPos / 8;
|
||||
uint8_t bitOff = 7 - (bw->BitPos % 8);
|
||||
if (value & (1u << i)) {
|
||||
bw->Data[bytePos] |= (1u << bitOff);
|
||||
}
|
||||
bw->BitPos++;
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Encode
|
||||
// =========================================================================
|
||||
|
||||
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] = {};
|
||||
|
||||
memset(out, 0, enc->FrameSize);
|
||||
|
||||
// 1. Analysis filterbank per channel.
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
AnalysisFilter(enc, pcm, ch, sb_samples[ch]);
|
||||
}
|
||||
|
||||
// 2. Scale factors from the subband samples.
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
ComputeScaleFactors(enc, sb_samples[ch], scale_factors[ch]);
|
||||
}
|
||||
|
||||
// 3. Joint-stereo decision + mid/side transform. A band is joined
|
||||
// when it saves scale-factor bits: sfL + sfR > sfM + sfS (the
|
||||
// reference encoder's criterion). Updates BOTH samples and scale
|
||||
// factors before bit allocation: the decoder re-derives the bit
|
||||
// widths from the transmitted scale factors, which must reflect
|
||||
// the FINAL (post-transform) samples, or the bitstream desyncs.
|
||||
// The last subband never joins (carries the side channel's escape).
|
||||
uint8_t joint = 0;
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
||||
for (int sb = 0; sb < enc->Subbands - 1; sb++) {
|
||||
int32_t mid[SBC_BLOCKS], side[SBC_BLOCKS];
|
||||
int32_t maxMid = 0, maxSide = 0;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
int32_t l = sb_samples[0][blk][sb], r = sb_samples[1][blk][sb];
|
||||
mid[blk] = (l >> 1) + (r >> 1);
|
||||
side[blk] = (l >> 1) - (r >> 1);
|
||||
int32_t am = mid[blk] < 0 ? -mid[blk] : mid[blk];
|
||||
int32_t as = side[blk] < 0 ? -side[blk] : side[blk];
|
||||
if (am > maxMid) maxMid = am;
|
||||
if (as > maxSide) maxSide = as;
|
||||
}
|
||||
int32_t sfm = 0, tmp = maxMid >> FRAC_BITS;
|
||||
while (tmp > 0) { sfm++; tmp >>= 1; }
|
||||
int32_t sfs = 0;
|
||||
tmp = maxSide >> FRAC_BITS;
|
||||
while (tmp > 0) { sfs++; tmp >>= 1; }
|
||||
|
||||
if (scale_factors[0][sb] + scale_factors[1][sb] > sfm + sfs) {
|
||||
joint |= (uint8_t)(1 << (enc->Subbands - 1 - sb));
|
||||
scale_factors[0][sb] = sfm;
|
||||
scale_factors[1][sb] = sfs;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
sb_samples[0][blk][sb] = mid[blk];
|
||||
sb_samples[1][blk][sb] = side[blk];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// 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.
|
||||
|
||||
BitWriter bw = {out, 32}; // packing starts at byte 4 (after 4-byte header)
|
||||
|
||||
// 5b. Join flags (joint stereo only).
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
||||
WriteBits(&bw, joint, enc->Subbands);
|
||||
}
|
||||
|
||||
// 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 = (uint32_t)scale_factors[ch][sb];
|
||||
if (sf > 15) sf = 15;
|
||||
WriteBits(&bw, sf, 4);
|
||||
}
|
||||
}
|
||||
|
||||
// 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);
|
||||
|
||||
// 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];
|
||||
uint32_t levels = (1u << bits[ch][sb]) - 1;
|
||||
// q = levels * (sample + range) / (2 * range), truncating,
|
||||
// with range = 2^(sf + FRAC_BITS) -- same mapping as the
|
||||
// reference encoder. The fractional sample bits feed
|
||||
// straight into the quantizer's precision.
|
||||
int32_t range = 1 << (sf + FRAC_BITS);
|
||||
int32_t quantized = (int32_t)
|
||||
(((int64_t)(sample + range) * levels) >> (sf + FRAC_BITS + 1));
|
||||
if (quantized < 0) quantized = 0;
|
||||
if (quantized > (int32_t)levels) quantized = (int32_t)levels;
|
||||
WriteBits(&bw, (uint32_t)quantized, bits[ch][sb]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// SBC frames are fixed-size for a given configuration.
|
||||
return enc->FrameSize;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Queries
|
||||
// =========================================================================
|
||||
|
||||
uint32_t GetFrameSize(const SbcEncoder* enc) {
|
||||
return enc->FrameSize;
|
||||
}
|
||||
|
||||
uint32_t GetSamplesPerFrame(const SbcEncoder* enc) {
|
||||
return enc->SamplesPerFrame;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -1,97 +0,0 @@
|
||||
/*
|
||||
* Sbc.hpp
|
||||
* SBC (Sub-Band Codec) encoder for Bluetooth A2DP
|
||||
* Fixed-point implementation (no FPU/SSE required)
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
|
||||
namespace Drivers::USB::Bluetooth::Sbc {
|
||||
|
||||
// =========================================================================
|
||||
// SBC configuration
|
||||
// =========================================================================
|
||||
|
||||
// Standard SBC parameters for A2DP
|
||||
constexpr int SBC_SUBBANDS = 8;
|
||||
constexpr int SBC_BLOCKS = 16;
|
||||
constexpr int SBC_CHANNELS = 2; // Stereo
|
||||
constexpr int SBC_BITPOOL = 53; // Standard quality
|
||||
|
||||
// Allocation method
|
||||
constexpr uint8_t ALLOC_SNR = 0;
|
||||
constexpr uint8_t ALLOC_LOUDNESS = 1;
|
||||
|
||||
// Channel mode
|
||||
constexpr uint8_t MODE_MONO = 0;
|
||||
constexpr uint8_t MODE_DUAL_CHANNEL = 1;
|
||||
constexpr uint8_t MODE_STEREO = 2;
|
||||
constexpr uint8_t MODE_JOINT_STEREO = 3;
|
||||
|
||||
// Sampling frequency
|
||||
constexpr uint8_t FREQ_16000 = 0;
|
||||
constexpr uint8_t FREQ_32000 = 1;
|
||||
constexpr uint8_t FREQ_44100 = 2;
|
||||
constexpr uint8_t FREQ_48000 = 3;
|
||||
|
||||
// SBC frame header
|
||||
struct SbcHeader {
|
||||
uint8_t SyncWord; // 0x9C
|
||||
uint8_t Config; // freq(2) | blocks(2) | mode(2) | alloc(1) | subbands(1)
|
||||
uint8_t Bitpool;
|
||||
uint8_t Crc;
|
||||
} __attribute__((packed));
|
||||
|
||||
// =========================================================================
|
||||
// Encoder state
|
||||
// =========================================================================
|
||||
|
||||
struct SbcEncoder {
|
||||
uint8_t Frequency;
|
||||
uint8_t Blocks;
|
||||
uint8_t ChannelMode;
|
||||
uint8_t AllocMethod;
|
||||
uint8_t Subbands;
|
||||
uint8_t Bitpool;
|
||||
uint8_t Channels;
|
||||
|
||||
// Analysis filter state (per-channel windowed buffer; X[0] = newest
|
||||
// sample, shifted down by Subbands each block)
|
||||
int32_t X[SBC_CHANNELS][SBC_SUBBANDS * 10];
|
||||
|
||||
// Computed frame size in bytes
|
||||
uint32_t FrameSize;
|
||||
|
||||
// Samples per frame
|
||||
uint32_t SamplesPerFrame; // blocks * subbands
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// Public API
|
||||
// =========================================================================
|
||||
|
||||
// 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
|
||||
// Returns number of bytes written to out
|
||||
uint32_t Encode(SbcEncoder* enc, const int16_t* pcm, uint8_t* out);
|
||||
|
||||
// Get the frame size in bytes for the current configuration
|
||||
uint32_t GetFrameSize(const SbcEncoder* enc);
|
||||
|
||||
// Get the number of PCM samples consumed per frame (per channel)
|
||||
uint32_t GetSamplesPerFrame(const SbcEncoder* enc);
|
||||
|
||||
}
|
||||
@@ -11,7 +11,7 @@
|
||||
#include "HidMouse.hpp"
|
||||
#include "MassStorage.hpp"
|
||||
#include "Hub.hpp"
|
||||
#include "Bluetooth/Bluetooth.hpp"
|
||||
#include <Drivers/Bluetooth/Bluetooth.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Memory/HHDM.hpp>
|
||||
|
||||
Reference in new issue
Block a user