pycyphal2 2.0.0.dev0__py3-none-any.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- pycyphal2/__init__.py +89 -0
- pycyphal2/_api.py +604 -0
- pycyphal2/_hash.py +204 -0
- pycyphal2/_header.py +349 -0
- pycyphal2/_node.py +1472 -0
- pycyphal2/_publisher.py +427 -0
- pycyphal2/_subscriber.py +430 -0
- pycyphal2/_transport.py +92 -0
- pycyphal2/can/__init__.py +43 -0
- pycyphal2/can/_interface.py +131 -0
- pycyphal2/can/_reassembly.py +158 -0
- pycyphal2/can/_transport.py +525 -0
- pycyphal2/can/_wire.py +376 -0
- pycyphal2/can/pythoncan.py +261 -0
- pycyphal2/can/socketcan.py +225 -0
- pycyphal2/py.typed +0 -0
- pycyphal2/udp.py +1000 -0
- pycyphal2-2.0.0.dev0.dist-info/METADATA +58 -0
- pycyphal2-2.0.0.dev0.dist-info/RECORD +22 -0
- pycyphal2-2.0.0.dev0.dist-info/WHEEL +5 -0
- pycyphal2-2.0.0.dev0.dist-info/licenses/LICENSE +20 -0
- pycyphal2-2.0.0.dev0.dist-info/top_level.txt +1 -0
pycyphal2/udp.py
ADDED
|
@@ -0,0 +1,1000 @@
|
|
|
1
|
+
"""
|
|
2
|
+
Cyphal/UDP transport — zero-config reliable pub/sub over IPv4 multicast.
|
|
3
|
+
|
|
4
|
+
```python
|
|
5
|
+
from pycyphal2.udp import UDPTransport
|
|
6
|
+
|
|
7
|
+
transport = UDPTransport.new() # auto-detects network interfaces to use
|
|
8
|
+
```
|
|
9
|
+
|
|
10
|
+
Pass the transport to `pycyphal2.Node.new()` to start a node.
|
|
11
|
+
|
|
12
|
+
`UDPTransport.new()` discovers usable IPv4 interfaces automatically and generates a random node identity.
|
|
13
|
+
For machine-local networking, use `UDPTransport.new_loopback()`.
|
|
14
|
+
|
|
15
|
+
Requires third-party dependencies — install with `pip install pycyphal2[udp]`.
|
|
16
|
+
"""
|
|
17
|
+
|
|
18
|
+
# This module is directly importable by the application (hence no underscore prefix), so its API must be spotless!
|
|
19
|
+
|
|
20
|
+
from __future__ import annotations
|
|
21
|
+
|
|
22
|
+
import asyncio
|
|
23
|
+
import logging
|
|
24
|
+
import os
|
|
25
|
+
import socket
|
|
26
|
+
import struct
|
|
27
|
+
import sys
|
|
28
|
+
from abc import ABC, abstractmethod
|
|
29
|
+
from collections import OrderedDict
|
|
30
|
+
from collections.abc import Callable, Iterable
|
|
31
|
+
from dataclasses import dataclass, field
|
|
32
|
+
from ipaddress import IPv4Address
|
|
33
|
+
|
|
34
|
+
import ifaddr
|
|
35
|
+
|
|
36
|
+
from . import Closable, ClosedError, Instant, Priority, SendError, eui64
|
|
37
|
+
from ._api import SUBJECT_ID_PINNED_MAX
|
|
38
|
+
from ._hash import CRC32C_INITIAL, CRC32C_OUTPUT_XOR, CRC32C_RESIDUE, crc32c_add, crc32c_full
|
|
39
|
+
from ._transport import SUBJECT_ID_MODULUS_23bit, SubjectWriter, Transport, TransportArrival
|
|
40
|
+
|
|
41
|
+
try:
|
|
42
|
+
import fcntl
|
|
43
|
+
except ImportError:
|
|
44
|
+
fcntl = None # type: ignore[assignment]
|
|
45
|
+
|
|
46
|
+
_logger = logging.getLogger(__name__)
|
|
47
|
+
|
|
48
|
+
UDP_PORT = 9382
|
|
49
|
+
HEADER_SIZE = 32
|
|
50
|
+
HEADER_VERSION = 2
|
|
51
|
+
IPv4_MCAST_PREFIX = 0xEF000000
|
|
52
|
+
IPv4_SUBJECT_ID_MAX = 0x7FFFFF
|
|
53
|
+
TRANSFER_ID_MASK = (1 << 48) - 1
|
|
54
|
+
_MULTICAST_TTL = 16
|
|
55
|
+
_SIOCGIFMTU = 0x8921
|
|
56
|
+
_CYPHAL_OVERHEAD_MAX = 100
|
|
57
|
+
_CYPHAL_MTU_LINK_MIN = 576
|
|
58
|
+
_RX_SESSION_LIFETIME_NS = round(30.0 * 1e9)
|
|
59
|
+
_RX_SLOT_COUNT = 8
|
|
60
|
+
_RX_TRANSFER_HISTORY_COUNT = 32
|
|
61
|
+
_SUBJECT_ID_MODULUS_MAX = IPv4_SUBJECT_ID_MAX - SUBJECT_ID_PINNED_MAX
|
|
62
|
+
|
|
63
|
+
|
|
64
|
+
# =====================================================================================================================
|
|
65
|
+
# Header Serialization / Deserialization
|
|
66
|
+
# =====================================================================================================================
|
|
67
|
+
|
|
68
|
+
|
|
69
|
+
@dataclass(frozen=True)
|
|
70
|
+
class _FrameHeader:
|
|
71
|
+
priority: int
|
|
72
|
+
transfer_id: int
|
|
73
|
+
sender_uid: int
|
|
74
|
+
frame_payload_offset: int
|
|
75
|
+
transfer_payload_size: int
|
|
76
|
+
prefix_crc: int
|
|
77
|
+
|
|
78
|
+
|
|
79
|
+
def _header_serialize(
|
|
80
|
+
priority: int,
|
|
81
|
+
transfer_id: int,
|
|
82
|
+
sender_uid: int,
|
|
83
|
+
frame_payload_offset: int,
|
|
84
|
+
transfer_payload_size: int,
|
|
85
|
+
prefix_crc: int,
|
|
86
|
+
) -> bytes:
|
|
87
|
+
"""Serialize a 32-byte Cyphal/UDP frame header."""
|
|
88
|
+
buf = bytearray(HEADER_SIZE)
|
|
89
|
+
buf[0] = HEADER_VERSION | ((priority & 0x07) << 5)
|
|
90
|
+
buf[1] = 0 # incompatibility | reserved
|
|
91
|
+
for i in range(6):
|
|
92
|
+
buf[2 + i] = (transfer_id >> (i * 8)) & 0xFF
|
|
93
|
+
struct.pack_into("<Q", buf, 8, sender_uid)
|
|
94
|
+
struct.pack_into("<I", buf, 16, frame_payload_offset)
|
|
95
|
+
struct.pack_into("<I", buf, 20, transfer_payload_size)
|
|
96
|
+
struct.pack_into("<I", buf, 24, prefix_crc)
|
|
97
|
+
struct.pack_into("<I", buf, 28, crc32c_full(memoryview(buf[:28])))
|
|
98
|
+
return bytes(buf)
|
|
99
|
+
|
|
100
|
+
|
|
101
|
+
def _header_deserialize(data: bytes | memoryview) -> _FrameHeader | None:
|
|
102
|
+
"""Deserialize a 32-byte frame header. Returns None on validation failure."""
|
|
103
|
+
# Wire data is untrusted: malformed headers are dropped here, never surfaced as exceptions.
|
|
104
|
+
if len(data) < HEADER_SIZE:
|
|
105
|
+
_logger.debug("UDP hdr drop short len=%d", len(data))
|
|
106
|
+
return None
|
|
107
|
+
# Validate header CRC (CRC of all 32 bytes must equal the residue constant)
|
|
108
|
+
if crc32c_full(memoryview(data[:HEADER_SIZE])) != CRC32C_RESIDUE:
|
|
109
|
+
_logger.debug("UDP hdr drop crc")
|
|
110
|
+
return None
|
|
111
|
+
head = data[0]
|
|
112
|
+
if (head & 0x1F) != HEADER_VERSION:
|
|
113
|
+
_logger.debug("UDP hdr drop version=%d", head & 0x1F)
|
|
114
|
+
return None
|
|
115
|
+
if (data[1] >> 5) != 0: # incompatibility bits
|
|
116
|
+
_logger.debug("UDP hdr drop incompatibility=%d", data[1] >> 5)
|
|
117
|
+
return None
|
|
118
|
+
priority = (head >> 5) & 0x07
|
|
119
|
+
transfer_id = 0
|
|
120
|
+
for i in range(6):
|
|
121
|
+
transfer_id |= data[2 + i] << (i * 8)
|
|
122
|
+
sender_uid = struct.unpack_from("<Q", data, 8)[0]
|
|
123
|
+
frame_payload_offset = struct.unpack_from("<I", data, 16)[0]
|
|
124
|
+
transfer_payload_size = struct.unpack_from("<I", data, 20)[0]
|
|
125
|
+
prefix_crc = struct.unpack_from("<I", data, 24)[0]
|
|
126
|
+
# Validate frame bounds
|
|
127
|
+
return _FrameHeader(priority, transfer_id, sender_uid, frame_payload_offset, transfer_payload_size, prefix_crc)
|
|
128
|
+
|
|
129
|
+
|
|
130
|
+
# =====================================================================================================================
|
|
131
|
+
# TX Segmentation
|
|
132
|
+
# =====================================================================================================================
|
|
133
|
+
|
|
134
|
+
|
|
135
|
+
def _segment_transfer(
|
|
136
|
+
priority: int, transfer_id: int, sender_uid: int, payload: bytes | memoryview, mtu: int
|
|
137
|
+
) -> list[bytes]:
|
|
138
|
+
"""Segment a transfer payload into wire-format frames (header + chunk each).
|
|
139
|
+
|
|
140
|
+
The ``mtu`` parameter is the max Cyphal frame payload size per frame (mtu_cyphal).
|
|
141
|
+
"""
|
|
142
|
+
payload = bytes(payload)
|
|
143
|
+
size = len(payload)
|
|
144
|
+
frames: list[bytes] = []
|
|
145
|
+
offset = 0
|
|
146
|
+
running_crc = CRC32C_INITIAL
|
|
147
|
+
while True:
|
|
148
|
+
progress = min(size - offset, mtu)
|
|
149
|
+
chunk = payload[offset : offset + progress]
|
|
150
|
+
running_crc = crc32c_add(running_crc, chunk)
|
|
151
|
+
header = _header_serialize(priority, transfer_id, sender_uid, offset, size, running_crc ^ CRC32C_OUTPUT_XOR)
|
|
152
|
+
frames.append(header + chunk)
|
|
153
|
+
offset += progress
|
|
154
|
+
if offset >= size:
|
|
155
|
+
break
|
|
156
|
+
return frames
|
|
157
|
+
|
|
158
|
+
|
|
159
|
+
# =====================================================================================================================
|
|
160
|
+
# RX Reassembly
|
|
161
|
+
# =====================================================================================================================
|
|
162
|
+
|
|
163
|
+
|
|
164
|
+
def _frame_is_valid(header: _FrameHeader, payload_chunk: bytes | memoryview) -> bool:
|
|
165
|
+
# This validator is part of the RX policy boundary: bad wire frames are rejected with False, not exceptions.
|
|
166
|
+
if header.frame_payload_offset == 0 and crc32c_full(payload_chunk) != header.prefix_crc:
|
|
167
|
+
return False
|
|
168
|
+
return (header.frame_payload_offset + len(payload_chunk)) <= header.transfer_payload_size
|
|
169
|
+
|
|
170
|
+
|
|
171
|
+
@dataclass(frozen=True)
|
|
172
|
+
class _Fragment:
|
|
173
|
+
offset: int
|
|
174
|
+
data: bytes
|
|
175
|
+
crc: int
|
|
176
|
+
|
|
177
|
+
@property
|
|
178
|
+
def end(self) -> int:
|
|
179
|
+
return self.offset + len(self.data)
|
|
180
|
+
|
|
181
|
+
|
|
182
|
+
@dataclass(frozen=True)
|
|
183
|
+
class _RxTransfer:
|
|
184
|
+
sender_uid: int
|
|
185
|
+
priority: int
|
|
186
|
+
payload: bytes
|
|
187
|
+
timestamp_ns: int
|
|
188
|
+
|
|
189
|
+
|
|
190
|
+
@dataclass
|
|
191
|
+
class _TransferSlot:
|
|
192
|
+
transfer_id: int
|
|
193
|
+
total_size: int
|
|
194
|
+
priority: int
|
|
195
|
+
ts_min_ns: int
|
|
196
|
+
ts_max_ns: int
|
|
197
|
+
covered_prefix: int = 0
|
|
198
|
+
crc_end: int = 0
|
|
199
|
+
crc: int = CRC32C_INITIAL
|
|
200
|
+
fragments: list[_Fragment] = field(default_factory=list)
|
|
201
|
+
|
|
202
|
+
@classmethod
|
|
203
|
+
def create(cls, header: _FrameHeader, timestamp_ns: int) -> _TransferSlot:
|
|
204
|
+
return cls(
|
|
205
|
+
transfer_id=header.transfer_id,
|
|
206
|
+
total_size=header.transfer_payload_size,
|
|
207
|
+
priority=header.priority,
|
|
208
|
+
ts_min_ns=timestamp_ns,
|
|
209
|
+
ts_max_ns=timestamp_ns,
|
|
210
|
+
)
|
|
211
|
+
|
|
212
|
+
def update(self, timestamp_ns: int, header: _FrameHeader, payload_chunk: bytes) -> bytes | None:
|
|
213
|
+
if self._accept_fragment(header.frame_payload_offset, payload_chunk, header.prefix_crc):
|
|
214
|
+
self.ts_max_ns = max(self.ts_max_ns, timestamp_ns)
|
|
215
|
+
self.ts_min_ns = min(self.ts_min_ns, timestamp_ns)
|
|
216
|
+
crc_end = header.frame_payload_offset + len(payload_chunk)
|
|
217
|
+
if crc_end >= self.crc_end:
|
|
218
|
+
self.crc_end = crc_end
|
|
219
|
+
self.crc = header.prefix_crc
|
|
220
|
+
if self.covered_prefix < self.total_size:
|
|
221
|
+
return None
|
|
222
|
+
return self._finalize_payload()
|
|
223
|
+
|
|
224
|
+
def _accept_fragment(self, offset: int, data: bytes, crc: int) -> bool:
|
|
225
|
+
left = offset
|
|
226
|
+
right = offset + len(data)
|
|
227
|
+
for frag in self.fragments:
|
|
228
|
+
if frag.offset <= left and frag.end >= right:
|
|
229
|
+
return False
|
|
230
|
+
|
|
231
|
+
left_neighbor = self._find_left_neighbor(left)
|
|
232
|
+
right_neighbor = self._find_right_neighbor(right)
|
|
233
|
+
left_size = len(left_neighbor.data) if left_neighbor is not None else 0
|
|
234
|
+
right_size = len(right_neighbor.data) if right_neighbor is not None else 0
|
|
235
|
+
accept = (
|
|
236
|
+
left_neighbor is None
|
|
237
|
+
or right_neighbor is None
|
|
238
|
+
or left_neighbor.end < right_neighbor.offset
|
|
239
|
+
or len(data) > min(left_size, right_size)
|
|
240
|
+
)
|
|
241
|
+
if not accept:
|
|
242
|
+
return False
|
|
243
|
+
|
|
244
|
+
v_left = min(left, left_neighbor.offset + 1) if left_neighbor is not None else left
|
|
245
|
+
v_right = max(right, max(right_neighbor.end, 1) - 1) if right_neighbor is not None else right
|
|
246
|
+
self.fragments = [frag for frag in self.fragments if not (frag.offset >= v_left and frag.end <= v_right)]
|
|
247
|
+
self.fragments.append(_Fragment(offset=offset, data=data, crc=crc))
|
|
248
|
+
self.fragments.sort(key=lambda frag: frag.offset)
|
|
249
|
+
self.covered_prefix = self._compute_covered_prefix()
|
|
250
|
+
return True
|
|
251
|
+
|
|
252
|
+
def _find_left_neighbor(self, left: int) -> _Fragment | None:
|
|
253
|
+
for frag in self.fragments:
|
|
254
|
+
if frag.end >= left:
|
|
255
|
+
return None if frag.offset >= left else frag
|
|
256
|
+
return None
|
|
257
|
+
|
|
258
|
+
def _find_right_neighbor(self, right: int) -> _Fragment | None:
|
|
259
|
+
candidate: _Fragment | None = None
|
|
260
|
+
for frag in self.fragments:
|
|
261
|
+
if frag.offset < right:
|
|
262
|
+
candidate = frag
|
|
263
|
+
else:
|
|
264
|
+
break
|
|
265
|
+
if candidate is not None and candidate.end <= right:
|
|
266
|
+
return None
|
|
267
|
+
return candidate
|
|
268
|
+
|
|
269
|
+
def _compute_covered_prefix(self) -> int:
|
|
270
|
+
covered = 0
|
|
271
|
+
for frag in self.fragments:
|
|
272
|
+
if frag.offset > covered:
|
|
273
|
+
break
|
|
274
|
+
covered = max(covered, frag.end)
|
|
275
|
+
return covered
|
|
276
|
+
|
|
277
|
+
def _finalize_payload(self) -> bytes | None:
|
|
278
|
+
offset = 0
|
|
279
|
+
parts: list[bytes] = []
|
|
280
|
+
for frag in self.fragments:
|
|
281
|
+
if frag.offset > offset:
|
|
282
|
+
return None
|
|
283
|
+
trim = offset - frag.offset
|
|
284
|
+
if trim >= len(frag.data):
|
|
285
|
+
continue
|
|
286
|
+
view = frag.data[trim:]
|
|
287
|
+
parts.append(view)
|
|
288
|
+
offset += len(view)
|
|
289
|
+
payload = b"".join(parts)
|
|
290
|
+
if len(payload) != self.total_size:
|
|
291
|
+
return None
|
|
292
|
+
if crc32c_full(payload) != self.crc:
|
|
293
|
+
return None
|
|
294
|
+
return payload
|
|
295
|
+
|
|
296
|
+
|
|
297
|
+
@dataclass
|
|
298
|
+
class _RxSession:
|
|
299
|
+
last_animated_ns: int
|
|
300
|
+
history: list[int] = field(default_factory=lambda: [0] * _RX_TRANSFER_HISTORY_COUNT)
|
|
301
|
+
history_current: int = 0
|
|
302
|
+
initialized: bool = False
|
|
303
|
+
slots: list[_TransferSlot | None] = field(default_factory=lambda: [None] * _RX_SLOT_COUNT)
|
|
304
|
+
|
|
305
|
+
def is_transfer_ejected(self, transfer_id: int) -> bool:
|
|
306
|
+
return transfer_id in self.history
|
|
307
|
+
|
|
308
|
+
def initialize_history(self, transfer_id: int) -> None:
|
|
309
|
+
value = (transfer_id - 1) & TRANSFER_ID_MASK
|
|
310
|
+
self.history = [value] * _RX_TRANSFER_HISTORY_COUNT
|
|
311
|
+
self.history_current = 0
|
|
312
|
+
self.initialized = True
|
|
313
|
+
|
|
314
|
+
def record_transfer_ejected(self, transfer_id: int) -> None:
|
|
315
|
+
self.history_current = (self.history_current + 1) % _RX_TRANSFER_HISTORY_COUNT
|
|
316
|
+
self.history[self.history_current] = transfer_id
|
|
317
|
+
|
|
318
|
+
def get_slot(self, timestamp_ns: int, header: _FrameHeader) -> tuple[int, _TransferSlot]:
|
|
319
|
+
for index, slot in enumerate(self.slots):
|
|
320
|
+
if slot is not None and slot.transfer_id == header.transfer_id:
|
|
321
|
+
return index, slot
|
|
322
|
+
for index, slot in enumerate(self.slots):
|
|
323
|
+
if slot is not None and timestamp_ns >= (slot.ts_max_ns + _RX_SESSION_LIFETIME_NS):
|
|
324
|
+
self.slots[index] = None
|
|
325
|
+
for index, slot in enumerate(self.slots):
|
|
326
|
+
if slot is None:
|
|
327
|
+
created = _TransferSlot.create(header, timestamp_ns)
|
|
328
|
+
self.slots[index] = created
|
|
329
|
+
return index, created
|
|
330
|
+
oldest_index = 0
|
|
331
|
+
oldest_slot: _TransferSlot | None = None
|
|
332
|
+
for index, slot in enumerate(self.slots):
|
|
333
|
+
if slot is None:
|
|
334
|
+
continue
|
|
335
|
+
if (oldest_slot is None) or (slot.ts_max_ns < oldest_slot.ts_max_ns):
|
|
336
|
+
oldest_index = index
|
|
337
|
+
oldest_slot = slot
|
|
338
|
+
if oldest_slot is None:
|
|
339
|
+
_logger.debug("UDP reasm slot fallback uid=%016x tid=%d", header.sender_uid, header.transfer_id)
|
|
340
|
+
created = _TransferSlot.create(header, timestamp_ns)
|
|
341
|
+
self.slots[oldest_index] = created
|
|
342
|
+
return oldest_index, created
|
|
343
|
+
|
|
344
|
+
|
|
345
|
+
class _RxReassembler:
|
|
346
|
+
"""Multi-frame transfer reassembly with per-sender session state."""
|
|
347
|
+
|
|
348
|
+
def __init__(self) -> None:
|
|
349
|
+
self._sessions: OrderedDict[int, _RxSession] = OrderedDict()
|
|
350
|
+
|
|
351
|
+
def accept(
|
|
352
|
+
self,
|
|
353
|
+
header: _FrameHeader,
|
|
354
|
+
payload_chunk: bytes,
|
|
355
|
+
*,
|
|
356
|
+
timestamp_ns: int | None = None,
|
|
357
|
+
frame_validated: bool = False,
|
|
358
|
+
) -> _RxTransfer | None:
|
|
359
|
+
timestamp_ns = Instant.now().ns if timestamp_ns is None else timestamp_ns
|
|
360
|
+
if not frame_validated and not _frame_is_valid(header, payload_chunk):
|
|
361
|
+
_logger.debug("UDP reasm drop invalid uid=%016x tid=%d", header.sender_uid, header.transfer_id)
|
|
362
|
+
return None
|
|
363
|
+
session: _RxSession | None = None
|
|
364
|
+
slot_index: int | None = None
|
|
365
|
+
try:
|
|
366
|
+
self._retire_one_stale_session(timestamp_ns)
|
|
367
|
+
session = self._sessions.get(header.sender_uid)
|
|
368
|
+
if session is None:
|
|
369
|
+
session = _RxSession(last_animated_ns=timestamp_ns)
|
|
370
|
+
self._sessions[header.sender_uid] = session
|
|
371
|
+
session.last_animated_ns = timestamp_ns
|
|
372
|
+
self._sessions.move_to_end(header.sender_uid, last=False)
|
|
373
|
+
if not session.initialized:
|
|
374
|
+
session.initialize_history(header.transfer_id)
|
|
375
|
+
if session.is_transfer_ejected(header.transfer_id):
|
|
376
|
+
_logger.debug("UDP reasm dup uid=%016x tid=%d", header.sender_uid, header.transfer_id)
|
|
377
|
+
return None
|
|
378
|
+
slot_index, slot = session.get_slot(timestamp_ns, header)
|
|
379
|
+
if (slot.total_size != header.transfer_payload_size) or (slot.priority != header.priority):
|
|
380
|
+
# Per RX policy, inconsistent per-transfer metadata is malformed wire input, not an exception path.
|
|
381
|
+
session.slots[slot_index] = None
|
|
382
|
+
_logger.debug("UDP reasm drop uid=%016x tid=%d reason=metadata", header.sender_uid, header.transfer_id)
|
|
383
|
+
return None
|
|
384
|
+
payload = slot.update(timestamp_ns, header, payload_chunk)
|
|
385
|
+
except Exception as ex:
|
|
386
|
+
if (session is not None) and (slot_index is not None):
|
|
387
|
+
session.slots[slot_index] = None
|
|
388
|
+
# RX state is driven by untrusted wire data; any malformed-input fault is downgraded to drop+debug.
|
|
389
|
+
_logger.debug(
|
|
390
|
+
"UDP reasm fault uid=%016x tid=%d %s", header.sender_uid, header.transfer_id, ex, exc_info=True
|
|
391
|
+
)
|
|
392
|
+
return None
|
|
393
|
+
if payload is None:
|
|
394
|
+
if (session is not None) and (slot_index is not None):
|
|
395
|
+
slot_state = session.slots[slot_index]
|
|
396
|
+
if (slot_state is not None) and (slot_state.covered_prefix >= slot_state.total_size):
|
|
397
|
+
# A fully covered but non-finalizable transfer is malformed on the wire, so we drop its slot here.
|
|
398
|
+
session.slots[slot_index] = None
|
|
399
|
+
_logger.debug(
|
|
400
|
+
"UDP reasm drop uid=%016x tid=%d reason=finalize", header.sender_uid, header.transfer_id
|
|
401
|
+
)
|
|
402
|
+
return None
|
|
403
|
+
if (session is None) or (slot_index is None):
|
|
404
|
+
_logger.debug("UDP reasm completion fallback uid=%016x tid=%d", header.sender_uid, header.transfer_id)
|
|
405
|
+
return None
|
|
406
|
+
session.record_transfer_ejected(header.transfer_id)
|
|
407
|
+
session.slots[slot_index] = None
|
|
408
|
+
_logger.debug("UDP reasm done uid=%016x tid=%d n=%d", header.sender_uid, header.transfer_id, len(payload))
|
|
409
|
+
return _RxTransfer(
|
|
410
|
+
sender_uid=header.sender_uid,
|
|
411
|
+
priority=slot.priority,
|
|
412
|
+
payload=payload,
|
|
413
|
+
timestamp_ns=slot.ts_min_ns,
|
|
414
|
+
)
|
|
415
|
+
|
|
416
|
+
def _retire_one_stale_session(self, timestamp_ns: int) -> None:
|
|
417
|
+
if not self._sessions:
|
|
418
|
+
return
|
|
419
|
+
oldest_uid = next(reversed(self._sessions))
|
|
420
|
+
oldest = self._sessions[oldest_uid]
|
|
421
|
+
if timestamp_ns >= (oldest.last_animated_ns + _RX_SESSION_LIFETIME_NS):
|
|
422
|
+
self._sessions.pop(oldest_uid)
|
|
423
|
+
_logger.debug("UDP reasm retire uid=%016x", oldest_uid)
|
|
424
|
+
|
|
425
|
+
|
|
426
|
+
# =====================================================================================================================
|
|
427
|
+
# Utilities
|
|
428
|
+
# =====================================================================================================================
|
|
429
|
+
|
|
430
|
+
|
|
431
|
+
def _make_subject_endpoint(subject_id: int) -> tuple[str, int]:
|
|
432
|
+
"""Return (multicast_ip, port) for a given subject_id."""
|
|
433
|
+
ip_int = IPv4_MCAST_PREFIX | (subject_id & IPv4_SUBJECT_ID_MAX)
|
|
434
|
+
return (str(IPv4Address(ip_int)), UDP_PORT)
|
|
435
|
+
|
|
436
|
+
|
|
437
|
+
def _get_iface_mtu(ifname: str) -> int:
|
|
438
|
+
"""Get link MTU via ioctl on Linux, default 1500 otherwise."""
|
|
439
|
+
if sys.platform == "linux" and fcntl is not None:
|
|
440
|
+
try:
|
|
441
|
+
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
442
|
+
try:
|
|
443
|
+
ifreq = struct.pack("256s", ifname.encode()[:15])
|
|
444
|
+
result = fcntl.ioctl(s.fileno(), _SIOCGIFMTU, ifreq)
|
|
445
|
+
return int(struct.unpack_from("i", result, 16)[0])
|
|
446
|
+
finally:
|
|
447
|
+
s.close()
|
|
448
|
+
except OSError:
|
|
449
|
+
pass
|
|
450
|
+
return 1500
|
|
451
|
+
|
|
452
|
+
|
|
453
|
+
def _get_default_iface_ip() -> IPv4Address | None:
|
|
454
|
+
"""Determine the default interface IP via the connect-to-1.1.1.1 trick."""
|
|
455
|
+
try:
|
|
456
|
+
s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
|
457
|
+
try:
|
|
458
|
+
s.connect(("1.1.1.1", 80))
|
|
459
|
+
return IPv4Address(s.getsockname()[0])
|
|
460
|
+
finally:
|
|
461
|
+
s.close()
|
|
462
|
+
except OSError:
|
|
463
|
+
return None
|
|
464
|
+
|
|
465
|
+
|
|
466
|
+
# =====================================================================================================================
|
|
467
|
+
# Interface
|
|
468
|
+
# =====================================================================================================================
|
|
469
|
+
|
|
470
|
+
|
|
471
|
+
@dataclass(frozen=True)
|
|
472
|
+
class Interface:
|
|
473
|
+
address: IPv4Address
|
|
474
|
+
mtu_link: int
|
|
475
|
+
"""Link-layer MTU. E.g., 1500 for Ethernet, ~64K for loopback."""
|
|
476
|
+
|
|
477
|
+
@property
|
|
478
|
+
def mtu_cyphal(self) -> int:
|
|
479
|
+
"""Max Cyphal frame payload: mtu_link - 60 (IPv4 max) - 8 (UDP) - 32 (Cyphal header)."""
|
|
480
|
+
assert self.mtu_link >= _CYPHAL_MTU_LINK_MIN
|
|
481
|
+
return self.mtu_link - _CYPHAL_OVERHEAD_MAX
|
|
482
|
+
|
|
483
|
+
|
|
484
|
+
# =====================================================================================================================
|
|
485
|
+
# Subject Writer / Listener
|
|
486
|
+
# =====================================================================================================================
|
|
487
|
+
|
|
488
|
+
|
|
489
|
+
class _UDPSubjectWriter(SubjectWriter):
|
|
490
|
+
def __init__(self, transport: _UDPTransportImpl, subject_id: int) -> None:
|
|
491
|
+
self._transport = transport
|
|
492
|
+
self._subject_id = subject_id
|
|
493
|
+
self._transfer_id = int.from_bytes(os.urandom(6), "little")
|
|
494
|
+
self._closed = False
|
|
495
|
+
|
|
496
|
+
async def __call__(self, deadline: Instant, priority: Priority, message: bytes | memoryview) -> None:
|
|
497
|
+
if self._closed:
|
|
498
|
+
raise ClosedError("Writer closed")
|
|
499
|
+
if self._transport.closed:
|
|
500
|
+
raise ClosedError("Transport closed")
|
|
501
|
+
|
|
502
|
+
mcast_ip, port = _make_subject_endpoint(self._subject_id)
|
|
503
|
+
transfer_id = self._transfer_id & TRANSFER_ID_MASK
|
|
504
|
+
self._transfer_id += 1
|
|
505
|
+
_logger.debug("Subject tx start sid=%d tid=%d bytes=%d", self._subject_id, transfer_id, len(message))
|
|
506
|
+
|
|
507
|
+
errors: list[Exception] = []
|
|
508
|
+
success_count = 0
|
|
509
|
+
for i, iface in enumerate(self._transport.interfaces):
|
|
510
|
+
mtu = iface.mtu_cyphal
|
|
511
|
+
frames = _segment_transfer(priority, transfer_id, self._transport.uid, message, mtu)
|
|
512
|
+
try:
|
|
513
|
+
for frame in frames:
|
|
514
|
+
await self._transport.async_sendto(self._transport.tx_socks[i], frame, (mcast_ip, port), deadline)
|
|
515
|
+
success_count += 1
|
|
516
|
+
except (OSError, SendError) as e:
|
|
517
|
+
errors.append(e)
|
|
518
|
+
|
|
519
|
+
if errors:
|
|
520
|
+
eg = ExceptionGroup("send failed on some interfaces", errors)
|
|
521
|
+
if success_count == 0:
|
|
522
|
+
_logger.error("Send failed on all interfaces for subject %d", self._subject_id)
|
|
523
|
+
raise SendError("send failed on all interfaces") from eg
|
|
524
|
+
_logger.warning(
|
|
525
|
+
"Send failed on %d/%d interfaces for subject %d",
|
|
526
|
+
len(errors),
|
|
527
|
+
len(errors) + success_count,
|
|
528
|
+
self._subject_id,
|
|
529
|
+
)
|
|
530
|
+
raise eg
|
|
531
|
+
|
|
532
|
+
_logger.debug("Subject tx done sid=%d tid=%d", self._subject_id, transfer_id)
|
|
533
|
+
|
|
534
|
+
def close(self) -> None:
|
|
535
|
+
if self._closed:
|
|
536
|
+
return
|
|
537
|
+
self._closed = True
|
|
538
|
+
self._transport.remove_subject_writer(self._subject_id, self)
|
|
539
|
+
_logger.debug("Subject writer closed for subject %d", self._subject_id)
|
|
540
|
+
|
|
541
|
+
|
|
542
|
+
class _UDPSubjectListener(Closable):
|
|
543
|
+
def __init__(
|
|
544
|
+
self, transport: _UDPTransportImpl, subject_id: int, handler: Callable[[TransportArrival], None]
|
|
545
|
+
) -> None:
|
|
546
|
+
self._transport = transport
|
|
547
|
+
self._subject_id = subject_id
|
|
548
|
+
self._handler = handler
|
|
549
|
+
self._closed = False
|
|
550
|
+
|
|
551
|
+
def close(self) -> None:
|
|
552
|
+
if self._closed:
|
|
553
|
+
return
|
|
554
|
+
self._closed = True
|
|
555
|
+
_logger.info("Subject listener closed for subject %d", self._subject_id)
|
|
556
|
+
self._transport.remove_subject_listener(self._subject_id, self._handler)
|
|
557
|
+
|
|
558
|
+
|
|
559
|
+
# =====================================================================================================================
|
|
560
|
+
# UDPTransport
|
|
561
|
+
# =====================================================================================================================
|
|
562
|
+
|
|
563
|
+
|
|
564
|
+
class UDPTransport(Transport, ABC):
|
|
565
|
+
"""
|
|
566
|
+
The public API of the Cyphal/UDP transport.
|
|
567
|
+
"""
|
|
568
|
+
|
|
569
|
+
@property
|
|
570
|
+
@abstractmethod
|
|
571
|
+
def uid(self) -> int:
|
|
572
|
+
"""The 64-bit globally unique ID of the local node."""
|
|
573
|
+
raise NotImplementedError
|
|
574
|
+
|
|
575
|
+
@property
|
|
576
|
+
@abstractmethod
|
|
577
|
+
def interfaces(self) -> list[Interface]:
|
|
578
|
+
"""List of (redundant) interfaces that the transport is operating over. Never empty."""
|
|
579
|
+
raise NotImplementedError
|
|
580
|
+
|
|
581
|
+
@staticmethod
|
|
582
|
+
def new(
|
|
583
|
+
interfaces: Iterable[Interface] | None = None,
|
|
584
|
+
uid: int | None = None,
|
|
585
|
+
*,
|
|
586
|
+
subject_id_modulus: int = SUBJECT_ID_MODULUS_23bit,
|
|
587
|
+
) -> UDPTransport:
|
|
588
|
+
"""
|
|
589
|
+
Constructs a new Cyphal/UDP transport instance that will operate over the specified local network interfaces.
|
|
590
|
+
|
|
591
|
+
If no interfaces are given (empty list or None, which is default), suitable interfaces will be automatically
|
|
592
|
+
detected. You can also use ``UDPTransport.list_interfaces()`` for a semi-automatic approach.
|
|
593
|
+
|
|
594
|
+
The UID is a globally unique 64-bit identifier of the local node. If not given, one will be generated randomly.
|
|
595
|
+
"""
|
|
596
|
+
# Resolve interfaces.
|
|
597
|
+
if not interfaces:
|
|
598
|
+
ifaces = UDPTransport.list_interfaces()
|
|
599
|
+
if not ifaces:
|
|
600
|
+
raise RuntimeError("No suitable network interfaces found")
|
|
601
|
+
interfaces = [ifaces[0]]
|
|
602
|
+
else:
|
|
603
|
+
interfaces = list(interfaces)
|
|
604
|
+
if not isinstance(interfaces, list) or not all(isinstance(i, Interface) for i in interfaces):
|
|
605
|
+
raise ValueError("interfaces must be an iterable of Interface instances")
|
|
606
|
+
|
|
607
|
+
# Resolve UID.
|
|
608
|
+
uid = uid or eui64()
|
|
609
|
+
if not isinstance(uid, int) or not (0 < uid < 2**64):
|
|
610
|
+
raise ValueError("uid must be a positive 64-bit integer")
|
|
611
|
+
|
|
612
|
+
return _UDPTransportImpl(interfaces=interfaces, uid=uid, subject_id_modulus=subject_id_modulus)
|
|
613
|
+
|
|
614
|
+
@staticmethod
|
|
615
|
+
def new_loopback() -> UDPTransport:
|
|
616
|
+
"""A simple wrapper that uses the local loopback interface."""
|
|
617
|
+
return UDPTransport.new([Interface(IPv4Address("127.0.0.1"), mtu_link=1500)])
|
|
618
|
+
|
|
619
|
+
@staticmethod
|
|
620
|
+
def list_interfaces() -> list[Interface]:
|
|
621
|
+
"""List usable IPv4 network interfaces. Default interface first, loopback last."""
|
|
622
|
+
default_ip = _get_default_iface_ip()
|
|
623
|
+
result: list[Interface] = []
|
|
624
|
+
for adapter in ifaddr.get_adapters():
|
|
625
|
+
for ip in adapter.ips:
|
|
626
|
+
if not isinstance(ip.ip, str):
|
|
627
|
+
_logger.info("Skipping non-string IP on %s: %r", adapter.name, ip.ip)
|
|
628
|
+
continue
|
|
629
|
+
try:
|
|
630
|
+
addr = IPv4Address(ip.ip)
|
|
631
|
+
except ValueError:
|
|
632
|
+
_logger.info("Skipping non-IPv4 address on %s: %s", adapter.name, ip.ip)
|
|
633
|
+
continue
|
|
634
|
+
mtu = _get_iface_mtu(adapter.name)
|
|
635
|
+
if mtu < _CYPHAL_MTU_LINK_MIN:
|
|
636
|
+
_logger.info("Skipping %s (%s): MTU %d < %d", adapter.name, addr, mtu, _CYPHAL_MTU_LINK_MIN)
|
|
637
|
+
continue
|
|
638
|
+
_logger.info("Found interface %s: %s, MTU=%d", adapter.name, addr, mtu)
|
|
639
|
+
result.append(Interface(address=addr, mtu_link=mtu))
|
|
640
|
+
|
|
641
|
+
def sort_key(iface: Interface) -> tuple[int, str]:
|
|
642
|
+
if default_ip is not None and iface.address == default_ip:
|
|
643
|
+
return 0, str(iface.address)
|
|
644
|
+
if iface.address.is_loopback:
|
|
645
|
+
return 2, str(iface.address)
|
|
646
|
+
return 1, str(iface.address)
|
|
647
|
+
|
|
648
|
+
result.sort(key=sort_key)
|
|
649
|
+
return result
|
|
650
|
+
|
|
651
|
+
|
|
652
|
+
class _UDPTransportImpl(UDPTransport):
|
|
653
|
+
def __init__(self, interfaces: Iterable[Interface], uid: int, subject_id_modulus: int) -> None:
|
|
654
|
+
if not (1 <= subject_id_modulus <= _SUBJECT_ID_MODULUS_MAX):
|
|
655
|
+
raise ValueError(f"subject_id_modulus must be in [1, {_SUBJECT_ID_MODULUS_MAX}] for Cyphal/UDP")
|
|
656
|
+
self._uid = uid
|
|
657
|
+
self._subject_id_modulus_val = subject_id_modulus
|
|
658
|
+
self._loop = asyncio.get_running_loop()
|
|
659
|
+
self._closed = False
|
|
660
|
+
|
|
661
|
+
self._interfaces: list[Interface] = list(interfaces)
|
|
662
|
+
if not self._interfaces:
|
|
663
|
+
_logger.error("Empty interfaces list provided")
|
|
664
|
+
raise ValueError("At least one network interface is required")
|
|
665
|
+
|
|
666
|
+
# Per-interface TX/unicast sockets
|
|
667
|
+
self._tx_socks: list[socket.socket] = []
|
|
668
|
+
self._self_endpoints: set[tuple[str, int]] = set()
|
|
669
|
+
for iface in self._interfaces:
|
|
670
|
+
sock = self._create_tx_socket(iface)
|
|
671
|
+
self._tx_socks.append(sock)
|
|
672
|
+
self._self_endpoints.add(sock.getsockname()[:2])
|
|
673
|
+
|
|
674
|
+
# Subject state
|
|
675
|
+
self._subject_handlers: dict[int, Callable[[TransportArrival], None]] = {}
|
|
676
|
+
self._subject_writers: dict[int, _UDPSubjectWriter] = {}
|
|
677
|
+
self._mcast_socks: dict[tuple[int, int], socket.socket] = {}
|
|
678
|
+
self._reassemblers: dict[int, _RxReassembler] = {}
|
|
679
|
+
|
|
680
|
+
# Unicast state
|
|
681
|
+
self._unicast_handler: Callable[[TransportArrival], None] | None = None
|
|
682
|
+
self._unicast_reassembler = _RxReassembler()
|
|
683
|
+
self._remote_endpoints: dict[tuple[int, int], tuple[str, int]] = {}
|
|
684
|
+
self._next_unicast_transfer_id = int.from_bytes(os.urandom(6), "little")
|
|
685
|
+
|
|
686
|
+
# Async RX tasks (platform-agnostic, replaces add_reader)
|
|
687
|
+
self._unicast_rx_tasks: list[asyncio.Task[None]] = []
|
|
688
|
+
self._mcast_rx_tasks: dict[tuple[int, int], asyncio.Task[None]] = {}
|
|
689
|
+
|
|
690
|
+
# Start unicast RX tasks on TX sockets
|
|
691
|
+
for i, sock in enumerate(self._tx_socks):
|
|
692
|
+
task = self._loop.create_task(self._unicast_rx_loop(sock, i))
|
|
693
|
+
self._unicast_rx_tasks.append(task)
|
|
694
|
+
|
|
695
|
+
_logger.info(
|
|
696
|
+
"UDPTransport initialized: uid=0x%016x, interfaces=%s, modulus=%d",
|
|
697
|
+
self._uid,
|
|
698
|
+
[str(i.address) for i in self._interfaces],
|
|
699
|
+
self._subject_id_modulus_val,
|
|
700
|
+
)
|
|
701
|
+
|
|
702
|
+
@staticmethod
|
|
703
|
+
def _create_tx_socket(iface: Interface) -> socket.socket:
|
|
704
|
+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
|
|
705
|
+
sock.setblocking(False)
|
|
706
|
+
sock.bind((str(iface.address), 0))
|
|
707
|
+
sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_TTL, _MULTICAST_TTL)
|
|
708
|
+
sock.setsockopt(socket.IPPROTO_IP, socket.IP_MULTICAST_IF, socket.inet_aton(str(iface.address)))
|
|
709
|
+
_logger.info("TX socket created on %s, bound to port %d", iface.address, sock.getsockname()[1])
|
|
710
|
+
return sock
|
|
711
|
+
|
|
712
|
+
@staticmethod
|
|
713
|
+
def _create_mcast_socket(subject_id: int, iface: Interface) -> socket.socket:
|
|
714
|
+
mcast_ip, port = _make_subject_endpoint(subject_id)
|
|
715
|
+
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM, socket.IPPROTO_UDP)
|
|
716
|
+
sock.setblocking(False)
|
|
717
|
+
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
|
|
718
|
+
if hasattr(socket, "SO_REUSEPORT"):
|
|
719
|
+
sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEPORT, 1)
|
|
720
|
+
# Bind to multicast group address on Linux; INADDR_ANY on Windows
|
|
721
|
+
if sys.platform == "win32":
|
|
722
|
+
sock.bind(("", port))
|
|
723
|
+
else:
|
|
724
|
+
sock.bind((mcast_ip, port))
|
|
725
|
+
# Join multicast group on the specific interface
|
|
726
|
+
mreq = socket.inet_aton(mcast_ip) + socket.inet_aton(str(iface.address))
|
|
727
|
+
sock.setsockopt(socket.IPPROTO_IP, socket.IP_ADD_MEMBERSHIP, mreq)
|
|
728
|
+
_logger.info("Multicast socket for subject %d on %s (%s:%d)", subject_id, iface.address, mcast_ip, port)
|
|
729
|
+
return sock
|
|
730
|
+
|
|
731
|
+
# -- Public accessors for internal classes --
|
|
732
|
+
|
|
733
|
+
@property
|
|
734
|
+
def closed(self) -> bool:
|
|
735
|
+
return self._closed
|
|
736
|
+
|
|
737
|
+
@property
|
|
738
|
+
def uid(self) -> int:
|
|
739
|
+
assert self._uid is not None
|
|
740
|
+
return self._uid
|
|
741
|
+
|
|
742
|
+
@property
|
|
743
|
+
def interfaces(self) -> list[Interface]:
|
|
744
|
+
return self._interfaces
|
|
745
|
+
|
|
746
|
+
@property
|
|
747
|
+
def tx_socks(self) -> list[socket.socket]:
|
|
748
|
+
return self._tx_socks
|
|
749
|
+
|
|
750
|
+
def __repr__(self) -> str:
|
|
751
|
+
addrs = ", ".join(str(i.address) for i in self._interfaces)
|
|
752
|
+
return f"UDPTransport(uid=0x{self._uid:016x}, interfaces=[{addrs}], modulus={self._subject_id_modulus_val})"
|
|
753
|
+
|
|
754
|
+
def remove_subject_listener(self, subject_id: int, handler: Callable[[TransportArrival], None]) -> None:
|
|
755
|
+
"""
|
|
756
|
+
Remove the handler for a subject; clean up sockets/tasks if none remains. Internal use only.
|
|
757
|
+
"""
|
|
758
|
+
if self._subject_handlers.get(subject_id) is not handler:
|
|
759
|
+
return
|
|
760
|
+
self._subject_handlers.pop(subject_id, None)
|
|
761
|
+
self._reassemblers.pop(subject_id, None)
|
|
762
|
+
for i in range(len(self._interfaces)):
|
|
763
|
+
key = (subject_id, i)
|
|
764
|
+
task = self._mcast_rx_tasks.pop(key, None)
|
|
765
|
+
if task is not None:
|
|
766
|
+
task.cancel()
|
|
767
|
+
sock = self._mcast_socks.pop(key, None)
|
|
768
|
+
if sock is not None:
|
|
769
|
+
sock.close()
|
|
770
|
+
|
|
771
|
+
def remove_subject_writer(self, subject_id: int, writer: _UDPSubjectWriter) -> None:
|
|
772
|
+
if self._subject_writers.get(subject_id) is writer:
|
|
773
|
+
self._subject_writers.pop(subject_id, None)
|
|
774
|
+
|
|
775
|
+
# -- Async sendto helper --
|
|
776
|
+
|
|
777
|
+
async def async_sendto(self, sock: socket.socket, data: bytes, addr: tuple[str, int], deadline: Instant) -> None:
|
|
778
|
+
"""Send a UDP datagram, suspending until writable or deadline exceeded."""
|
|
779
|
+
remaining_ns = deadline.ns - Instant.now().ns
|
|
780
|
+
if remaining_ns <= 0:
|
|
781
|
+
raise SendError("Deadline exceeded")
|
|
782
|
+
try:
|
|
783
|
+
await asyncio.wait_for(self._loop.sock_sendto(sock, data, addr), timeout=remaining_ns * 1e-9)
|
|
784
|
+
except asyncio.TimeoutError:
|
|
785
|
+
raise SendError("Deadline exceeded waiting for socket writability")
|
|
786
|
+
|
|
787
|
+
# -- Transport ABC --
|
|
788
|
+
|
|
789
|
+
@property
|
|
790
|
+
def subject_id_modulus(self) -> int:
|
|
791
|
+
return self._subject_id_modulus_val
|
|
792
|
+
|
|
793
|
+
def subject_listen(self, subject_id: int, handler: Callable[[TransportArrival], None]) -> Closable:
|
|
794
|
+
if subject_id in self._subject_handlers:
|
|
795
|
+
raise ValueError(f"Subject {subject_id} already has an active listener")
|
|
796
|
+
_logger.info("Subscribing to subject %d", subject_id)
|
|
797
|
+
self._subject_handlers[subject_id] = handler
|
|
798
|
+
for i, iface in enumerate(self._interfaces):
|
|
799
|
+
key = (subject_id, i)
|
|
800
|
+
sock = self._create_mcast_socket(subject_id, iface)
|
|
801
|
+
self._mcast_socks[key] = sock
|
|
802
|
+
task = self._loop.create_task(self._mcast_rx_loop(sock, subject_id, i))
|
|
803
|
+
self._mcast_rx_tasks[key] = task
|
|
804
|
+
return _UDPSubjectListener(self, subject_id, handler)
|
|
805
|
+
|
|
806
|
+
def subject_advertise(self, subject_id: int) -> SubjectWriter:
|
|
807
|
+
if subject_id in self._subject_writers:
|
|
808
|
+
raise ValueError(f"Subject {subject_id} already has an active writer")
|
|
809
|
+
_logger.info("Advertising subject %d", subject_id)
|
|
810
|
+
writer = _UDPSubjectWriter(self, subject_id)
|
|
811
|
+
self._subject_writers[subject_id] = writer
|
|
812
|
+
return writer
|
|
813
|
+
|
|
814
|
+
def unicast_listen(self, handler: Callable[[TransportArrival], None]) -> None:
|
|
815
|
+
self._unicast_handler = handler
|
|
816
|
+
_logger.info("Unicast listener set")
|
|
817
|
+
|
|
818
|
+
async def unicast(self, deadline: Instant, priority: Priority, remote_id: int, message: bytes | memoryview) -> None:
|
|
819
|
+
if self._closed:
|
|
820
|
+
raise ClosedError("Transport closed")
|
|
821
|
+
transfer_id = self._next_unicast_transfer_id & TRANSFER_ID_MASK
|
|
822
|
+
self._next_unicast_transfer_id += 1
|
|
823
|
+
_logger.debug("Unicast tx start rid=%016x tid=%d bytes=%d", remote_id, transfer_id, len(message))
|
|
824
|
+
|
|
825
|
+
errors: list[Exception] = []
|
|
826
|
+
success_count = 0
|
|
827
|
+
for i, iface in enumerate(self._interfaces):
|
|
828
|
+
ep = self._remote_endpoints.get((remote_id, i))
|
|
829
|
+
if ep is None:
|
|
830
|
+
_logger.debug("Unicast tx skip rid=%016x iface=%d reason=no-endpoint", remote_id, i)
|
|
831
|
+
continue
|
|
832
|
+
frames = _segment_transfer(priority, transfer_id, self._uid, message, iface.mtu_cyphal)
|
|
833
|
+
try:
|
|
834
|
+
for frame in frames:
|
|
835
|
+
await self.async_sendto(self._tx_socks[i], frame, ep, deadline)
|
|
836
|
+
success_count += 1
|
|
837
|
+
except (OSError, SendError) as e:
|
|
838
|
+
errors.append(e)
|
|
839
|
+
|
|
840
|
+
if success_count == 0:
|
|
841
|
+
if errors:
|
|
842
|
+
raise SendError("Unicast failed on all interfaces") from errors[0]
|
|
843
|
+
_logger.warning("No endpoint known for remote_id=0x%016x", remote_id)
|
|
844
|
+
raise SendError("No endpoint known for remote_id")
|
|
845
|
+
if errors:
|
|
846
|
+
raise ExceptionGroup("unicast send failed on some interfaces", errors)
|
|
847
|
+
_logger.debug("Unicast sent %d frames to remote_id=0x%016x", len(frames), remote_id)
|
|
848
|
+
|
|
849
|
+
def close(self) -> None:
|
|
850
|
+
if self._closed:
|
|
851
|
+
return
|
|
852
|
+
self._closed = True
|
|
853
|
+
_logger.info("Closing UDPTransport uid=0x%016x", self._uid)
|
|
854
|
+
for task in self._unicast_rx_tasks:
|
|
855
|
+
task.cancel()
|
|
856
|
+
self._unicast_rx_tasks.clear()
|
|
857
|
+
for task in self._mcast_rx_tasks.values():
|
|
858
|
+
task.cancel()
|
|
859
|
+
self._mcast_rx_tasks.clear()
|
|
860
|
+
for sock in self._tx_socks:
|
|
861
|
+
sock.close()
|
|
862
|
+
for sock in self._mcast_socks.values():
|
|
863
|
+
sock.close()
|
|
864
|
+
self._mcast_socks.clear()
|
|
865
|
+
self._tx_socks.clear()
|
|
866
|
+
self._subject_handlers.clear()
|
|
867
|
+
self._subject_writers.clear()
|
|
868
|
+
self._reassemblers.clear()
|
|
869
|
+
|
|
870
|
+
# -- Internal async RX loops --
|
|
871
|
+
|
|
872
|
+
async def _mcast_rx_loop(self, sock: socket.socket, subject_id: int, iface_idx: int) -> None:
|
|
873
|
+
"""Async receive loop for a multicast socket. Runs until cancelled or transport is closed."""
|
|
874
|
+
try:
|
|
875
|
+
while not self._closed:
|
|
876
|
+
try:
|
|
877
|
+
data, addr = await self._loop.sock_recvfrom(sock, 65536)
|
|
878
|
+
except OSError:
|
|
879
|
+
if self._closed:
|
|
880
|
+
break
|
|
881
|
+
_logger.debug("Multicast recv error on subject %d iface %d", subject_id, iface_idx)
|
|
882
|
+
await asyncio.sleep(0.1)
|
|
883
|
+
continue
|
|
884
|
+
src_ip, src_port = addr[0], addr[1]
|
|
885
|
+
if (src_ip, src_port) in self._self_endpoints:
|
|
886
|
+
_logger.debug("Multicast drop self sid=%d iface=%d", subject_id, iface_idx)
|
|
887
|
+
continue # Self-send filter
|
|
888
|
+
self._process_subject_datagram(data, src_ip, src_port, subject_id, iface_idx, Instant.now())
|
|
889
|
+
except asyncio.CancelledError:
|
|
890
|
+
_logger.debug("Multicast rx cancelled sid=%d iface=%d", subject_id, iface_idx)
|
|
891
|
+
|
|
892
|
+
async def _unicast_rx_loop(self, sock: socket.socket, iface_idx: int) -> None:
|
|
893
|
+
"""Async receive loop for a unicast socket. Runs until cancelled or transport is closed."""
|
|
894
|
+
try:
|
|
895
|
+
while not self._closed:
|
|
896
|
+
try:
|
|
897
|
+
data, addr = await self._loop.sock_recvfrom(sock, 65536)
|
|
898
|
+
except OSError:
|
|
899
|
+
if self._closed:
|
|
900
|
+
break
|
|
901
|
+
_logger.debug("Unicast recv error on iface %d", iface_idx)
|
|
902
|
+
await asyncio.sleep(0.1)
|
|
903
|
+
continue
|
|
904
|
+
src_ip, src_port = addr[0], addr[1]
|
|
905
|
+
self._process_unicast_datagram(data, src_ip, src_port, iface_idx, Instant.now())
|
|
906
|
+
except asyncio.CancelledError:
|
|
907
|
+
_logger.debug("Unicast rx cancelled iface=%d", iface_idx)
|
|
908
|
+
|
|
909
|
+
def _learn_remote_endpoint(self, remote_id: int, iface_idx: int, src_ip: str, src_port: int) -> None:
|
|
910
|
+
existing = self._remote_endpoints.get((remote_id, iface_idx))
|
|
911
|
+
self._remote_endpoints[(remote_id, iface_idx)] = (src_ip, src_port)
|
|
912
|
+
if existing != (src_ip, src_port):
|
|
913
|
+
_logger.info("Remote endpoint rid=%016x iface=%d ep=%s:%d", remote_id, iface_idx, src_ip, src_port)
|
|
914
|
+
|
|
915
|
+
def _process_unicast_datagram(
|
|
916
|
+
self, data: bytes, src_ip: str, src_port: int, iface_idx: int, timestamp: Instant | None = None
|
|
917
|
+
) -> None:
|
|
918
|
+
try:
|
|
919
|
+
if len(data) < HEADER_SIZE:
|
|
920
|
+
# Malformed wire inputs are dropped in-place to keep the receive path exception-free.
|
|
921
|
+
_logger.debug("Unicast rx drop short iface=%d len=%d", iface_idx, len(data))
|
|
922
|
+
return
|
|
923
|
+
header = _header_deserialize(data[:HEADER_SIZE])
|
|
924
|
+
if header is None:
|
|
925
|
+
_logger.debug("Unicast rx drop bad-header iface=%d len=%d", iface_idx, len(data))
|
|
926
|
+
return
|
|
927
|
+
payload_chunk = data[HEADER_SIZE:]
|
|
928
|
+
if not _frame_is_valid(header, payload_chunk):
|
|
929
|
+
_logger.debug("Unicast rx drop bad-frame iface=%d rid=%016x", iface_idx, header.sender_uid)
|
|
930
|
+
return
|
|
931
|
+
timestamp = Instant.now() if timestamp is None else timestamp
|
|
932
|
+
self._learn_remote_endpoint(header.sender_uid, iface_idx, src_ip, src_port)
|
|
933
|
+
# Keep a local fault boundary here so future wire-triggered bugs still degrade to drop+debug.
|
|
934
|
+
result = self._unicast_reassembler.accept(
|
|
935
|
+
header, payload_chunk, timestamp_ns=timestamp.ns, frame_validated=True
|
|
936
|
+
)
|
|
937
|
+
arrival = None
|
|
938
|
+
if result is not None:
|
|
939
|
+
arrival = TransportArrival(
|
|
940
|
+
timestamp=Instant(ns=result.timestamp_ns),
|
|
941
|
+
priority=Priority(result.priority),
|
|
942
|
+
remote_id=result.sender_uid,
|
|
943
|
+
message=result.payload,
|
|
944
|
+
)
|
|
945
|
+
except Exception as ex:
|
|
946
|
+
_logger.debug("Unicast rx fault iface=%d %s", iface_idx, ex, exc_info=True)
|
|
947
|
+
return
|
|
948
|
+
if arrival is not None and self._unicast_handler is not None:
|
|
949
|
+
_logger.debug("Unicast transfer complete from sender_uid=0x%016x", arrival.remote_id)
|
|
950
|
+
self._unicast_handler(arrival)
|
|
951
|
+
|
|
952
|
+
def _process_subject_datagram(
|
|
953
|
+
self,
|
|
954
|
+
data: bytes,
|
|
955
|
+
src_ip: str,
|
|
956
|
+
src_port: int,
|
|
957
|
+
subject_id: int,
|
|
958
|
+
iface_idx: int,
|
|
959
|
+
timestamp: Instant | None = None,
|
|
960
|
+
) -> None:
|
|
961
|
+
try:
|
|
962
|
+
if len(data) < HEADER_SIZE:
|
|
963
|
+
# Malformed wire inputs are dropped in-place to keep the receive path exception-free.
|
|
964
|
+
_logger.debug("Subject rx drop short sid=%d iface=%d len=%d", subject_id, iface_idx, len(data))
|
|
965
|
+
return
|
|
966
|
+
header = _header_deserialize(data[:HEADER_SIZE])
|
|
967
|
+
if header is None:
|
|
968
|
+
_logger.debug("Subject rx drop bad-header sid=%d iface=%d len=%d", subject_id, iface_idx, len(data))
|
|
969
|
+
return
|
|
970
|
+
payload_chunk = data[HEADER_SIZE:]
|
|
971
|
+
if not _frame_is_valid(header, payload_chunk):
|
|
972
|
+
_logger.debug(
|
|
973
|
+
"Subject rx drop bad-frame sid=%d iface=%d rid=%016x", subject_id, iface_idx, header.sender_uid
|
|
974
|
+
)
|
|
975
|
+
return
|
|
976
|
+
timestamp = Instant.now() if timestamp is None else timestamp
|
|
977
|
+
self._learn_remote_endpoint(header.sender_uid, iface_idx, src_ip, src_port)
|
|
978
|
+
reassembler = self._reassemblers.get(subject_id)
|
|
979
|
+
if reassembler is None:
|
|
980
|
+
reassembler = _RxReassembler()
|
|
981
|
+
self._reassemblers[subject_id] = reassembler
|
|
982
|
+
_logger.debug("Subject reasm create sid=%d", subject_id)
|
|
983
|
+
# Keep a local fault boundary here so future wire-triggered bugs still degrade to drop+debug.
|
|
984
|
+
result = reassembler.accept(header, payload_chunk, timestamp_ns=timestamp.ns, frame_validated=True)
|
|
985
|
+
handler = self._subject_handlers.get(subject_id)
|
|
986
|
+
arrival = None
|
|
987
|
+
if result is not None:
|
|
988
|
+
arrival = TransportArrival(
|
|
989
|
+
timestamp=Instant(ns=result.timestamp_ns),
|
|
990
|
+
priority=Priority(result.priority),
|
|
991
|
+
remote_id=result.sender_uid,
|
|
992
|
+
message=result.payload,
|
|
993
|
+
)
|
|
994
|
+
except Exception as ex:
|
|
995
|
+
_logger.debug("Subject rx fault sid=%d iface=%d %s", subject_id, iface_idx, ex, exc_info=True)
|
|
996
|
+
return
|
|
997
|
+
if arrival is not None:
|
|
998
|
+
_logger.debug("Subject %d transfer complete from sender_uid=0x%016x", subject_id, arrival.remote_id)
|
|
999
|
+
if handler is not None:
|
|
1000
|
+
handler(arrival)
|