From ccb0991762975d7fd522dfaa6b32df921cee2430 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?D=C3=A1niel=20Buga?= Date: Mon, 27 May 2024 09:24:45 +0200 Subject: [PATCH 1/3] Fragmentation support for HCI/ACL packets --- pybleno/hci_socket/Bindings.py | 7 +-- pybleno/hci_socket/Hci.py | 101 ++++++++++++++++++++++++------- pybleno/hci_socket/constants2.py | 4 ++ 3 files changed, 83 insertions(+), 29 deletions(-) diff --git a/pybleno/hci_socket/Bindings.py b/pybleno/hci_socket/Bindings.py index 5492cbe..63b8b5d 100644 --- a/pybleno/hci_socket/Bindings.py +++ b/pybleno/hci_socket/Bindings.py @@ -104,12 +104,7 @@ def onAddressChange(self, address): self.emit('addressChange', [address]) def onReadLocalVersion(self, hciVer, hciRev, lmpVer, manufacturer, lmpSubVer): - if (manufacturer == 2): - # Intel Corporation - self._gatt.maxMtu = 23 - elif (manufacturer == 93): - # Realtek Semiconductor Corporation - self._gatt.maxMtu = 23 + pass def onAdvertisingStart(self, error): self.emit('advertisingStart', [error]) diff --git a/pybleno/hci_socket/Hci.py b/pybleno/hci_socket/Hci.py index 61785a8..ac621e8 100644 --- a/pybleno/hci_socket/Hci.py +++ b/pybleno/hci_socket/Hci.py @@ -21,8 +21,9 @@ def __init__(self): self._isDevUp = None self._state = None self._deviceId = None - - self._handleBuffers = {} + # le-u min payload size + l2cap header size + # see Bluetooth spec 4.2 [Vol 3, Part A, Chapter 4] + self._acl_mtu = 23 + 4 self.on('stateChange', self.onStateChange) @@ -45,6 +46,15 @@ def init(self): # def io_thread(self): # while True: + def initDev(self): + self.setEventMask() + self.setLeEventMask() + self.readLocalVersion() + self.writeLeHostSupported() + self.readLeHostSupported() + self.readBdAddr() + self.leReadBufferSize() + # pass def setSocketFilter(self): @@ -310,21 +320,54 @@ def readRssi(self, handle): # debug('read rssi - writing: ' + cmd.toString('hex')) self.write(cmd) - def writeAclDataPkt(self, handle, cid, data): - pkt = array.array('B', [0] * (9 + len(data))) + def leReadBufferSize(self): + pkt = array.array("B", [0] * 4) # header - writeUInt8(pkt, HCI_ACLDATA_PKT, 0) - writeUInt16LE(pkt, handle | ACL_START_NO_FLUSH << 12, 1) - writeUInt16LE(pkt, len(data) + 4, 3) # data length 1 - writeUInt16LE(pkt, len(data), 5) # data length 2 - writeUInt16LE(pkt, cid, 7) + writeUInt8(pkt, HCI_COMMAND_PKT, 0) + writeUInt16LE(pkt, LE_READ_BUFFER_SIZE_CMD, 1) + writeUInt8(pkt, 0x0, 3) # data length 0 - copy(data, pkt, 9) + # debug('le read buffer size - writing: ' + pkt.toString('hex')) + self.write(pkt) + + def readBufferSize(self): + pkt = array.array("B", [0] * 4) - # debug('write acl data pkt - writing: ' + pkt.toString('hex')) + # header + writeUInt8(pkt, HCI_COMMAND_PKT, 0) + writeUInt16LE(pkt, READ_BUFFER_SIZE_CMD, 1) + writeUInt8(pkt, 0x0, 3) # data length 0 + + # debug('read buffer size - writing: ' + pkt.toString('hex')) self.write(pkt) + def writeAclDataPkt(self, handle, cid, data): + hf = handle | ACL_START_NO_FLUSH << 12 + l2capPdu = array.array("B", [0] * (4 + len(data))) + + # header + writeUInt16LE(l2capPdu, len(data), 0) + writeUInt16LE(l2capPdu, cid, 2) + copy(data, l2capPdu, 4) + fragId = 0 + + while len(l2capPdu) > 0: + frag = l2capPdu[0 : self._acl_mtu] + l2capPdu = l2capPdu[self._acl_mtu :] + + # hci header + pkt = array.array("B", [0] * (5 + len(frag))) + writeUInt8(pkt, HCI_ACLDATA_PKT, 0) + writeUInt16LE(pkt, hf, 1) + hf |= ACL_CONT << 12 + writeUInt16LE(pkt, len(frag), 3) + copy(frag, pkt, 5) + + # debug('write acl data pkt frag ' + fragId + ' - writing: ' + pkt.toString('hex')) + self.write(pkt) + fragId += 1 + def write(self, pkt): # print 'WRITING: %s' % ''.join(format(x, '02x') for x in pkt) self._socket.write(pkt) @@ -441,12 +484,7 @@ def onSocketError(self, error): def processCmdCompleteEvent(self, cmd, status, result): # handle if cmd == RESET_CMD: - self.setEventMask() - self.setLeEventMask() - self.readLocalVersion() - self.writeLeHostSupported() - self.readLeHostSupported() - self.readBdAddr() + self.initDev() elif cmd == READ_LE_HOST_SUPPORTED_CMD: if status == 0: le = readUInt8(result, 0) @@ -501,6 +539,28 @@ def processCmdCompleteEvent(self, cmd, status, result): # debug('\t\t\thandle = ' + handle) self.emit('leLtkNegReply', [handle]) + elif cmd == LE_READ_BUFFER_SIZE_CMD: + if not status: + self.processLeReadBufferSize(result) + elif cmd == READ_BUFFER_SIZE_CMD: + if not status: + acl_mtu = readUInt16LE(result, 0) + # sanity + if acl_mtu: + # debug('br/edr acl_mtu = ' + acl_mtu) + self._acl_mtu = acl_mtu + + def processLeReadBufferSize(self, result): + acl_mtu = readUInt16LE(result, 0) + # acl_queue = readUInt8(result, 2) + if not acl_mtu: + # as per Bluetooth specs + # print("falling back to br/edr buffer size") + self.readBufferSize() + else: + # print(f"le acl_mtu = {acl_mtu}") + # print(f"le acl_queue = {acl_queue}") + self._acl_mtu = acl_mtu def processLeMetaEvent(self, eventType, status, data): if eventType == EVT_LE_CONN_COMPLETE: @@ -561,12 +621,7 @@ def on_socket_started(self): self._isDevUp = isDevUp if isDevUp: self.setSocketFilter() - self.setEventMask() - self.setLeEventMask() - self.readLocalVersion() - self.writeLeHostSupported() - self.readLeHostSupported() - self.readBdAddr() + self.initDev() else: self.emit('stateChange', ['poweredOff']) diff --git a/pybleno/hci_socket/constants2.py b/pybleno/hci_socket/constants2.py index 8a35a8f..ce1730e 100644 --- a/pybleno/hci_socket/constants2.py +++ b/pybleno/hci_socket/constants2.py @@ -26,6 +26,7 @@ OGF_INFO_PARAM = 0x04 OCF_READ_LOCAL_VERSION = 0x0001 +OCF_READ_BUFFER_SIZE = 0x0005 OCF_READ_BD_ADDR = 0x0009 OGF_STATUS_PARAM = 0x05 @@ -33,6 +34,7 @@ OGF_LE_CTL = 0x08 OCF_LE_SET_EVENT_MASK = 0x0001 +OCF_LE_READ_BUFFER_SIZE = 0x0002 OCF_LE_SET_ADVERTISING_PARAMETERS = 0x0006 OCF_LE_SET_ADVERTISING_DATA = 0x0008 OCF_LE_SET_SCAN_RESPONSE_DATA = 0x0009 @@ -47,11 +49,13 @@ WRITE_LE_HOST_SUPPORTED_CMD = OCF_WRITE_LE_HOST_SUPPORTED | OGF_HOST_CTL << 10 READ_LOCAL_VERSION_CMD = OCF_READ_LOCAL_VERSION | (OGF_INFO_PARAM << 10) +READ_BUFFER_SIZE_CMD = OCF_READ_BUFFER_SIZE | (OGF_INFO_PARAM << 10) READ_BD_ADDR_CMD = OCF_READ_BD_ADDR | (OGF_INFO_PARAM << 10) READ_RSSI_CMD = OCF_READ_RSSI | OGF_STATUS_PARAM << 10 LE_SET_EVENT_MASK_CMD = OCF_LE_SET_EVENT_MASK | OGF_LE_CTL << 10 +LE_READ_BUFFER_SIZE_CMD = OCF_LE_READ_BUFFER_SIZE | OGF_LE_CTL << 10 LE_SET_ADVERTISING_PARAMETERS_CMD = OCF_LE_SET_ADVERTISING_PARAMETERS | OGF_LE_CTL << 10 LE_SET_ADVERTISING_DATA_CMD = OCF_LE_SET_ADVERTISING_DATA | OGF_LE_CTL << 10 LE_SET_SCAN_RESPONSE_DATA_CMD = OCF_LE_SET_SCAN_RESPONSE_DATA | OGF_LE_CTL << 10 From 9d9e8437e8116941de7efa08cade753f815097f3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?D=C3=A1niel=20Buga?= Date: Mon, 27 May 2024 09:31:45 +0200 Subject: [PATCH 2/3] HCI/ACL flow control --- pybleno/hci_socket/AclStream.py | 2 +- pybleno/hci_socket/Hci.py | 135 +++++++++++++++++++++++++------ pybleno/hci_socket/constants2.py | 1 + 3 files changed, 114 insertions(+), 24 deletions(-) diff --git a/pybleno/hci_socket/AclStream.py b/pybleno/hci_socket/AclStream.py index 0c72338..0de94c6 100644 --- a/pybleno/hci_socket/AclStream.py +++ b/pybleno/hci_socket/AclStream.py @@ -11,7 +11,7 @@ def __init__(self, hci, handle, localAddressType, localAddress, remoteAddressTyp self._smp = Smp(self, localAddressType, localAddress, remoteAddressType, remoteAddress) def write(self, cid, data): - self._hci.writeAclDataPkt(self._handle, cid, data) + self._hci.queueAclDataPkt(self._handle, cid, data) def push(self, cid, data): if data: diff --git a/pybleno/hci_socket/Hci.py b/pybleno/hci_socket/Hci.py index ac621e8..9d15b7b 100644 --- a/pybleno/hci_socket/Hci.py +++ b/pybleno/hci_socket/Hci.py @@ -23,7 +23,10 @@ def __init__(self): self._deviceId = None # le-u min payload size + l2cap header size # see Bluetooth spec 4.2 [Vol 3, Part A, Chapter 4] - self._acl_mtu = 23 + 4 + self._aclMtu = 23 + 4 + self._aclMaxInProgress = 1 + + self.resetBuffers() self.on('stateChange', self.onStateChange) @@ -45,6 +48,7 @@ def init(self): # def io_thread(self): # while True: + # pass def initDev(self): self.setEventMask() @@ -55,12 +59,20 @@ def initDev(self): self.readBdAddr() self.leReadBufferSize() - # pass + def resetBuffers(self): + self._handleAclsInProgress = {} + self._handleBuffers = {} + self._aclOutQueue = [] def setSocketFilter(self): typeMask = (1 << HCI_EVENT_PKT) | (1 << HCI_ACLDATA_PKT) - eventMask1 = (1 << EVT_DISCONN_COMPLETE) | (1 << EVT_ENCRYPT_CHANGE) | (1 << EVT_CMD_COMPLETE) | ( - 1 << EVT_CMD_STATUS) + eventMask1 = ( + (1 << EVT_DISCONN_COMPLETE) + | (1 << EVT_ENCRYPT_CHANGE) + | (1 << EVT_CMD_COMPLETE) + | (1 << EVT_CMD_STATUS) + | (1 << EVT_NUMBER_OF_COMPLETED_PACKETS) + ) eventMask2 = (1 << (EVT_LE_META_EVENT - 32)) opcode = 0 @@ -342,7 +354,7 @@ def readBufferSize(self): # debug('read buffer size - writing: ' + pkt.toString('hex')) self.write(pkt) - def writeAclDataPkt(self, handle, cid, data): + def queueAclDataPkt(self, handle, cid, data): hf = handle | ACL_START_NO_FLUSH << 12 l2capPdu = array.array("B", [0] * (4 + len(data))) @@ -353,8 +365,8 @@ def writeAclDataPkt(self, handle, cid, data): fragId = 0 while len(l2capPdu) > 0: - frag = l2capPdu[0 : self._acl_mtu] - l2capPdu = l2capPdu[self._acl_mtu :] + frag = l2capPdu[0 : self._aclMtu] + l2capPdu = l2capPdu[self._aclMtu :] # hci header pkt = array.array("B", [0] * (5 + len(frag))) @@ -364,10 +376,38 @@ def writeAclDataPkt(self, handle, cid, data): writeUInt16LE(pkt, len(frag), 3) copy(frag, pkt, 5) - # debug('write acl data pkt frag ' + fragId + ' - writing: ' + pkt.toString('hex')) - self.write(pkt) + self._aclOutQueue.append({"handle": handle, "pkt": pkt, "fragId": fragId}) fragId += 1 + self.pushAclOutQueue() + + def pushAclOutQueue(self): + inProgress = 0 + for count in self._handleAclsInProgress.values(): + inProgress += count + + while inProgress < self._aclMaxInProgress and len(self._aclOutQueue): + inProgress = inProgress + 1 + self.writeOneAclDataPkt() + + # if (inProgress >= self._aclMaxInProgress and self._aclOutQueue.length): + # printf("acl out queue congested") + # printf("\tin progress = {inProgress}") + # printf("\twaiting = {self._aclOutQueue.length}") + + def writeOneAclDataPkt(self): + pkt = self._aclOutQueue.pop(0) + self._handleAclsInProgress[pkt["handle"]] += 1 + # debug( + # "write acl data pkt frag " + # + pkt.fragId + # + " handle " + # + pkt.handle + # + " - writing: " + # + pkt.pkt.toString("hex") + # ) + self._socket.write(pkt["pkt"]) + def write(self, pkt): # print 'WRITING: %s' % ''.join(format(x, '02x') for x in pkt) self._socket.write(pkt) @@ -403,6 +443,27 @@ def onSocketData(self, data): # print('\t\thandle = ' + `handle`) # print('\t\treason = ' + `reason`) + # As per Bluetooth Core specs: + # When the Host receives a Disconnection Complete, Disconnection Physical + # Link Complete or Disconnection Logical Link Complete event, the Host shall + # assume that all unacknowledged HCI Data Packets that have been sent to the + # Controller for the returned Handle have been flushed, and that the + # corresponding data buffers have been freed. + del self._handleAclsInProgress[handle] + aclOutQueue = [] + discarded = 0 + for pkt in self._aclOutQueue: + if pkt["handle"] != handle: + aclOutQueue.append(pkt) + else: + discarded += 1 + + # if discarded: + # debug('\t\tacls discarded = ' + discarded); + + self._aclOutQueue = aclOutQueue + self.pushAclOutQueue() + self.emit('disconnComplete', [handle, reason]) elif subEventType == EVT_ENCRYPT_CHANGE: @@ -413,19 +474,19 @@ def onSocketData(self, data): # debug('\t\tencrypt = ' + encrypt) self.emit('encryptChange', [handle, encrypt]) elif subEventType == EVT_CMD_COMPLETE: - # cmd = data.readUInt16LE(4) + # ncmd = readUInt8(data, 3) cmd = readUInt16LE(data, 4) # status = data.readUInt8(6) status = readUInt8(data, 6) # result = data.slice(7) result = data[7:] - # debug('\t\tcmd = ' + cmd) + # debug('\t\tncmd = ' + ncmd) # debug('\t\tstatus = ' + status) # debug('\t\tresult = ' + result.toString('hex')) # print('\t\tcmd = ' + `cmd`) # print('\t\tstatus = ' + `status`) - # print('\t\tresult = ' + `result`); + # print('\t\tresult = ' + `result`); self.processCmdCompleteEvent(cmd, status, result) elif subEventType == EVT_LE_META_EVENT: @@ -438,6 +499,28 @@ def onSocketData(self, data): # debug('\t\tLE meta event data = ' + leMetaEventData.toString('hex')) self.processLeMetaEvent(leMetaEventType, leMetaEventStatus, leMetaEventData) + + elif subEventType == EVT_NUMBER_OF_COMPLETED_PACKETS: + handles = readUInt8(data, 3) + for pkt in range(handles): + handle = readUInt16LE(data, 4 + pkt * 4) + pkts = readUInt16LE(data, 6 + pkt * 4) + # debug("\thandle = " + handle); + # debug("\t\tcompleted = " + pkts); + if handle not in self._handleAclsInProgress: + # debug("\t\talready closed") + continue + + if pkts > self._handleAclsInProgress[handle]: + # Linux kernel may send acl packets by itself, so be ready for underflow + self._handleAclsInProgress[handle] = 0 + else: + self._handleAclsInProgress[handle] -= pkts + + # debug("\t\tin progress = " + self._handleAclsInProgress[handle]); + + self.pushAclOutQueue() + elif HCI_ACLDATA_PKT == eventType: flags = readUInt16LE(data, 1) >> 12 handle = readUInt16LE(data, 1) & 0x0fff @@ -531,7 +614,7 @@ def processCmdCompleteEvent(self, cmd, status, result): # debug('\t\t\thandle = ' + handle) # debug('\t\t\trssi = ' + rssi) # print('\t\t\thandle = ' + `handle`) - # print('\t\t\trssi = ' + `rssi`); + # print('\t\t\trssi = ' + `rssi`); self.emit('rssiRead', [handle, rssi]) elif cmd == LE_LTK_NEG_REPLY_CMD: @@ -544,23 +627,27 @@ def processCmdCompleteEvent(self, cmd, status, result): self.processLeReadBufferSize(result) elif cmd == READ_BUFFER_SIZE_CMD: if not status: - acl_mtu = readUInt16LE(result, 0) + aclMtu = readUInt16LE(result, 0) + aclMaxInProgress = readUInt16LE(result, 3) # sanity - if acl_mtu: - # debug('br/edr acl_mtu = ' + acl_mtu) - self._acl_mtu = acl_mtu + if aclMtu and aclMaxInProgress: + # debug('br/edr acl mtu = ' + aclMtu) + # debug('br/edr acl max pkts = ' + aclMaxInProgress) + self._aclMtu = aclMtu + self._aclMaxInProgress = aclMaxInProgress def processLeReadBufferSize(self, result): - acl_mtu = readUInt16LE(result, 0) - # acl_queue = readUInt8(result, 2) - if not acl_mtu: + aclMtu = readUInt16LE(result, 0) + aclMaxInProgress = readUInt8(result, 2) + if not aclMtu: # as per Bluetooth specs # print("falling back to br/edr buffer size") self.readBufferSize() else: - # print(f"le acl_mtu = {acl_mtu}") - # print(f"le acl_queue = {acl_queue}") - self._acl_mtu = acl_mtu + # print(f"le acl_mtu = {aclMtu}") + # print(f"le acl_queue = {aclMaxInProgress}") + self._aclMtu = aclMtu + self._aclMaxInProgress = aclMaxInProgress def processLeMetaEvent(self, eventType, status, data): if eventType == EVT_LE_CONN_COMPLETE: @@ -589,6 +676,8 @@ def processLeConnComplete(self, status, data): # debug('\t\t\tsupervision timeout = ' + supervisionTimeout) # debug('\t\t\tmaster clock accuracy = ' + masterClockAccuracy) + self._handleAclsInProgress[handle] = 0 + self.emit('leConnComplete', [status, handle, role, addressType, address, interval, latency, supervisionTimeout, masterClockAccuracy]) diff --git a/pybleno/hci_socket/constants2.py b/pybleno/hci_socket/constants2.py index ce1730e..665f2b3 100644 --- a/pybleno/hci_socket/constants2.py +++ b/pybleno/hci_socket/constants2.py @@ -10,6 +10,7 @@ EVT_ENCRYPT_CHANGE = 0x08 EVT_CMD_COMPLETE = 0x0e EVT_CMD_STATUS = 0x0f +EVT_NUMBER_OF_COMPLETED_PACKETS = 0x13 EVT_LE_META_EVENT = 0x3e EVT_LE_CONN_COMPLETE = 0x01 From 81dd643721cbdb1cd1644950d6298c374b6294f7 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?D=C3=A1niel=20Buga?= Date: Mon, 27 May 2024 14:17:29 +0200 Subject: [PATCH 3/3] Prevent exception when connection handle changes --- pybleno/hci_socket/Hci.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/pybleno/hci_socket/Hci.py b/pybleno/hci_socket/Hci.py index 9d15b7b..88c5408 100644 --- a/pybleno/hci_socket/Hci.py +++ b/pybleno/hci_socket/Hci.py @@ -397,6 +397,9 @@ def pushAclOutQueue(self): def writeOneAclDataPkt(self): pkt = self._aclOutQueue.pop(0) + if pkt["handle"] not in self._handleAclsInProgress: + return + self._handleAclsInProgress[pkt["handle"]] += 1 # debug( # "write acl data pkt frag "