Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly
Sign up
Appearance settings

Commit f6c5238

Browse files
committed
Rewrite handleAclDataPkt to correctly handle fragments
1 parent 9263b3a commit f6c5238

File tree

2 files changed

+105
-55
lines changed

2 files changed

+105
-55
lines changed

‎src/utility/HCI.cpp‎

Lines changed: 103 additions & 54 deletions
Original file line numberDiff line numberDiff line change
@@ -101,7 +101,8 @@ String commandToString(LE_COMMAND command){
101101
HCIClass::HCIClass() :
102102
_debug(NULL),
103103
_recvIndex(0),
104-
_pendingPkt(0)
104+
_pendingPkt(0),
105+
_l2CapPduBufferSize(0)
105106
{
106107
}
107108

@@ -718,87 +719,135 @@ int HCIClass::sendCommand(uint16_t opcode, uint8_t plen, void* parameters)
718719
void HCIClass::handleAclDataPkt(uint8_t /*plen*/, uint8_t pdata[])
719720
{
720721
struct __attribute__ ((packed)) HCIACLHdr {
721-
uint16_t handle;
722-
uint16_t dlen;
723-
uint16_t len;
724-
uint16_t cid;
725-
} *aclHdr = (HCIACLHdr*)pdata;
722+
uint16_t connectionHandleWithFlags;
723+
uint16_t dlen; // dlen + 4 = plen (dlen is the size of the ACL SDU)
724+
} *aclHeader = (HCIACLHdr*)pdata;
726725

726+
uint8_t bcFlag = (aclHeader->connectionHandleWithFlags & 0xc000) >> 14;
727+
uint8_t pbFlag = (aclHeader->connectionHandleWithFlags & 0x3000) >> 12;
728+
uint16_t connectionHandle = aclHeader->connectionHandleWithFlags & 0x0fff;
727729

728-
uint16_t aclFlags = (aclHdr->handle & 0xf000) >> 12;
730+
uint8_t *aclSdu = &pdata[sizeof(HCIACLHdr)];
729731

730-
if ((aclHdr->dlen - 4) != aclHdr->len) {
731-
// packet is fragmented
732-
if (aclFlags != 0x01) {
733-
// copy into ACL buffer
734-
memcpy(_aclPktBuffer, &_recvBuffer[1], sizeof(HCIACLHdr) + aclHdr->dlen - 4);
735-
} else {
736-
// copy next chunk into the buffer
737-
HCIACLHdr* aclBufferHeader = (HCIACLHdr*)_aclPktBuffer;
732+
#ifdef _BLE_TRACE_
733+
Serial.print("Acl packet bcFlag = ");
734+
Serial.print(bcFlag, BIN);
735+
Serial.print(" pbFlag = ");
736+
Serial.print(pbFlag, BIN);
737+
Serial.print(" connectionHandle = ");
738+
Serial.print(connectionHandle, HEX);
739+
Serial.print(" dlen = ");
740+
Serial.println(aclHeader->dlen, DEC);
741+
#endif
738742

739-
memcpy(&_aclPktBuffer[sizeof(HCIACLHdr) + aclBufferHeader->dlen - 4], &_recvBuffer[1 + sizeof(aclHdr->handle) + sizeof(aclHdr->dlen)], aclHdr->dlen);
743+
// Pointer to the L2CAP PDU (might be reconstructed from multiple fragments)
744+
uint8_t *l2CapPdu;
745+
uint8_t l2CapPduSize;
740746

741-
aclBufferHeader->dlen += aclHdr->dlen;
742-
aclHdr = aclBufferHeader;
743-
}
744-
}
747+
if (pbFlag == 0b10) {
748+
// "First automatically flushable packet" = Start of our L2CAP PDU
749+
750+
l2CapPdu = aclSdu;
751+
l2CapPduSize = aclHeader->dlen;
752+
} else if (pbFlag == 0b01) {
753+
// "Continuing Fragment" = Continued L2CAP PDU
754+
#ifdef _BLE_TRACE_
755+
Serial.print("Continued packet. Appending to L2CAP PDU buffer (previously ");
756+
Serial.print(_l2CapPduBufferSize, DEC);
757+
Serial.println(" bytes in buffer)");
758+
#endif
759+
// If we receive a fragment, we always need to append it to the L2CAP PDU buffer
760+
memcpy(&_l2CapPduBuffer[_l2CapPduBufferSize], aclSdu, aclHeader->dlen);
761+
_l2CapPduBufferSize += aclHeader->dlen;
745762

746-
if ((aclHdr->dlen - 4) != aclHdr->len) {
763+
l2CapPdu = _l2CapPduBuffer;
764+
l2CapPduSize = _l2CapPduBufferSize;
765+
} else {
766+
// I don't think other values are allowed for BLE
747767
#ifdef _BLE_TRACE_
748-
Serial.println("Don't have full packet yet");
749-
Serial.print("Handle: ");
750-
btct.printBytes((uint8_t*)&aclHdr->handle,2);
751-
Serial.print("dlen: ");
752-
btct.printBytes((uint8_t*)&aclHdr->dlen,2);
753-
Serial.print("len: ");
754-
btct.printBytes((uint8_t*)&aclHdr->len,2);
755-
Serial.print("cid: ");
756-
btct.printBytes((uint8_t*)&aclHdr->cid,2);
768+
Serial.println("Invalid pbFlag, discarding packet");
757769
#endif
758-
// don't have the full packet yet
759770
return;
760771
}
761772

762-
if (aclHdr->cid == ATT_CID) {
763-
if (aclFlags == 0x01) {
764-
// use buffered packet
765-
ATT.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &_aclPktBuffer[sizeof(HCIACLHdr)]);
766-
} else {
767-
// use the recv buffer
768-
ATT.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]);
769-
}
770-
} else if (aclHdr->cid == SIGNALING_CID) {
773+
// We now have a valid L2CAP header in l2CapPdu and can parse the headers
774+
struct __attribute__ ((packed)) HCIL2CapHdr {
775+
uint16_t len; // size of the L2CAP SDU
776+
uint16_t cid;
777+
} *l2CapHeader = (HCIL2CapHdr*)l2CapPdu;
778+
771779
#ifdef _BLE_TRACE_
772-
Serial.println("Signaling");
780+
Serial.print("Received ");
781+
Serial.print(l2CapPduSize - 4, DEC);
782+
Serial.print("B/");
783+
Serial.print(l2CapHeader->len, DEC);
784+
Serial.print("B of the L2CAP SDU. CID = ");
785+
Serial.println(l2CapHeader->cid, HEX);
773786
#endif
774-
L2CAPSignaling.handleData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]);
775-
} elseif (aclHdr->cid == SECURITY_CID){
776-
// Security manager
787+
788+
// -4 because the buffer is the L2CAP PDU (with L2CAP header). The len field is only the L2CAP SDU (without L2CAP header).
789+
if (l2CapPduSize - 4 != l2CapHeader->len) {
777790
#ifdef _BLE_TRACE_
778-
Serial.println("Security data");
791+
Serial.println("L2CAP SDU incomplete");
779792
#endif
780-
if (aclFlags == 0x1){
781-
L2CAPSignaling.handleSecurityData(aclHdr->handle & 0x0fff, aclHdr->len, &_aclPktBuffer[sizeof(HCIACLHdr)]);
782-
}else{
783-
L2CAPSignaling.handleSecurityData(aclHdr->handle & 0x0fff, aclHdr->len, &_recvBuffer[1 + sizeof(HCIACLHdr)]);
793+
794+
// If this is a first packet, we have not copied it into the buffer yet
795+
if (pbFlag == 0b10) {
796+
#ifdef _BLE_TRACE_
797+
Serial.println("Storing first packet to L2CAP PDU buffer");
798+
if (_l2CapPduBufferSize != 0) {
799+
Serial.print("Warning: Discarding ");
800+
Serial.print(_l2CapPduBufferSize, DEC);
801+
Serial.println(" bytes from buffer");
802+
}
803+
#endif
804+
805+
memcpy(_l2CapPduBuffer, l2CapPdu, l2CapPduSize);
806+
_l2CapPduBufferSize = l2CapPduSize;
784807
}
785808

786-
}else {
809+
// We need to wait for the missing parts of the L2CAP SDU
810+
return;
811+
}
812+
813+
#ifdef _BLE_TRACE_
814+
Serial.println("L2CAP SDU complete");
815+
#endif
816+
817+
if (l2CapHeader->cid == ATT_CID) {
818+
#ifdef _BLE_TRACE_
819+
Serial.println("CID: ATT");
820+
#endif
821+
ATT.handleData(connectionHandle, l2CapHeader->len, &l2CapPdu[sizeof(HCIL2CapHdr)]);
822+
} else if (l2CapHeader->cid == SIGNALING_CID) {
823+
#ifdef _BLE_TRACE_
824+
Serial.println("CID: SIGNALING");
825+
#endif
826+
L2CAPSignaling.handleData(connectionHandle, l2CapHeader->len, &l2CapPdu[sizeof(HCIL2CapHdr)]);
827+
} else if (l2CapHeader->cid == SECURITY_CID) {
828+
// Security manager
829+
#ifdef _BLE_TRACE_
830+
Serial.println("CID: SECURITY");
831+
#endif
832+
L2CAPSignaling.handleSecurityData(connectionHandle, l2CapHeader->len, &l2CapPdu[sizeof(HCIL2CapHdr)]);
833+
} else {
787834
struct __attribute__ ((packed)) {
788835
uint8_t op;
789836
uint8_t id;
790837
uint16_t length;
791838
uint16_t reason;
792839
uint16_t localCid;
793840
uint16_t remoteCid;
794-
} l2capRejectCid= { 0x01, 0x00, 0x006, 0x0002, aclHdr->cid, 0x0000 };
841+
} l2capRejectCid= { 0x01, 0x00, 0x006, 0x0002, l2CapHeader->cid, 0x0000 };
795842
#ifdef _BLE_TRACE_
796-
Serial.print("rejecting packet cid: 0x");
797-
Serial.println(aclHdr->cid,HEX);
843+
Serial.println("Rejecting packet cid");
798844
#endif
799845

800-
sendAclPkt(aclHdr->handle & 0x0fff, 0x0005, sizeof(l2capRejectCid), &l2capRejectCid);
846+
sendAclPkt(connectionHandle, 0x0005, sizeof(l2capRejectCid), &l2capRejectCid);
801847
}
848+
849+
// We have processed everything in the buffer. Discard the contents.
850+
_l2CapPduBufferSize = 0;
802851
}
803852

804853
void HCIClass::handleNumCompPkts(uint16_t /*handle*/, uint16_t numPkts)

‎src/utility/HCI.h‎

Lines changed: 2 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -159,7 +159,8 @@ class HCIClass {
159159
uint8_t _maxPkt;
160160
uint8_t _pendingPkt;
161161

162-
uint8_t _aclPktBuffer[255];
162+
uint8_t _l2CapPduBuffer[255];
163+
uint8_t _l2CapPduBufferSize;
163164
};
164165

165166
extern HCIClass& HCI;

0 commit comments

Comments
(0)

AltStyle によって変換されたページ (->オリジナル) /