680 lines
31 KiB
C++
680 lines
31 KiB
C++
/*
|
|
* L2cap.cpp
|
|
* Bluetooth L2CAP implementation
|
|
* Copyright (c) 2026 Daniel Hammer
|
|
*/
|
|
|
|
#include "L2cap.hpp"
|
|
#include <Arch/Cpu.hpp>
|
|
#include "Hci.hpp"
|
|
#include "A2dp.hpp"
|
|
#include "Avrcp.hpp"
|
|
#include <Drivers/USB/Xhci.hpp>
|
|
#include <Terminal/Terminal.hpp>
|
|
#include <CppLib/Stream.hpp>
|
|
#include <Libraries/Memory.hpp>
|
|
#include <Timekeeping/Timer.hpp>
|
|
|
|
using namespace Kt;
|
|
|
|
namespace Drivers::USB::Bluetooth::L2cap {
|
|
|
|
// =========================================================================
|
|
// State
|
|
// =========================================================================
|
|
|
|
static uint16_t g_aclHandle = 0;
|
|
static bool g_initialized = false;
|
|
static uint8_t g_sigIdentifier = 1;
|
|
|
|
// Channel table
|
|
static ChannelInfo g_channels[MAX_CHANNELS] = {};
|
|
static uint16_t g_nextCid = CID_DYNAMIC_START;
|
|
|
|
// =========================================================================
|
|
// Helpers
|
|
// =========================================================================
|
|
|
|
static uint8_t NextIdentifier() {
|
|
uint8_t id = g_sigIdentifier++;
|
|
if (id == 0) id = g_sigIdentifier++;
|
|
if (g_sigIdentifier == 0) g_sigIdentifier = 1;
|
|
return id;
|
|
}
|
|
|
|
static uint16_t AllocCid() {
|
|
// Dynamic CIDs are 0x0040..0xFFFF and must not collide with an active
|
|
// channel after wraparound.
|
|
for (uint32_t tries = 0; tries <= 0xFFC0; tries++) {
|
|
if (g_nextCid < CID_DYNAMIC_START) g_nextCid = CID_DYNAMIC_START;
|
|
uint16_t candidate = g_nextCid++;
|
|
bool used = false;
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == candidate) {
|
|
used = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!used) return candidate;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
static ChannelInfo* AllocChannel(uint16_t psm) {
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (!g_channels[i].Active) {
|
|
g_channels[i].Active = true;
|
|
g_channels[i].LocalCid = AllocCid();
|
|
if (g_channels[i].LocalCid == 0) {
|
|
g_channels[i].Active = false;
|
|
return nullptr;
|
|
}
|
|
g_channels[i].RemoteCid = 0;
|
|
g_channels[i].Psm = psm;
|
|
g_channels[i].RemoteMtu = 672; // Default L2CAP MTU
|
|
g_channels[i].Configured = false;
|
|
g_channels[i].LocalConfigDone = false;
|
|
g_channels[i].RemoteConfigDone = false;
|
|
g_channels[i].ConnRspResult = 0xFFFF;
|
|
g_channels[i].ConnRspStatus = 0;
|
|
return &g_channels[i];
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
// Send L2CAP signaling command
|
|
static bool SendSignal(uint8_t code, uint8_t identifier,
|
|
const uint8_t* payload, uint16_t payloadLen) {
|
|
// L2CAP header + Signal header + payload
|
|
uint16_t sigLen = sizeof(SignalHeader) + payloadLen;
|
|
uint16_t totalPayload = sizeof(L2capHeader) + sigLen;
|
|
|
|
uint8_t buf[128] = {};
|
|
if (totalPayload > sizeof(buf)) return false;
|
|
auto* l2hdr = (L2capHeader*)buf;
|
|
l2hdr->Length = sigLen;
|
|
l2hdr->ChannelId = CID_SIGNALING;
|
|
|
|
auto* sig = (SignalHeader*)(buf + sizeof(L2capHeader));
|
|
sig->Code = code;
|
|
sig->Identifier = identifier;
|
|
sig->Length = payloadLen;
|
|
|
|
if (payload && payloadLen > 0) {
|
|
memcpy(buf + sizeof(L2capHeader) + sizeof(SignalHeader), payload, payloadLen);
|
|
}
|
|
|
|
return Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_NON_FLUSH,
|
|
buf, totalPayload);
|
|
}
|
|
|
|
// =========================================================================
|
|
// Initialize
|
|
// =========================================================================
|
|
|
|
void Initialize(uint16_t aclHandle) {
|
|
g_aclHandle = aclHandle;
|
|
g_initialized = true;
|
|
g_sigIdentifier = 1;
|
|
g_nextCid = CID_DYNAMIC_START;
|
|
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active)
|
|
Avrcp::OnChannelClosed(g_channels[i].LocalCid);
|
|
g_channels[i].Active = false;
|
|
}
|
|
|
|
KernelLogStream(OK, "BT-L2CAP") << "Initialized for ACL handle " << (uint64_t)aclHandle;
|
|
}
|
|
|
|
void OnDisconnected(uint16_t aclHandle) {
|
|
if (!g_initialized || aclHandle != g_aclHandle) return;
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active)
|
|
Avrcp::OnChannelClosed(g_channels[i].LocalCid);
|
|
g_channels[i].Active = false;
|
|
}
|
|
g_aclHandle = 0;
|
|
g_initialized = false;
|
|
}
|
|
|
|
// =========================================================================
|
|
// ProcessPacket
|
|
// =========================================================================
|
|
|
|
void ProcessPacket(uint16_t aclHandle, const uint8_t* data, uint16_t len) {
|
|
if (!g_initialized || aclHandle != g_aclHandle) return;
|
|
if (len < sizeof(L2capHeader)) return;
|
|
|
|
auto* l2hdr = (const L2capHeader*)data;
|
|
uint16_t l2len = l2hdr->Length;
|
|
uint16_t cid = l2hdr->ChannelId;
|
|
const uint8_t* payload = data + sizeof(L2capHeader);
|
|
|
|
if (l2len + sizeof(L2capHeader) > len) return;
|
|
|
|
if (cid == CID_SIGNALING) {
|
|
// L2CAP signaling channel. One packet may carry MULTIPLE commands
|
|
// back to back -- a headset commonly bundles its Config Response
|
|
// (to our request) with its own Config Request in a single packet.
|
|
// Loop over all of them, or we'd handle one and silently drop the
|
|
// rest (which left our channel half-configured: localCfg=0).
|
|
uint16_t sOff = 0;
|
|
while (sOff + sizeof(SignalHeader) <= l2len) {
|
|
auto* sig = (const SignalHeader*)(payload + sOff);
|
|
const uint8_t* sigPayload = payload + sOff + sizeof(SignalHeader);
|
|
uint16_t sigPayloadLen = sig->Length;
|
|
// Clamp (don't bail) so a length that doesn't exactly match the
|
|
// packet never skips this command -- the cases bounds-check their
|
|
// own reads. Advancing by the clamped length ends the loop after
|
|
// the last real command.
|
|
uint16_t avail = (uint16_t)(l2len - sOff - sizeof(SignalHeader));
|
|
if (sigPayloadLen > avail) sigPayloadLen = avail;
|
|
|
|
switch (sig->Code) {
|
|
case SIG_CONN_REQ: {
|
|
if (sigPayloadLen >= 4) {
|
|
uint16_t psm = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
|
uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8);
|
|
|
|
KernelLogStream(INFO, "BT-L2CAP") << "Connection Request: PSM="
|
|
<< base::hex << (uint64_t)psm << " srcCID=" << (uint64_t)srcCid;
|
|
|
|
// Accept connections for AVDTP, SDP, and AVRCP control
|
|
if (psm == PSM_AVDTP || psm == PSM_SDP || psm == PSM_AVCTP) {
|
|
// Detect a duplicate source CID instead of creating
|
|
// two local channels that address the same endpoint.
|
|
// Same-PSM duplicates are normal retransmissions;
|
|
// a different PSM reusing the CID is invalid.
|
|
ChannelInfo* existing = nullptr;
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active
|
|
&& g_channels[i].RemoteCid == srcCid) {
|
|
existing = &g_channels[i];
|
|
break;
|
|
}
|
|
}
|
|
auto* ch = existing ? nullptr : AllocChannel(psm);
|
|
if (existing && existing->Psm == psm) {
|
|
// Retransmission of a request whose response was
|
|
// lost: repeat SUCCESS for the original channel,
|
|
// and repeat our config request as well.
|
|
uint8_t rsp[8] = {
|
|
(uint8_t)existing->LocalCid,
|
|
(uint8_t)(existing->LocalCid >> 8),
|
|
(uint8_t)srcCid, (uint8_t)(srcCid >> 8),
|
|
0, 0, 0, 0
|
|
};
|
|
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
|
uint8_t cfgReq[4] = {
|
|
(uint8_t)srcCid, (uint8_t)(srcCid >> 8), 0, 0
|
|
};
|
|
SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4);
|
|
} else if (existing) {
|
|
uint8_t rsp[8] = {
|
|
0, 0, (uint8_t)srcCid, (uint8_t)(srcCid >> 8),
|
|
0x07, 0x00, 0, 0 // source CID already allocated
|
|
};
|
|
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
|
} else if (ch) {
|
|
ch->RemoteCid = srcCid;
|
|
|
|
// Send Connection Response (success)
|
|
uint8_t rsp[8] = {};
|
|
rsp[0] = (uint8_t)(ch->LocalCid & 0xFF);
|
|
rsp[1] = (uint8_t)(ch->LocalCid >> 8);
|
|
rsp[2] = (uint8_t)(srcCid & 0xFF);
|
|
rsp[3] = (uint8_t)(srcCid >> 8);
|
|
rsp[4] = 0; rsp[5] = 0; // Result: success
|
|
rsp[6] = 0; rsp[7] = 0; // Status: no info
|
|
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
|
|
|
// As the acceptor we must ALSO send our own
|
|
// Configuration Request -- otherwise our side
|
|
// never reaches LocalConfigDone and the channel
|
|
// never becomes Configured (OnChannelReady never
|
|
// fires). Address the remote's CID.
|
|
uint8_t cfgReq[4] = {};
|
|
cfgReq[0] = (uint8_t)(srcCid & 0xFF);
|
|
cfgReq[1] = (uint8_t)(srcCid >> 8);
|
|
cfgReq[2] = 0; cfgReq[3] = 0; // Flags
|
|
SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4);
|
|
} else {
|
|
uint8_t rsp[8] = {
|
|
0, 0, (uint8_t)srcCid, (uint8_t)(srcCid >> 8),
|
|
0x04, 0x00, 0, 0 // no resources
|
|
};
|
|
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
|
}
|
|
} else {
|
|
// Reject: PSM not supported
|
|
uint8_t rsp[8] = {};
|
|
rsp[0] = 0; rsp[1] = 0; // Dest CID = 0
|
|
rsp[2] = (uint8_t)(srcCid & 0xFF);
|
|
rsp[3] = (uint8_t)(srcCid >> 8);
|
|
rsp[4] = 0x02; rsp[5] = 0; // Result: PSM not supported
|
|
rsp[6] = 0; rsp[7] = 0;
|
|
SendSignal(SIG_CONN_RSP, sig->Identifier, rsp, 8);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SIG_CONN_RSP: {
|
|
if (sigPayloadLen >= 8) {
|
|
uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
|
uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8);
|
|
uint16_t result = (uint16_t)sigPayload[4] | ((uint16_t)sigPayload[5] << 8);
|
|
// Status (bytes 6-7) explains a PENDING (result=1): 0=no
|
|
// info, 1=authentication pending, 2=authorization pending.
|
|
uint16_t status = (sigPayloadLen >= 8)
|
|
? ((uint16_t)sigPayload[6] | ((uint16_t)sigPayload[7] << 8)) : 0;
|
|
KernelLogStream(INFO, "BT-L2CAP") << "Connection Response: dstCID="
|
|
<< base::hex << (uint64_t)dstCid << " result=" << (uint64_t)result
|
|
<< " status=" << (uint64_t)status << base::dec;
|
|
|
|
if (result == CONN_SUCCESS) {
|
|
// Find our channel by srcCid (which is our local CID)
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) {
|
|
g_channels[i].ConnRspResult = result;
|
|
g_channels[i].ConnRspStatus = status;
|
|
g_channels[i].RemoteCid = dstCid;
|
|
|
|
// Send Configuration Request
|
|
uint8_t cfgReq[4] = {};
|
|
cfgReq[0] = (uint8_t)(dstCid & 0xFF);
|
|
cfgReq[1] = (uint8_t)(dstCid >> 8);
|
|
cfgReq[2] = 0; cfgReq[3] = 0; // Flags
|
|
SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4);
|
|
break;
|
|
}
|
|
}
|
|
} else if (result == CONN_PENDING && dstCid != 0) {
|
|
// A PENDING response may already carry the remote's
|
|
// allocated CID. Record it (but do NOT configure --
|
|
// the final SUCCESS on this channel does that): the
|
|
// diagnostics then show the peer engaged, and if the
|
|
// dial is later abandoned, FreeChannel knows to send
|
|
// a Disconnect for the half-open remote endpoint.
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) {
|
|
g_channels[i].ConnRspResult = result;
|
|
g_channels[i].ConnRspStatus = status;
|
|
g_channels[i].RemoteCid = dstCid;
|
|
break;
|
|
}
|
|
}
|
|
} else {
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active
|
|
&& g_channels[i].LocalCid == srcCid) {
|
|
g_channels[i].ConnRspResult = result;
|
|
g_channels[i].ConnRspStatus = status;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SIG_CONFIG_REQ: {
|
|
if (sigPayloadLen >= 4) {
|
|
uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
|
|
|
// Find channel
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == dstCid) {
|
|
g_channels[i].RemoteConfigDone = true;
|
|
|
|
// Parse MTU option if present
|
|
uint16_t cfgOffset = 4; // Skip dstCid + flags
|
|
while (cfgOffset + 2 <= sigPayloadLen) {
|
|
uint8_t optType = sigPayload[cfgOffset];
|
|
uint8_t optLen = sigPayload[cfgOffset + 1];
|
|
if (optType == 0x01 && optLen == 2 && cfgOffset + 4 <= sigPayloadLen) {
|
|
g_channels[i].RemoteMtu = (uint16_t)sigPayload[cfgOffset + 2]
|
|
| ((uint16_t)sigPayload[cfgOffset + 3] << 8);
|
|
}
|
|
cfgOffset += 2 + optLen;
|
|
}
|
|
|
|
// Send Config Response (success)
|
|
uint8_t rsp[6] = {};
|
|
rsp[0] = (uint8_t)(g_channels[i].RemoteCid & 0xFF);
|
|
rsp[1] = (uint8_t)(g_channels[i].RemoteCid >> 8);
|
|
rsp[2] = 0; rsp[3] = 0; // Flags
|
|
rsp[4] = 0; rsp[5] = 0; // Result: success
|
|
SendSignal(SIG_CONFIG_RSP, sig->Identifier, rsp, 6);
|
|
|
|
if (g_channels[i].LocalConfigDone && g_channels[i].RemoteConfigDone) {
|
|
g_channels[i].Configured = true;
|
|
KernelLogStream(OK, "BT-L2CAP") << "Channel "
|
|
<< (uint64_t)g_channels[i].LocalCid << " configured";
|
|
|
|
// Notify A2DP if this is an AVDTP channel
|
|
if (g_channels[i].Psm == PSM_AVDTP) {
|
|
A2dp::OnChannelReady(g_channels[i].LocalCid);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SIG_CONFIG_RSP: {
|
|
if (sigPayloadLen >= 6) {
|
|
// The Config Response's first field is the "Source CID".
|
|
// In a RECEIVED Config Response it echoes back OUR local
|
|
// channel endpoint (the device that sent the Config
|
|
// Request -- us), NOT the remote's CID -- exactly like the
|
|
// SIG_CONN_RSP handler above matches srcCid to LocalCid.
|
|
// Matching RemoteCid here was THE bug that pinned every
|
|
// AVDTP channel at localCfg=0: a success Config Response
|
|
// arrived but matched no channel, so LocalConfigDone never
|
|
// got set and the channel never became Configured.
|
|
uint16_t srcCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
|
uint16_t result = (uint16_t)sigPayload[4] | ((uint16_t)sigPayload[5] << 8);
|
|
|
|
if (result != CFG_SUCCESS) {
|
|
KernelLogStream(WARNING, "BT-L2CAP") << "Config Response result="
|
|
<< base::hex << (uint64_t)result << base::dec
|
|
<< " (our config rejected)";
|
|
}
|
|
|
|
if (result == CFG_SUCCESS) {
|
|
bool matched = false;
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == srcCid) {
|
|
matched = true;
|
|
g_channels[i].LocalConfigDone = true;
|
|
if (g_channels[i].LocalConfigDone && g_channels[i].RemoteConfigDone) {
|
|
g_channels[i].Configured = true;
|
|
KernelLogStream(OK, "BT-L2CAP") << "Channel "
|
|
<< (uint64_t)g_channels[i].LocalCid << " configured";
|
|
|
|
if (g_channels[i].Psm == PSM_AVDTP) {
|
|
A2dp::OnChannelReady(g_channels[i].LocalCid);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
if (!matched) {
|
|
KernelLogStream(WARNING, "BT-L2CAP")
|
|
<< "Config Response (success) for unknown CID="
|
|
<< base::hex << (uint64_t)srcCid << base::dec;
|
|
}
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SIG_DISCONN_REQ: {
|
|
if (sigPayloadLen >= 4) {
|
|
uint16_t dstCid = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
|
uint16_t srcCid = (uint16_t)sigPayload[2] | ((uint16_t)sigPayload[3] << 8);
|
|
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == dstCid) {
|
|
uint16_t closedCid = g_channels[i].LocalCid;
|
|
g_channels[i].Active = false;
|
|
A2dp::OnChannelClosed(closedCid);
|
|
Avrcp::OnChannelClosed(closedCid);
|
|
break;
|
|
}
|
|
}
|
|
|
|
// Send Disconnect Response
|
|
uint8_t rsp[4] = {};
|
|
rsp[0] = (uint8_t)(dstCid & 0xFF);
|
|
rsp[1] = (uint8_t)(dstCid >> 8);
|
|
rsp[2] = (uint8_t)(srcCid & 0xFF);
|
|
rsp[3] = (uint8_t)(srcCid >> 8);
|
|
SendSignal(SIG_DISCONN_RSP, sig->Identifier, rsp, 4);
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SIG_INFO_REQ: {
|
|
if (sigPayloadLen >= 2) {
|
|
uint16_t infoType = (uint16_t)sigPayload[0] | ((uint16_t)sigPayload[1] << 8);
|
|
|
|
if (infoType == 0x0002) {
|
|
// Extended features mask
|
|
uint8_t rsp[8] = {};
|
|
rsp[0] = 0x02; rsp[1] = 0x00; // InfoType
|
|
rsp[2] = 0x00; rsp[3] = 0x00; // Result: success
|
|
rsp[4] = 0x00; rsp[5] = 0x00; // Features: none
|
|
rsp[6] = 0x00; rsp[7] = 0x00;
|
|
SendSignal(SIG_INFO_RSP, sig->Identifier, rsp, 8);
|
|
} else {
|
|
// Not supported
|
|
uint8_t rsp[4] = {};
|
|
rsp[0] = (uint8_t)(infoType & 0xFF);
|
|
rsp[1] = (uint8_t)(infoType >> 8);
|
|
rsp[2] = 0x01; rsp[3] = 0x00; // Result: not supported
|
|
SendSignal(SIG_INFO_RSP, sig->Identifier, rsp, 4);
|
|
}
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SIG_DISCONN_RSP:
|
|
// Response to a Disconnect Request we sent (e.g. closing the
|
|
// SDP client channel after a query); nothing left to do.
|
|
break;
|
|
|
|
case SIG_COMMAND_REJECT:
|
|
case SIG_ECHO_RSP:
|
|
case SIG_INFO_RSP:
|
|
// Terminal responses to requests sent by us. No response
|
|
// is required; in particular, never reject a response.
|
|
break;
|
|
|
|
case SIG_ECHO_REQ: {
|
|
// Echo Request must be answered (echo the payload back) or
|
|
// a peer probing the link times out on us.
|
|
SendSignal(SIG_ECHO_RSP, sig->Identifier, sigPayload, sigPayloadLen);
|
|
break;
|
|
}
|
|
|
|
default: {
|
|
// A signaling command we do not handle. Log it: a peer that
|
|
// sends something exotic while holding our dials at
|
|
// "authorization pending" is exactly what we need to see.
|
|
KernelLogStream(INFO, "BT-L2CAP") << "Unhandled signaling code="
|
|
<< base::hex << (uint64_t)sig->Code << base::dec
|
|
<< " len=" << (uint64_t)sigPayloadLen;
|
|
// Command Reject, reason 0x0000 = command not understood.
|
|
// Silence leaves the peer waiting for its signaling timer.
|
|
uint8_t reject[2] = {0x00, 0x00};
|
|
SendSignal(SIG_COMMAND_REJECT, sig->Identifier, reject, 2);
|
|
break;
|
|
}
|
|
}
|
|
sOff += sizeof(SignalHeader) + sigPayloadLen;
|
|
}
|
|
} else {
|
|
// Data on a dynamic channel
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == cid) {
|
|
if (g_channels[i].Psm == PSM_AVDTP) {
|
|
A2dp::ProcessAvdtp(cid, payload, l2len);
|
|
} else if (g_channels[i].Psm == PSM_SDP) {
|
|
A2dp::ProcessSdp(cid, payload, l2len);
|
|
} else if (g_channels[i].Psm == PSM_AVCTP) {
|
|
Avrcp::ProcessAvctp(cid, payload, l2len);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// =========================================================================
|
|
// Connect
|
|
// =========================================================================
|
|
|
|
uint16_t Connect(uint16_t psm) {
|
|
if (!g_initialized) return 0;
|
|
|
|
auto* ch = AllocChannel(psm);
|
|
if (!ch) return 0;
|
|
|
|
// Send Connection Request
|
|
uint8_t req[4] = {};
|
|
req[0] = (uint8_t)(psm & 0xFF);
|
|
req[1] = (uint8_t)(psm >> 8);
|
|
req[2] = (uint8_t)(ch->LocalCid & 0xFF);
|
|
req[3] = (uint8_t)(ch->LocalCid >> 8);
|
|
if (!SendSignal(SIG_CONN_REQ, NextIdentifier(), req, 4)) {
|
|
ch->Active = false;
|
|
return 0;
|
|
}
|
|
|
|
return ch->LocalCid;
|
|
}
|
|
|
|
// =========================================================================
|
|
// WaitConfigured
|
|
// =========================================================================
|
|
|
|
bool WaitConfigured(uint16_t localCid, uint32_t timeoutMs) {
|
|
uint64_t start = Timekeeping::GetMilliseconds();
|
|
|
|
while (Timekeeping::GetMilliseconds() - start < timeoutMs) {
|
|
Xhci::PollEvents();
|
|
Hci::DrainEvents(); // process incoming ACL (the Connection Response!)
|
|
|
|
auto* ch = GetChannel(localCid);
|
|
if (ch && ch->Configured) return true;
|
|
|
|
for (int j = 0; j < 100; j++) {
|
|
Arch::CompilerBarrier();
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
// =========================================================================
|
|
// SendData
|
|
// =========================================================================
|
|
|
|
bool SendData(uint16_t localCid, const uint8_t* data, uint16_t len) {
|
|
if (!g_initialized) return false;
|
|
|
|
auto* ch = GetChannel(localCid);
|
|
if (!ch || !ch->Configured) return false;
|
|
|
|
// Build L2CAP packet
|
|
uint16_t totalLen = sizeof(L2capHeader) + len;
|
|
uint8_t buf[1024] = {};
|
|
if (totalLen > sizeof(buf)) return false;
|
|
|
|
auto* l2hdr = (L2capHeader*)buf;
|
|
l2hdr->Length = len;
|
|
l2hdr->ChannelId = ch->RemoteCid;
|
|
|
|
if (data && len > 0) {
|
|
memcpy(buf + sizeof(L2capHeader), data, len);
|
|
}
|
|
|
|
return Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_FLUSH, buf, totalLen);
|
|
}
|
|
|
|
// =========================================================================
|
|
// Channel queries
|
|
// =========================================================================
|
|
|
|
ChannelInfo* GetChannel(uint16_t localCid) {
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == localCid) {
|
|
return &g_channels[i];
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
bool FreeChannel(uint16_t localCid) {
|
|
// Reclaim a channel slot from a failed/abandoned dial during A2DP
|
|
// bring-up retries. Two cases:
|
|
// - RemoteCid == 0: the remote never acknowledged our CONN_REQ (ignored,
|
|
// or answered only with a stuck PENDING that never completed), so its
|
|
// side was never created and no Disconnect is owed -- just free it.
|
|
// - RemoteCid != 0: a CONN_RSP success arrived (the peer allocated its
|
|
// CID) but the channel never finished configuring, so the peer holds a
|
|
// half-open channel. Send it a Disconnect Request before freeing, or
|
|
// BOTH our slot leaks (the old RemoteCid==0-only guard left it Active)
|
|
// AND the peer keeps a dangling channel.
|
|
// We deliberately do NOT reset g_nextCid, so a late response for this
|
|
// (now freed) CID matches nothing rather than cross-wiring the retry.
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].LocalCid == localCid) {
|
|
if (g_channels[i].RemoteCid != 0) {
|
|
uint8_t req[4] = {};
|
|
req[0] = (uint8_t)(g_channels[i].RemoteCid & 0xFF);
|
|
req[1] = (uint8_t)(g_channels[i].RemoteCid >> 8);
|
|
req[2] = (uint8_t)(g_channels[i].LocalCid & 0xFF);
|
|
req[3] = (uint8_t)(g_channels[i].LocalCid >> 8);
|
|
SendSignal(SIG_DISCONN_REQ, NextIdentifier(), req, 4);
|
|
}
|
|
g_channels[i].Active = false;
|
|
A2dp::OnChannelClosed(localCid);
|
|
Avrcp::OnChannelClosed(localCid);
|
|
return true;
|
|
}
|
|
}
|
|
return false;
|
|
}
|
|
|
|
ChannelInfo* FindChannelByPsm(uint16_t psm) {
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].Psm == psm) {
|
|
return &g_channels[i];
|
|
}
|
|
}
|
|
return nullptr;
|
|
}
|
|
|
|
uint16_t FindConfiguredAvdtpChannelExcept(uint16_t exceptCid) {
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].Configured
|
|
&& g_channels[i].Psm == PSM_AVDTP
|
|
&& g_channels[i].LocalCid != exceptCid) {
|
|
return g_channels[i].LocalCid;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
uint16_t FindAvdtpChannelExcept(uint16_t exceptCid) {
|
|
for (int i = 0; i < MAX_CHANNELS; i++) {
|
|
if (g_channels[i].Active && g_channels[i].Psm == PSM_AVDTP
|
|
&& g_channels[i].LocalCid != exceptCid) {
|
|
return g_channels[i].LocalCid;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
uint16_t GetAclHandle() {
|
|
return g_aclHandle;
|
|
}
|
|
|
|
uint16_t ConnRspResult(uint16_t localCid) {
|
|
auto* ch = GetChannel(localCid);
|
|
return ch ? ch->ConnRspResult : 0xFFFF;
|
|
}
|
|
|
|
uint16_t ConnRspStatus(uint16_t localCid) {
|
|
auto* ch = GetChannel(localCid);
|
|
return ch ? ch->ConnRspStatus : 0;
|
|
}
|
|
|
|
}
|