423 lines
17 KiB
C++
423 lines
17 KiB
C++
/*
|
|
* 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;
|
|
}
|
|
|
|
}
|