L2CAP: FCS Implementation

Author: zxzxwu

bumble/host.py

@@ -707,7 +707,9 @@ async def send_command(command: hci.HCI_Command) -> None:
 
         asyncio.create_task(send_command(command))
 
-    def send_l2cap_pdu(self, connection_handle: int, cid: int, pdu: bytes) -> None:
+    def send_l2cap_pdu(
+        self, connection_handle: int, cid: int, pdu: bytes, with_fcs: bool = False
+    ) -> None:
         if not (connection := self.connections.get(connection_handle)):
             logger.warning(f'connection 0x{connection_handle:04X} not found')
             return
@@ -719,26 +721,23 @@ def send_l2cap_pdu(self, connection_handle: int, cid: int, pdu: bytes) -> None:
             return
 
         # Create a PDU
-        l2cap_pdu = bytes(L2CAP_PDU(cid, pdu))
+        l2cap_pdu = L2CAP_PDU(cid, pdu).to_bytes(with_fcs)
 
         # Send the data to the controller via ACL packets
-        bytes_remaining = len(l2cap_pdu)
-        offset = 0
-        pb_flag = 0
-        while bytes_remaining:
-            data_total_length = min(bytes_remaining, packet_queue.max_packet_size)
+        max_packet_size = packet_queue.max_packet_size
+        for offset in range(0, len(l2cap_pdu), max_packet_size):
+            pdu = l2cap_pdu[offset : offset + max_packet_size]
             acl_packet = hci.HCI_AclDataPacket(
                 connection_handle=connection_handle,
-                pb_flag=pb_flag,
+                pb_flag=1 if offset > 0 else 0,
                 bc_flag=0,
-                data_total_length=data_total_length,
-                data=l2cap_pdu[offset : offset + data_total_length],
+                data_total_length=len(pdu),
+                data=pdu,
+            )
+            logger.debug(
+                '>>> ACL packet enqueue: (Handle=0x%04X) %s', connection_handle, pdu
             )
-            logger.debug(f'>>> ACL packet enqueue: (CID={cid}) {acl_packet}')
             packet_queue.enqueue(acl_packet, connection_handle)
-            pb_flag = 1
-            offset += data_total_length
-            bytes_remaining -= data_total_length
 
     def get_data_packet_queue(self, connection_handle: int) -> DataPacketQueue | None:
         if connection := self.connections.get(connection_handle):

bumble/l2cap.py

@@ -219,20 +219,41 @@ class L2CAP_PDU:
     See Bluetooth spec @ Vol 3, Part A - 3 DATA PACKET FORMAT
     '''
 
-    @staticmethod
-    def from_bytes(data: bytes) -> L2CAP_PDU:
+    @classmethod
+    def from_bytes(cls, data: bytes) -> L2CAP_PDU:
         # Check parameters
         if len(data) < 4:
             raise InvalidPacketError('not enough data for L2CAP header')
 
-        _, l2cap_pdu_cid = struct.unpack_from('<HH', data, 0)
-        l2cap_pdu_payload = data[4:]
+        length, l2cap_pdu_cid = struct.unpack_from('<HH', data, 0)
+        l2cap_pdu_payload = data[4 : 4 + length]
 
-        return L2CAP_PDU(l2cap_pdu_cid, l2cap_pdu_payload)
+        return cls(l2cap_pdu_cid, l2cap_pdu_payload)
 
     def __bytes__(self) -> bytes:
-        header = struct.pack('<HH', len(self.payload), self.cid)
-        return header + self.payload
+        return self.to_bytes(with_fcs=False)
+
+    @classmethod
+    def crc16(self, data: bytes) -> int:
+        crc = 0x0000
+        for byte in data:
+            crc ^= byte
+            for _ in range(8):
+                if (crc & 0x0001) > 0:
+                    crc = (crc >> 1) ^ 0xA001
+                else:
+                    crc = crc >> 1
+        return crc
+
+    def to_bytes(self, with_fcs: bool = False) -> bytes:
+        length = len(self.payload)
+        if with_fcs:
+            length += 2
+        header = struct.pack('<HH', length, self.cid)
+        body = header + self.payload
+        if with_fcs:
+            body += struct.pack('<H', self.crc16(body))
+        return body
 
     def __init__(self, cid: int, payload: bytes) -> None:
         self.cid = cid
@@ -1120,6 +1141,7 @@ def __init__(
         self.connection_result = None
         self.disconnection_result = None
         self.sink = None
+        self.fcs_enabled = False
         self.spec = spec
         if spec.mode == TransmissionMode.BASIC:
             self.processor = BaseProcessor(self)
@@ -1138,12 +1160,17 @@ def write(self, sdu: bytes) -> None:
     def send_pdu(self, pdu: Union[SupportsBytes, bytes]) -> None:
         if self.state != self.State.OPEN:
             raise InvalidStateError('channel not open')
-        self.manager.send_pdu(self.connection, self.destination_cid, pdu)
+        self.manager.send_pdu(
+            self.connection, self.destination_cid, pdu, self.fcs_enabled
+        )
 
     def send_control_frame(self, frame: L2CAP_Control_Frame) -> None:
         self.manager.send_control_frame(self.connection, self.signaling_cid, frame)
 
     def on_pdu(self, pdu: bytes) -> None:
+        if self.fcs_enabled:
+            # Drop FCS.
+            pdu = pdu[:-2]
         self.processor.on_pdu(pdu)
 
     def on_sdu(self, sdu: bytes) -> None:
@@ -1308,7 +1335,12 @@ def on_configure_request(self, request: L2CAP_Configure_Request) -> None:
                     peer_mps,
                 ) = struct.unpack_from('<BBBHHH', option[1])
                 logger.debug(
-                    'Peer requests Retransmission or Flow Control: mode=%s, tx_window_size=%s, retransmission_timeout=%s, monitor_timeout=%s, mps=%s',
+                    'Peer requests Retransmission or Flow Control: mode=%s,'
+                    ' tx_window_size=%s,'
+                    ' max_retransmission=%s,'
+                    ' retransmission_timeout=%s,'
+                    ' monitor_timeout=%s,'
+                    ' mps=%s',
                     TransmissionMode(mode).name,
                     peer_tx_window_size,
                     peer_max_retransmission,
@@ -1330,8 +1362,7 @@ def on_configure_request(self, request: L2CAP_Configure_Request) -> None:
                     else:
                         logger.error("Enhanced Retransmission Mode is not enabled")
                         result = L2CAP_Configure_Response.Result.FAILURE_REJECTED
-                        replied_options.clear()
-                        replied_options.append(option)
+                        replied_options = [option]
                         break
                 else:
                     logger.error(
@@ -1340,8 +1371,23 @@ def on_configure_request(self, request: L2CAP_Configure_Request) -> None:
                     result = (
                         L2CAP_Configure_Response.Result.FAILURE_UNACCEPTABLE_PARAMETERS
                     )
-                    replied_options.clear()
+                    replied_options = [option]
+                    break
+            elif option[0] == L2CAP_Configure_Request.ParameterType.FCS:
+                enabled = option[1][0] != 0
+                logger.debug("Peer requests FCS: %s", enabled)
+                if (
+                    L2CAP_Information_Request.ExtendedFeatures.FCS_OPTION
+                    in self.manager.extended_features
+                ):
+                    self.fcs_enabled = enabled
                     replied_options.append(option)
+                else:
+                    logger.error("Frame Check Sequence is not supported")
+                    result = (
+                        L2CAP_Configure_Response.Result.FAILURE_UNACCEPTABLE_PARAMETERS
+                    )
+                    replied_options = [option]
                     break
             else:
                 logger.debug(
@@ -1350,8 +1396,7 @@ def on_configure_request(self, request: L2CAP_Configure_Request) -> None:
                     option[1].hex(),
                 )
                 result = L2CAP_Configure_Response.Result.FAILURE_UNKNOWN_OPTIONS
-                replied_options.clear()
-                replied_options.append(option)
+                replied_options = [option]
                 break
 
         self.send_control_frame(
@@ -2061,15 +2106,21 @@ def on_disconnection(self, connection_handle: int, _reason: int) -> None:
         if connection_handle in self.identifiers:
             del self.identifiers[connection_handle]
 
-    def send_pdu(self, connection, cid: int, pdu: Union[SupportsBytes, bytes]) -> None:
+    def send_pdu(
+        self,
+        connection,
+        cid: int,
+        pdu: Union[SupportsBytes, bytes],
+        with_fcs: bool = False,
+    ) -> None:
         pdu_str = pdu.hex() if isinstance(pdu, bytes) else str(pdu)
         pdu_bytes = bytes(pdu)
         logger.debug(
             f'{color(">>> Sending L2CAP PDU", "blue")} '
             f'on connection [0x{connection.handle:04X}] (CID={cid}) '
             f'{connection.peer_address}: {len(pdu_bytes)} bytes, {pdu_str}'
         )
-        self.host.send_l2cap_pdu(connection.handle, cid, pdu_bytes)
+        self.host.send_l2cap_pdu(connection.handle, cid, pdu_bytes, with_fcs)
 
     def on_pdu(self, connection: Connection, cid: int, pdu: bytes) -> None:
         if cid in (L2CAP_SIGNALING_CID, L2CAP_LE_SIGNALING_CID):

examples/run_l2cap_server.py renamed to examples/run_classic_l2cap.py

@@ -17,30 +17,35 @@
 # -----------------------------------------------------------------------------
 import asyncio
 import sys
-
-from typing import Optional
+import argparse
 
 import bumble.logging
-from bumble.device import Device
+from bumble import core
 from bumble import l2cap
+from bumble.device import Device
 from bumble.transport import open_transport
 
 
 # -----------------------------------------------------------------------------
-async def main() -> None:
+async def main(
+    config_file: str, transport: str, mode: int, peer_address: str, psm: int
+) -> None:
 
     print('<<< connecting to HCI...')
-    async with await open_transport(sys.argv[2]) as hci_transport:
+    async with await open_transport(transport) as hci_transport:
         print('<<< connected')
 
         # Create a device
         device = Device.from_config_file_with_hci(
-            sys.argv[1], hci_transport.source, hci_transport.sink
+            config_file, hci_transport.source, hci_transport.sink
         )
         device.classic_enabled = True
         device.l2cap_channel_manager.extended_features.add(
             l2cap.L2CAP_Information_Request.ExtendedFeatures.ENHANCED_RETRANSMISSION_MODE
         )
+        device.l2cap_channel_manager.extended_features.add(
+            l2cap.L2CAP_Information_Request.ExtendedFeatures.FCS_OPTION
+        )
 
         # Start the controller
         await device.power_on()
@@ -63,12 +68,22 @@ def on_sdu(sdu: bytes):
 
         server = device.create_l2cap_server(
             spec=l2cap.ClassicChannelSpec(
-                mode=l2cap.TransmissionMode.ENHANCED_RETRANSMISSION
+                mode=l2cap.TransmissionMode(mode), psm=psm if psm else None
             ),
             handler=on_connection,
         )
         print(f'Listen L2CAP on channel {server.psm}')
 
+        if peer_address:
+            connection = await device.connect(
+                peer_address, transport=core.PhysicalTransport.BR_EDR
+            )
+            connection.create_l2cap_channel(
+                spec=l2cap.ClassicChannelSpec(
+                    mode=l2cap.TransmissionMode(mode), psm=psm
+                )
+            )
+
         while sdu := await asyncio.to_thread(lambda: input('>>> ')):
             if channels:
                 channels[0].write(sdu.encode())
@@ -78,4 +93,13 @@ def on_sdu(sdu: bytes):
 
 # -----------------------------------------------------------------------------
 bumble.logging.setup_basic_logging('INFO')
-asyncio.run(main())
+parser = argparse.ArgumentParser()
+parser.add_argument('config')
+parser.add_argument('transport')
+parser.add_argument('-p', '--peer_address', default='')
+parser.add_argument(
+    '-m', '--mode', default=l2cap.TransmissionMode.ENHANCED_RETRANSMISSION
+)
+parser.add_argument('--psm', default=0)
+args = parser.parse_args(sys.argv[1:])
+asyncio.run(main(args.config, args.transport, args.mode, args.peer_address, args.psm))

tests/l2cap_test.py

@@ -347,10 +347,19 @@ def on_channel(channel):
 async def test_enhanced_retransmission_channel():
     devices = TwoDevices()
     await devices.setup_connection()
+    devices[0].l2cap_channel_manager.extended_features.add(
+        l2cap.L2CAP_Information_Request.ExtendedFeatures.ENHANCED_RETRANSMISSION_MODE
+    )
+    devices[1].l2cap_channel_manager.extended_features.add(
+        l2cap.L2CAP_Information_Request.ExtendedFeatures.ENHANCED_RETRANSMISSION_MODE
+    )
 
     server_channels = asyncio.Queue[l2cap.ClassicChannel]()
     server = devices.devices[1].create_l2cap_server(
-        spec=l2cap.ClassicChannelSpec(), handler=server_channels.put_nowait
+        spec=l2cap.ClassicChannelSpec(
+            mode=l2cap.TransmissionMode.ENHANCED_RETRANSMISSION
+        ),
+        handler=server_channels.put_nowait,
     )
     client_channel = await devices.connections[0].create_l2cap_channel(
         spec=l2cap.ClassicChannelSpec(
@@ -375,6 +384,20 @@ async def test_enhanced_retransmission_channel():
         assert (await sinks[0].get()) == b'456'
 
 
+# -----------------------------------------------------------------------------
+@pytest.mark.parametrize(
+    'cid, payload, expected',
+    [
+        (0x0040, '020000010203040506070809', '0E0040000200000102030405060708093861'),
+        (0x0040, '0101', '040040000101D414'),
+    ],
+)
+def test_fcs(cid: int, payload: str, expected: str):
+    '''Core Spec 6.1, Vol 3, Part A, 3.3.5. Frame Check Sequence.'''
+    pdu = l2cap.L2CAP_PDU(cid, bytes.fromhex(payload))
+    assert pdu.to_bytes(with_fcs=True) == bytes.fromhex(expected)
+
+
 # -----------------------------------------------------------------------------
 async def run():
     test_helpers()