diff --git a/cflib/crtp/bletransport.py b/cflib/crtp/bletransport.py new file mode 100644 index 000000000..1bb7be700 --- /dev/null +++ b/cflib/crtp/bletransport.py @@ -0,0 +1,54 @@ +"""Crazyflie BLE CRTP framing, independent of an IO backend. + +Copyright (C) 2026. Licensed under GPL-3.0-or-later, as part of cflib. +""" + + +class FrameError(ValueError): + """A BLE fragment does not follow the documented CRTP wire format.""" + + +class DownDecoder: + """Reassemble one packet at a time; malformed input discards partial state.""" + + def __init__(self): + self.reset() + + def reset(self): + """Discard any incomplete packet at a connection or error boundary.""" + self.pid = None + self.expected = 0 + self.data = bytearray() + + def feed(self, value): + """Return a complete packet, None for a partial one, or raise FrameError.""" + try: + if not 2 <= len(value) <= 20: + raise FrameError('Invalid BLE notification size') + control = value[0] + pid = (control >> 5) & 3 + if control & 0x80: + if self.pid is not None: + raise FrameError('New start before previous packet completed') + self.pid, self.expected = pid, (control & 31) + 1 + elif self.pid is None or pid != self.pid or control & 31: + raise FrameError('Missing start, wrong PID, or invalid continuation') + self.data.extend(value[1:]) + if len(self.data) > self.expected: + raise FrameError('Notification exceeds declared packet length') + if len(self.data) == self.expected: + packet = bytes(self.data) + self.reset() + return packet + return None + except FrameError: + self.reset() + raise + + +def fragment_up(packet, pid): + """Encode a 1-31 byte CRTP packet into 20-byte ATT fragments using a 2-bit PID.""" + if not 1 <= len(packet) <= 31: + raise FrameError('CRTP packet must contain 1..31 bytes') + first = bytes([0x80 | ((pid & 3) << 5) | (len(packet) - 1)]) + packet[:19] + return [first] + ([bytes([(pid & 3) << 5]) + packet[19:]] if len(packet) > 19 else []) diff --git a/test/crtp/test_bletransport.py b/test/crtp/test_bletransport.py new file mode 100644 index 000000000..8c9eadb65 --- /dev/null +++ b/test/crtp/test_bletransport.py @@ -0,0 +1,58 @@ +"""BLE codec tests: no Bleak import, platform backend or hardware.""" +import unittest + +from cflib.crtp.bletransport import DownDecoder +from cflib.crtp.bletransport import fragment_up +from cflib.crtp.bletransport import FrameError + + +class CodecTests(unittest.TestCase): + def test_all_lengths_and_pids_roundtrip(self): + for size in range(1, 32): + for pid in range(4): + packet = bytes(range(size)) + decoder = DownDecoder() + result = None + for frame in fragment_up(packet, pid): + result = decoder.feed(frame) + self.assertEqual(result, packet) + + def test_corruption_rejected(self): + decoder = DownDecoder() + decoder.feed(fragment_up(bytes(31), 1)[0]) + with self.assertRaises(FrameError): + decoder.feed(b'\x00\x01') + with self.assertRaises(FrameError): + decoder.feed(b'\x80\0\0') + + def test_rejects_invalid_packet_and_fragment_sizes(self): + for packet in (b'', bytes(32)): + with self.assertRaises(FrameError): + fragment_up(packet, 0) + for fragment in (b'', b'\x80', bytes(21)): + with self.assertRaises(FrameError): + DownDecoder().feed(fragment) + + def test_rejects_orphan_continuation_and_recovers(self): + decoder = DownDecoder() + with self.assertRaises(FrameError): + decoder.feed(b'\x00\x01') + packet = b'\xfcdata' + self.assertEqual(decoder.feed(fragment_up(packet, 2)[0]), packet) + + def test_new_start_during_partial_packet_rejects_and_resets(self): + decoder = DownDecoder() + first = fragment_up(bytes(range(31)), 1)[0] + self.assertIsNone(decoder.feed(first)) + with self.assertRaises(FrameError): + decoder.feed(first) + packet = b'\xfcrecovered' + self.assertEqual(decoder.feed(fragment_up(packet, 3)[0]), packet) + + def test_reset_discards_incomplete_packet(self): + decoder = DownDecoder() + fragments = fragment_up(bytes(range(31)), 0) + self.assertIsNone(decoder.feed(fragments[0])) + decoder.reset() + with self.assertRaises(FrameError): + decoder.feed(fragments[1])