feat: Intel HDA audio driver, audio streaming syscalls, userspace Music app, fixes and improvements, rudimentary Bluetooth support
This commit is contained in:
@@ -0,0 +1,500 @@
|
||||
/*
|
||||
* A2dp.cpp
|
||||
* Bluetooth A2DP / AVDTP implementation
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "A2dp.hpp"
|
||||
#include "Sbc.hpp"
|
||||
#include "L2cap.hpp"
|
||||
#include "Hci.hpp"
|
||||
#include <Drivers/USB/Xhci.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
#include <Timekeeping/ApicTimer.hpp>
|
||||
|
||||
using namespace Kt;
|
||||
|
||||
namespace Drivers::USB::Bluetooth::A2dp {
|
||||
|
||||
// =========================================================================
|
||||
// AVDTP constants
|
||||
// =========================================================================
|
||||
|
||||
// AVDTP signal IDs
|
||||
constexpr uint8_t AVDTP_DISCOVER = 0x01;
|
||||
constexpr uint8_t AVDTP_GET_CAPABILITIES = 0x02;
|
||||
constexpr uint8_t AVDTP_SET_CONFIGURATION = 0x03;
|
||||
constexpr uint8_t AVDTP_GET_CONFIGURATION = 0x04;
|
||||
constexpr uint8_t AVDTP_RECONFIGURE = 0x05;
|
||||
constexpr uint8_t AVDTP_OPEN = 0x06;
|
||||
constexpr uint8_t AVDTP_START = 0x07;
|
||||
constexpr uint8_t AVDTP_CLOSE = 0x08;
|
||||
constexpr uint8_t AVDTP_SUSPEND = 0x09;
|
||||
constexpr uint8_t AVDTP_ABORT = 0x0A;
|
||||
|
||||
// AVDTP message types
|
||||
constexpr uint8_t MSG_COMMAND = 0x00;
|
||||
constexpr uint8_t MSG_GENERAL_REJECT = 0x01;
|
||||
constexpr uint8_t MSG_RESPONSE_ACCEPT = 0x02;
|
||||
constexpr uint8_t MSG_RESPONSE_REJECT = 0x03;
|
||||
|
||||
// AVDTP packet types
|
||||
constexpr uint8_t PKT_SINGLE = 0x00;
|
||||
|
||||
// Service category IDs
|
||||
constexpr uint8_t CAT_MEDIA_TRANSPORT = 0x01;
|
||||
constexpr uint8_t CAT_MEDIA_CODEC = 0x07;
|
||||
|
||||
// Media type
|
||||
constexpr uint8_t MEDIA_AUDIO = 0x00;
|
||||
|
||||
// Codec type
|
||||
constexpr uint8_t CODEC_SBC = 0x00;
|
||||
|
||||
// SBC capability octets
|
||||
// Octet 0: Sampling Frequency (bits 7-4) | Channel Mode (bits 3-0)
|
||||
// Octet 1: Block Length (bits 7-4) | Subbands (bits 3-2) | Alloc Method (bits 1-0)
|
||||
// Octet 2: Min Bitpool
|
||||
// Octet 3: Max Bitpool
|
||||
|
||||
// =========================================================================
|
||||
// State
|
||||
// =========================================================================
|
||||
|
||||
static State g_state = State::Idle;
|
||||
static uint16_t g_sigCid = 0; // L2CAP CID for AVDTP signaling
|
||||
static uint16_t g_mediaCid = 0; // L2CAP CID for AVDTP media transport
|
||||
static uint8_t g_txLabel = 1;
|
||||
static uint8_t g_remoteSeid = 0; // Remote stream endpoint ID
|
||||
static uint8_t g_localSeid = 1; // Our local SEID
|
||||
|
||||
// SBC encoder
|
||||
static Sbc::SbcEncoder g_sbcEncoder = {};
|
||||
static bool g_sbcInitialized = false;
|
||||
|
||||
// Media packet state
|
||||
static uint16_t g_seqNum = 0;
|
||||
static uint32_t g_timestamp = 0;
|
||||
|
||||
// Volume
|
||||
static int g_volume = 80;
|
||||
|
||||
// AVDTP response tracking
|
||||
static volatile bool g_avdtpResponseReady = false;
|
||||
static uint8_t g_avdtpResponseBuf[128] = {};
|
||||
static uint32_t g_avdtpResponseLen = 0;
|
||||
|
||||
// =========================================================================
|
||||
// AVDTP signaling helpers
|
||||
// =========================================================================
|
||||
|
||||
static void SendAvdtpCommand(uint8_t signalId, const uint8_t* payload, uint16_t len) {
|
||||
uint8_t buf[128] = {};
|
||||
|
||||
// AVDTP single packet header
|
||||
buf[0] = (g_txLabel << 4) | (PKT_SINGLE << 2) | MSG_COMMAND;
|
||||
buf[1] = signalId;
|
||||
g_txLabel = (g_txLabel + 1) & 0x0F;
|
||||
|
||||
if (payload && len > 0) {
|
||||
memcpy(&buf[2], payload, len);
|
||||
}
|
||||
|
||||
L2cap::SendData(g_sigCid, buf, 2 + len);
|
||||
}
|
||||
|
||||
static void SendAvdtpResponse(uint8_t txLabel, uint8_t signalId,
|
||||
const uint8_t* payload, uint16_t len) {
|
||||
uint8_t buf[128] = {};
|
||||
|
||||
buf[0] = (txLabel << 4) | (PKT_SINGLE << 2) | MSG_RESPONSE_ACCEPT;
|
||||
buf[1] = signalId;
|
||||
|
||||
if (payload && len > 0) {
|
||||
memcpy(&buf[2], payload, len);
|
||||
}
|
||||
|
||||
L2cap::SendData(g_sigCid, buf, 2 + len);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// WaitAvdtpResponse
|
||||
// =========================================================================
|
||||
|
||||
static bool WaitAvdtpResponse(uint32_t timeoutMs = 3000) {
|
||||
g_avdtpResponseReady = false;
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
if (g_avdtpResponseReady) return true;
|
||||
for (int j = 0; j < 100; j++) {
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// AVDTP signaling procedures
|
||||
// =========================================================================
|
||||
|
||||
static bool AvdtpDiscover() {
|
||||
SendAvdtpCommand(AVDTP_DISCOVER, nullptr, 0);
|
||||
|
||||
if (!WaitAvdtpResponse()) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Discover timeout";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Parse discover response to find audio sink SEID
|
||||
// Response format: each SEP is 2 bytes:
|
||||
// Byte 0: SEID(6) | InUse(1) | Rsvd(1)
|
||||
// Byte 1: MediaType(4) | SEPType(4) (SEPType: 0=Source, 1=Sink)
|
||||
if (g_avdtpResponseLen >= 4) {
|
||||
for (uint32_t i = 2; i + 1 < g_avdtpResponseLen; i += 2) {
|
||||
uint8_t seid = (g_avdtpResponseBuf[i] >> 2) & 0x3F;
|
||||
bool inUse = (g_avdtpResponseBuf[i] >> 1) & 1;
|
||||
uint8_t mediaType = (g_avdtpResponseBuf[i + 1] >> 4) & 0x0F;
|
||||
uint8_t sepType = g_avdtpResponseBuf[i + 1] & 0x0F;
|
||||
|
||||
if (mediaType == MEDIA_AUDIO && sepType == 0x01 && !inUse) {
|
||||
g_remoteSeid = seid;
|
||||
KernelLogStream(INFO, "BT-A2DP") << "Found audio sink SEID="
|
||||
<< (uint64_t)seid;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "No audio sink SEP found";
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool AvdtpGetCapabilities() {
|
||||
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
||||
SendAvdtpCommand(AVDTP_GET_CAPABILITIES, payload, 1);
|
||||
return WaitAvdtpResponse();
|
||||
}
|
||||
|
||||
static bool AvdtpSetConfiguration() {
|
||||
// Set Configuration payload:
|
||||
// ACP SEID (1 byte) | INT SEID (1 byte) | Service Capabilities...
|
||||
uint8_t payload[12] = {};
|
||||
payload[0] = (g_remoteSeid << 2); // ACP SEID
|
||||
payload[1] = (g_localSeid << 2); // INT SEID
|
||||
|
||||
// Media Transport capability (no data)
|
||||
payload[2] = CAT_MEDIA_TRANSPORT; // Category
|
||||
payload[3] = 0; // Length
|
||||
|
||||
// Media Codec capability (SBC)
|
||||
payload[4] = CAT_MEDIA_CODEC; // Category
|
||||
payload[5] = 6; // Length
|
||||
payload[6] = (MEDIA_AUDIO << 4); // Media Type
|
||||
payload[7] = CODEC_SBC; // Codec Type
|
||||
// SBC codec info (4 bytes)
|
||||
// Sampling: 44.1kHz (bit 5), Channel mode: Joint Stereo (bit 0)
|
||||
payload[8] = 0x21; // 44.1kHz | Joint Stereo
|
||||
// Block length: 16 (bit 7), Subbands: 8 (bit 1), Alloc: Loudness (bit 0)
|
||||
payload[9] = 0x83; // 16 blocks | 8 subbands | Loudness
|
||||
payload[10] = 2; // Min bitpool
|
||||
payload[11] = 53; // Max bitpool
|
||||
|
||||
SendAvdtpCommand(AVDTP_SET_CONFIGURATION, payload, 12);
|
||||
|
||||
if (!WaitAvdtpResponse()) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP SetConfiguration timeout";
|
||||
return false;
|
||||
}
|
||||
|
||||
g_state = State::Configured;
|
||||
KernelLogStream(OK, "BT-A2DP") << "Stream configured";
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool AvdtpOpen() {
|
||||
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
||||
SendAvdtpCommand(AVDTP_OPEN, payload, 1);
|
||||
|
||||
if (!WaitAvdtpResponse()) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Open timeout";
|
||||
return false;
|
||||
}
|
||||
|
||||
g_state = State::Open;
|
||||
KernelLogStream(OK, "BT-A2DP") << "Stream opened";
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool AvdtpStart() {
|
||||
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
||||
SendAvdtpCommand(AVDTP_START, payload, 1);
|
||||
|
||||
if (!WaitAvdtpResponse()) {
|
||||
KernelLogStream(WARNING, "BT-A2DP") << "AVDTP Start timeout";
|
||||
return false;
|
||||
}
|
||||
|
||||
g_state = State::Streaming;
|
||||
KernelLogStream(OK, "BT-A2DP") << "Streaming started";
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// OnChannelReady — called by L2CAP when an AVDTP channel is configured
|
||||
// =========================================================================
|
||||
|
||||
void OnChannelReady(uint16_t l2capCid) {
|
||||
if (g_sigCid == 0) {
|
||||
// First AVDTP channel is signaling
|
||||
g_sigCid = l2capCid;
|
||||
KernelLogStream(OK, "BT-A2DP") << "AVDTP signaling channel ready: CID="
|
||||
<< (uint64_t)l2capCid;
|
||||
|
||||
// Auto-discover remote SEPs
|
||||
g_state = State::Discovering;
|
||||
if (AvdtpDiscover()) {
|
||||
AvdtpGetCapabilities();
|
||||
AvdtpSetConfiguration();
|
||||
}
|
||||
} else if (g_mediaCid == 0) {
|
||||
// Second AVDTP channel is media transport
|
||||
g_mediaCid = l2capCid;
|
||||
KernelLogStream(OK, "BT-A2DP") << "AVDTP media channel ready: CID="
|
||||
<< (uint64_t)l2capCid;
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ProcessAvdtp — handle AVDTP signaling packets
|
||||
// =========================================================================
|
||||
|
||||
void ProcessAvdtp(const uint8_t* data, uint16_t len) {
|
||||
if (len < 2) return;
|
||||
|
||||
uint8_t txLabel = (data[0] >> 4) & 0x0F;
|
||||
uint8_t pktType = (data[0] >> 2) & 0x03;
|
||||
uint8_t msgType = data[0] & 0x03;
|
||||
uint8_t signalId = data[1] & 0x3F;
|
||||
|
||||
if (msgType == MSG_RESPONSE_ACCEPT || msgType == MSG_RESPONSE_REJECT) {
|
||||
// This is a response to our command
|
||||
memcpy(g_avdtpResponseBuf, data, len > sizeof(g_avdtpResponseBuf) ? sizeof(g_avdtpResponseBuf) : len);
|
||||
g_avdtpResponseLen = len;
|
||||
g_avdtpResponseReady = true;
|
||||
return;
|
||||
}
|
||||
|
||||
// Handle incoming commands
|
||||
if (msgType == MSG_COMMAND) {
|
||||
switch (signalId) {
|
||||
case AVDTP_DISCOVER: {
|
||||
// Respond with our local SEP (audio source)
|
||||
uint8_t rsp[2] = {};
|
||||
rsp[0] = (g_localSeid << 2); // SEID, not in use
|
||||
rsp[1] = (MEDIA_AUDIO << 4) | 0x00; // Audio, Source
|
||||
SendAvdtpResponse(txLabel, AVDTP_DISCOVER, rsp, 2);
|
||||
break;
|
||||
}
|
||||
|
||||
case AVDTP_GET_CAPABILITIES: {
|
||||
// Respond with our SBC capabilities
|
||||
uint8_t rsp[10] = {};
|
||||
rsp[0] = CAT_MEDIA_TRANSPORT;
|
||||
rsp[1] = 0;
|
||||
rsp[2] = CAT_MEDIA_CODEC;
|
||||
rsp[3] = 6;
|
||||
rsp[4] = (MEDIA_AUDIO << 4);
|
||||
rsp[5] = CODEC_SBC;
|
||||
rsp[6] = 0x21; // 44.1kHz | Joint Stereo
|
||||
rsp[7] = 0x83; // 16 blocks | 8 subbands | Loudness
|
||||
rsp[8] = 2; // Min bitpool
|
||||
rsp[9] = 53; // Max bitpool
|
||||
SendAvdtpResponse(txLabel, AVDTP_GET_CAPABILITIES, rsp, 10);
|
||||
break;
|
||||
}
|
||||
|
||||
case AVDTP_SET_CONFIGURATION: {
|
||||
// Accept configuration from remote
|
||||
if (len >= 4) {
|
||||
g_remoteSeid = (data[2] >> 2) & 0x3F;
|
||||
g_state = State::Configured;
|
||||
SendAvdtpResponse(txLabel, AVDTP_SET_CONFIGURATION, nullptr, 0);
|
||||
KernelLogStream(OK, "BT-A2DP") << "Remote configured stream, SEID="
|
||||
<< (uint64_t)g_remoteSeid;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case AVDTP_OPEN: {
|
||||
g_state = State::Open;
|
||||
SendAvdtpResponse(txLabel, AVDTP_OPEN, nullptr, 0);
|
||||
KernelLogStream(OK, "BT-A2DP") << "Remote opened stream";
|
||||
|
||||
// The media transport channel will be set up via L2CAP after this
|
||||
break;
|
||||
}
|
||||
|
||||
case AVDTP_START: {
|
||||
g_state = State::Streaming;
|
||||
SendAvdtpResponse(txLabel, AVDTP_START, nullptr, 0);
|
||||
KernelLogStream(OK, "BT-A2DP") << "Remote started streaming";
|
||||
break;
|
||||
}
|
||||
|
||||
case AVDTP_CLOSE: {
|
||||
g_state = State::Idle;
|
||||
SendAvdtpResponse(txLabel, AVDTP_CLOSE, nullptr, 0);
|
||||
break;
|
||||
}
|
||||
|
||||
case AVDTP_SUSPEND: {
|
||||
g_state = State::Open;
|
||||
SendAvdtpResponse(txLabel, AVDTP_SUSPEND, nullptr, 0);
|
||||
break;
|
||||
}
|
||||
|
||||
case AVDTP_ABORT: {
|
||||
g_state = State::Idle;
|
||||
SendAvdtpResponse(txLabel, AVDTP_ABORT, nullptr, 0);
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ConfigureStream
|
||||
// =========================================================================
|
||||
|
||||
bool ConfigureStream(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample) {
|
||||
Sbc::Init(&g_sbcEncoder, sampleRate, channels, bitsPerSample);
|
||||
g_sbcInitialized = true;
|
||||
g_seqNum = 0;
|
||||
g_timestamp = 0;
|
||||
|
||||
KernelLogStream(OK, "BT-A2DP") << "SBC encoder initialized: "
|
||||
<< (uint64_t)sampleRate << "Hz " << (uint64_t)bitsPerSample << "-bit "
|
||||
<< (uint64_t)channels << "ch";
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// StartStream / StopStream
|
||||
// =========================================================================
|
||||
|
||||
bool StartStream() {
|
||||
if (g_state == State::Open || g_state == State::Configured) {
|
||||
if (g_state == State::Configured) {
|
||||
if (!AvdtpOpen()) return false;
|
||||
}
|
||||
return AvdtpStart();
|
||||
}
|
||||
return (g_state == State::Streaming);
|
||||
}
|
||||
|
||||
bool StopStream() {
|
||||
if (g_state == State::Streaming) {
|
||||
uint8_t payload[1] = {(uint8_t)(g_remoteSeid << 2)};
|
||||
SendAvdtpCommand(AVDTP_SUSPEND, payload, 1);
|
||||
WaitAvdtpResponse(1000);
|
||||
g_state = State::Open;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// WriteAudio — encode PCM to SBC and stream over Bluetooth
|
||||
// =========================================================================
|
||||
|
||||
int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen) {
|
||||
if (!g_sbcInitialized || g_state != State::Streaming || g_mediaCid == 0) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
uint32_t samplesPerFrame = Sbc::GetSamplesPerFrame(&g_sbcEncoder);
|
||||
uint32_t bytesPerFrame = samplesPerFrame * g_sbcEncoder.Channels * 2; // 16-bit samples
|
||||
uint32_t sbcFrameSize = Sbc::GetFrameSize(&g_sbcEncoder);
|
||||
|
||||
// Apply volume scaling to PCM data
|
||||
// We work on a local copy for volume adjustment
|
||||
int16_t scaledPcm[512]; // Max ~128 samples * 2 channels = 256 samples
|
||||
if (bytesPerFrame > sizeof(scaledPcm)) return -1;
|
||||
|
||||
uint32_t consumed = 0;
|
||||
|
||||
while (consumed + bytesPerFrame <= pcmLen) {
|
||||
// Copy and scale by volume
|
||||
const int16_t* src = (const int16_t*)(pcmData + consumed);
|
||||
uint32_t numSamples = samplesPerFrame * g_sbcEncoder.Channels;
|
||||
for (uint32_t i = 0; i < numSamples; i++) {
|
||||
scaledPcm[i] = (int16_t)(((int32_t)src[i] * g_volume) / 100);
|
||||
}
|
||||
|
||||
// Build media packet: RTP-like header (12 bytes) + SBC payload header (1 byte) + SBC frames
|
||||
uint8_t mediaPkt[256] = {};
|
||||
|
||||
// Simplified media packet header (AVDTP media packet)
|
||||
// Byte 0: V=2, P=0, X=0, CC=0 -> 0x80
|
||||
// Byte 1: M=0, PT=96 -> 0x60
|
||||
// Bytes 2-3: Sequence number
|
||||
// Bytes 4-7: Timestamp
|
||||
// Bytes 8-11: SSRC
|
||||
// Byte 12: SBC payload header (number of SBC frames)
|
||||
mediaPkt[0] = 0x80;
|
||||
mediaPkt[1] = 0x60;
|
||||
mediaPkt[2] = (uint8_t)(g_seqNum >> 8);
|
||||
mediaPkt[3] = (uint8_t)(g_seqNum & 0xFF);
|
||||
mediaPkt[4] = (uint8_t)(g_timestamp >> 24);
|
||||
mediaPkt[5] = (uint8_t)(g_timestamp >> 16);
|
||||
mediaPkt[6] = (uint8_t)(g_timestamp >> 8);
|
||||
mediaPkt[7] = (uint8_t)(g_timestamp & 0xFF);
|
||||
mediaPkt[8] = 0; mediaPkt[9] = 0; mediaPkt[10] = 0; mediaPkt[11] = 0x01; // SSRC
|
||||
mediaPkt[12] = 1; // Number of SBC frames in this packet
|
||||
|
||||
// Encode SBC frame
|
||||
uint32_t encodedSize = Sbc::Encode(&g_sbcEncoder, scaledPcm, &mediaPkt[13]);
|
||||
|
||||
uint32_t totalLen = 13 + encodedSize;
|
||||
|
||||
// Send via L2CAP on media channel
|
||||
L2cap::SendData(g_mediaCid, mediaPkt, (uint16_t)totalLen);
|
||||
|
||||
g_seqNum++;
|
||||
g_timestamp += samplesPerFrame;
|
||||
consumed += bytesPerFrame;
|
||||
}
|
||||
|
||||
return (int)consumed;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// State queries
|
||||
// =========================================================================
|
||||
|
||||
State GetState() {
|
||||
return g_state;
|
||||
}
|
||||
|
||||
bool IsStreaming() {
|
||||
return (g_state == State::Streaming);
|
||||
}
|
||||
|
||||
int GetVolume() {
|
||||
return g_volume;
|
||||
}
|
||||
|
||||
void SetVolume(int percent) {
|
||||
if (percent < 0) percent = 0;
|
||||
if (percent > 100) percent = 100;
|
||||
g_volume = percent;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* 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
|
||||
// =========================================================================
|
||||
|
||||
// Called by L2CAP when an AVDTP channel becomes ready
|
||||
void OnChannelReady(uint16_t l2capCid);
|
||||
|
||||
// Process an AVDTP signaling packet
|
||||
void ProcessAvdtp(const uint8_t* data, uint16_t len);
|
||||
|
||||
// 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
|
||||
bool StopStream();
|
||||
|
||||
// Write PCM audio data to the Bluetooth audio stream
|
||||
// Returns number of bytes consumed
|
||||
int WriteAudio(const uint8_t* pcmData, uint32_t pcmLen);
|
||||
|
||||
// Get current state
|
||||
State GetState();
|
||||
|
||||
// Check if currently streaming
|
||||
bool IsStreaming();
|
||||
|
||||
// Get volume (0-100)
|
||||
int GetVolume();
|
||||
|
||||
// Set volume (0-100)
|
||||
void SetVolume(int percent);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,329 @@
|
||||
/*
|
||||
* Bluetooth.cpp
|
||||
* Top-level Bluetooth subsystem — adapter registration and Intel BT initialization
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "Bluetooth.hpp"
|
||||
#include "Hci.hpp"
|
||||
#include "A2dp.hpp"
|
||||
#include <Drivers/USB/Xhci.hpp>
|
||||
#include <Drivers/USB/UsbDevice.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
#include <Timekeeping/ApicTimer.hpp>
|
||||
|
||||
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] = {};
|
||||
|
||||
// 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(uint8_t slotId) {
|
||||
KernelLogStream(INFO, "BT") << "Intel Bluetooth adapter detected";
|
||||
|
||||
// Intel BT controllers require HCI Reset before they respond to
|
||||
// vendor-specific commands. This mirrors the Linux btintel driver
|
||||
// sequence: Reset → Read Version → (firmware load) → Reset.
|
||||
if (!Hci::Reset()) {
|
||||
KernelLogStream(ERROR, "BT") << "Initial HCI Reset failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Read standard HCI version -- if this fails, the controller is likely
|
||||
// in bootloader mode where only vendor commands are accepted.
|
||||
Hci::LocalVersion lver = {};
|
||||
bool hciVersionOk = Hci::ReadLocalVersion(&lver);
|
||||
if (hciVersionOk) {
|
||||
KernelLogStream(INFO, "BT") << "HCI version=" << (uint64_t)lver.HciVersion
|
||||
<< " rev=" << base::hex << (uint64_t)lver.HciRevision
|
||||
<< " LMP=" << (uint64_t)lver.LmpVersion
|
||||
<< " manufacturer=" << (uint64_t)lver.Manufacturer
|
||||
<< " subver=" << (uint64_t)lver.LmpSubversion << base::dec;
|
||||
}
|
||||
|
||||
// Read Intel version to check firmware state
|
||||
Hci::IntelVersion ver = {};
|
||||
if (!Hci::ReadIntelVersion(&ver)) {
|
||||
KernelLogStream(WARNING, "BT") << "Failed to read Intel BT version";
|
||||
} else {
|
||||
KernelLogStream(INFO, "BT") << "Intel BT: HW variant=" << (uint64_t)ver.HwVariant
|
||||
<< " FW variant=" << base::hex << (uint64_t)ver.FwVariant
|
||||
<< " FW rev=" << (uint64_t)ver.FwRevision << "."
|
||||
<< (uint64_t)ver.FwBuildNum << base::dec;
|
||||
|
||||
if (ver.FwVariant == 0x23) {
|
||||
KernelLogStream(OK, "BT") << "Intel BT firmware already loaded (operational mode)";
|
||||
} else if (ver.FwVariant == 0x06) {
|
||||
KernelLogStream(WARNING, "BT") << "Intel BT in bootloader mode, firmware not loaded";
|
||||
KernelLogStream(WARNING, "BT") << "Bluetooth will have limited functionality without firmware";
|
||||
} else if (!hciVersionOk) {
|
||||
// Standard HCI commands failed AND Intel version is zeros/unknown
|
||||
// -> controller is in bootloader mode, needs firmware download
|
||||
KernelLogStream(WARNING, "BT") << "Intel BT in bootloader mode (FW not loaded by UEFI)";
|
||||
KernelLogStream(WARNING, "BT") << "Bluetooth requires firmware download for full functionality";
|
||||
} else {
|
||||
KernelLogStream(INFO, "BT") << "Intel BT firmware variant: "
|
||||
<< base::hex << (uint64_t)ver.FwVariant;
|
||||
}
|
||||
}
|
||||
|
||||
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();
|
||||
asm volatile("pause" ::: "memory");
|
||||
}
|
||||
|
||||
// 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.
|
||||
Hci::StartEventPipe();
|
||||
|
||||
// Intel-specific initialization (includes HCI Reset)
|
||||
bool didReset = false;
|
||||
if (IsIntelBt(dev->VendorId, dev->ProductId)) {
|
||||
if (InitIntelBluetooth(slotId)) {
|
||||
didReset = true; // InitIntelBluetooth already sent HCI Reset
|
||||
} else {
|
||||
KernelLogStream(WARNING, "BT") << "Intel BT init failed, continuing with basic HCI";
|
||||
}
|
||||
}
|
||||
|
||||
// Standard HCI Reset (skip if Intel init already did one)
|
||||
if (!didReset) {
|
||||
if (!Hci::Reset()) {
|
||||
KernelLogStream(ERROR, "BT") << "HCI Reset failed";
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
|
||||
// Set local name
|
||||
Hci::WriteLocalName("MontaukOS");
|
||||
|
||||
// Set class of device: Audio (Major Service: Audio, Major Class: Audio/Video)
|
||||
// CoD: 0x240404 = Rendering | Audio | Wearable Headset (common for audio devices)
|
||||
// For a computer acting as audio source:
|
||||
// 0x200408 = Audio service | Audio/Video class | Portable Audio
|
||||
Hci::WriteClassOfDevice(0x200408);
|
||||
|
||||
// Enable Simple Secure Pairing
|
||||
Hci::WriteSSPMode(1);
|
||||
|
||||
// Set event mask to receive relevant events
|
||||
uint8_t eventMask[8] = {0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x1F, 0x00, 0x20};
|
||||
Hci::SendCommand(Hci::OP_SET_EVENT_MASK, eventMask, 8);
|
||||
Hci::WaitCommandComplete(Hci::OP_SET_EVENT_MASK);
|
||||
|
||||
// Enable inquiry + page scan (discoverable and connectable)
|
||||
Hci::WriteScanEnable(0x03);
|
||||
|
||||
g_initialized = true;
|
||||
KernelLogStream(OK, "BT") << "Bluetooth adapter initialized successfully";
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Public queries
|
||||
// =========================================================================
|
||||
|
||||
bool IsInitialized() {
|
||||
return g_initialized;
|
||||
}
|
||||
|
||||
uint8_t GetSlotId() {
|
||||
return g_slotId;
|
||||
}
|
||||
|
||||
const uint8_t* GetBdAddr() {
|
||||
return g_bdAddr;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// 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)
|
||||
uint8_t duration = (uint8_t)(timeoutMs / 1280);
|
||||
if (duration < 1) duration = 1;
|
||||
if (duration > 30) duration = 30;
|
||||
|
||||
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++) {
|
||||
asm volatile("pause" ::: "memory");
|
||||
}
|
||||
}
|
||||
|
||||
// Cancel if still running
|
||||
if (Hci::IsInquiryActive()) {
|
||||
Hci::CancelInquiry();
|
||||
}
|
||||
|
||||
return Hci::GetInquiryResults(buf, maxCount);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Connect — initiate ACL connection
|
||||
// =========================================================================
|
||||
|
||||
int Connect(const uint8_t* bdAddr, uint32_t timeoutMs) {
|
||||
if (!g_initialized || !bdAddr) return -1;
|
||||
|
||||
if (!Hci::CreateConnection(bdAddr)) return -1;
|
||||
|
||||
// Wait for Connection Complete event
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
Hci::DrainEvents();
|
||||
|
||||
// Check connection table for matching BD_ADDR
|
||||
for (int i = 0; i < Hci::MAX_CONNECTIONS; i++) {
|
||||
auto* conn = Hci::GetConnectionByIndex(i);
|
||||
if (conn && conn->Active) {
|
||||
bool match = true;
|
||||
for (int j = 0; j < 6; j++) {
|
||||
if (conn->BdAddr[j] != bdAddr[j]) { match = false; break; }
|
||||
}
|
||||
if (match) return 0;
|
||||
}
|
||||
}
|
||||
|
||||
for (int j = 0; j < 200; j++) {
|
||||
asm volatile("pause" ::: "memory");
|
||||
}
|
||||
}
|
||||
|
||||
return -1; // Timeout
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// 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) {
|
||||
bool match = true;
|
||||
for (int j = 0; j < 6; j++) {
|
||||
if (conn->BdAddr[j] != bdAddr[j]) { match = false; break; }
|
||||
}
|
||||
if (match) {
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,39 @@
|
||||
/*
|
||||
* 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);
|
||||
|
||||
// Query adapter state
|
||||
bool IsInitialized();
|
||||
uint8_t GetSlotId();
|
||||
|
||||
// Get Bluetooth device address (6 bytes)
|
||||
const uint8_t* GetBdAddr();
|
||||
|
||||
// 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 on success (connection established), -1 on failure
|
||||
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);
|
||||
|
||||
}
|
||||
@@ -0,0 +1,818 @@
|
||||
/*
|
||||
* Hci.cpp
|
||||
* Bluetooth HCI transport over USB
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "Hci.hpp"
|
||||
#include "L2cap.hpp"
|
||||
#include <Drivers/USB/Xhci.hpp>
|
||||
#include <Drivers/USB/UsbDevice.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Memory/HHDM.hpp>
|
||||
#include <Memory/PageFrameAllocator.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
#include <Timekeeping/ApicTimer.hpp>
|
||||
|
||||
using namespace Kt;
|
||||
|
||||
namespace Drivers::USB::Bluetooth::Hci {
|
||||
|
||||
// =========================================================================
|
||||
// State
|
||||
// =========================================================================
|
||||
|
||||
static uint8_t g_slotId = 0;
|
||||
static bool g_initialized = false;
|
||||
|
||||
// Event receive buffer (filled by xHCI interrupt IN callback)
|
||||
static uint8_t g_eventBuf[256] = {};
|
||||
static volatile uint32_t g_eventLen = 0;
|
||||
static volatile bool g_eventReady = false;
|
||||
|
||||
// ACL receive buffer
|
||||
static uint8_t g_aclRxBuf[1024] = {};
|
||||
static volatile uint32_t g_aclRxLen = 0;
|
||||
static volatile bool g_aclRxReady = false;
|
||||
|
||||
// ACL transmit DMA buffer
|
||||
static uint8_t* g_aclTxBuf = nullptr;
|
||||
static uint64_t g_aclTxBufPhys = 0;
|
||||
|
||||
// HCI command DMA buffer (separate from ACL to avoid conflicts)
|
||||
static uint8_t* g_cmdDmaBuf = nullptr;
|
||||
static uint64_t g_cmdDmaBufPhys = 0;
|
||||
|
||||
// Connection table
|
||||
static ConnectionInfo g_connections[MAX_CONNECTIONS] = {};
|
||||
|
||||
// ACL buffer size (from controller)
|
||||
static uint16_t g_aclMaxLen = 0;
|
||||
static uint16_t g_aclMaxNum = 0;
|
||||
static volatile uint16_t g_aclPendingCount = 0;
|
||||
|
||||
// Inquiry results
|
||||
static InquiryDevice g_inquiryResults[MAX_INQUIRY_RESULTS] = {};
|
||||
static volatile int g_inquiryResultCount = 0;
|
||||
static volatile bool g_inquiryActive = false;
|
||||
|
||||
// =========================================================================
|
||||
// USB transfer callback
|
||||
// =========================================================================
|
||||
|
||||
static void TransferCallback(uint8_t slotId, uint8_t epDci,
|
||||
const uint8_t* data, uint32_t length,
|
||||
uint32_t completionCode) {
|
||||
if (slotId != g_slotId) return;
|
||||
|
||||
auto* dev = Xhci::GetDevice(slotId);
|
||||
if (!dev) return;
|
||||
|
||||
uint8_t intDci = dev->InterruptEpNum ? (dev->InterruptEpNum * 2 + 1) : 0;
|
||||
uint8_t bulkInDci = dev->BulkInEpNum ? (dev->BulkInEpNum * 2 + 1) : 0;
|
||||
|
||||
if (epDci == intDci && data && length > 0) {
|
||||
// HCI Event received on interrupt IN.
|
||||
// Dispatch asynchronous events (inquiry results, connection events,
|
||||
// etc.) immediately so they are never lost. Only buffer
|
||||
// Command Complete / Command Status events — those are consumed
|
||||
// by WaitCommandComplete / WaitCommandStatus.
|
||||
uint8_t evtCode = (length >= 1) ? data[0] : 0;
|
||||
|
||||
if (evtCode == EVT_COMMAND_COMPLETE || evtCode == EVT_COMMAND_STATUS) {
|
||||
uint32_t copyLen = length;
|
||||
if (copyLen > sizeof(g_eventBuf)) copyLen = sizeof(g_eventBuf);
|
||||
memcpy(g_eventBuf, data, copyLen);
|
||||
g_eventLen = copyLen;
|
||||
g_eventReady = true;
|
||||
} else {
|
||||
// Process immediately (inquiry results, connection events, etc.)
|
||||
ProcessEvent(data, length);
|
||||
}
|
||||
|
||||
// Re-queue interrupt transfer for next event
|
||||
Xhci::QueueInterruptTransfer(slotId);
|
||||
} else if (epDci == bulkInDci && data && length > 0) {
|
||||
// ACL data received on bulk IN
|
||||
uint32_t copyLen = length;
|
||||
if (copyLen > sizeof(g_aclRxBuf)) copyLen = sizeof(g_aclRxBuf);
|
||||
memcpy(g_aclRxBuf, data, copyLen);
|
||||
g_aclRxLen = copyLen;
|
||||
g_aclRxReady = true;
|
||||
|
||||
// Re-queue bulk IN transfer
|
||||
Xhci::QueueBulkInTransfer(slotId, nullptr, 0, dev->BulkInMaxPacket);
|
||||
} else if (epDci == (dev->BulkOutEpNum ? (uint8_t)(dev->BulkOutEpNum * 2) : (uint8_t)0)) {
|
||||
// Bulk OUT completion — decrement pending count
|
||||
if (g_aclPendingCount > 0) g_aclPendingCount--;
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Busy wait with event polling
|
||||
// =========================================================================
|
||||
|
||||
static void BusyWaitMs(uint64_t ms) {
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - start < ms) {
|
||||
asm volatile("pause" ::: "memory");
|
||||
}
|
||||
}
|
||||
|
||||
// Poll for events while waiting
|
||||
static void PollWait(uint32_t ms) {
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
while (Timekeeping::GetMilliseconds() - start < ms) {
|
||||
Xhci::PollEvents();
|
||||
for (int j = 0; j < 100; j++) {
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Initialize
|
||||
// =========================================================================
|
||||
|
||||
void Initialize(uint8_t slotId) {
|
||||
g_slotId = slotId;
|
||||
|
||||
// Register our transfer callback
|
||||
Xhci::RegisterTransferCallback(slotId, TransferCallback);
|
||||
|
||||
// Allocate DMA buffers for HCI commands and ACL data
|
||||
g_cmdDmaBuf = (uint8_t*)Memory::g_pfa->AllocateZeroed();
|
||||
g_cmdDmaBufPhys = Memory::SubHHDM(g_cmdDmaBuf);
|
||||
|
||||
g_aclTxBuf = (uint8_t*)Memory::g_pfa->AllocateZeroed();
|
||||
g_aclTxBufPhys = Memory::SubHHDM(g_aclTxBuf);
|
||||
|
||||
// NOTE: Do NOT queue interrupt IN or bulk IN transfers here.
|
||||
// The BT controller is not yet HCI-initialized and may misbehave.
|
||||
// Call StartEventPipe() after HCI Reset and initial setup.
|
||||
|
||||
g_initialized = true;
|
||||
KernelLogStream(OK, "BT-HCI") << "HCI transport initialized on slot " << (uint64_t)slotId;
|
||||
}
|
||||
|
||||
// Start receiving HCI events and ACL data — call after HCI init sequence
|
||||
void StartEventPipe() {
|
||||
if (!g_initialized) return;
|
||||
|
||||
// Queue initial interrupt IN transfer for HCI events
|
||||
Xhci::QueueInterruptTransfer(g_slotId);
|
||||
|
||||
// Queue initial bulk IN transfer for ACL data
|
||||
auto* dev = Xhci::GetDevice(g_slotId);
|
||||
if (dev && dev->BulkInEpNum) {
|
||||
Xhci::QueueBulkInTransfer(g_slotId, nullptr, 0, dev->BulkInMaxPacket);
|
||||
}
|
||||
|
||||
KernelLogStream(INFO, "BT-HCI") << "Event pipe started (interrupt IN + bulk IN)";
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// SendCommand — via USB control transfer on EP0
|
||||
// =========================================================================
|
||||
|
||||
bool SendCommand(uint16_t opcode, const uint8_t* params, uint8_t paramLen) {
|
||||
if (!g_initialized || !g_cmdDmaBuf) return false;
|
||||
|
||||
// HCI command packet: opcode (2) + paramLen (1) + params
|
||||
// USB-BT spec: HCI commands are sent via control transfer
|
||||
// bmRequestType = 0x20 (Host-to-device, Class, Device)
|
||||
// bRequest = 0x00
|
||||
// wValue = 0, wIndex = 0
|
||||
// wLength = sizeof(CommandHeader) + paramLen
|
||||
|
||||
// Use DMA-allocated buffer (not stack) for the command data.
|
||||
// xHCI reads from this buffer via DMA for OUT transfers.
|
||||
memset(g_cmdDmaBuf, 0, 512);
|
||||
g_cmdDmaBuf[0] = (uint8_t)(opcode & 0xFF);
|
||||
g_cmdDmaBuf[1] = (uint8_t)(opcode >> 8);
|
||||
g_cmdDmaBuf[2] = paramLen;
|
||||
if (params && paramLen > 0) {
|
||||
memcpy(&g_cmdDmaBuf[3], params, paramLen);
|
||||
}
|
||||
|
||||
uint16_t totalLen = 3 + paramLen;
|
||||
|
||||
uint32_t cc = Xhci::ControlTransfer(g_slotId,
|
||||
0x20, // bmRequestType: Host-to-device, Class, Device
|
||||
0x00, // bRequest: 0
|
||||
0x0000, // wValue
|
||||
0x0000, // wIndex
|
||||
totalLen,
|
||||
g_cmdDmaBuf,
|
||||
false); // dirIn = false (host to device)
|
||||
|
||||
if (cc != Xhci::CC_SUCCESS) {
|
||||
KernelLogStream(WARNING, "BT-HCI") << "SendCommand failed, opcode="
|
||||
<< base::hex << (uint64_t)opcode << " cc=" << base::dec << (uint64_t)cc;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// WaitCommandComplete
|
||||
// =========================================================================
|
||||
|
||||
bool WaitCommandComplete(uint16_t opcode, uint8_t* outParams,
|
||||
uint8_t maxLen, uint32_t timeoutMs) {
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
|
||||
if (g_eventReady) {
|
||||
g_eventReady = false;
|
||||
|
||||
if (g_eventLen >= 2) {
|
||||
uint8_t evtCode = g_eventBuf[0];
|
||||
uint8_t evtParamLen = g_eventBuf[1];
|
||||
|
||||
if (evtCode == EVT_COMMAND_COMPLETE && evtParamLen >= 3) {
|
||||
// Command Complete: NumPkts(1) + Opcode(2) + Status(1) + Params
|
||||
uint16_t evtOpcode = (uint16_t)g_eventBuf[3] | ((uint16_t)g_eventBuf[4] << 8);
|
||||
if (evtOpcode == opcode) {
|
||||
if (outParams && maxLen > 0) {
|
||||
// Copy params starting after the status byte
|
||||
uint8_t availLen = (evtParamLen > 4) ? (evtParamLen - 4) : 0;
|
||||
uint8_t copyLen = (availLen < maxLen) ? availLen : maxLen;
|
||||
// Include status byte + return params
|
||||
copyLen = (evtParamLen > 3) ? (evtParamLen - 3) : 0;
|
||||
if (copyLen > maxLen) copyLen = maxLen;
|
||||
memcpy(outParams, &g_eventBuf[5], copyLen);
|
||||
}
|
||||
// Check status
|
||||
uint8_t status = g_eventBuf[5];
|
||||
if (status != 0) {
|
||||
KernelLogStream(WARNING, "BT-HCI") << "Command Complete status="
|
||||
<< (uint64_t)status << " opcode=" << base::hex << (uint64_t)opcode;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
for (int j = 0; j < 100; j++) {
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
}
|
||||
|
||||
KernelLogStream(WARNING, "BT-HCI") << "WaitCommandComplete timeout, opcode="
|
||||
<< base::hex << (uint64_t)opcode;
|
||||
return false;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// WaitCommandStatus
|
||||
// =========================================================================
|
||||
|
||||
bool WaitCommandStatus(uint16_t opcode, uint32_t timeoutMs) {
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
|
||||
if (g_eventReady) {
|
||||
g_eventReady = false;
|
||||
|
||||
if (g_eventLen >= 2) {
|
||||
uint8_t evtCode = g_eventBuf[0];
|
||||
uint8_t evtParamLen = g_eventBuf[1];
|
||||
|
||||
if (evtCode == EVT_COMMAND_STATUS && evtParamLen >= 4) {
|
||||
uint8_t status = g_eventBuf[2];
|
||||
uint16_t evtOpcode = (uint16_t)g_eventBuf[4] | ((uint16_t)g_eventBuf[5] << 8);
|
||||
if (evtOpcode == opcode) {
|
||||
return (status == 0);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
for (int j = 0; j < 100; j++) {
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// SendAcl — via USB bulk OUT
|
||||
// =========================================================================
|
||||
|
||||
bool SendAcl(uint16_t handle, uint16_t pbFlag, const uint8_t* data, uint16_t len) {
|
||||
if (!g_initialized || !g_aclTxBuf) return false;
|
||||
if (len + sizeof(AclHeader) > 4096) return false; // Single page DMA buffer
|
||||
|
||||
// Build ACL packet in DMA buffer
|
||||
auto* hdr = (AclHeader*)g_aclTxBuf;
|
||||
hdr->HandleFlags = (handle & 0x0FFF) | pbFlag;
|
||||
hdr->DataLength = len;
|
||||
if (data && len > 0) {
|
||||
memcpy(g_aclTxBuf + sizeof(AclHeader), data, len);
|
||||
}
|
||||
|
||||
uint32_t totalLen = sizeof(AclHeader) + len;
|
||||
|
||||
g_aclPendingCount++;
|
||||
Xhci::QueueBulkOutTransfer(g_slotId, g_aclTxBuf, g_aclTxBufPhys, totalLen);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ProcessEvent — handle HCI events
|
||||
// =========================================================================
|
||||
|
||||
void ProcessEvent(const uint8_t* data, uint32_t len) {
|
||||
if (len < 2) return;
|
||||
|
||||
uint8_t evtCode = data[0];
|
||||
uint8_t evtParamLen = data[1];
|
||||
const uint8_t* params = data + 2;
|
||||
|
||||
switch (evtCode) {
|
||||
case EVT_CONNECTION_COMPLETE: {
|
||||
if (evtParamLen >= 11) {
|
||||
uint8_t status = params[0];
|
||||
uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8);
|
||||
const uint8_t* bdAddr = ¶ms[3];
|
||||
uint8_t linkType = params[9];
|
||||
|
||||
KernelLogStream(INFO, "BT-HCI") << "Connection Complete: status="
|
||||
<< (uint64_t)status << " handle=" << (uint64_t)handle
|
||||
<< " link=" << (uint64_t)linkType;
|
||||
|
||||
if (status == 0) {
|
||||
// Find empty connection slot
|
||||
for (int i = 0; i < MAX_CONNECTIONS; i++) {
|
||||
if (!g_connections[i].Active) {
|
||||
g_connections[i].Active = true;
|
||||
g_connections[i].Handle = handle;
|
||||
memcpy(g_connections[i].BdAddr, bdAddr, 6);
|
||||
g_connections[i].LinkType = linkType;
|
||||
g_connections[i].Encrypted = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// Initialize L2CAP for this connection
|
||||
L2cap::Initialize(handle);
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_DISCONNECTION_COMPLETE: {
|
||||
if (evtParamLen >= 4) {
|
||||
uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8);
|
||||
uint8_t reason = params[3];
|
||||
|
||||
KernelLogStream(INFO, "BT-HCI") << "Disconnection: handle="
|
||||
<< (uint64_t)handle << " reason=" << (uint64_t)reason;
|
||||
|
||||
for (int i = 0; i < MAX_CONNECTIONS; i++) {
|
||||
if (g_connections[i].Active && g_connections[i].Handle == handle) {
|
||||
g_connections[i].Active = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_CONNECTION_REQUEST: {
|
||||
if (evtParamLen >= 10) {
|
||||
const uint8_t* bdAddr = ¶ms[0];
|
||||
uint8_t linkType = params[9];
|
||||
|
||||
KernelLogStream(INFO, "BT-HCI") << "Connection Request: link="
|
||||
<< (uint64_t)linkType;
|
||||
|
||||
// Auto-accept ACL connections
|
||||
if (linkType == 0x01) {
|
||||
AcceptConnection(bdAddr, 0x01); // Role = slave
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_NUM_COMPLETED_PACKETS: {
|
||||
if (evtParamLen >= 1) {
|
||||
uint8_t numHandles = params[0];
|
||||
for (int i = 0; i < numHandles && (3 + i * 4) < evtParamLen; i++) {
|
||||
uint16_t completed = (uint16_t)params[3 + i * 4]
|
||||
| ((uint16_t)params[4 + i * 4] << 8);
|
||||
if (g_aclPendingCount >= completed) {
|
||||
g_aclPendingCount -= completed;
|
||||
} else {
|
||||
g_aclPendingCount = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_IO_CAPABILITY_REQUEST: {
|
||||
if (evtParamLen >= 6) {
|
||||
// Reply with NoInputNoOutput for simple pairing
|
||||
uint8_t reply[9] = {};
|
||||
memcpy(reply, ¶ms[0], 6); // BD_ADDR
|
||||
reply[6] = 0x03; // IO Capability: NoInputNoOutput
|
||||
reply[7] = 0x00; // OOB data not present
|
||||
reply[8] = 0x00; // Authentication requirements: MITM not required
|
||||
SendCommand(OP_IO_CAPABILITY_REPLY, reply, 9);
|
||||
WaitCommandComplete(OP_IO_CAPABILITY_REPLY, nullptr, 0, 1000);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_USER_CONFIRM_REQUEST: {
|
||||
if (evtParamLen >= 6) {
|
||||
// Auto-confirm
|
||||
SendCommand(OP_USER_CONFIRM_REPLY, ¶ms[0], 6);
|
||||
WaitCommandComplete(OP_USER_CONFIRM_REPLY, nullptr, 0, 1000);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_INQUIRY_COMPLETE: {
|
||||
g_inquiryActive = false;
|
||||
KernelLogStream(INFO, "BT-HCI") << "Inquiry complete, "
|
||||
<< (uint64_t)g_inquiryResultCount << " device(s) found";
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_INQUIRY_RESULT: {
|
||||
// Standard inquiry result: NumResp(1) + per-device(14 bytes each)
|
||||
if (evtParamLen >= 1) {
|
||||
uint8_t numResp = params[0];
|
||||
for (int i = 0; i < numResp && g_inquiryResultCount < MAX_INQUIRY_RESULTS; i++) {
|
||||
const uint8_t* entry = ¶ms[1 + i * 14];
|
||||
auto& dev = g_inquiryResults[g_inquiryResultCount];
|
||||
memset(&dev, 0, sizeof(dev));
|
||||
memcpy(dev.BdAddr, entry, 6);
|
||||
dev.ClassOfDevice = (uint32_t)entry[9]
|
||||
| ((uint32_t)entry[10] << 8)
|
||||
| ((uint32_t)entry[11] << 16);
|
||||
dev.Rssi = -128; // Unknown for standard inquiry
|
||||
g_inquiryResultCount++;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_INQUIRY_RESULT_RSSI: {
|
||||
// Inquiry Result with RSSI: NumResp(1) + per-device(15 bytes each)
|
||||
if (evtParamLen >= 1) {
|
||||
uint8_t numResp = params[0];
|
||||
for (int i = 0; i < numResp && g_inquiryResultCount < MAX_INQUIRY_RESULTS; i++) {
|
||||
const uint8_t* entry = ¶ms[1 + i * 15];
|
||||
auto& dev = g_inquiryResults[g_inquiryResultCount];
|
||||
memset(&dev, 0, sizeof(dev));
|
||||
memcpy(dev.BdAddr, entry, 6);
|
||||
dev.ClassOfDevice = (uint32_t)entry[9]
|
||||
| ((uint32_t)entry[10] << 8)
|
||||
| ((uint32_t)entry[11] << 16);
|
||||
dev.Rssi = (int8_t)entry[14];
|
||||
g_inquiryResultCount++;
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_EXTENDED_INQUIRY_RESULT: {
|
||||
// Extended Inquiry Result: NumResp(1) + BD_ADDR(6) + PSRM(1) + reserved(1)
|
||||
// + CoD(3) + ClockOff(2) + RSSI(1) + EIR(240)
|
||||
if (evtParamLen >= 15 && g_inquiryResultCount < MAX_INQUIRY_RESULTS) {
|
||||
auto& dev = g_inquiryResults[g_inquiryResultCount];
|
||||
memset(&dev, 0, sizeof(dev));
|
||||
memcpy(dev.BdAddr, ¶ms[1], 6);
|
||||
dev.ClassOfDevice = (uint32_t)params[9]
|
||||
| ((uint32_t)params[10] << 8)
|
||||
| ((uint32_t)params[11] << 16);
|
||||
dev.Rssi = (int8_t)params[14];
|
||||
|
||||
// Parse EIR data for device name
|
||||
const uint8_t* eir = ¶ms[15];
|
||||
int eirLen = evtParamLen - 15;
|
||||
int pos = 0;
|
||||
while (pos < eirLen && pos < 240) {
|
||||
uint8_t len = eir[pos];
|
||||
if (len == 0) break;
|
||||
if (pos + 1 + len > eirLen) break;
|
||||
uint8_t type = eir[pos + 1];
|
||||
// Type 0x08 = Shortened Local Name, 0x09 = Complete Local Name
|
||||
if (type == 0x08 || type == 0x09) {
|
||||
int nameLen = len - 1;
|
||||
if (nameLen > 63) nameLen = 63;
|
||||
memcpy(dev.Name, &eir[pos + 2], nameLen);
|
||||
dev.Name[nameLen] = '\0';
|
||||
}
|
||||
pos += 1 + len;
|
||||
}
|
||||
|
||||
g_inquiryResultCount++;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case EVT_ENCRYPT_CHANGE: {
|
||||
if (evtParamLen >= 4) {
|
||||
uint16_t handle = (uint16_t)params[1] | ((uint16_t)params[2] << 8);
|
||||
uint8_t encryption = params[3];
|
||||
|
||||
for (int i = 0; i < MAX_CONNECTIONS; i++) {
|
||||
if (g_connections[i].Active && g_connections[i].Handle == handle) {
|
||||
g_connections[i].Encrypted = (encryption != 0);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ProcessAcl — handle incoming ACL data
|
||||
// =========================================================================
|
||||
|
||||
void ProcessAcl(const uint8_t* data, uint32_t len) {
|
||||
if (len < sizeof(AclHeader)) return;
|
||||
|
||||
auto* hdr = (const AclHeader*)data;
|
||||
uint16_t handle = hdr->HandleFlags & 0x0FFF;
|
||||
uint16_t pbFlag = hdr->HandleFlags & 0x3000;
|
||||
uint16_t dataLen = hdr->DataLength;
|
||||
|
||||
if (dataLen + sizeof(AclHeader) > len) return;
|
||||
|
||||
// Dispatch to L2CAP
|
||||
L2cap::ProcessPacket(handle, data + sizeof(AclHeader), dataLen);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Connection management
|
||||
// =========================================================================
|
||||
|
||||
ConnectionInfo* GetConnection(uint16_t handle) {
|
||||
for (int i = 0; i < MAX_CONNECTIONS; i++) {
|
||||
if (g_connections[i].Active && g_connections[i].Handle == handle) {
|
||||
return &g_connections[i];
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
ConnectionInfo* GetActiveConnection() {
|
||||
for (int i = 0; i < MAX_CONNECTIONS; i++) {
|
||||
if (g_connections[i].Active) {
|
||||
return &g_connections[i];
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
ConnectionInfo* GetConnectionByIndex(int index) {
|
||||
if (index < 0 || index >= MAX_CONNECTIONS) return nullptr;
|
||||
return &g_connections[index];
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Convenience HCI commands
|
||||
// =========================================================================
|
||||
|
||||
bool Reset() {
|
||||
if (!SendCommand(OP_RESET, nullptr, 0)) return false;
|
||||
BusyWaitMs(100);
|
||||
return WaitCommandComplete(OP_RESET, nullptr, 0, 5000);
|
||||
}
|
||||
|
||||
bool ReadBdAddr(uint8_t* addr) {
|
||||
if (!SendCommand(OP_READ_BD_ADDR, nullptr, 0)) return false;
|
||||
uint8_t params[7] = {};
|
||||
if (!WaitCommandComplete(OP_READ_BD_ADDR, params, sizeof(params))) return false;
|
||||
// params[0] = status, params[1..6] = BD_ADDR
|
||||
if (params[0] != 0) return false;
|
||||
memcpy(addr, ¶ms[1], 6);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ReadLocalVersion(LocalVersion* ver) {
|
||||
if (!SendCommand(OP_READ_LOCAL_VERSION, nullptr, 0)) return false;
|
||||
uint8_t params[9] = {};
|
||||
if (!WaitCommandComplete(OP_READ_LOCAL_VERSION, params, sizeof(params))) return false;
|
||||
if (params[0] != 0) return false;
|
||||
if (ver) {
|
||||
ver->Status = params[0];
|
||||
ver->HciVersion = params[1];
|
||||
ver->HciRevision = (uint16_t)params[2] | ((uint16_t)params[3] << 8);
|
||||
ver->LmpVersion = params[4];
|
||||
ver->Manufacturer = (uint16_t)params[5] | ((uint16_t)params[6] << 8);
|
||||
ver->LmpSubversion = (uint16_t)params[7] | ((uint16_t)params[8] << 8);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ReadIntelVersion(IntelVersion* ver) {
|
||||
// Newer Intel BT controllers (AX200/AX201/AX211, THP+) require a
|
||||
// parameter byte of 0xFF for 0xFC05 to return the full version in
|
||||
// TLV format. Try with the parameter first; fall back to the
|
||||
// legacy (no-param) format if the command fails.
|
||||
uint8_t param = 0xFF;
|
||||
bool sent = SendCommand(OP_INTEL_READ_VERSION, ¶m, 1);
|
||||
if (!sent) {
|
||||
// Fallback: legacy format (no parameter)
|
||||
sent = SendCommand(OP_INTEL_READ_VERSION, nullptr, 0);
|
||||
}
|
||||
if (!sent) return false;
|
||||
|
||||
uint8_t params[32] = {};
|
||||
if (!WaitCommandComplete(OP_INTEL_READ_VERSION, params, sizeof(params))) return false;
|
||||
|
||||
// Log raw response for diagnostics
|
||||
KernelLogStream(INFO, "BT-HCI") << "Intel version raw: "
|
||||
<< base::hex
|
||||
<< (uint64_t)params[0] << " " << (uint64_t)params[1] << " "
|
||||
<< (uint64_t)params[2] << " " << (uint64_t)params[3] << " "
|
||||
<< (uint64_t)params[4] << " " << (uint64_t)params[5] << " "
|
||||
<< (uint64_t)params[6] << " " << (uint64_t)params[7] << " "
|
||||
<< (uint64_t)params[8] << " " << (uint64_t)params[9]
|
||||
<< base::dec;
|
||||
|
||||
if (ver) memcpy(ver, params, sizeof(IntelVersion));
|
||||
return true;
|
||||
}
|
||||
|
||||
bool WriteLocalName(const char* name) {
|
||||
uint8_t params[248] = {};
|
||||
int i = 0;
|
||||
for (; i < 247 && name[i]; i++) params[i] = name[i];
|
||||
params[i] = '\0';
|
||||
if (!SendCommand(OP_WRITE_LOCAL_NAME, params, 248)) return false;
|
||||
return WaitCommandComplete(OP_WRITE_LOCAL_NAME);
|
||||
}
|
||||
|
||||
bool WriteClassOfDevice(uint32_t cod) {
|
||||
uint8_t params[3] = {
|
||||
(uint8_t)(cod & 0xFF),
|
||||
(uint8_t)((cod >> 8) & 0xFF),
|
||||
(uint8_t)((cod >> 16) & 0xFF)
|
||||
};
|
||||
if (!SendCommand(OP_WRITE_CLASS_OF_DEVICE, params, 3)) return false;
|
||||
return WaitCommandComplete(OP_WRITE_CLASS_OF_DEVICE);
|
||||
}
|
||||
|
||||
bool WriteScanEnable(uint8_t mode) {
|
||||
if (!SendCommand(OP_WRITE_SCAN_ENABLE, &mode, 1)) return false;
|
||||
return WaitCommandComplete(OP_WRITE_SCAN_ENABLE);
|
||||
}
|
||||
|
||||
bool WriteSSPMode(uint8_t mode) {
|
||||
if (!SendCommand(OP_WRITE_SSP_MODE, &mode, 1)) return false;
|
||||
return WaitCommandComplete(OP_WRITE_SSP_MODE);
|
||||
}
|
||||
|
||||
bool AcceptConnection(const uint8_t* bdAddr, uint8_t role) {
|
||||
uint8_t params[7];
|
||||
memcpy(params, bdAddr, 6);
|
||||
params[6] = role;
|
||||
if (!SendCommand(OP_ACCEPT_CONN_REQ, params, 7)) return false;
|
||||
return WaitCommandStatus(OP_ACCEPT_CONN_REQ);
|
||||
}
|
||||
|
||||
bool Disconnect(uint16_t handle, uint8_t reason) {
|
||||
uint8_t params[3] = {
|
||||
(uint8_t)(handle & 0xFF),
|
||||
(uint8_t)((handle >> 8) & 0xFF),
|
||||
reason
|
||||
};
|
||||
if (!SendCommand(OP_DISCONNECT, params, 3)) return false;
|
||||
return WaitCommandStatus(OP_DISCONNECT);
|
||||
}
|
||||
|
||||
bool ReadBufferSize(uint16_t* aclLen, uint8_t* scoLen,
|
||||
uint16_t* aclNum, uint16_t* scoNum) {
|
||||
if (!SendCommand(OP_READ_BUFFER_SIZE, nullptr, 0)) return false;
|
||||
uint8_t params[8] = {};
|
||||
if (!WaitCommandComplete(OP_READ_BUFFER_SIZE, params, sizeof(params))) return false;
|
||||
if (params[0] != 0) return false;
|
||||
if (aclLen) *aclLen = (uint16_t)params[1] | ((uint16_t)params[2] << 8);
|
||||
if (scoLen) *scoLen = params[3];
|
||||
if (aclNum) *aclNum = (uint16_t)params[4] | ((uint16_t)params[5] << 8);
|
||||
if (scoNum) *scoNum = (uint16_t)params[6] | ((uint16_t)params[7] << 8);
|
||||
g_aclMaxLen = (uint16_t)params[1] | ((uint16_t)params[2] << 8);
|
||||
g_aclMaxNum = (uint16_t)params[4] | ((uint16_t)params[5] << 8);
|
||||
return true;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Inquiry (device discovery)
|
||||
// =========================================================================
|
||||
|
||||
bool StartInquiry(uint8_t durationUnits) {
|
||||
g_inquiryResultCount = 0;
|
||||
g_inquiryActive = true;
|
||||
|
||||
// HCI Inquiry: LAP(3) + InquiryLength(1) + NumResponses(1)
|
||||
// GIAC LAP = 0x9E8B33
|
||||
uint8_t params[5] = {
|
||||
0x33, 0x8B, 0x9E, // LAP (General Inquiry Access Code)
|
||||
durationUnits, // Duration in 1.28s units
|
||||
0x00 // Unlimited responses
|
||||
};
|
||||
|
||||
if (!SendCommand(OP_INQUIRY, params, 5)) {
|
||||
g_inquiryActive = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Inquiry uses Command Status (not Command Complete)
|
||||
if (!WaitCommandStatus(OP_INQUIRY)) {
|
||||
g_inquiryActive = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CancelInquiry() {
|
||||
if (!g_inquiryActive) return true;
|
||||
if (!SendCommand(OP_INQUIRY_CANCEL, nullptr, 0)) return false;
|
||||
WaitCommandComplete(OP_INQUIRY_CANCEL, nullptr, 0, 2000);
|
||||
g_inquiryActive = false;
|
||||
return true;
|
||||
}
|
||||
|
||||
int GetInquiryResults(InquiryDevice* buf, int maxCount) {
|
||||
int count = g_inquiryResultCount;
|
||||
if (count > maxCount) count = maxCount;
|
||||
if (buf && count > 0) {
|
||||
memcpy(buf, g_inquiryResults, count * sizeof(InquiryDevice));
|
||||
}
|
||||
return count;
|
||||
}
|
||||
|
||||
void ClearInquiryResults() {
|
||||
g_inquiryResultCount = 0;
|
||||
}
|
||||
|
||||
bool IsInquiryActive() {
|
||||
return g_inquiryActive;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Create ACL connection
|
||||
// =========================================================================
|
||||
|
||||
void DrainEvents() {
|
||||
// Discard any unconsumed Command Complete/Status events that weren't
|
||||
// picked up by WaitCommandComplete/WaitCommandStatus.
|
||||
if (g_eventReady) {
|
||||
g_eventReady = false;
|
||||
}
|
||||
|
||||
// Drain ACL data
|
||||
if (g_aclRxReady) {
|
||||
g_aclRxReady = false;
|
||||
if (g_aclRxLen > 0) {
|
||||
ProcessAcl(g_aclRxBuf, g_aclRxLen);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool CreateConnection(const uint8_t* bdAddr) {
|
||||
// HCI Create Connection:
|
||||
// BD_ADDR(6) + PacketType(2) + PSRM(1) + reserved(1) + ClockOffset(2) + AllowRoleSwitch(1)
|
||||
uint8_t params[13] = {};
|
||||
memcpy(params, bdAddr, 6);
|
||||
// Packet types: DM1, DH1, DM3, DH3, DM5, DH5
|
||||
params[6] = 0x18; // CC18 = allow DM1, DH1, DM3, DH3, DM5, DH5
|
||||
params[7] = 0xCC;
|
||||
params[8] = 0x02; // Page Scan Repetition Mode R2
|
||||
params[9] = 0x00; // Reserved
|
||||
params[10] = 0x00; // Clock offset
|
||||
params[11] = 0x00;
|
||||
params[12] = 0x01; // Allow role switch
|
||||
|
||||
if (!SendCommand(OP_CREATE_CONNECTION, params, 13)) return false;
|
||||
|
||||
// Create Connection uses Command Status, then Connection Complete event
|
||||
return WaitCommandStatus(OP_CREATE_CONNECTION, 5000);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,251 @@
|
||||
/*
|
||||
* 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_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_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;
|
||||
|
||||
// =========================================================================
|
||||
// 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_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;
|
||||
|
||||
// =========================================================================
|
||||
// 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)
|
||||
void StartEventPipe();
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
// Accept an incoming connection
|
||||
bool AcceptConnection(const uint8_t* bdAddr, uint8_t role);
|
||||
|
||||
// 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();
|
||||
|
||||
}
|
||||
@@ -0,0 +1,422 @@
|
||||
/*
|
||||
* L2cap.cpp
|
||||
* Bluetooth L2CAP implementation
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "L2cap.hpp"
|
||||
#include "Hci.hpp"
|
||||
#include "A2dp.hpp"
|
||||
#include <Drivers/USB/Xhci.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
#include <Timekeeping/ApicTimer.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;
|
||||
|
||||
// Signaling response tracking
|
||||
static volatile bool g_sigResponseReady = false;
|
||||
static uint8_t g_sigResponseBuf[64] = {};
|
||||
static uint32_t g_sigResponseLen = 0;
|
||||
|
||||
// =========================================================================
|
||||
// Helpers
|
||||
// =========================================================================
|
||||
|
||||
static uint16_t AllocCid() {
|
||||
return g_nextCid++;
|
||||
}
|
||||
|
||||
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();
|
||||
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;
|
||||
return &g_channels[i];
|
||||
}
|
||||
}
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
// Send L2CAP signaling command
|
||||
static void 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] = {};
|
||||
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);
|
||||
}
|
||||
|
||||
Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_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++) {
|
||||
g_channels[i].Active = false;
|
||||
}
|
||||
|
||||
KernelLogStream(OK, "BT-L2CAP") << "Initialized for ACL handle " << (uint64_t)aclHandle;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// ProcessPacket
|
||||
// =========================================================================
|
||||
|
||||
void ProcessPacket(uint16_t aclHandle, const uint8_t* data, uint16_t len) {
|
||||
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
|
||||
if (l2len < sizeof(SignalHeader)) return;
|
||||
|
||||
auto* sig = (const SignalHeader*)payload;
|
||||
const uint8_t* sigPayload = payload + sizeof(SignalHeader);
|
||||
uint16_t sigPayloadLen = sig->Length;
|
||||
|
||||
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
|
||||
if (psm == PSM_AVDTP || psm == PSM_SDP) {
|
||||
auto* ch = AllocChannel(psm);
|
||||
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);
|
||||
}
|
||||
} 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);
|
||||
|
||||
KernelLogStream(INFO, "BT-L2CAP") << "Connection Response: dstCID="
|
||||
<< base::hex << (uint64_t)dstCid << " result=" << (uint64_t)result;
|
||||
|
||||
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].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, g_sigIdentifier++, cfgReq, 4);
|
||||
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) {
|
||||
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) {
|
||||
for (int i = 0; i < MAX_CHANNELS; i++) {
|
||||
if (g_channels[i].Active && g_channels[i].RemoteCid == srcCid) {
|
||||
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;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
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) {
|
||||
g_channels[i].Active = false;
|
||||
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;
|
||||
}
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
} 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(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);
|
||||
SendSignal(SIG_CONN_REQ, g_sigIdentifier++, req, 4);
|
||||
|
||||
return ch->LocalCid;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// WaitConfigured
|
||||
// =========================================================================
|
||||
|
||||
bool WaitConfigured(uint16_t localCid, uint32_t timeoutMs) {
|
||||
uint64_t start = Timekeeping::GetMilliseconds();
|
||||
|
||||
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
||||
Xhci::PollEvents();
|
||||
|
||||
auto* ch = GetChannel(localCid);
|
||||
if (ch && ch->Configured) return true;
|
||||
|
||||
for (int j = 0; j < 100; j++) {
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
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 GetAclHandle() {
|
||||
return g_aclHandle;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,111 @@
|
||||
/*
|
||||
* 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_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_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;
|
||||
};
|
||||
|
||||
constexpr int MAX_CHANNELS = 8;
|
||||
|
||||
// =========================================================================
|
||||
// Public API
|
||||
// =========================================================================
|
||||
|
||||
// Initialize L2CAP for a new HCI connection
|
||||
void Initialize(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);
|
||||
|
||||
// Get the ACL handle
|
||||
uint16_t GetAclHandle();
|
||||
|
||||
}
|
||||
@@ -0,0 +1,416 @@
|
||||
/*
|
||||
* 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 {
|
||||
|
||||
// =========================================================================
|
||||
// Fixed-point constants (Q1.30 format for filter coefficients)
|
||||
// =========================================================================
|
||||
|
||||
// Scale factor: 1.0 = (1 << 15) in Q15.16
|
||||
constexpr int32_t FP_ONE = (1 << 15);
|
||||
|
||||
// SBC 8-subband analysis filter prototype coefficients (Q1.30)
|
||||
// These are the 80 windowed prototype coefficients from the SBC spec
|
||||
// Scaled to Q15.16 for our fixed-point arithmetic
|
||||
static const int32_t g_proto8[80] = {
|
||||
0x0002, 0x0005, 0x000A, 0x0014, 0x0023, 0x0038, 0x0054, 0x0078,
|
||||
0x00A5, 0x00DB, 0x011B, 0x0164, 0x01B5, 0x020E, 0x026D, 0x02D0,
|
||||
0x0335, 0x039A, 0x03FC, 0x0458, 0x04AB, 0x04F1, 0x0527, 0x054A,
|
||||
0x0557, 0x054A, 0x0527, 0x04F1, 0x04AB, 0x0458, 0x03FC, 0x039A,
|
||||
0x0335, 0x02D0, 0x026D, 0x020E, 0x01B5, 0x0164, 0x011B, 0x00DB,
|
||||
0x00A5, 0x0078, 0x0054, 0x0038, 0x0023, 0x0014, 0x000A, 0x0005,
|
||||
0x0002, -0x0002, -0x0005, -0x000A, -0x0014, -0x0023, -0x0038, -0x0054,
|
||||
-0x0078, -0x00A5, -0x00DB, -0x011B, -0x0164, -0x01B5, -0x020E, -0x026D,
|
||||
-0x02D0, -0x0335, -0x039A, -0x03FC, -0x0458, -0x04AB, -0x04F1, -0x0527,
|
||||
-0x054A, -0x0557, -0x054A, -0x0527, -0x04F1, -0x04AB, -0x0458, -0x03FC,
|
||||
};
|
||||
|
||||
// Cosine matrix for 8-subband DCT-II (Q15.16)
|
||||
// cos_matrix[k][i] = cos((k + 0.5) * (2*i + 1) * PI / 16) * FP_ONE
|
||||
static const int32_t g_cosMatrix8[8][16] = {
|
||||
{ 32138, 31650, 30679, 29246, 27381, 25126, 22529, 19644, 16531, 13254, 9882, 6484, 3134, -199, -3509, -6758},
|
||||
{ 30679, 25126, 16531, 6484, -3509,-13254,-22529,-29246,-32138,-31650,-27381,-19644, -9882, 199, 9882, 19644},
|
||||
{ 27381, 13254, -3509,-19644,-30679,-32138,-22529, -6484, 9882, 25126, 32138, 29246, 16531, -199,-16531,-29246},
|
||||
{ 22529, -199,-22529, -199, 22529, 199,-22529, -199, 22529, 199,-22529, -199, 22529, 199,-22529, -199},
|
||||
{ 16531,-13254,-30679, 6484, 32138, -199,-32138, -6484, 30679, 13254,-16531,-25126, 3509, 29246, 9882,-27381},
|
||||
{ 9882,-25126,-16531, 29246, 3509,-32138, 9882, 25126,-16531,-29246, 3509, 32138, -9882,-25126, 16531, 29246},
|
||||
{ 3134,-31650, 27381, -6484,-22529, 32138,-13254, -9882, 30679,-29246, 6484, 19644,-32138, 16531, 3509,-25126},
|
||||
{ -3509, 32138,-22529, -6484, 30679,-27381, 3509, 25126,-32138, 16531, 9882,-30679, 22529, -199,-25126, 32138},
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// 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;
|
||||
enc->FrameSize = (headerBits + dataBits + 7) / 8;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Analysis filter bank (8 subbands)
|
||||
// =========================================================================
|
||||
|
||||
static void AnalysisFilter(SbcEncoder* enc, const int16_t* pcm, int ch,
|
||||
int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS]) {
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
// Shift in new samples
|
||||
int pos = enc->XPos[ch];
|
||||
for (int i = enc->Subbands - 1; i >= 0; i--) {
|
||||
pos = (pos + 1) % (enc->Subbands * 10);
|
||||
enc->X[ch][pos] = (int32_t)pcm[blk * enc->Subbands * enc->Channels + i * enc->Channels + ch];
|
||||
}
|
||||
enc->XPos[ch] = pos;
|
||||
|
||||
// Windowing and partial calculation
|
||||
int32_t Z[2 * SBC_SUBBANDS];
|
||||
for (int i = 0; i < 2 * enc->Subbands; i++) {
|
||||
Z[i] = 0;
|
||||
for (int j = 0; j < 5; j++) {
|
||||
int idx = (pos + i + j * 2 * enc->Subbands) % (enc->Subbands * 10);
|
||||
int protoIdx = i + j * 2 * enc->Subbands;
|
||||
if (protoIdx < 80) {
|
||||
Z[i] += (int32_t)(((int64_t)enc->X[ch][idx] * g_proto8[protoIdx]) >> 15);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Matrixing (DCT)
|
||||
for (int k = 0; k < enc->Subbands; k++) {
|
||||
int32_t sum = 0;
|
||||
for (int i = 0; i < 2 * enc->Subbands; i++) {
|
||||
sum += (int32_t)(((int64_t)Z[i] * g_cosMatrix8[k][i]) >> 15);
|
||||
}
|
||||
sb_samples[blk][k] = sum;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Bit allocation (Loudness method)
|
||||
// =========================================================================
|
||||
|
||||
static void BitAllocation(SbcEncoder* enc,
|
||||
int32_t sb_samples[SBC_BLOCKS][SBC_SUBBANDS],
|
||||
int ch,
|
||||
int32_t scale_factors[SBC_SUBBANDS],
|
||||
uint8_t bits[SBC_SUBBANDS]) {
|
||||
// Compute scale factors
|
||||
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;
|
||||
}
|
||||
|
||||
// Find scale factor (highest bit position)
|
||||
scale_factors[sb] = 0;
|
||||
int32_t tmp = maxVal;
|
||||
while (tmp > 0) {
|
||||
scale_factors[sb]++;
|
||||
tmp >>= 1;
|
||||
}
|
||||
}
|
||||
|
||||
// Loudness offset table for 8 subbands (from SBC spec)
|
||||
static const int8_t loudness_offset_8[4][8] = {
|
||||
{-2, 0, 0, 0, 0, 0, 0, 1}, // 16kHz
|
||||
{-3, 0, 0, 0, 0, 0, 1, 2}, // 32kHz
|
||||
{-4, 0, 0, 0, 0, 0, 1, 2}, // 44.1kHz
|
||||
{-4, 0, 0, 0, 0, 0, 1, 2}, // 48kHz
|
||||
};
|
||||
|
||||
// Compute bitneed
|
||||
int32_t bitneed[SBC_SUBBANDS];
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (enc->AllocMethod == ALLOC_LOUDNESS) {
|
||||
bitneed[sb] = scale_factors[sb] - loudness_offset_8[enc->Frequency][sb];
|
||||
} else {
|
||||
bitneed[sb] = scale_factors[sb];
|
||||
}
|
||||
}
|
||||
|
||||
// Bit allocation loop
|
||||
int32_t bitcount = 0;
|
||||
int32_t slicecount = 0;
|
||||
int32_t bitslice = (int32_t)(scale_factors[0] > 0 ? scale_factors[0] : 1);
|
||||
|
||||
// Find max bitneed
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (bitneed[sb] > bitslice) bitslice = bitneed[sb];
|
||||
}
|
||||
bitslice++;
|
||||
|
||||
// Iterative allocation
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) bits[sb] = 0;
|
||||
|
||||
while (true) {
|
||||
bitslice--;
|
||||
bitcount = 0;
|
||||
slicecount = 0;
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (bitneed[sb] >= bitslice + 1 && bitneed[sb] < bitslice + 16) {
|
||||
if (bitneed[sb] == bitslice + 1) {
|
||||
bitcount += 2;
|
||||
slicecount++;
|
||||
} else {
|
||||
bitcount++;
|
||||
slicecount++;
|
||||
}
|
||||
}
|
||||
}
|
||||
if (bitcount + slicecount >= enc->Bitpool) break;
|
||||
if (bitslice <= -16) break;
|
||||
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (bitneed[sb] >= bitslice + 1 && bitneed[sb] < bitslice + 16) {
|
||||
if (bitneed[sb] == bitslice + 1) {
|
||||
bits[sb] = 2;
|
||||
} else if (bits[sb] < 16) {
|
||||
bits[sb]++;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Distribute remaining bits
|
||||
int32_t remaining = enc->Bitpool - bitcount;
|
||||
for (int sb = 0; sb < enc->Subbands && remaining > 0; sb++) {
|
||||
if (bits[sb] >= 2 && bits[sb] < 16) {
|
||||
bits[sb]++;
|
||||
remaining--;
|
||||
} else if (bitneed[sb] == bitslice && bits[sb] == 0) {
|
||||
bits[sb] = 2;
|
||||
remaining -= 2;
|
||||
if (remaining < 0) { bits[sb] = 0; break; }
|
||||
}
|
||||
}
|
||||
|
||||
for (int sb = 0; sb < enc->Subbands && remaining > 0; sb++) {
|
||||
if (bits[sb] < 16) {
|
||||
bits[sb]++;
|
||||
remaining--;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// 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];
|
||||
|
||||
// Clear output
|
||||
memset(out, 0, enc->FrameSize);
|
||||
|
||||
// Analysis filter for each channel
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
AnalysisFilter(enc, pcm, ch, sb_samples[ch]);
|
||||
BitAllocation(enc, sb_samples[ch], ch, scale_factors[ch], bits[ch]);
|
||||
}
|
||||
|
||||
// Joint stereo processing
|
||||
uint8_t joint = 0;
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
||||
for (int sb = 0; sb < enc->Subbands - 1; sb++) {
|
||||
// Simple heuristic: use joint coding if it saves bits
|
||||
int32_t maxMid = 0, maxSide = 0;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
int32_t mid = (sb_samples[0][blk][sb] + sb_samples[1][blk][sb]) / 2;
|
||||
int32_t side = (sb_samples[0][blk][sb] - sb_samples[1][blk][sb]) / 2;
|
||||
if (mid < 0) mid = -mid;
|
||||
if (side < 0) side = -side;
|
||||
if (mid > maxMid) maxMid = mid;
|
||||
if (side > maxSide) maxSide = side;
|
||||
}
|
||||
int32_t maxOrig = 0;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
int32_t v0 = sb_samples[0][blk][sb]; if (v0 < 0) v0 = -v0;
|
||||
int32_t v1 = sb_samples[1][blk][sb]; if (v1 < 0) v1 = -v1;
|
||||
if (v0 > maxOrig) maxOrig = v0;
|
||||
if (v1 > maxOrig) maxOrig = v1;
|
||||
}
|
||||
if (maxMid + maxSide < maxOrig) {
|
||||
joint |= (1 << (enc->Subbands - 1 - sb));
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
int32_t l = sb_samples[0][blk][sb];
|
||||
int32_t r = sb_samples[1][blk][sb];
|
||||
sb_samples[0][blk][sb] = (l + r) / 2;
|
||||
sb_samples[1][blk][sb] = (l - r) / 2;
|
||||
}
|
||||
// Recalculate scale factors for joint channels
|
||||
for (int ch = 0; ch < 2; ch++) {
|
||||
int32_t maxVal = 0;
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
int32_t val = sb_samples[ch][blk][sb];
|
||||
if (val < 0) val = -val;
|
||||
if (val > maxVal) maxVal = val;
|
||||
}
|
||||
scale_factors[ch][sb] = 0;
|
||||
int32_t tmp = maxVal;
|
||||
while (tmp > 0) { scale_factors[ch][sb]++; tmp >>= 1; }
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Pack SBC frame header
|
||||
out[0] = 0x9C; // Sync word
|
||||
out[1] = (enc->Frequency << 6) | ((enc->Blocks == 4 ? 0 : enc->Blocks == 8 ? 1 : enc->Blocks == 12 ? 2 : 3) << 4)
|
||||
| (enc->ChannelMode << 2) | (enc->AllocMethod << 1) | (enc->Subbands == 8 ? 1 : 0);
|
||||
out[2] = enc->Bitpool;
|
||||
|
||||
// CRC (computed over header bytes 1-2 and scale factors)
|
||||
// Will be filled after scale factors are packed
|
||||
|
||||
BitWriter bw = {out, 32}; // Start after 4-byte header
|
||||
|
||||
// Joint stereo flags
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) {
|
||||
WriteBits(&bw, joint, enc->Subbands);
|
||||
}
|
||||
|
||||
// Pack scale factors (4 bits each)
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
uint32_t sf = scale_factors[ch][sb];
|
||||
if (sf > 15) sf = 15;
|
||||
WriteBits(&bw, sf, 4);
|
||||
}
|
||||
}
|
||||
|
||||
// Compute CRC (over bytes 1, 2, and scale factor bits)
|
||||
uint32_t crcBits = 16 + (enc->Channels * enc->Subbands * 4);
|
||||
if (enc->ChannelMode == MODE_JOINT_STEREO) crcBits += enc->Subbands;
|
||||
out[3] = SbcCrc8(&out[1], (crcBits + 7) / 8, crcBits % 8 ? crcBits % 8 : 8);
|
||||
|
||||
// Pack audio samples
|
||||
for (int blk = 0; blk < enc->Blocks; blk++) {
|
||||
for (int ch = 0; ch < enc->Channels; ch++) {
|
||||
for (int sb = 0; sb < enc->Subbands; sb++) {
|
||||
if (bits[ch][sb] == 0) continue;
|
||||
|
||||
int32_t sf = scale_factors[ch][sb];
|
||||
int32_t sample = sb_samples[ch][blk][sb];
|
||||
|
||||
// Quantize: levels = (1 << bits) - 1
|
||||
uint32_t levels = (1u << bits[ch][sb]) - 1;
|
||||
int32_t quantized;
|
||||
|
||||
if (sf > 0) {
|
||||
// Normalize and quantize
|
||||
int32_t maxRange = (1 << sf);
|
||||
quantized = (int32_t)(((int64_t)(sample + maxRange) * levels) / (2 * maxRange));
|
||||
} else {
|
||||
quantized = levels / 2;
|
||||
}
|
||||
|
||||
if (quantized < 0) quantized = 0;
|
||||
if (quantized > (int32_t)levels) quantized = (int32_t)levels;
|
||||
|
||||
WriteBits(&bw, (uint32_t)quantized, bits[ch][sb]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Pad to byte boundary
|
||||
uint32_t totalBytes = (bw.BitPos + 7) / 8;
|
||||
return totalBytes > enc->FrameSize ? enc->FrameSize : totalBytes;
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
// Queries
|
||||
// =========================================================================
|
||||
|
||||
uint32_t GetFrameSize(const SbcEncoder* enc) {
|
||||
return enc->FrameSize;
|
||||
}
|
||||
|
||||
uint32_t GetSamplesPerFrame(const SbcEncoder* enc) {
|
||||
return enc->SamplesPerFrame;
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
/*
|
||||
* 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)
|
||||
int32_t X[SBC_CHANNELS][SBC_SUBBANDS * 10];
|
||||
int XPos[SBC_CHANNELS];
|
||||
|
||||
// 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);
|
||||
|
||||
// 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);
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user