fix: improve Bluetooth reliability
This commit is contained in:
daniel committed
1 parent
4627ac92fd
commit
18122136dd
20 files changed
+1870
-587
No files matched your search
@@ -26,11 +26,6 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
static bool g_initialized = false;
|
||||
static uint8_t g_sigIdentifier = 1;
|
||||
|
||||
// Diagnostic: result of the most recent L2CAP Connection Response we got for
|
||||
// an outgoing connection (0=success, 1=pending, 2=PSM-unsupported,
|
||||
// 3=security-block, 4=no-resources). 0xFFFF = none received yet.
|
||||
static volatile uint16_t g_lastConnRspResult = 0xFFFF;
|
||||
|
||||
// Channel table
|
||||
static ChannelInfo g_channels[MAX_CHANNELS] = {};
|
||||
static uint16_t g_nextCid = CID_DYNAMIC_START;
|
||||
@@ -39,8 +34,29 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
// 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() {
|
||||
return g_nextCid++;
|
||||
// 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) {
|
||||
@@ -48,12 +64,18 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
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];
|
||||
}
|
||||
}
|
||||
@@ -61,13 +83,14 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
}
|
||||
|
||||
// Send L2CAP signaling command
|
||||
static void SendSignal(uint8_t code, uint8_t identifier,
|
||||
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;
|
||||
@@ -81,8 +104,8 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
memcpy(buf + sizeof(L2capHeader) + sizeof(SignalHeader), payload, payloadLen);
|
||||
}
|
||||
|
||||
Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_FLUSH,
|
||||
buf, totalPayload);
|
||||
return Hci::SendAcl(g_aclHandle, Hci::ACL_PB_FIRST_NON_FLUSH,
|
||||
buf, totalPayload);
|
||||
}
|
||||
|
||||
// =========================================================================
|
||||
@@ -96,17 +119,31 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
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;
|
||||
@@ -145,8 +182,41 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
|
||||
// Accept connections for AVDTP, SDP, and AVRCP control
|
||||
if (psm == PSM_AVDTP || psm == PSM_SDP || psm == PSM_AVCTP) {
|
||||
auto* ch = AllocChannel(psm);
|
||||
if (ch) {
|
||||
// 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)
|
||||
@@ -168,7 +238,13 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
cfgReq[0] = (uint8_t)(srcCid & 0xFF);
|
||||
cfgReq[1] = (uint8_t)(srcCid >> 8);
|
||||
cfgReq[2] = 0; cfgReq[3] = 0; // Flags
|
||||
SendSignal(SIG_CONFIG_REQ, g_sigIdentifier++, cfgReq, 4);
|
||||
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
|
||||
@@ -193,8 +269,6 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
// info, 1=authentication pending, 2=authorization pending.
|
||||
uint16_t status = (sigPayloadLen >= 8)
|
||||
? ((uint16_t)sigPayload[6] | ((uint16_t)sigPayload[7] << 8)) : 0;
|
||||
g_lastConnRspResult = result;
|
||||
|
||||
KernelLogStream(INFO, "BT-L2CAP") << "Connection Response: dstCID="
|
||||
<< base::hex << (uint64_t)dstCid << " result=" << (uint64_t)result
|
||||
<< " status=" << (uint64_t)status << base::dec;
|
||||
@@ -203,6 +277,8 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
// 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
|
||||
@@ -210,7 +286,7 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
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);
|
||||
SendSignal(SIG_CONFIG_REQ, NextIdentifier(), cfgReq, 4);
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -223,10 +299,21 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
// 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;
|
||||
@@ -333,7 +420,10 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -378,6 +468,13 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
// 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.
|
||||
@@ -392,6 +489,10 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
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;
|
||||
}
|
||||
}
|
||||
@@ -402,7 +503,7 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
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);
|
||||
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) {
|
||||
@@ -421,8 +522,6 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
uint16_t Connect(uint16_t psm) {
|
||||
if (!g_initialized) return 0;
|
||||
|
||||
g_lastConnRspResult = 0xFFFF; // reset diagnostic for this attempt
|
||||
|
||||
auto* ch = AllocChannel(psm);
|
||||
if (!ch) return 0;
|
||||
|
||||
@@ -432,7 +531,10 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
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);
|
||||
if (!SendSignal(SIG_CONN_REQ, NextIdentifier(), req, 4)) {
|
||||
ch->Active = false;
|
||||
return 0;
|
||||
}
|
||||
|
||||
return ch->LocalCid;
|
||||
}
|
||||
@@ -518,9 +620,11 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
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, g_sigIdentifier++, req, 4);
|
||||
SendSignal(SIG_DISCONN_REQ, NextIdentifier(), req, 4);
|
||||
}
|
||||
g_channels[i].Active = false;
|
||||
A2dp::OnChannelClosed(localCid);
|
||||
Avrcp::OnChannelClosed(localCid);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
@@ -547,12 +651,28 @@ namespace Drivers::USB::Bluetooth::L2cap {
|
||||
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 LastConnRspResult() {
|
||||
return g_lastConnRspResult;
|
||||
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;
|
||||
}
|
||||
|
||||
}
|
||||
Reference in new issue
Block a user