feat: Intel HDA audio driver, audio streaming syscalls, userspace Music app, fixes and improvements, rudimentary Bluetooth support
This commit is contained in:
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,284 @@
|
||||
/*
|
||||
* IntelHda.hpp
|
||||
* Intel High Definition Audio controller driver
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <Pci/Pci.hpp>
|
||||
|
||||
namespace Drivers::Audio::IntelHda {
|
||||
|
||||
// =========================================================================
|
||||
// HDA controller registers (memory-mapped via BAR0)
|
||||
// Ref: Intel High Definition Audio Specification, Rev 1.0a
|
||||
// =========================================================================
|
||||
|
||||
// Global registers
|
||||
constexpr uint32_t REG_GCAP = 0x00; // Global Capabilities (16-bit)
|
||||
constexpr uint32_t REG_VMIN = 0x02; // Minor Version (8-bit)
|
||||
constexpr uint32_t REG_VMAJ = 0x03; // Major Version (8-bit)
|
||||
constexpr uint32_t REG_OUTPAY = 0x04; // Output Payload Capability (16-bit)
|
||||
constexpr uint32_t REG_INPAY = 0x06; // Input Payload Capability (16-bit)
|
||||
constexpr uint32_t REG_GCTL = 0x08; // Global Control (32-bit)
|
||||
constexpr uint32_t REG_WAKEEN = 0x0C; // Wake Enable (16-bit)
|
||||
constexpr uint32_t REG_STATESTS = 0x0E; // State Change Status (16-bit)
|
||||
constexpr uint32_t REG_GSTS = 0x10; // Global Status (16-bit)
|
||||
constexpr uint32_t REG_INTCTL = 0x20; // Interrupt Control (32-bit)
|
||||
constexpr uint32_t REG_INTSTS = 0x24; // Interrupt Status (32-bit)
|
||||
constexpr uint32_t REG_WALCLK = 0x30; // Wall Clock Counter (32-bit)
|
||||
constexpr uint32_t REG_SSYNC = 0x38; // Stream Synchronization (32-bit)
|
||||
|
||||
// CORB registers
|
||||
constexpr uint32_t REG_CORBLBASE = 0x40; // CORB Lower Base Address (32-bit)
|
||||
constexpr uint32_t REG_CORBUBASE = 0x44; // CORB Upper Base Address (32-bit)
|
||||
constexpr uint32_t REG_CORBWP = 0x48; // CORB Write Pointer (16-bit)
|
||||
constexpr uint32_t REG_CORBRP = 0x4A; // CORB Read Pointer (16-bit)
|
||||
constexpr uint32_t REG_CORBCTL = 0x4C; // CORB Control (8-bit)
|
||||
constexpr uint32_t REG_CORBSTS = 0x4D; // CORB Status (8-bit)
|
||||
constexpr uint32_t REG_CORBSIZE = 0x4E; // CORB Size (8-bit)
|
||||
|
||||
// RIRB registers
|
||||
constexpr uint32_t REG_RIRBLBASE = 0x50; // RIRB Lower Base Address (32-bit)
|
||||
constexpr uint32_t REG_RIRBUBASE = 0x54; // RIRB Upper Base Address (32-bit)
|
||||
constexpr uint32_t REG_RIRBWP = 0x58; // RIRB Write Pointer (16-bit)
|
||||
constexpr uint32_t REG_RINTCNT = 0x5A; // Response Interrupt Count (16-bit)
|
||||
constexpr uint32_t REG_RIRBCTL = 0x5C; // RIRB Control (8-bit)
|
||||
constexpr uint32_t REG_RIRBSTS = 0x5D; // RIRB Status (8-bit)
|
||||
constexpr uint32_t REG_RIRBSIZE = 0x5E; // RIRB Size (8-bit)
|
||||
|
||||
// Immediate command interface
|
||||
constexpr uint32_t REG_ICW = 0x60; // Immediate Command Write (32-bit)
|
||||
constexpr uint32_t REG_IRR = 0x64; // Immediate Response Read (32-bit)
|
||||
constexpr uint32_t REG_ICS = 0x68; // Immediate Command Status (16-bit)
|
||||
|
||||
// DMA Position Buffer
|
||||
constexpr uint32_t REG_DPIBLBASE = 0x70; // DMA Position Lower Base (32-bit)
|
||||
constexpr uint32_t REG_DPIBUBASE = 0x74; // DMA Position Upper Base (32-bit)
|
||||
|
||||
// GCTL register bits
|
||||
constexpr uint32_t GCTL_CRST = (1u << 0); // Controller Reset
|
||||
constexpr uint32_t GCTL_FCNTRL = (1u << 1); // Flush Control
|
||||
constexpr uint32_t GCTL_UNSOL = (1u << 8); // Accept Unsolicited Responses
|
||||
|
||||
// INTCTL register bits
|
||||
constexpr uint32_t INTCTL_GIE = (1u << 31); // Global Interrupt Enable
|
||||
constexpr uint32_t INTCTL_CIE = (1u << 30); // Controller Interrupt Enable
|
||||
|
||||
// CORBCTL register bits
|
||||
constexpr uint8_t CORBCTL_RUN = (1u << 1); // CORB DMA Engine Run
|
||||
constexpr uint8_t CORBCTL_MEIE = (1u << 0); // Memory Error Interrupt Enable
|
||||
|
||||
// RIRBCTL register bits
|
||||
constexpr uint8_t RIRBCTL_RUN = (1u << 1); // RIRB DMA Engine Run
|
||||
constexpr uint8_t RIRBCTL_RINTCTL = (1u << 0); // Response Interrupt Control
|
||||
|
||||
// RIRBSTS register bits
|
||||
constexpr uint8_t RIRBSTS_RINTFL = (1u << 0); // Response Interrupt
|
||||
|
||||
// CORBRP register bits
|
||||
constexpr uint16_t CORBRP_RST = (1u << 15); // CORB Read Pointer Reset
|
||||
|
||||
// RIRBWP register bits
|
||||
constexpr uint16_t RIRBWP_RST = (1u << 15); // RIRB Write Pointer Reset
|
||||
|
||||
// =========================================================================
|
||||
// Stream Descriptor registers (offset = 0x80 + streamIndex * 0x20)
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint32_t SD_BASE = 0x80;
|
||||
constexpr uint32_t SD_SIZE = 0x20;
|
||||
|
||||
// Stream descriptor register offsets (relative to stream base)
|
||||
constexpr uint32_t SD_CTL = 0x00; // Control (24-bit: bytes 0,1,2)
|
||||
constexpr uint32_t SD_STS = 0x03; // Status (8-bit)
|
||||
constexpr uint32_t SD_LPIB = 0x04; // Link Position in Current Buffer (32-bit)
|
||||
constexpr uint32_t SD_CBL = 0x08; // Cyclic Buffer Length (32-bit)
|
||||
constexpr uint32_t SD_LVI = 0x0C; // Last Valid Index (16-bit)
|
||||
constexpr uint32_t SD_FIFOS = 0x10; // FIFO Size (16-bit, read-only)
|
||||
constexpr uint32_t SD_FMT = 0x12; // Format (16-bit)
|
||||
constexpr uint32_t SD_BDPL = 0x18; // BDL Pointer Lower (32-bit)
|
||||
constexpr uint32_t SD_BDPU = 0x1C; // BDL Pointer Upper (32-bit)
|
||||
|
||||
// Stream control bits (CTL is 24-bit, accessed as 3 bytes)
|
||||
constexpr uint8_t SD_CTL0_SRST = (1u << 0); // Stream Reset
|
||||
constexpr uint8_t SD_CTL0_RUN = (1u << 1); // Stream Run
|
||||
constexpr uint8_t SD_CTL0_IOCE = (1u << 2); // Interrupt On Completion Enable
|
||||
constexpr uint8_t SD_CTL0_FEIE = (1u << 3); // FIFO Error Interrupt Enable
|
||||
constexpr uint8_t SD_CTL0_DEIE = (1u << 4); // Descriptor Error Interrupt Enable
|
||||
// CTL byte 2 (offset +2): bits 23:20 = Stream Number (1-15)
|
||||
|
||||
// Stream status bits
|
||||
constexpr uint8_t SD_STS_BCIS = (1u << 2); // Buffer Completion Interrupt Status
|
||||
constexpr uint8_t SD_STS_FIFOE = (1u << 3); // FIFO Error
|
||||
constexpr uint8_t SD_STS_DESE = (1u << 4); // Descriptor Error
|
||||
|
||||
// =========================================================================
|
||||
// Stream format register encoding (SD_FMT, 16-bit)
|
||||
// =========================================================================
|
||||
|
||||
// Bit 15: Stream Type (0=PCM, 1=non-PCM)
|
||||
// Bits 14: Base Rate (0=48kHz, 1=44.1kHz)
|
||||
// Bits 13:11: Sample Rate Multiplier (0=x1, 1=x2, 2=x3, 3=x4)
|
||||
// Bits 10:8: Sample Rate Divisor (0=/1, 1=/2, 2=/3, ..., 7=/8)
|
||||
// Bits 7:4: Bits Per Sample (000=8, 001=16, 010=20, 011=24, 100=32)
|
||||
// Bits 3:0: Number of Channels - 1
|
||||
|
||||
constexpr uint16_t FMT_BASE_44K = (1u << 14);
|
||||
constexpr uint16_t FMT_BASE_48K = 0;
|
||||
|
||||
// =========================================================================
|
||||
// Buffer Descriptor List Entry (16 bytes)
|
||||
// =========================================================================
|
||||
|
||||
struct BdlEntry {
|
||||
uint64_t Address; // Physical address of buffer
|
||||
uint32_t Length; // Length in bytes
|
||||
uint32_t Ioc; // Bit 0: Interrupt on Completion
|
||||
} __attribute__((packed));
|
||||
|
||||
static_assert(sizeof(BdlEntry) == 16, "BDL entry must be 16 bytes");
|
||||
|
||||
// =========================================================================
|
||||
// RIRB response entry (8 bytes)
|
||||
// =========================================================================
|
||||
|
||||
struct RirbEntry {
|
||||
uint32_t Response;
|
||||
uint32_t ResponseEx; // Bits 3:0 = Codec Address, Bit 4 = Unsolicited
|
||||
} __attribute__((packed));
|
||||
|
||||
static_assert(sizeof(RirbEntry) == 8, "RIRB entry must be 8 bytes");
|
||||
|
||||
// =========================================================================
|
||||
// HDA Codec verbs
|
||||
// =========================================================================
|
||||
|
||||
// Verb construction: (codec << 28) | (nid << 20) | verb
|
||||
// Get Parameter: verb = 0xF0000 | paramId
|
||||
// Set Converter Format: verb = 0x20000 | format
|
||||
// Set Amp Gain/Mute: verb = 0x30000 | payload
|
||||
// Set Converter Stream/Channel: verb = 0x70600 | (stream << 4) | channel
|
||||
// Set Pin Widget Control: verb = 0x70700 | value
|
||||
// Set EAPD/BTL Enable: verb = 0x70C00 | value
|
||||
// Set Power State: verb = 0x70500 | state
|
||||
// Get Config Default: verb = 0xF1C00
|
||||
// Set Connection Select: verb = 0x70100 | index
|
||||
|
||||
// Parameter IDs
|
||||
constexpr uint32_t PARAM_VENDOR_ID = 0x00;
|
||||
constexpr uint32_t PARAM_REVISION_ID = 0x02;
|
||||
constexpr uint32_t PARAM_SUB_NODE_COUNT = 0x04;
|
||||
constexpr uint32_t PARAM_FN_GROUP_TYPE = 0x05;
|
||||
constexpr uint32_t PARAM_AUDIO_WIDGET_CAP = 0x09;
|
||||
constexpr uint32_t PARAM_PCM_RATES = 0x0A;
|
||||
constexpr uint32_t PARAM_STREAM_FORMATS = 0x0B;
|
||||
constexpr uint32_t PARAM_PIN_CAPS = 0x0C;
|
||||
constexpr uint32_t PARAM_INPUT_AMP_CAP = 0x0D;
|
||||
constexpr uint32_t PARAM_CONN_LIST_LEN = 0x0E;
|
||||
constexpr uint32_t PARAM_POWER_STATES = 0x0F;
|
||||
constexpr uint32_t PARAM_OUTPUT_AMP_CAP = 0x12;
|
||||
|
||||
// Widget types (bits 23:20 of Audio Widget Capabilities)
|
||||
constexpr uint8_t WIDGET_AUDIO_OUTPUT = 0x0;
|
||||
constexpr uint8_t WIDGET_AUDIO_INPUT = 0x1;
|
||||
constexpr uint8_t WIDGET_AUDIO_MIXER = 0x2;
|
||||
constexpr uint8_t WIDGET_AUDIO_SELECTOR = 0x3;
|
||||
constexpr uint8_t WIDGET_PIN_COMPLEX = 0x4;
|
||||
constexpr uint8_t WIDGET_POWER = 0x5;
|
||||
constexpr uint8_t WIDGET_VOLUME_KNOB = 0x6;
|
||||
constexpr uint8_t WIDGET_BEEP_GEN = 0x7;
|
||||
constexpr uint8_t WIDGET_VENDOR_DEFINED = 0xF;
|
||||
|
||||
// Pin default config: device type (bits 23:20)
|
||||
constexpr uint8_t PIN_DEV_LINE_OUT = 0x0;
|
||||
constexpr uint8_t PIN_DEV_SPEAKER = 0x1;
|
||||
constexpr uint8_t PIN_DEV_HP_OUT = 0x2;
|
||||
constexpr uint8_t PIN_DEV_CD = 0x3;
|
||||
constexpr uint8_t PIN_DEV_SPDIF_OUT = 0x4;
|
||||
constexpr uint8_t PIN_DEV_LINE_IN = 0x8;
|
||||
constexpr uint8_t PIN_DEV_MIC_IN = 0xA;
|
||||
|
||||
// Pin widget control bits
|
||||
constexpr uint8_t PIN_CTL_ENABLE_OUTPUT = (1u << 6); // OUT Enable
|
||||
constexpr uint8_t PIN_CTL_ENABLE_INPUT = (1u << 5); // IN Enable
|
||||
constexpr uint8_t PIN_CTL_ENABLE_HP = (1u << 7); // Headphone enable
|
||||
|
||||
// EAPD/BTL bits
|
||||
constexpr uint8_t EAPD_ENABLE = (1u << 1);
|
||||
|
||||
// Amp gain/mute verb payload bits
|
||||
constexpr uint16_t AMP_SET_OUTPUT = (1u << 15);
|
||||
constexpr uint16_t AMP_SET_INPUT = (1u << 14);
|
||||
constexpr uint16_t AMP_SET_LEFT = (1u << 13);
|
||||
constexpr uint16_t AMP_SET_RIGHT = (1u << 12);
|
||||
constexpr uint16_t AMP_MUTE = (1u << 7);
|
||||
|
||||
// =========================================================================
|
||||
// Ring buffer sizes
|
||||
// =========================================================================
|
||||
|
||||
constexpr int CORB_ENTRIES = 256; // 256 entries * 4 bytes = 1024 bytes
|
||||
constexpr int RIRB_ENTRIES = 256; // 256 entries * 8 bytes = 2048 bytes
|
||||
constexpr int BDL_MAX_ENTRIES = 256; // Max BDL entries per stream
|
||||
|
||||
// =========================================================================
|
||||
// DMA buffer configuration
|
||||
// =========================================================================
|
||||
|
||||
constexpr int BUFFER_COUNT = 2; // Double-buffered
|
||||
constexpr int BUFFER_SIZE = 0x4000; // 16 KiB per buffer segment
|
||||
constexpr int TOTAL_BUFFER_SIZE = BUFFER_COUNT * BUFFER_SIZE;
|
||||
|
||||
// =========================================================================
|
||||
// MSI configuration
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint8_t MSI_IRQ = 27; // IRQ slot 27 = vector 59
|
||||
constexpr uint32_t MSI_VECTOR = 59;
|
||||
constexpr uint32_t MSI_ADDR_BASE = 0xFEE00000;
|
||||
|
||||
// =========================================================================
|
||||
// Audio stream state (one active output stream)
|
||||
// =========================================================================
|
||||
|
||||
struct AudioStream {
|
||||
bool Active;
|
||||
uint32_t SampleRate;
|
||||
uint8_t Channels;
|
||||
uint8_t BitsPerSample;
|
||||
uint8_t StreamIndex; // HDA stream index
|
||||
uint8_t StreamTag; // HDA stream tag (1-15)
|
||||
volatile uint32_t WritePos; // Write position in ring buffer (bytes)
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// Public API
|
||||
// =========================================================================
|
||||
|
||||
bool Probe(const Pci::PciDevice& dev);
|
||||
bool IsInitialized();
|
||||
|
||||
// Returns the codec vendor/device ID (vendor in upper 16 bits, device in lower 16).
|
||||
// Returns 0 if no codec was found.
|
||||
uint32_t GetCodecVendorId();
|
||||
|
||||
// Open an output stream. Returns stream handle (0) or -1 on failure.
|
||||
int Open(uint32_t sampleRate, uint8_t channels, uint8_t bitsPerSample);
|
||||
|
||||
// Close an output stream.
|
||||
void Close(int handle);
|
||||
|
||||
// Write PCM sample data to the stream buffer.
|
||||
// Returns number of bytes written (may be less than requested if buffer full).
|
||||
int Write(int handle, const uint8_t* data, uint32_t size);
|
||||
|
||||
// Control commands
|
||||
constexpr int AUDIO_CTL_SET_VOLUME = 0; // value: 0-100
|
||||
constexpr int AUDIO_CTL_GET_VOLUME = 1;
|
||||
constexpr int AUDIO_CTL_GET_POS = 2; // returns playback position in bytes
|
||||
constexpr int AUDIO_CTL_PAUSE = 3; // value: 1=pause, 0=resume
|
||||
int Control(int handle, int cmd, int value);
|
||||
|
||||
};
|
||||
@@ -11,7 +11,9 @@
|
||||
#include <Drivers/Net/E1000E.hpp>
|
||||
#include <Drivers/USB/Xhci.hpp>
|
||||
#include <Drivers/Storage/Ahci.hpp>
|
||||
#include <Drivers/Storage/Nvme.hpp>
|
||||
#include <Drivers/Storage/Gpt.hpp>
|
||||
#include <Drivers/Audio/IntelHda.hpp>
|
||||
#include <Graphics/Cursor.hpp>
|
||||
#include <Net/Net.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
@@ -65,6 +67,14 @@ namespace Drivers {
|
||||
return Storage::Ahci::Probe(dev);
|
||||
}
|
||||
|
||||
static bool ProbeNvme(const Pci::PciDevice& dev) {
|
||||
return Storage::Nvme::Probe(dev);
|
||||
}
|
||||
|
||||
static bool ProbeIntelHda(const Pci::PciDevice& dev) {
|
||||
return Audio::IntelHda::Probe(dev);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Driver table
|
||||
// -------------------------------------------------------------------------
|
||||
@@ -126,6 +136,32 @@ namespace Drivers {
|
||||
Pci::ProbePhase::Normal,
|
||||
ProbeAhci,
|
||||
},
|
||||
// Order 6: NVMe — Normal phase, match class=0x01/0x08/0x02 (NVM Express)
|
||||
{
|
||||
"NVMe",
|
||||
0, // VendorId (any)
|
||||
0x01, // ClassCode (Mass Storage)
|
||||
0x08, // SubClass (Non-Volatile Memory)
|
||||
0x02, // ProgIf (NVM Express)
|
||||
nullptr,
|
||||
0,
|
||||
Pci::ProbePhase::Normal,
|
||||
ProbeNvme,
|
||||
},
|
||||
// Order 7: Intel HDA — Normal phase, match vendor=0x8086 + class=0x04 (Multimedia)
|
||||
// SubClass 0x01 = "Multimedia audio controller" (most Intel HDA)
|
||||
// SubClass 0x03 = "Audio device" (HDA-compatible)
|
||||
{
|
||||
"IntelHDA",
|
||||
0x8086, // VendorId (Intel)
|
||||
0x04, // ClassCode (Multimedia)
|
||||
0xFF, // SubClass (any — covers both 0x01 and 0x03)
|
||||
0xFF, // ProgIf (any)
|
||||
nullptr,
|
||||
0,
|
||||
Pci::ProbePhase::Normal,
|
||||
ProbeIntelHda,
|
||||
},
|
||||
};
|
||||
|
||||
static constexpr uint16_t g_driverTableCount = sizeof(g_driverTable) / sizeof(g_driverTable[0]);
|
||||
@@ -163,4 +199,10 @@ namespace Drivers {
|
||||
Storage::Gpt::ProbeAll();
|
||||
}
|
||||
|
||||
void InitializeAudio() {
|
||||
// HDA driver initializes during ProbeNormal().
|
||||
// Bluetooth audio (A2DP) initializes automatically during USB enumeration
|
||||
// when a Bluetooth adapter is detected by the xHCI driver.
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -14,7 +14,8 @@ namespace Drivers {
|
||||
// Post-probe: wire up GPU framebuffer to cursor subsystem.
|
||||
void InitializeGraphics();
|
||||
|
||||
// Probe PCI devices for Normal-phase drivers (xHCI, E1000, E1000E, AHCI).
|
||||
// Probe PCI devices for Normal-phase drivers (xHCI, E1000, E1000E, AHCI, NVMe, IntelHDA).
|
||||
// USB class drivers (Bluetooth, etc.) initialize automatically during xHCI enumeration.
|
||||
void ProbeNormal();
|
||||
|
||||
// Post-probe: initialize network stack.
|
||||
@@ -23,4 +24,7 @@ namespace Drivers {
|
||||
// Post-probe: register SATA drives with VFS.
|
||||
void InitializeStorage();
|
||||
|
||||
// Post-probe: initialize audio subsystem.
|
||||
void InitializeAudio();
|
||||
|
||||
}
|
||||
|
||||
@@ -0,0 +1,829 @@
|
||||
/*
|
||||
* Nvme.cpp
|
||||
* NVM Express (NVMe) storage driver
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#include "Nvme.hpp"
|
||||
#include "BlockDevice.hpp"
|
||||
#include <Pci/Pci.hpp>
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Memory/HHDM.hpp>
|
||||
#include <Memory/Paging.hpp>
|
||||
#include <Memory/PageFrameAllocator.hpp>
|
||||
#include <Libraries/Memory.hpp>
|
||||
#include <Hal/Apic/Interrupts.hpp>
|
||||
#include <Hal/Apic/IoApic.hpp>
|
||||
|
||||
using namespace Kt;
|
||||
|
||||
namespace Drivers::Storage::Nvme {
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Driver state
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static bool g_initialized = false;
|
||||
static volatile uint8_t* g_mmioBase = nullptr;
|
||||
static uint32_t g_doorbellStride = 0; // in bytes (4 << DSTRD)
|
||||
static uint16_t g_maxQueueEntries = 0; // CAP.MQES + 1
|
||||
|
||||
// Admin queue state
|
||||
static SqEntry* g_adminSq = nullptr;
|
||||
static CqEntry* g_adminCq = nullptr;
|
||||
static uint64_t g_adminSqPhys = 0;
|
||||
static uint64_t g_adminCqPhys = 0;
|
||||
static uint16_t g_adminSqTail = 0;
|
||||
static uint16_t g_adminCqHead = 0;
|
||||
static uint8_t g_adminCqPhase = 1; // Expected phase bit starts at 1
|
||||
static uint16_t g_adminCmdId = 0;
|
||||
|
||||
// I/O queue state (single I/O queue pair)
|
||||
static SqEntry* g_ioSq = nullptr;
|
||||
static CqEntry* g_ioCq = nullptr;
|
||||
static uint64_t g_ioSqPhys = 0;
|
||||
static uint64_t g_ioCqPhys = 0;
|
||||
static uint16_t g_ioSqTail = 0;
|
||||
static uint16_t g_ioCqHead = 0;
|
||||
static uint8_t g_ioCqPhase = 1;
|
||||
static uint16_t g_ioCmdId = 0;
|
||||
static uint16_t g_ioSqDepth = 0;
|
||||
static uint16_t g_ioCqDepth = 0;
|
||||
|
||||
// Namespace state
|
||||
static int g_nsCount = 0;
|
||||
static NamespaceInfo g_namespaces[MAX_NAMESPACES] = {};
|
||||
|
||||
// Controller identify data
|
||||
static uint32_t g_mdts = 0; // Max Data Transfer Size in pages (0 = unlimited)
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Register access
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static void WriteReg32(uint32_t reg, uint32_t value) {
|
||||
*(volatile uint32_t*)(g_mmioBase + reg) = value;
|
||||
}
|
||||
|
||||
static uint32_t ReadReg32(uint32_t reg) {
|
||||
return *(volatile uint32_t*)(g_mmioBase + reg);
|
||||
}
|
||||
|
||||
static void WriteReg64(uint32_t reg, uint64_t value) {
|
||||
*(volatile uint32_t*)(g_mmioBase + reg) = (uint32_t)(value & 0xFFFFFFFF);
|
||||
*(volatile uint32_t*)(g_mmioBase + reg + 4) = (uint32_t)(value >> 32);
|
||||
}
|
||||
|
||||
static uint64_t ReadReg64(uint32_t reg) {
|
||||
uint32_t lo = *(volatile uint32_t*)(g_mmioBase + reg);
|
||||
uint32_t hi = *(volatile uint32_t*)(g_mmioBase + reg + 4);
|
||||
return (uint64_t)lo | ((uint64_t)hi << 32);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Doorbell registers
|
||||
// NVMe spec: SQ y Tail Doorbell offset = 0x1000 + (2y * doorbellStride)
|
||||
// CQ y Head Doorbell offset = 0x1000 + ((2y+1) * doorbellStride)
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static void WriteSqTailDoorbell(uint16_t queueId, uint16_t value) {
|
||||
uint32_t offset = 0x1000 + (2 * queueId) * g_doorbellStride;
|
||||
*(volatile uint32_t*)(g_mmioBase + offset) = value;
|
||||
}
|
||||
|
||||
static void WriteCqHeadDoorbell(uint16_t queueId, uint16_t value) {
|
||||
uint32_t offset = 0x1000 + (2 * queueId + 1) * g_doorbellStride;
|
||||
*(volatile uint32_t*)(g_mmioBase + offset) = value;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// DMA buffer allocation
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static void* AllocateDmaBuffer(uint64_t& outPhys, int pages = 1) {
|
||||
void* virt;
|
||||
if (pages == 1) {
|
||||
virt = Memory::g_pfa->AllocateZeroed();
|
||||
} else {
|
||||
virt = Memory::g_pfa->ReallocConsecutive(nullptr, pages);
|
||||
memset(virt, 0, pages * 0x1000);
|
||||
}
|
||||
outPhys = Memory::SubHHDM(virt);
|
||||
return virt;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Admin command submission
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static void SubmitAdminCommand(SqEntry& cmd) {
|
||||
cmd.CommandId = g_adminCmdId++;
|
||||
g_adminSq[g_adminSqTail] = cmd;
|
||||
g_adminSqTail = (g_adminSqTail + 1) % ADMIN_QUEUE_DEPTH;
|
||||
WriteSqTailDoorbell(0, g_adminSqTail);
|
||||
}
|
||||
|
||||
static bool WaitAdminCompletion(CqEntry& out) {
|
||||
for (int i = 0; i < 5000000; i++) {
|
||||
CqEntry* cqe = &g_adminCq[g_adminCqHead];
|
||||
uint16_t status = cqe->Status;
|
||||
|
||||
// Check phase bit matches expected
|
||||
if ((status & CQE_PHASE_BIT) == g_adminCqPhase) {
|
||||
out = *cqe;
|
||||
|
||||
// Advance CQ head
|
||||
g_adminCqHead++;
|
||||
if (g_adminCqHead >= ADMIN_QUEUE_DEPTH) {
|
||||
g_adminCqHead = 0;
|
||||
g_adminCqPhase ^= 1; // Toggle expected phase
|
||||
}
|
||||
WriteCqHeadDoorbell(0, g_adminCqHead);
|
||||
|
||||
// Check status code (bits 15:1, 0 = success)
|
||||
if (status & CQE_STATUS_MASK) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Admin command failed, status="
|
||||
<< base::hex << (uint64_t)(status >> 1);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
|
||||
KernelLogStream(ERROR, "NVMe") << "Admin command timeout";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Submit an admin command and wait for completion
|
||||
static bool AdminCommand(SqEntry& cmd, CqEntry& cqe) {
|
||||
SubmitAdminCommand(cmd);
|
||||
return WaitAdminCompletion(cqe);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// I/O command submission
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static void SubmitIoCommand(SqEntry& cmd) {
|
||||
cmd.CommandId = g_ioCmdId++;
|
||||
g_ioSq[g_ioSqTail] = cmd;
|
||||
g_ioSqTail = (g_ioSqTail + 1) % g_ioSqDepth;
|
||||
WriteSqTailDoorbell(1, g_ioSqTail);
|
||||
}
|
||||
|
||||
static bool WaitIoCompletion(CqEntry& out) {
|
||||
for (int i = 0; i < 5000000; i++) {
|
||||
CqEntry* cqe = &g_ioCq[g_ioCqHead];
|
||||
uint16_t status = cqe->Status;
|
||||
|
||||
if ((status & CQE_PHASE_BIT) == g_ioCqPhase) {
|
||||
out = *cqe;
|
||||
|
||||
g_ioCqHead++;
|
||||
if (g_ioCqHead >= g_ioCqDepth) {
|
||||
g_ioCqHead = 0;
|
||||
g_ioCqPhase ^= 1;
|
||||
}
|
||||
WriteCqHeadDoorbell(1, g_ioCqHead);
|
||||
|
||||
if (status & CQE_STATUS_MASK) {
|
||||
KernelLogStream(ERROR, "NVMe") << "I/O command failed, status="
|
||||
<< base::hex << (uint64_t)(status >> 1);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
|
||||
KernelLogStream(ERROR, "NVMe") << "I/O command timeout";
|
||||
return false;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Controller disable/enable
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static bool DisableController() {
|
||||
uint32_t cc = ReadReg32(REG_CC);
|
||||
cc &= ~CC_EN;
|
||||
WriteReg32(REG_CC, cc);
|
||||
|
||||
// Wait for CSTS.RDY to become 0
|
||||
for (int i = 0; i < 5000000; i++) {
|
||||
uint32_t csts = ReadReg32(REG_CSTS);
|
||||
if (csts & CSTS_CFS) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Controller fatal status during disable";
|
||||
return false;
|
||||
}
|
||||
if (!(csts & CSTS_RDY)) {
|
||||
return true;
|
||||
}
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
|
||||
KernelLogStream(ERROR, "NVMe") << "Controller disable timeout";
|
||||
return false;
|
||||
}
|
||||
|
||||
static bool EnableController() {
|
||||
// Configure CC: enable, NVM command set, 4 KiB pages,
|
||||
// SQ entry size = 64 bytes (2^6), CQ entry size = 16 bytes (2^4)
|
||||
uint32_t cc = CC_EN | CC_CSS_NVM | CC_AMS_RR | CC_SHN_NONE;
|
||||
cc |= (0u << CC_MPS_SHIFT); // MPS = 0 => 4 KiB pages
|
||||
cc |= (6u << CC_IOSQES_SHIFT); // SQ entry = 2^6 = 64 bytes
|
||||
cc |= (4u << CC_IOCQES_SHIFT); // CQ entry = 2^4 = 16 bytes
|
||||
WriteReg32(REG_CC, cc);
|
||||
|
||||
// Wait for CSTS.RDY
|
||||
for (int i = 0; i < 5000000; i++) {
|
||||
uint32_t csts = ReadReg32(REG_CSTS);
|
||||
if (csts & CSTS_CFS) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Controller fatal status during enable";
|
||||
return false;
|
||||
}
|
||||
if (csts & CSTS_RDY) {
|
||||
return true;
|
||||
}
|
||||
asm volatile("" ::: "memory");
|
||||
}
|
||||
|
||||
KernelLogStream(ERROR, "NVMe") << "Controller enable timeout";
|
||||
return false;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Admin queue setup
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static bool SetupAdminQueues() {
|
||||
// Allocate Admin Submission Queue (ADMIN_QUEUE_DEPTH * 64 bytes)
|
||||
// At 32 entries * 64 bytes = 2048 bytes, fits in 1 page
|
||||
g_adminSq = (SqEntry*)AllocateDmaBuffer(g_adminSqPhys);
|
||||
|
||||
// Allocate Admin Completion Queue (ADMIN_QUEUE_DEPTH * 16 bytes)
|
||||
// At 32 entries * 16 bytes = 512 bytes, fits in 1 page
|
||||
g_adminCq = (CqEntry*)AllocateDmaBuffer(g_adminCqPhys);
|
||||
|
||||
g_adminSqTail = 0;
|
||||
g_adminCqHead = 0;
|
||||
g_adminCqPhase = 1;
|
||||
g_adminCmdId = 0;
|
||||
|
||||
// Set Admin Queue Attributes
|
||||
// AQA: bits 27:16 = ACQS (CQ size - 1), bits 11:0 = ASQS (SQ size - 1)
|
||||
uint32_t aqa = ((ADMIN_QUEUE_DEPTH - 1) << 16) | (ADMIN_QUEUE_DEPTH - 1);
|
||||
WriteReg32(REG_AQA, aqa);
|
||||
|
||||
// Set Admin SQ and CQ base addresses
|
||||
WriteReg64(REG_ASQ, g_adminSqPhys);
|
||||
WriteReg64(REG_ACQ, g_adminCqPhys);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Identify Controller
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static bool IdentifyController() {
|
||||
uint64_t identPhys;
|
||||
uint8_t* identData = (uint8_t*)AllocateDmaBuffer(identPhys);
|
||||
|
||||
SqEntry cmd = {};
|
||||
cmd.Opcode = ADMIN_IDENTIFY;
|
||||
cmd.Nsid = 0;
|
||||
cmd.Prp1 = identPhys;
|
||||
cmd.Prp2 = 0;
|
||||
cmd.Cdw10 = IDENTIFY_CNS_CONTROLLER;
|
||||
|
||||
CqEntry cqe;
|
||||
if (!AdminCommand(cmd, cqe)) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Identify Controller failed";
|
||||
Memory::g_pfa->Free(identData);
|
||||
return false;
|
||||
}
|
||||
|
||||
// Parse controller data (4096 bytes)
|
||||
// Bytes 24-63: Serial Number (20 bytes, space-padded ASCII)
|
||||
// Bytes 64-103: Model Number (40 bytes, space-padded ASCII)
|
||||
// Bytes 104-111: Firmware Revision (8 bytes)
|
||||
// Byte 77: MDTS (Maximum Data Transfer Size, in units of CAP.MPSMIN pages)
|
||||
|
||||
// Model string
|
||||
char model[41];
|
||||
memcpy(model, identData + 24, 40);
|
||||
model[40] = '\0';
|
||||
// Trim trailing spaces
|
||||
for (int i = 39; i >= 0; i--) {
|
||||
if (model[i] == ' ' || model[i] == '\0') {
|
||||
model[i] = '\0';
|
||||
} else {
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// MDTS
|
||||
uint8_t mdtsPower = identData[77];
|
||||
if (mdtsPower > 0) {
|
||||
g_mdts = (1u << mdtsPower); // In minimum page size units (4 KiB pages)
|
||||
} else {
|
||||
g_mdts = 0; // No limit
|
||||
}
|
||||
|
||||
// NN (Number of Namespaces): bytes 516-519
|
||||
uint32_t nn = *(uint32_t*)(identData + 516);
|
||||
|
||||
KernelLogStream(OK, "NVMe") << "Controller: " << model;
|
||||
KernelLogStream(INFO, "NVMe") << "MDTS: " << (g_mdts ? (uint64_t)(g_mdts * 4) : 0)
|
||||
<< " KiB, Namespaces: " << (uint64_t)nn;
|
||||
|
||||
// Copy model to all namespaces (will be used during registration)
|
||||
for (int i = 0; i < MAX_NAMESPACES; i++) {
|
||||
memcpy(g_namespaces[i].Model, model, 41);
|
||||
}
|
||||
|
||||
// Enumerate active namespaces
|
||||
if (nn > (uint32_t)MAX_NAMESPACES) nn = MAX_NAMESPACES;
|
||||
|
||||
for (uint32_t nsid = 1; nsid <= nn; nsid++) {
|
||||
// Identify Namespace
|
||||
uint64_t nsIdentPhys;
|
||||
uint8_t* nsIdentData = (uint8_t*)AllocateDmaBuffer(nsIdentPhys);
|
||||
|
||||
SqEntry nsCmd = {};
|
||||
nsCmd.Opcode = ADMIN_IDENTIFY;
|
||||
nsCmd.Nsid = nsid;
|
||||
nsCmd.Prp1 = nsIdentPhys;
|
||||
nsCmd.Prp2 = 0;
|
||||
nsCmd.Cdw10 = IDENTIFY_CNS_NAMESPACE;
|
||||
|
||||
CqEntry nsCqe;
|
||||
if (!AdminCommand(nsCmd, nsCqe)) {
|
||||
KernelLogStream(WARNING, "NVMe") << "Identify Namespace " << (uint64_t)nsid << " failed";
|
||||
Memory::g_pfa->Free(nsIdentData);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Bytes 0-7: NSZE (Namespace Size in LBAs)
|
||||
uint64_t nsze = *(uint64_t*)(nsIdentData + 0);
|
||||
|
||||
if (nsze == 0) {
|
||||
Memory::g_pfa->Free(nsIdentData);
|
||||
continue;
|
||||
}
|
||||
|
||||
// Byte 26: NLBAF (Number of LBA Formats, 0-based)
|
||||
// Byte 25:24 - FLBAS (Formatted LBA Size)
|
||||
// bits 3:0 = index of the LBA format in use
|
||||
uint8_t flbas = nsIdentData[26];
|
||||
uint8_t lbaFmtIdx = flbas & 0x0F;
|
||||
|
||||
// LBA Format descriptors start at byte 128, each is 4 bytes
|
||||
// Bits 23:16 = LBADS (LBA Data Size as power of 2)
|
||||
uint32_t lbaFmt = *(uint32_t*)(nsIdentData + 128 + lbaFmtIdx * 4);
|
||||
uint8_t lbads = (lbaFmt >> 16) & 0xFF;
|
||||
uint32_t sectorSize = (1u << lbads);
|
||||
|
||||
int idx = g_nsCount;
|
||||
g_namespaces[idx].Active = true;
|
||||
g_namespaces[idx].Nsid = nsid;
|
||||
g_namespaces[idx].SectorCount = nsze;
|
||||
g_namespaces[idx].SectorSize = sectorSize;
|
||||
|
||||
// Compute max transfer in blocks
|
||||
if (g_mdts > 0) {
|
||||
g_namespaces[idx].MaxTransferBlocks = (g_mdts * 0x1000) / sectorSize;
|
||||
} else {
|
||||
// Conservative default: 128 sectors (64 KiB for 512-byte sectors)
|
||||
g_namespaces[idx].MaxTransferBlocks = 128;
|
||||
}
|
||||
|
||||
g_nsCount++;
|
||||
|
||||
uint64_t sizeBytes = nsze * sectorSize;
|
||||
uint64_t sizeMB = sizeBytes / (1024 * 1024);
|
||||
uint64_t sizeGB = sizeMB / 1024;
|
||||
|
||||
if (sizeGB > 0) {
|
||||
KernelLogStream(OK, "NVMe") << "Namespace " << (uint64_t)nsid
|
||||
<< ": " << sizeGB << " GiB (" << (uint64_t)sectorSize << " B/sector)";
|
||||
} else {
|
||||
KernelLogStream(OK, "NVMe") << "Namespace " << (uint64_t)nsid
|
||||
<< ": " << sizeMB << " MiB (" << (uint64_t)sectorSize << " B/sector)";
|
||||
}
|
||||
|
||||
Memory::g_pfa->Free(nsIdentData);
|
||||
}
|
||||
|
||||
Memory::g_pfa->Free(identData);
|
||||
return g_nsCount > 0;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Create I/O queues
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static bool SetNumberOfQueues(uint16_t& sqCount, uint16_t& cqCount) {
|
||||
SqEntry cmd = {};
|
||||
cmd.Opcode = ADMIN_SET_FEATURES;
|
||||
cmd.Cdw10 = FEATURE_NUM_QUEUES;
|
||||
// CDW11: bits 31:16 = number of CQs requested (0-based)
|
||||
// bits 15:0 = number of SQs requested (0-based)
|
||||
cmd.Cdw11 = ((uint32_t)(cqCount - 1) << 16) | (sqCount - 1);
|
||||
|
||||
CqEntry cqe;
|
||||
if (!AdminCommand(cmd, cqe)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Result DW0 contains allocated counts (0-based)
|
||||
sqCount = (cqe.Result & 0xFFFF) + 1;
|
||||
cqCount = ((cqe.Result >> 16) & 0xFFFF) + 1;
|
||||
return true;
|
||||
}
|
||||
|
||||
static bool CreateIoQueues() {
|
||||
// Request 1 SQ + 1 CQ
|
||||
uint16_t sqCount = 1;
|
||||
uint16_t cqCount = 1;
|
||||
if (!SetNumberOfQueues(sqCount, cqCount)) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Set Number of Queues failed";
|
||||
return false;
|
||||
}
|
||||
|
||||
KernelLogStream(INFO, "NVMe") << "Allocated " << (uint64_t)sqCount
|
||||
<< " SQ(s), " << (uint64_t)cqCount << " CQ(s)";
|
||||
|
||||
// Determine queue depths (capped by controller max)
|
||||
g_ioSqDepth = IO_QUEUE_DEPTH;
|
||||
g_ioCqDepth = IO_QUEUE_DEPTH;
|
||||
if (g_ioSqDepth > g_maxQueueEntries) g_ioSqDepth = g_maxQueueEntries;
|
||||
if (g_ioCqDepth > g_maxQueueEntries) g_ioCqDepth = g_maxQueueEntries;
|
||||
|
||||
// Allocate I/O CQ
|
||||
int cqPages = ((uint32_t)g_ioCqDepth * sizeof(CqEntry) + 0xFFF) / 0x1000;
|
||||
g_ioCq = (CqEntry*)AllocateDmaBuffer(g_ioCqPhys, cqPages);
|
||||
g_ioCqHead = 0;
|
||||
g_ioCqPhase = 1;
|
||||
|
||||
// Create I/O Completion Queue (queue ID = 1)
|
||||
{
|
||||
SqEntry cmd = {};
|
||||
cmd.Opcode = ADMIN_CREATE_IO_CQ;
|
||||
cmd.Prp1 = g_ioCqPhys;
|
||||
// CDW10: bits 31:16 = queue size (0-based), bits 15:0 = queue ID
|
||||
cmd.Cdw10 = ((uint32_t)(g_ioCqDepth - 1) << 16) | 1;
|
||||
// CDW11: bit 0 = physically contiguous, bit 1 = interrupts enabled
|
||||
// bits 31:16 = interrupt vector
|
||||
cmd.Cdw11 = (1u << 0) | (1u << 1) | (0u << 16);
|
||||
|
||||
CqEntry cqe;
|
||||
if (!AdminCommand(cmd, cqe)) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Create I/O CQ failed";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
// Allocate I/O SQ
|
||||
int sqPages = ((uint32_t)g_ioSqDepth * sizeof(SqEntry) + 0xFFF) / 0x1000;
|
||||
g_ioSq = (SqEntry*)AllocateDmaBuffer(g_ioSqPhys, sqPages);
|
||||
g_ioSqTail = 0;
|
||||
g_ioCmdId = 0;
|
||||
|
||||
// Create I/O Submission Queue (queue ID = 1, linked to CQ ID = 1)
|
||||
{
|
||||
SqEntry cmd = {};
|
||||
cmd.Opcode = ADMIN_CREATE_IO_SQ;
|
||||
cmd.Prp1 = g_ioSqPhys;
|
||||
// CDW10: bits 31:16 = queue size (0-based), bits 15:0 = queue ID
|
||||
cmd.Cdw10 = ((uint32_t)(g_ioSqDepth - 1) << 16) | 1;
|
||||
// CDW11: bit 0 = physically contiguous, bits 31:16 = CQ ID
|
||||
cmd.Cdw11 = (1u << 0) | (1u << 16);
|
||||
|
||||
CqEntry cqe;
|
||||
if (!AdminCommand(cmd, cqe)) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Create I/O SQ failed";
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
KernelLogStream(OK, "NVMe") << "I/O queues created (depth " << (uint64_t)g_ioSqDepth << ")";
|
||||
return true;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Interrupt handler
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static void HandleInterrupt(uint8_t irq) {
|
||||
(void)irq;
|
||||
// NVMe uses polling-based completion in this driver.
|
||||
// The interrupt handler just acknowledges the interrupt.
|
||||
// Completions are consumed in the polling loops above.
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// MSI setup
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
static bool SetupMsi(uint8_t bus, uint8_t dev, uint8_t func) {
|
||||
uint8_t cap = Pci::FindCapability(bus, dev, func, Pci::PCI_CAP_MSI);
|
||||
if (cap == 0) {
|
||||
KernelLogStream(INFO, "NVMe") << "MSI capability not found";
|
||||
return false;
|
||||
}
|
||||
|
||||
uint16_t msgCtrl = Pci::LegacyRead16(bus, dev, func, cap + 2);
|
||||
bool is64bit = (msgCtrl & (1 << 7)) != 0;
|
||||
|
||||
Pci::LegacyWrite32(bus, dev, func, cap + 4, MSI_ADDR_BASE);
|
||||
|
||||
if (is64bit) {
|
||||
Pci::LegacyWrite32(bus, dev, func, cap + 8, 0);
|
||||
Pci::LegacyWrite16(bus, dev, func, cap + 12, MSI_VECTOR);
|
||||
} else {
|
||||
Pci::LegacyWrite16(bus, dev, func, cap + 8, MSI_VECTOR);
|
||||
}
|
||||
|
||||
msgCtrl &= ~(0x70); // Single message
|
||||
msgCtrl |= (1 << 0); // MSI Enable
|
||||
Pci::LegacyWrite16(bus, dev, func, cap + 2, msgCtrl);
|
||||
|
||||
uint16_t pciCmd = Pci::LegacyRead16(bus, dev, func, (uint8_t)Pci::PCI_REG_COMMAND);
|
||||
pciCmd |= Pci::PCI_CMD_INTX_DISABLE;
|
||||
Pci::LegacyWrite16(bus, dev, func, (uint8_t)Pci::PCI_REG_COMMAND, pciCmd);
|
||||
|
||||
Hal::RegisterIrqHandler(MSI_IRQ, HandleInterrupt);
|
||||
|
||||
KernelLogStream(OK, "NVMe") << "MSI enabled: vector " << base::dec << (uint64_t)MSI_VECTOR;
|
||||
return true;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Probe (PCI driver entry point)
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
bool Probe(const Pci::PciDevice& dev) {
|
||||
if (g_initialized) return false;
|
||||
|
||||
KernelLogStream(OK, "NVMe") << "Found NVMe controller at PCI "
|
||||
<< base::hex << (uint64_t)dev.Bus << ":"
|
||||
<< (uint64_t)dev.Device << "." << (uint64_t)dev.Function
|
||||
<< " (" << (uint64_t)dev.VendorId << ":" << (uint64_t)dev.DeviceId << ")";
|
||||
|
||||
// NVMe uses BAR0 for MMIO registers
|
||||
uint64_t mmioPhys = Pci::ReadBar0(dev.Bus, dev.Device, dev.Function);
|
||||
if (mmioPhys == 0) {
|
||||
KernelLogStream(ERROR, "NVMe") << "BAR0 is zero";
|
||||
return false;
|
||||
}
|
||||
|
||||
KernelLogStream(INFO, "NVMe") << "BAR0 physical: " << base::hex << mmioPhys;
|
||||
|
||||
// Map MMIO region. NVMe requires at least the controller registers (0x1000)
|
||||
// plus doorbell registers. Map 16 KiB to cover admin + a few I/O doorbells.
|
||||
constexpr uint64_t MmioSize = 0x4000;
|
||||
for (uint64_t offset = 0; offset < MmioSize; offset += 0x1000) {
|
||||
Memory::VMM::g_paging->MapMMIO(mmioPhys + offset, Memory::HHDM(mmioPhys + offset));
|
||||
}
|
||||
|
||||
g_mmioBase = (volatile uint8_t*)Memory::HHDM(mmioPhys);
|
||||
|
||||
// Enable bus mastering and memory space
|
||||
Pci::EnableBusMaster(dev.Bus, dev.Device, dev.Function);
|
||||
|
||||
// Read capabilities
|
||||
uint64_t cap = ReadReg64(REG_CAP);
|
||||
g_maxQueueEntries = (uint16_t)((cap & CAP_MQES_MASK) + 1);
|
||||
uint32_t dstrd = (uint32_t)((cap & CAP_DSTRD_MASK) >> CAP_DSTRD_SHIFT);
|
||||
g_doorbellStride = 4u << dstrd;
|
||||
|
||||
// Read version
|
||||
uint32_t vs = ReadReg32(REG_VS);
|
||||
uint32_t vsMajor = (vs >> 16) & 0xFFFF;
|
||||
uint32_t vsMinor = (vs >> 8) & 0xFF;
|
||||
uint32_t vsTertiary = vs & 0xFF;
|
||||
|
||||
KernelLogStream(INFO, "NVMe") << "Version: " << base::dec
|
||||
<< (uint64_t)vsMajor << "." << (uint64_t)vsMinor << "." << (uint64_t)vsTertiary;
|
||||
KernelLogStream(INFO, "NVMe") << "Max queue entries: " << (uint64_t)g_maxQueueEntries
|
||||
<< ", Doorbell stride: " << (uint64_t)g_doorbellStride << " bytes";
|
||||
|
||||
// Step 1: Disable controller
|
||||
if (!DisableController()) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Failed to disable controller";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Step 2: Set up MSI
|
||||
bool hasMsi = SetupMsi(dev.Bus, dev.Device, dev.Function);
|
||||
if (!hasMsi) {
|
||||
uint8_t irqLine = Pci::LegacyRead8(dev.Bus, dev.Device, dev.Function,
|
||||
(uint8_t)Pci::PCI_REG_INTERRUPT);
|
||||
if (irqLine != 0xFF) {
|
||||
KernelLogStream(INFO, "NVMe") << "Using legacy IRQ " << base::dec << (uint64_t)irqLine;
|
||||
Hal::RegisterIrqHandler(irqLine, HandleInterrupt);
|
||||
Hal::IoApic::UnmaskIrq(Hal::IoApic::GetGsiForIrq(irqLine));
|
||||
}
|
||||
}
|
||||
|
||||
// Step 3: Set up admin queues
|
||||
if (!SetupAdminQueues()) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Failed to set up admin queues";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Step 4: Enable controller
|
||||
if (!EnableController()) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Failed to enable controller";
|
||||
return false;
|
||||
}
|
||||
|
||||
KernelLogStream(OK, "NVMe") << "Controller enabled and ready";
|
||||
|
||||
// Step 5: Identify controller and namespaces
|
||||
if (!IdentifyController()) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Failed to identify controller/namespaces";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Step 6: Create I/O queues
|
||||
if (!CreateIoQueues()) {
|
||||
KernelLogStream(ERROR, "NVMe") << "Failed to create I/O queues";
|
||||
return false;
|
||||
}
|
||||
|
||||
// Step 7: Register namespaces as block devices
|
||||
for (int i = 0; i < g_nsCount; i++) {
|
||||
Storage::BlockDevice bdev = {};
|
||||
bdev.ReadSectors = [](void* ctx, uint64_t lba, uint32_t count, void* buffer) -> bool {
|
||||
return ReadSectors((int)(uintptr_t)ctx, lba, count, buffer);
|
||||
};
|
||||
bdev.WriteSectors = [](void* ctx, uint64_t lba, uint32_t count, const void* buffer) -> bool {
|
||||
return WriteSectors((int)(uintptr_t)ctx, lba, count, buffer);
|
||||
};
|
||||
bdev.Ctx = (void*)(uintptr_t)i;
|
||||
bdev.SectorCount = g_namespaces[i].SectorCount;
|
||||
bdev.SectorSize = (uint16_t)g_namespaces[i].SectorSize;
|
||||
memcpy(bdev.Model, g_namespaces[i].Model, 41);
|
||||
Storage::RegisterBlockDevice(bdev);
|
||||
}
|
||||
|
||||
g_initialized = true;
|
||||
|
||||
KernelLogStream(OK, "NVMe") << "Initialization complete: "
|
||||
<< base::dec << (uint64_t)g_nsCount << " namespace(s) ready";
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Public API: Read/Write
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
bool IsInitialized() {
|
||||
return g_initialized;
|
||||
}
|
||||
|
||||
int GetNamespaceCount() {
|
||||
return g_nsCount;
|
||||
}
|
||||
|
||||
const NamespaceInfo* GetNamespaceInfo(int ns) {
|
||||
if (ns < 0 || ns >= g_nsCount || !g_namespaces[ns].Active) {
|
||||
return nullptr;
|
||||
}
|
||||
return &g_namespaces[ns];
|
||||
}
|
||||
|
||||
bool ReadSectors(int ns, uint64_t lba, uint32_t count, void* buffer) {
|
||||
if (!g_initialized || ns < 0 || ns >= g_nsCount || !g_namespaces[ns].Active) {
|
||||
return false;
|
||||
}
|
||||
if (count == 0 || buffer == nullptr) return false;
|
||||
|
||||
// Limit to max transfer size
|
||||
uint32_t maxBlocks = g_namespaces[ns].MaxTransferBlocks;
|
||||
if (count > maxBlocks) {
|
||||
KernelLogStream(ERROR, "NVMe") << "ReadSectors: count " << count
|
||||
<< " exceeds max " << maxBlocks;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t sectorSize = g_namespaces[ns].SectorSize;
|
||||
uint32_t totalBytes = count * sectorSize;
|
||||
int pagesNeeded = (totalBytes + 0xFFF) / 0x1000;
|
||||
|
||||
uint64_t dmaPhys;
|
||||
void* dmaVirt = AllocateDmaBuffer(dmaPhys, pagesNeeded);
|
||||
|
||||
// Build NVMe Read command
|
||||
SqEntry cmd = {};
|
||||
cmd.Opcode = IO_CMD_READ;
|
||||
cmd.Nsid = g_namespaces[ns].Nsid;
|
||||
cmd.Prp1 = dmaPhys;
|
||||
|
||||
// PRP2: If transfer spans more than one page, set PRP2
|
||||
if (totalBytes > 0x1000) {
|
||||
// For transfers spanning exactly 2 pages, PRP2 = second page address
|
||||
// For transfers spanning more pages, PRP2 should point to a PRP list.
|
||||
// Since our DMA buffer is physically contiguous, we can point to the
|
||||
// second page directly for 2-page transfers. For larger transfers,
|
||||
// we build a PRP list.
|
||||
if (pagesNeeded == 2) {
|
||||
cmd.Prp2 = dmaPhys + 0x1000;
|
||||
} else {
|
||||
// Build a PRP list (array of physical page addresses)
|
||||
uint64_t prpListPhys;
|
||||
uint64_t* prpList = (uint64_t*)AllocateDmaBuffer(prpListPhys);
|
||||
for (int i = 1; i < pagesNeeded; i++) {
|
||||
prpList[i - 1] = dmaPhys + (uint64_t)i * 0x1000;
|
||||
}
|
||||
cmd.Prp2 = prpListPhys;
|
||||
}
|
||||
}
|
||||
|
||||
// CDW10-11: Starting LBA (64-bit)
|
||||
cmd.Cdw10 = (uint32_t)(lba & 0xFFFFFFFF);
|
||||
cmd.Cdw11 = (uint32_t)(lba >> 32);
|
||||
// CDW12: bits 15:0 = Number of Logical Blocks (0-based)
|
||||
cmd.Cdw12 = count - 1;
|
||||
|
||||
SubmitIoCommand(cmd);
|
||||
|
||||
CqEntry cqe;
|
||||
bool ok = WaitIoCompletion(cqe);
|
||||
if (ok) {
|
||||
memcpy(buffer, dmaVirt, totalBytes);
|
||||
}
|
||||
|
||||
// Free PRP list if we allocated one
|
||||
if (pagesNeeded > 2 && cmd.Prp2 != 0 && cmd.Prp2 != dmaPhys + 0x1000) {
|
||||
Memory::g_pfa->Free((void*)Memory::HHDM(cmd.Prp2));
|
||||
}
|
||||
|
||||
Memory::g_pfa->Free(dmaVirt, pagesNeeded);
|
||||
return ok;
|
||||
}
|
||||
|
||||
bool WriteSectors(int ns, uint64_t lba, uint32_t count, const void* buffer) {
|
||||
if (!g_initialized || ns < 0 || ns >= g_nsCount || !g_namespaces[ns].Active) {
|
||||
return false;
|
||||
}
|
||||
if (count == 0 || buffer == nullptr) return false;
|
||||
|
||||
uint32_t maxBlocks = g_namespaces[ns].MaxTransferBlocks;
|
||||
if (count > maxBlocks) {
|
||||
KernelLogStream(ERROR, "NVMe") << "WriteSectors: count " << count
|
||||
<< " exceeds max " << maxBlocks;
|
||||
return false;
|
||||
}
|
||||
|
||||
uint32_t sectorSize = g_namespaces[ns].SectorSize;
|
||||
uint32_t totalBytes = count * sectorSize;
|
||||
int pagesNeeded = (totalBytes + 0xFFF) / 0x1000;
|
||||
|
||||
uint64_t dmaPhys;
|
||||
void* dmaVirt = AllocateDmaBuffer(dmaPhys, pagesNeeded);
|
||||
memcpy(dmaVirt, buffer, totalBytes);
|
||||
|
||||
// Build NVMe Write command
|
||||
SqEntry cmd = {};
|
||||
cmd.Opcode = IO_CMD_WRITE;
|
||||
cmd.Nsid = g_namespaces[ns].Nsid;
|
||||
cmd.Prp1 = dmaPhys;
|
||||
|
||||
if (totalBytes > 0x1000) {
|
||||
if (pagesNeeded == 2) {
|
||||
cmd.Prp2 = dmaPhys + 0x1000;
|
||||
} else {
|
||||
uint64_t prpListPhys;
|
||||
uint64_t* prpList = (uint64_t*)AllocateDmaBuffer(prpListPhys);
|
||||
for (int i = 1; i < pagesNeeded; i++) {
|
||||
prpList[i - 1] = dmaPhys + (uint64_t)i * 0x1000;
|
||||
}
|
||||
cmd.Prp2 = prpListPhys;
|
||||
}
|
||||
}
|
||||
|
||||
cmd.Cdw10 = (uint32_t)(lba & 0xFFFFFFFF);
|
||||
cmd.Cdw11 = (uint32_t)(lba >> 32);
|
||||
cmd.Cdw12 = count - 1;
|
||||
|
||||
SubmitIoCommand(cmd);
|
||||
|
||||
CqEntry cqe;
|
||||
bool ok = WaitIoCompletion(cqe);
|
||||
|
||||
if (pagesNeeded > 2 && cmd.Prp2 != 0 && cmd.Prp2 != dmaPhys + 0x1000) {
|
||||
Memory::g_pfa->Free((void*)Memory::HHDM(cmd.Prp2));
|
||||
}
|
||||
|
||||
Memory::g_pfa->Free(dmaVirt, pagesNeeded);
|
||||
return ok;
|
||||
}
|
||||
|
||||
};
|
||||
@@ -0,0 +1,193 @@
|
||||
/*
|
||||
* Nvme.hpp
|
||||
* NVM Express (NVMe) storage driver
|
||||
* Copyright (c) 2026 Daniel Hammer
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include <cstdint>
|
||||
#include <Pci/Pci.hpp>
|
||||
|
||||
namespace Drivers::Storage::Nvme {
|
||||
|
||||
// =========================================================================
|
||||
// NVMe controller registers (memory-mapped via BAR0)
|
||||
// Ref: NVM Express Base Specification 2.0
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint32_t REG_CAP = 0x00; // Controller Capabilities (64-bit)
|
||||
constexpr uint32_t REG_VS = 0x08; // Version
|
||||
constexpr uint32_t REG_INTMS = 0x0C; // Interrupt Mask Set
|
||||
constexpr uint32_t REG_INTMC = 0x10; // Interrupt Mask Clear
|
||||
constexpr uint32_t REG_CC = 0x14; // Controller Configuration
|
||||
constexpr uint32_t REG_CSTS = 0x1C; // Controller Status
|
||||
constexpr uint32_t REG_AQA = 0x24; // Admin Queue Attributes
|
||||
constexpr uint32_t REG_ASQ = 0x28; // Admin SQ Base Address (64-bit)
|
||||
constexpr uint32_t REG_ACQ = 0x30; // Admin CQ Base Address (64-bit)
|
||||
|
||||
// CAP register fields (64-bit)
|
||||
constexpr uint64_t CAP_MQES_MASK = 0xFFFF; // Max Queue Entries Supported (0-based)
|
||||
constexpr int CAP_DSTRD_SHIFT = 32; // Doorbell Stride (2 ^ (2 + DSTRD))
|
||||
constexpr uint64_t CAP_DSTRD_MASK = 0xFULL << 32;
|
||||
constexpr int CAP_MPSMIN_SHIFT = 48; // Memory Page Size Minimum
|
||||
constexpr uint64_t CAP_MPSMIN_MASK = 0xFULL << 48;
|
||||
constexpr int CAP_MPSMAX_SHIFT = 52; // Memory Page Size Maximum
|
||||
constexpr uint64_t CAP_MPSMAX_MASK = 0xFULL << 52;
|
||||
constexpr uint64_t CAP_CSS_NVM = (1ULL << 37); // NVM Command Set supported
|
||||
|
||||
// CC register fields
|
||||
constexpr uint32_t CC_EN = (1u << 0); // Enable
|
||||
constexpr uint32_t CC_CSS_NVM = (0u << 4); // NVM Command Set
|
||||
constexpr uint32_t CC_MPS_SHIFT = 7; // Memory Page Size (2 ^ (12 + MPS))
|
||||
constexpr uint32_t CC_AMS_RR = (0u << 11); // Arbitration: Round Robin
|
||||
constexpr uint32_t CC_SHN_NONE = (0u << 14); // No shutdown notification
|
||||
constexpr uint32_t CC_SHN_NORMAL = (1u << 14); // Normal shutdown
|
||||
constexpr uint32_t CC_IOSQES_SHIFT = 16; // I/O SQ Entry Size (2^n)
|
||||
constexpr uint32_t CC_IOCQES_SHIFT = 20; // I/O CQ Entry Size (2^n)
|
||||
|
||||
// CSTS register fields
|
||||
constexpr uint32_t CSTS_RDY = (1u << 0); // Ready
|
||||
constexpr uint32_t CSTS_CFS = (1u << 1); // Controller Fatal Status
|
||||
constexpr uint32_t CSTS_SHST_MASK = (3u << 2); // Shutdown Status
|
||||
constexpr uint32_t CSTS_SHST_NORMAL = (0u << 2);
|
||||
constexpr uint32_t CSTS_SHST_COMPLETE = (2u << 2);
|
||||
|
||||
// =========================================================================
|
||||
// Submission Queue Entry (64 bytes)
|
||||
// =========================================================================
|
||||
|
||||
struct SqEntry {
|
||||
// Dword 0: Command Dword 0
|
||||
uint8_t Opcode;
|
||||
uint8_t Flags; // Fused (1:0), PSDT (7:6)
|
||||
uint16_t CommandId;
|
||||
|
||||
// Dword 1
|
||||
uint32_t Nsid; // Namespace ID
|
||||
|
||||
// Dwords 2-3
|
||||
uint64_t Reserved;
|
||||
|
||||
// Dwords 4-5: Metadata Pointer
|
||||
uint64_t Mptr;
|
||||
|
||||
// Dwords 6-9: Data Pointer (PRP1, PRP2)
|
||||
uint64_t Prp1;
|
||||
uint64_t Prp2;
|
||||
|
||||
// Dwords 10-15: Command specific
|
||||
uint32_t Cdw10;
|
||||
uint32_t Cdw11;
|
||||
uint32_t Cdw12;
|
||||
uint32_t Cdw13;
|
||||
uint32_t Cdw14;
|
||||
uint32_t Cdw15;
|
||||
} __attribute__((packed));
|
||||
|
||||
static_assert(sizeof(SqEntry) == 64, "SqEntry must be 64 bytes");
|
||||
|
||||
// =========================================================================
|
||||
// Completion Queue Entry (16 bytes)
|
||||
// =========================================================================
|
||||
|
||||
struct CqEntry {
|
||||
uint32_t Result; // Command-specific result (DW0)
|
||||
uint32_t Reserved;
|
||||
uint16_t SqHead; // SQ Head Pointer
|
||||
uint16_t SqId; // SQ Identifier
|
||||
uint16_t CommandId; // Command Identifier
|
||||
uint16_t Status; // Status Field (bit 0 = Phase Tag)
|
||||
} __attribute__((packed));
|
||||
|
||||
static_assert(sizeof(CqEntry) == 16, "CqEntry must be 16 bytes");
|
||||
|
||||
// Status field: Phase bit is bit 0; status code is bits 15:1
|
||||
constexpr uint16_t CQE_PHASE_BIT = (1u << 0);
|
||||
constexpr uint16_t CQE_STATUS_MASK = 0xFFFE; // bits 15:1
|
||||
|
||||
// =========================================================================
|
||||
// Admin opcodes
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint8_t ADMIN_DELETE_IO_SQ = 0x00;
|
||||
constexpr uint8_t ADMIN_CREATE_IO_SQ = 0x01;
|
||||
constexpr uint8_t ADMIN_DELETE_IO_CQ = 0x04;
|
||||
constexpr uint8_t ADMIN_CREATE_IO_CQ = 0x05;
|
||||
constexpr uint8_t ADMIN_IDENTIFY = 0x06;
|
||||
constexpr uint8_t ADMIN_SET_FEATURES = 0x09;
|
||||
|
||||
// =========================================================================
|
||||
// NVM I/O opcodes
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint8_t IO_CMD_READ = 0x02;
|
||||
constexpr uint8_t IO_CMD_WRITE = 0x01;
|
||||
|
||||
// =========================================================================
|
||||
// Identify CNS values
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint32_t IDENTIFY_CNS_NAMESPACE = 0x00;
|
||||
constexpr uint32_t IDENTIFY_CNS_CONTROLLER = 0x01;
|
||||
|
||||
// =========================================================================
|
||||
// Feature identifiers
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint32_t FEATURE_NUM_QUEUES = 0x07;
|
||||
|
||||
// =========================================================================
|
||||
// Queue parameters
|
||||
// =========================================================================
|
||||
|
||||
constexpr int ADMIN_QUEUE_DEPTH = 32; // Admin queue entries
|
||||
constexpr int IO_QUEUE_DEPTH = 64; // I/O queue entries
|
||||
constexpr int MAX_NAMESPACES = 8;
|
||||
|
||||
// =========================================================================
|
||||
// Namespace info
|
||||
// =========================================================================
|
||||
|
||||
struct NamespaceInfo {
|
||||
bool Active;
|
||||
uint32_t Nsid;
|
||||
uint64_t SectorCount; // Total LBAs (NSZE)
|
||||
uint32_t SectorSize; // Bytes per LBA
|
||||
uint32_t MaxTransferBlocks; // MDTS in blocks
|
||||
char Model[41];
|
||||
};
|
||||
|
||||
// =========================================================================
|
||||
// MSI configuration (use a different IRQ slot than AHCI)
|
||||
// =========================================================================
|
||||
|
||||
constexpr uint8_t MSI_IRQ = 26; // IRQ slot 26 = vector 58
|
||||
constexpr uint32_t MSI_VECTOR = 58;
|
||||
constexpr uint32_t MSI_ADDR_BASE = 0xFEE00000;
|
||||
|
||||
// =========================================================================
|
||||
// Public API
|
||||
// =========================================================================
|
||||
|
||||
// Probe a PCI device (called by driver matching framework)
|
||||
bool Probe(const Pci::PciDevice& dev);
|
||||
|
||||
// Check if the driver was initialized
|
||||
bool IsInitialized();
|
||||
|
||||
// Get number of active namespaces
|
||||
int GetNamespaceCount();
|
||||
|
||||
// Read sectors from an NVMe namespace
|
||||
// ns: namespace index, lba: starting LBA, count: sector count (max 128)
|
||||
// buffer: destination buffer
|
||||
// Returns true on success
|
||||
bool ReadSectors(int ns, uint64_t lba, uint32_t count, void* buffer);
|
||||
|
||||
// Write sectors to an NVMe namespace
|
||||
bool WriteSectors(int ns, uint64_t lba, uint32_t count, const void* buffer);
|
||||
|
||||
// Get info about a specific namespace
|
||||
const NamespaceInfo* GetNamespaceInfo(int ns);
|
||||
|
||||
};
|
||||
@@ -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);
|
||||
|
||||
}
|
||||
@@ -8,6 +8,7 @@
|
||||
#include "Xhci.hpp"
|
||||
#include "HidKeyboard.hpp"
|
||||
#include "HidMouse.hpp"
|
||||
#include "Bluetooth/Bluetooth.hpp"
|
||||
#include <Terminal/Terminal.hpp>
|
||||
#include <CppLib/Stream.hpp>
|
||||
#include <Memory/HHDM.hpp>
|
||||
@@ -300,8 +301,9 @@ namespace Drivers::USB::UsbDevice {
|
||||
return 0;
|
||||
}
|
||||
|
||||
dev->VendorId = devDesc.idVendor;
|
||||
dev->ProductId = devDesc.idProduct;
|
||||
dev->VendorId = devDesc.idVendor;
|
||||
dev->ProductId = devDesc.idProduct;
|
||||
dev->DeviceClass = devDesc.bDeviceClass;
|
||||
|
||||
KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId
|
||||
<< ": VID:PID = " << base::hex << (uint64_t)devDesc.idVendor
|
||||
@@ -338,7 +340,10 @@ namespace Drivers::USB::UsbDevice {
|
||||
// -----------------------------------------------------------------
|
||||
uint16_t offset = 0;
|
||||
bool foundHid = false;
|
||||
bool foundBt = false;
|
||||
bool foundEp = false;
|
||||
bool foundBulkIn = false;
|
||||
bool foundBulkOut = false;
|
||||
uint16_t hidReportDescLen = 0;
|
||||
|
||||
while (offset + 2 <= totalLen) {
|
||||
@@ -348,9 +353,10 @@ namespace Drivers::USB::UsbDevice {
|
||||
|
||||
if (type == DESC_INTERFACE && offset + sizeof(InterfaceDescriptor) <= totalLen) {
|
||||
auto* iface = (InterfaceDescriptor*)&cfgBuf[offset];
|
||||
// Reset foundHid at each new interface boundary so we don't
|
||||
// accidentally pick up endpoints from a different interface.
|
||||
// Reset at each new interface boundary
|
||||
foundHid = false;
|
||||
foundBt = false;
|
||||
|
||||
if (!foundEp &&
|
||||
iface->bInterfaceClass == CLASS_HID &&
|
||||
iface->bInterfaceSubClass == SUBCLASS_BOOT) {
|
||||
@@ -359,6 +365,18 @@ namespace Drivers::USB::UsbDevice {
|
||||
dev->InterfaceProtocol = iface->bInterfaceProtocol;
|
||||
foundHid = true;
|
||||
}
|
||||
|
||||
// Bluetooth HCI interface (class 0xE0, subclass 0x01, protocol 0x01)
|
||||
if (iface->bInterfaceClass == CLASS_WIRELESS &&
|
||||
iface->bInterfaceSubClass == SUBCLASS_RF &&
|
||||
iface->bInterfaceProtocol == PROTOCOL_BLUETOOTH) {
|
||||
if (dev->InterfaceClass == 0 || dev->InterfaceClass == CLASS_WIRELESS) {
|
||||
dev->InterfaceClass = iface->bInterfaceClass;
|
||||
dev->InterfaceSubClass = iface->bInterfaceSubClass;
|
||||
dev->InterfaceProtocol = iface->bInterfaceProtocol;
|
||||
}
|
||||
foundBt = true;
|
||||
}
|
||||
}
|
||||
|
||||
// HID descriptor (0x21): extract report descriptor length
|
||||
@@ -367,21 +385,55 @@ namespace Drivers::USB::UsbDevice {
|
||||
| ((uint16_t)cfgBuf[offset + 8] << 8);
|
||||
}
|
||||
|
||||
if (type == DESC_ENDPOINT && foundHid && !foundEp &&
|
||||
offset + sizeof(EndpointDescriptor) <= totalLen) {
|
||||
if (type == DESC_ENDPOINT && offset + sizeof(EndpointDescriptor) <= totalLen) {
|
||||
auto* ep = (EndpointDescriptor*)&cfgBuf[offset];
|
||||
if ((ep->bEndpointAddress & EP_DIR_IN) &&
|
||||
(ep->bmAttributes & EP_XFER_TYPE_MASK) == EP_XFER_INTERRUPT) {
|
||||
uint8_t xferType = ep->bmAttributes & EP_XFER_TYPE_MASK;
|
||||
bool isIn = (ep->bEndpointAddress & EP_DIR_IN) != 0;
|
||||
|
||||
// HID interrupt IN endpoint
|
||||
if (foundHid && !foundEp && isIn && xferType == EP_XFER_INTERRUPT) {
|
||||
dev->InterruptEpNum = ep->bEndpointAddress & 0x0F;
|
||||
dev->InterruptMaxPacket = ep->wMaxPacketSize & 0x7FF;
|
||||
dev->InterruptInterval = ep->bInterval;
|
||||
foundEp = true;
|
||||
}
|
||||
|
||||
// Bluetooth endpoints
|
||||
if (foundBt) {
|
||||
if (isIn && xferType == EP_XFER_INTERRUPT && !foundEp) {
|
||||
// HCI event pipe (interrupt IN)
|
||||
dev->InterruptEpNum = ep->bEndpointAddress & 0x0F;
|
||||
dev->InterruptMaxPacket = ep->wMaxPacketSize & 0x7FF;
|
||||
dev->InterruptInterval = ep->bInterval;
|
||||
foundEp = true;
|
||||
} else if (isIn && xferType == EP_XFER_BULK && !foundBulkIn) {
|
||||
// ACL data IN pipe (bulk IN)
|
||||
dev->BulkInEpNum = ep->bEndpointAddress & 0x0F;
|
||||
dev->BulkInMaxPacket = ep->wMaxPacketSize & 0x7FF;
|
||||
foundBulkIn = true;
|
||||
} else if (!isIn && xferType == EP_XFER_BULK && !foundBulkOut) {
|
||||
// ACL data OUT pipe (bulk OUT)
|
||||
dev->BulkOutEpNum = ep->bEndpointAddress & 0x0F;
|
||||
dev->BulkOutMaxPacket = ep->wMaxPacketSize & 0x7FF;
|
||||
foundBulkOut = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
offset += len;
|
||||
}
|
||||
|
||||
// For Bluetooth devices, also check device class for correct identification
|
||||
// Some BT adapters use bDeviceClass=0xE0 at device level
|
||||
if (!foundBt && devDesc.bDeviceClass == CLASS_WIRELESS &&
|
||||
devDesc.bDeviceSubClass == SUBCLASS_RF &&
|
||||
devDesc.bDeviceProtocol == PROTOCOL_BLUETOOTH) {
|
||||
dev->InterfaceClass = CLASS_WIRELESS;
|
||||
dev->InterfaceSubClass = SUBCLASS_RF;
|
||||
dev->InterfaceProtocol = PROTOCOL_BLUETOOTH;
|
||||
foundBt = true;
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------
|
||||
// Step 8: SET_CONFIGURATION
|
||||
// -----------------------------------------------------------------
|
||||
@@ -394,57 +446,106 @@ namespace Drivers::USB::UsbDevice {
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------
|
||||
// Step 9: Configure Endpoint (if HID interrupt endpoint was found)
|
||||
// Step 9: Configure Endpoints
|
||||
// -----------------------------------------------------------------
|
||||
if (foundEp) {
|
||||
// Device Context Index for an IN endpoint: DCI = EpNum * 2 + 1
|
||||
uint8_t dci = dev->InterruptEpNum * 2 + 1;
|
||||
|
||||
if (foundEp || foundBulkIn || foundBulkOut) {
|
||||
auto* inputCtx2 = (Xhci::InputContext*)Memory::g_pfa->AllocateZeroed();
|
||||
|
||||
// ICC: Add slot context (bit 0) and the interrupt endpoint (bit dci)
|
||||
inputCtx2->ICC.AddFlags = (1 << 0) | (1 << dci);
|
||||
// Start with slot context
|
||||
inputCtx2->ICC.AddFlags = (1 << 0);
|
||||
|
||||
// Copy the current slot context from the output context
|
||||
inputCtx2->Slot = dev->OutputContext->Slot;
|
||||
|
||||
// Update Context Entries in slot context to at least cover this DCI
|
||||
uint32_t newCtxEntries = dci;
|
||||
// Track the highest DCI to set Context Entries
|
||||
uint32_t maxDci = 0;
|
||||
|
||||
// --- Configure Interrupt IN endpoint ---
|
||||
if (foundEp) {
|
||||
uint8_t dci = dev->InterruptEpNum * 2 + 1;
|
||||
inputCtx2->ICC.AddFlags |= (1 << dci);
|
||||
if (dci > maxDci) maxDci = dci;
|
||||
|
||||
// Allocate interrupt transfer ring
|
||||
auto* intRing = (Xhci::TRB*)Memory::g_pfa->AllocateZeroed();
|
||||
dev->InterruptRing = intRing;
|
||||
dev->InterruptRingPhys = Memory::SubHHDM(intRing);
|
||||
dev->InterruptRingEnqueue = 0;
|
||||
dev->InterruptRingCCS = true;
|
||||
|
||||
Xhci::TRB& intLink = intRing[Xhci::XFER_RING_SIZE - 1];
|
||||
intLink.Parameter0 = (uint32_t)(dev->InterruptRingPhys & 0xFFFFFFFF);
|
||||
intLink.Parameter1 = (uint32_t)(dev->InterruptRingPhys >> 32);
|
||||
intLink.Status = 0;
|
||||
intLink.Control = (Xhci::TRB_LINK << Xhci::TRB_TYPE_SHIFT) | Xhci::TRB_ENT;
|
||||
|
||||
auto& epCtx = inputCtx2->EP[dci - 1];
|
||||
uint32_t xhciInterval = ConvertInterval(speed, dev->InterruptInterval);
|
||||
epCtx.Field0 = (xhciInterval << 16);
|
||||
epCtx.Field1 = (3 << 1)
|
||||
| (Xhci::EP_TYPE_INTERRUPT_IN << 3)
|
||||
| ((uint32_t)dev->InterruptMaxPacket << 16);
|
||||
epCtx.TRDequeuePtr = dev->InterruptRingPhys | 1;
|
||||
epCtx.Field2 = dev->InterruptMaxPacket;
|
||||
}
|
||||
|
||||
// --- Configure Bulk IN endpoint ---
|
||||
if (foundBulkIn) {
|
||||
uint8_t dci = dev->BulkInEpNum * 2 + 1;
|
||||
inputCtx2->ICC.AddFlags |= (1 << dci);
|
||||
if (dci > maxDci) maxDci = dci;
|
||||
|
||||
auto* bulkInRing = (Xhci::TRB*)Memory::g_pfa->AllocateZeroed();
|
||||
dev->BulkInRing = bulkInRing;
|
||||
dev->BulkInRingPhys = Memory::SubHHDM(bulkInRing);
|
||||
dev->BulkInRingEnqueue = 0;
|
||||
dev->BulkInRingCCS = true;
|
||||
|
||||
Xhci::TRB& biLink = bulkInRing[Xhci::XFER_RING_SIZE - 1];
|
||||
biLink.Parameter0 = (uint32_t)(dev->BulkInRingPhys & 0xFFFFFFFF);
|
||||
biLink.Parameter1 = (uint32_t)(dev->BulkInRingPhys >> 32);
|
||||
biLink.Status = 0;
|
||||
biLink.Control = (Xhci::TRB_LINK << Xhci::TRB_TYPE_SHIFT) | Xhci::TRB_ENT;
|
||||
|
||||
auto& epCtx = inputCtx2->EP[dci - 1];
|
||||
epCtx.Field0 = 0;
|
||||
epCtx.Field1 = (3 << 1)
|
||||
| (Xhci::EP_TYPE_BULK_IN << 3)
|
||||
| ((uint32_t)dev->BulkInMaxPacket << 16);
|
||||
epCtx.TRDequeuePtr = dev->BulkInRingPhys | 1;
|
||||
epCtx.Field2 = dev->BulkInMaxPacket;
|
||||
}
|
||||
|
||||
// --- Configure Bulk OUT endpoint ---
|
||||
if (foundBulkOut) {
|
||||
uint8_t dci = dev->BulkOutEpNum * 2;
|
||||
inputCtx2->ICC.AddFlags |= (1 << dci);
|
||||
if (dci > maxDci) maxDci = dci;
|
||||
|
||||
auto* bulkOutRing = (Xhci::TRB*)Memory::g_pfa->AllocateZeroed();
|
||||
dev->BulkOutRing = bulkOutRing;
|
||||
dev->BulkOutRingPhys = Memory::SubHHDM(bulkOutRing);
|
||||
dev->BulkOutRingEnqueue = 0;
|
||||
dev->BulkOutRingCCS = true;
|
||||
|
||||
Xhci::TRB& boLink = bulkOutRing[Xhci::XFER_RING_SIZE - 1];
|
||||
boLink.Parameter0 = (uint32_t)(dev->BulkOutRingPhys & 0xFFFFFFFF);
|
||||
boLink.Parameter1 = (uint32_t)(dev->BulkOutRingPhys >> 32);
|
||||
boLink.Status = 0;
|
||||
boLink.Control = (Xhci::TRB_LINK << Xhci::TRB_TYPE_SHIFT) | Xhci::TRB_ENT;
|
||||
|
||||
auto& epCtx = inputCtx2->EP[dci - 1];
|
||||
epCtx.Field0 = 0;
|
||||
epCtx.Field1 = (3 << 1)
|
||||
| (Xhci::EP_TYPE_BULK_OUT << 3)
|
||||
| ((uint32_t)dev->BulkOutMaxPacket << 16);
|
||||
epCtx.TRDequeuePtr = dev->BulkOutRingPhys | 1;
|
||||
epCtx.Field2 = dev->BulkOutMaxPacket;
|
||||
}
|
||||
|
||||
// Update Context Entries to cover the highest DCI
|
||||
inputCtx2->Slot.Field0 = (inputCtx2->Slot.Field0 & ~(0x1Fu << 27))
|
||||
| (newCtxEntries << 27);
|
||||
|
||||
// Allocate interrupt transfer ring
|
||||
auto* intRing = (Xhci::TRB*)Memory::g_pfa->AllocateZeroed();
|
||||
dev->InterruptRing = intRing;
|
||||
dev->InterruptRingPhys = Memory::SubHHDM(intRing);
|
||||
dev->InterruptRingEnqueue = 0;
|
||||
dev->InterruptRingCCS = true;
|
||||
|
||||
// Set up Link TRB at last position
|
||||
Xhci::TRB& intLink = intRing[Xhci::XFER_RING_SIZE - 1];
|
||||
intLink.Parameter0 = (uint32_t)(dev->InterruptRingPhys & 0xFFFFFFFF);
|
||||
intLink.Parameter1 = (uint32_t)(dev->InterruptRingPhys >> 32);
|
||||
intLink.Status = 0;
|
||||
intLink.Control = (Xhci::TRB_LINK << Xhci::TRB_TYPE_SHIFT) | Xhci::TRB_ENT;
|
||||
|
||||
// Endpoint Context for the interrupt IN endpoint
|
||||
auto& epCtx = inputCtx2->EP[dci - 1]; // EP array is 0-indexed, DCI 1 = EP[0]
|
||||
|
||||
// Field0: Interval (bits 23:16) — convert bInterval to xHCI encoding
|
||||
uint32_t xhciInterval = ConvertInterval(speed, dev->InterruptInterval);
|
||||
epCtx.Field0 = (xhciInterval << 16);
|
||||
|
||||
// Field1: CErr=3 (bits 2:1), EP Type=Interrupt IN=7 (bits 5:3),
|
||||
// Max Packet Size (bits 31:16)
|
||||
epCtx.Field1 = (3 << 1)
|
||||
| (Xhci::EP_TYPE_INTERRUPT_IN << 3)
|
||||
| ((uint32_t)dev->InterruptMaxPacket << 16);
|
||||
|
||||
// TR Dequeue Pointer with DCS=1
|
||||
epCtx.TRDequeuePtr = dev->InterruptRingPhys | 1;
|
||||
|
||||
// Average TRB Length
|
||||
epCtx.Field2 = dev->InterruptMaxPacket;
|
||||
| (maxDci << 27);
|
||||
|
||||
// Send Configure Endpoint command
|
||||
Xhci::TRB cfgTrb = {};
|
||||
@@ -462,9 +563,18 @@ namespace Drivers::USB::UsbDevice {
|
||||
return 0;
|
||||
}
|
||||
|
||||
KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId
|
||||
<< ": Interrupt EP " << (uint64_t)dev->InterruptEpNum
|
||||
<< " configured (DCI " << (uint64_t)dci << ")";
|
||||
if (foundEp) {
|
||||
KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId
|
||||
<< ": Interrupt EP " << (uint64_t)dev->InterruptEpNum << " configured";
|
||||
}
|
||||
if (foundBulkIn) {
|
||||
KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId
|
||||
<< ": Bulk IN EP " << (uint64_t)dev->BulkInEpNum << " configured";
|
||||
}
|
||||
if (foundBulkOut) {
|
||||
KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId
|
||||
<< ": Bulk OUT EP " << (uint64_t)dev->BulkOutEpNum << " configured";
|
||||
}
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------
|
||||
@@ -473,7 +583,7 @@ namespace Drivers::USB::UsbDevice {
|
||||
// Set Boot Protocol for keyboards only.
|
||||
// Mice stay in Report Protocol (the default) for scroll wheel support;
|
||||
// HidMouse parses the HID Report Descriptor to handle variable formats.
|
||||
if (foundEp && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) {
|
||||
if (foundEp && dev->InterfaceClass == CLASS_HID && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) {
|
||||
cc = Xhci::ControlTransfer(slotId, REQTYPE_CLASS_IFACE, REQ_SET_PROTOCOL,
|
||||
0, 0, 0, nullptr, false);
|
||||
if (cc != Xhci::CC_SUCCESS) {
|
||||
@@ -503,7 +613,7 @@ namespace Drivers::USB::UsbDevice {
|
||||
// -----------------------------------------------------------------
|
||||
// Step 11: SET_IDLE(0) -- only report on changes (no idle reports)
|
||||
// -----------------------------------------------------------------
|
||||
if (foundEp && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) {
|
||||
if (foundEp && dev->InterfaceClass == CLASS_HID && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) {
|
||||
// wValue upper byte = duration (0 = indefinite), lower byte = report ID
|
||||
cc = Xhci::ControlTransfer(slotId, REQTYPE_CLASS_IFACE, REQ_SET_IDLE,
|
||||
(0 << 8), 0, 0, nullptr, false);
|
||||
@@ -514,24 +624,33 @@ namespace Drivers::USB::UsbDevice {
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------
|
||||
// Step 12: Queue first interrupt transfer
|
||||
// Step 12: Queue first interrupt transfer (HID only)
|
||||
// Bluetooth manages its own interrupt/bulk transfers via StartEventPipe()
|
||||
// -----------------------------------------------------------------
|
||||
if (foundEp) {
|
||||
if (foundEp && !foundBt) {
|
||||
Xhci::QueueInterruptTransfer(slotId);
|
||||
}
|
||||
|
||||
// -----------------------------------------------------------------
|
||||
// Step 13: Register with the appropriate HID driver
|
||||
// Step 13: Register with the appropriate class driver
|
||||
// -----------------------------------------------------------------
|
||||
if (dev->InterfaceProtocol == PROTOCOL_KEYBOARD) {
|
||||
if (dev->InterfaceClass == CLASS_HID && dev->InterfaceProtocol == PROTOCOL_KEYBOARD) {
|
||||
HidKeyboard::RegisterDevice(slotId);
|
||||
KernelLogStream(OK, "USB") << "Slot " << (uint64_t)slotId << ": HID Boot Keyboard";
|
||||
} else if (dev->InterfaceProtocol == PROTOCOL_MOUSE) {
|
||||
} else if (dev->InterfaceClass == CLASS_HID && dev->InterfaceProtocol == PROTOCOL_MOUSE) {
|
||||
HidMouse::RegisterDevice(slotId);
|
||||
KernelLogStream(OK, "USB") << "Slot " << (uint64_t)slotId << ": HID Boot Mouse";
|
||||
} else if (dev->InterfaceClass == CLASS_WIRELESS &&
|
||||
dev->InterfaceSubClass == SUBCLASS_RF &&
|
||||
dev->InterfaceProtocol == PROTOCOL_BLUETOOTH) {
|
||||
Bluetooth::RegisterAdapter(slotId);
|
||||
KernelLogStream(OK, "USB") << "Slot " << (uint64_t)slotId << ": Bluetooth Adapter"
|
||||
<< " VID:" << base::hex << (uint64_t)dev->VendorId
|
||||
<< " PID:" << (uint64_t)dev->ProductId << base::dec;
|
||||
} else if (foundEp) {
|
||||
KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId
|
||||
<< ": HID device, protocol=" << (uint64_t)dev->InterfaceProtocol;
|
||||
<< ": USB device, class=" << (uint64_t)dev->InterfaceClass
|
||||
<< " protocol=" << (uint64_t)dev->InterfaceProtocol;
|
||||
} else {
|
||||
KernelLogStream(INFO, "USB") << "Slot " << (uint64_t)slotId
|
||||
<< ": Non-HID device, class=" << (uint64_t)devDesc.bDeviceClass;
|
||||
|
||||
@@ -80,6 +80,11 @@ namespace Drivers::USB::UsbDevice {
|
||||
constexpr uint8_t PROTOCOL_KEYBOARD = 0x01;
|
||||
constexpr uint8_t PROTOCOL_MOUSE = 0x02;
|
||||
|
||||
// Wireless Controller class (Bluetooth)
|
||||
constexpr uint8_t CLASS_WIRELESS = 0xE0;
|
||||
constexpr uint8_t SUBCLASS_RF = 0x01;
|
||||
constexpr uint8_t PROTOCOL_BLUETOOTH = 0x01;
|
||||
|
||||
// USB standard requests (bRequest)
|
||||
constexpr uint8_t REQ_GET_DESCRIPTOR = 0x06;
|
||||
constexpr uint8_t REQ_SET_CONFIGURATION = 0x09;
|
||||
@@ -100,6 +105,9 @@ namespace Drivers::USB::UsbDevice {
|
||||
|
||||
// Endpoint transfer type mask
|
||||
constexpr uint8_t EP_XFER_TYPE_MASK = 0x03;
|
||||
constexpr uint8_t EP_XFER_CONTROL = 0x00;
|
||||
constexpr uint8_t EP_XFER_ISOCH = 0x01;
|
||||
constexpr uint8_t EP_XFER_BULK = 0x02;
|
||||
constexpr uint8_t EP_XFER_INTERRUPT = 0x03;
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
+180
-18
@@ -98,6 +98,13 @@ namespace Drivers::USB::Xhci {
|
||||
static uint8_t* g_interruptDataBuf[MAX_SLOTS + 1] = {};
|
||||
static uint64_t g_interruptDataBufPhys[MAX_SLOTS + 1] = {};
|
||||
|
||||
// Bulk IN transfer data buffers (per slot)
|
||||
static uint8_t* g_bulkInDataBuf[MAX_SLOTS + 1] = {};
|
||||
static uint64_t g_bulkInDataBufPhys[MAX_SLOTS + 1] = {};
|
||||
|
||||
// Transfer callbacks for non-HID class drivers (per slot)
|
||||
static TransferCallback g_transferCallbacks[MAX_SLOTS + 1] = {};
|
||||
|
||||
// Scratchpad buffer array
|
||||
static uint64_t* g_scratchpadBufs = nullptr;
|
||||
|
||||
@@ -174,6 +181,36 @@ namespace Drivers::USB::Xhci {
|
||||
}
|
||||
}
|
||||
|
||||
// Advance bulk IN ring enqueue pointer, activating Link TRB when reached
|
||||
static void AdvanceBulkInRing(UsbDeviceInfo& dev) {
|
||||
dev.BulkInRingEnqueue++;
|
||||
if (dev.BulkInRingEnqueue >= XFER_RING_SIZE - 1) {
|
||||
TRB& link = dev.BulkInRing[XFER_RING_SIZE - 1];
|
||||
if (dev.BulkInRingCCS) {
|
||||
link.Control |= TRB_CYCLE_BIT;
|
||||
} else {
|
||||
link.Control &= ~TRB_CYCLE_BIT;
|
||||
}
|
||||
dev.BulkInRingCCS = !dev.BulkInRingCCS;
|
||||
dev.BulkInRingEnqueue = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// Advance bulk OUT ring enqueue pointer, activating Link TRB when reached
|
||||
static void AdvanceBulkOutRing(UsbDeviceInfo& dev) {
|
||||
dev.BulkOutRingEnqueue++;
|
||||
if (dev.BulkOutRingEnqueue >= XFER_RING_SIZE - 1) {
|
||||
TRB& link = dev.BulkOutRing[XFER_RING_SIZE - 1];
|
||||
if (dev.BulkOutRingCCS) {
|
||||
link.Control |= TRB_CYCLE_BIT;
|
||||
} else {
|
||||
link.Control &= ~TRB_CYCLE_BIT;
|
||||
}
|
||||
dev.BulkOutRingCCS = !dev.BulkOutRingCCS;
|
||||
dev.BulkOutRingEnqueue = 0;
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// Forward declarations
|
||||
// -------------------------------------------------------------------------
|
||||
@@ -279,37 +316,62 @@ namespace Drivers::USB::Xhci {
|
||||
g_xferCompletionCode = completionCode;
|
||||
g_xferCompleted = true;
|
||||
} else if (slotId > 0 && slotId <= MAX_SLOTS && g_devices[slotId].Active) {
|
||||
// Interrupt IN endpoint completion
|
||||
UsbDeviceInfo& dev = g_devices[slotId];
|
||||
|
||||
if (completionCode == CC_SUCCESS || completionCode == CC_SHORT_PACKET) {
|
||||
uint16_t len = dev.InterruptMaxPacket;
|
||||
|
||||
// Residual byte count: bits 23:0 of Status gives residual
|
||||
// Compute actual transfer length from residual
|
||||
uint32_t residual = evt.Status & 0x00FFFFFF;
|
||||
if (residual < len) {
|
||||
len = dev.InterruptMaxPacket - (uint16_t)residual;
|
||||
}
|
||||
|
||||
// Dispatch to HID driver based on interface protocol
|
||||
if (dev.InterfaceClass == UsbDevice::CLASS_HID) {
|
||||
if (dev.InterfaceProtocol == UsbDevice::PROTOCOL_KEYBOARD) {
|
||||
HidKeyboard::ProcessReport(g_interruptDataBuf[slotId], len);
|
||||
} else if (dev.InterfaceProtocol == UsbDevice::PROTOCOL_MOUSE) {
|
||||
HidMouse::ProcessReport(g_interruptDataBuf[slotId], len);
|
||||
// Check if this is a bulk IN endpoint completion
|
||||
uint8_t bulkInDci = dev.BulkInEpNum ? (dev.BulkInEpNum * 2 + 1) : 0;
|
||||
uint8_t bulkOutDci = dev.BulkOutEpNum ? (dev.BulkOutEpNum * 2) : 0;
|
||||
uint8_t intDci = dev.InterruptEpNum ? (dev.InterruptEpNum * 2 + 1) : 0;
|
||||
|
||||
if (epDci == bulkInDci && g_transferCallbacks[slotId]) {
|
||||
// Bulk IN — dispatch via registered callback
|
||||
uint16_t len = dev.BulkInMaxPacket;
|
||||
if (residual < len) len = dev.BulkInMaxPacket - (uint16_t)residual;
|
||||
g_transferCallbacks[slotId](slotId, epDci,
|
||||
g_bulkInDataBuf[slotId], len, completionCode);
|
||||
} else if (epDci == bulkOutDci && g_transferCallbacks[slotId]) {
|
||||
// Bulk OUT completion — notify callback
|
||||
g_transferCallbacks[slotId](slotId, epDci,
|
||||
nullptr, 0, completionCode);
|
||||
} else if (epDci == intDci) {
|
||||
// Interrupt IN — HID or callback dispatch
|
||||
uint16_t len = dev.InterruptMaxPacket;
|
||||
if (residual < len) len = dev.InterruptMaxPacket - (uint16_t)residual;
|
||||
|
||||
if (dev.InterfaceClass == UsbDevice::CLASS_HID) {
|
||||
if (dev.InterfaceProtocol == UsbDevice::PROTOCOL_KEYBOARD) {
|
||||
HidKeyboard::ProcessReport(g_interruptDataBuf[slotId], len);
|
||||
} else if (dev.InterfaceProtocol == UsbDevice::PROTOCOL_MOUSE) {
|
||||
HidMouse::ProcessReport(g_interruptDataBuf[slotId], len);
|
||||
}
|
||||
// Re-queue for next HID report
|
||||
QueueInterruptTransfer(slotId);
|
||||
} else if (g_transferCallbacks[slotId]) {
|
||||
// Callback is responsible for re-queuing
|
||||
g_transferCallbacks[slotId](slotId, epDci,
|
||||
g_interruptDataBuf[slotId], len, completionCode);
|
||||
}
|
||||
} else if (g_transferCallbacks[slotId]) {
|
||||
// Unknown endpoint — try callback
|
||||
g_transferCallbacks[slotId](slotId, epDci,
|
||||
nullptr, 0, completionCode);
|
||||
}
|
||||
} else {
|
||||
KernelLogStream(WARNING, "xHCI") << "Transfer error on slot "
|
||||
<< base::dec << (uint64_t)slotId << " ep " << (uint64_t)epDci
|
||||
<< " cc=" << (uint64_t)completionCode;
|
||||
|
||||
// Notify callback of errors too
|
||||
if (g_transferCallbacks[slotId]) {
|
||||
g_transferCallbacks[slotId](slotId, epDci,
|
||||
nullptr, 0, completionCode);
|
||||
}
|
||||
}
|
||||
|
||||
// Always re-queue the next interrupt transfer so the
|
||||
// device can continue delivering reports. Transient
|
||||
// errors (stall, babble, etc.) must not permanently
|
||||
// kill the transfer chain.
|
||||
QueueInterruptTransfer(slotId);
|
||||
}
|
||||
break;
|
||||
}
|
||||
@@ -495,6 +557,31 @@ namespace Drivers::USB::Xhci {
|
||||
}
|
||||
|
||||
KernelLogStream(WARNING, "xHCI") << "Control transfer timeout on slot " << base::dec << (uint64_t)slotId;
|
||||
|
||||
// Recover EP0 ring: Stop Endpoint, then Set TR Dequeue Pointer
|
||||
// so that subsequent control transfers on this slot still work.
|
||||
TRB stopTrb = {};
|
||||
stopTrb.Control = (TRB_STOP_ENDPOINT << TRB_TYPE_SHIFT)
|
||||
| ((uint32_t)slotId << 24)
|
||||
| (1 << 16); // DCI=1 (EP0) in bits 20:16
|
||||
SendCommand(stopTrb);
|
||||
|
||||
// Reset the enqueue pointer to the start and set the dequeue pointer
|
||||
// to match so the ring is back in sync.
|
||||
uint64_t newDeq = dev.EP0RingPhys
|
||||
+ (uint64_t)dev.EP0RingEnqueue * sizeof(TRB);
|
||||
if (dev.EP0RingCCS) {
|
||||
newDeq |= 1; // DCS bit
|
||||
}
|
||||
|
||||
TRB deqTrb = {};
|
||||
deqTrb.Parameter0 = (uint32_t)(newDeq & 0xFFFFFFFF);
|
||||
deqTrb.Parameter1 = (uint32_t)(newDeq >> 32);
|
||||
deqTrb.Control = (TRB_SET_TR_DEQUEUE << TRB_TYPE_SHIFT)
|
||||
| ((uint32_t)slotId << 24)
|
||||
| (1 << 16); // DCI=1 (EP0)
|
||||
SendCommand(deqTrb);
|
||||
|
||||
return 0xFF;
|
||||
}
|
||||
|
||||
@@ -534,6 +621,81 @@ namespace Drivers::USB::Xhci {
|
||||
WriteDoorbell(slotId, target);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// QueueBulkInTransfer
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
void QueueBulkInTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length) {
|
||||
if (slotId == 0 || slotId > MAX_SLOTS || !g_devices[slotId].Active) return;
|
||||
|
||||
UsbDeviceInfo& dev = g_devices[slotId];
|
||||
if (!dev.BulkInRing || dev.BulkInEpNum == 0) return;
|
||||
|
||||
// If caller provides nullptr, use the per-slot bulk IN DMA buffer
|
||||
if (data == nullptr) {
|
||||
if (g_bulkInDataBuf[slotId] == nullptr) {
|
||||
g_bulkInDataBuf[slotId] = AllocateDmaBuffer(g_bulkInDataBufPhys[slotId]);
|
||||
}
|
||||
data = g_bulkInDataBuf[slotId];
|
||||
dataPhys = g_bulkInDataBufPhys[slotId];
|
||||
}
|
||||
|
||||
TRB& trb = dev.BulkInRing[dev.BulkInRingEnqueue];
|
||||
trb.Parameter0 = (uint32_t)(dataPhys & 0xFFFFFFFF);
|
||||
trb.Parameter1 = (uint32_t)(dataPhys >> 32);
|
||||
trb.Status = length;
|
||||
|
||||
uint32_t control = (TRB_NORMAL << TRB_TYPE_SHIFT) | TRB_IOC | TRB_ISP;
|
||||
if (dev.BulkInRingCCS) {
|
||||
control |= TRB_CYCLE_BIT;
|
||||
}
|
||||
trb.Control = control;
|
||||
|
||||
AdvanceBulkInRing(dev);
|
||||
|
||||
// Ring doorbell: DCI for bulk IN = EpNum * 2 + 1
|
||||
uint8_t target = dev.BulkInEpNum * 2 + 1;
|
||||
WriteDoorbell(slotId, target);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// QueueBulkOutTransfer
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
void QueueBulkOutTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length) {
|
||||
if (slotId == 0 || slotId > MAX_SLOTS || !g_devices[slotId].Active) return;
|
||||
|
||||
UsbDeviceInfo& dev = g_devices[slotId];
|
||||
if (!dev.BulkOutRing || dev.BulkOutEpNum == 0) return;
|
||||
|
||||
TRB& trb = dev.BulkOutRing[dev.BulkOutRingEnqueue];
|
||||
trb.Parameter0 = (uint32_t)(dataPhys & 0xFFFFFFFF);
|
||||
trb.Parameter1 = (uint32_t)(dataPhys >> 32);
|
||||
trb.Status = length;
|
||||
|
||||
uint32_t control = (TRB_NORMAL << TRB_TYPE_SHIFT) | TRB_IOC;
|
||||
if (dev.BulkOutRingCCS) {
|
||||
control |= TRB_CYCLE_BIT;
|
||||
}
|
||||
trb.Control = control;
|
||||
|
||||
AdvanceBulkOutRing(dev);
|
||||
|
||||
// Ring doorbell: DCI for bulk OUT = EpNum * 2
|
||||
uint8_t target = dev.BulkOutEpNum * 2;
|
||||
WriteDoorbell(slotId, target);
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// RegisterTransferCallback
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
void RegisterTransferCallback(uint8_t slotId, TransferCallback cb) {
|
||||
if (slotId > 0 && slotId <= MAX_SLOTS) {
|
||||
g_transferCallbacks[slotId] = cb;
|
||||
}
|
||||
}
|
||||
|
||||
// -------------------------------------------------------------------------
|
||||
// RingDoorbell
|
||||
// -------------------------------------------------------------------------
|
||||
|
||||
@@ -138,6 +138,8 @@ namespace Drivers::USB::Xhci {
|
||||
constexpr uint32_t TRB_CONFIGURE_ENDPOINT = 12;
|
||||
constexpr uint32_t TRB_EVALUATE_CONTEXT = 13;
|
||||
constexpr uint32_t TRB_RESET_ENDPOINT = 14;
|
||||
constexpr uint32_t TRB_STOP_ENDPOINT = 15;
|
||||
constexpr uint32_t TRB_SET_TR_DEQUEUE = 16;
|
||||
constexpr uint32_t TRB_NOOP_CMD = 23;
|
||||
constexpr uint32_t TRB_TRANSFER_EVENT = 32;
|
||||
constexpr uint32_t TRB_COMMAND_COMPLETION = 33;
|
||||
@@ -235,6 +237,7 @@ namespace Drivers::USB::Xhci {
|
||||
uint8_t InterfaceClass;
|
||||
uint8_t InterfaceSubClass;
|
||||
uint8_t InterfaceProtocol;
|
||||
uint8_t DeviceClass; // bDeviceClass from device descriptor
|
||||
|
||||
// Interrupt IN endpoint
|
||||
uint8_t InterruptEpNum; // Endpoint number (1-15)
|
||||
@@ -247,6 +250,22 @@ namespace Drivers::USB::Xhci {
|
||||
uint32_t InterruptRingEnqueue;
|
||||
bool InterruptRingCCS; // Current Cycle State
|
||||
|
||||
// Bulk IN endpoint
|
||||
uint8_t BulkInEpNum;
|
||||
uint16_t BulkInMaxPacket;
|
||||
TRB* BulkInRing;
|
||||
uint64_t BulkInRingPhys;
|
||||
uint32_t BulkInRingEnqueue;
|
||||
bool BulkInRingCCS;
|
||||
|
||||
// Bulk OUT endpoint
|
||||
uint8_t BulkOutEpNum;
|
||||
uint16_t BulkOutMaxPacket;
|
||||
TRB* BulkOutRing;
|
||||
uint64_t BulkOutRingPhys;
|
||||
uint32_t BulkOutRingEnqueue;
|
||||
bool BulkOutRingCCS;
|
||||
|
||||
// EP0 transfer ring
|
||||
TRB* EP0Ring;
|
||||
uint64_t EP0RingPhys;
|
||||
@@ -258,6 +277,14 @@ namespace Drivers::USB::Xhci {
|
||||
uint64_t OutputContextPhys;
|
||||
};
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Transfer callback for non-HID class drivers (Bluetooth, etc.)
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
using TransferCallback = void (*)(uint8_t slotId, uint8_t epDci,
|
||||
const uint8_t* data, uint32_t length,
|
||||
uint32_t completionCode);
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Public API
|
||||
// ---------------------------------------------------------------------------
|
||||
@@ -285,6 +312,13 @@ namespace Drivers::USB::Xhci {
|
||||
// Queue an interrupt IN transfer on a device's interrupt endpoint
|
||||
void QueueInterruptTransfer(uint8_t slotId);
|
||||
|
||||
// Queue a bulk transfer on a device's bulk IN or OUT endpoint
|
||||
void QueueBulkInTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length);
|
||||
void QueueBulkOutTransfer(uint8_t slotId, uint8_t* data, uint64_t dataPhys, uint32_t length);
|
||||
|
||||
// Register a transfer callback for a specific slot (used by non-HID class drivers)
|
||||
void RegisterTransferCallback(uint8_t slotId, TransferCallback cb);
|
||||
|
||||
// Ring a doorbell
|
||||
void RingDoorbell(uint8_t slotId, uint8_t target);
|
||||
|
||||
|
||||
Reference in New Issue
Block a user