import can import asyncio from dataclasses import dataclass @dataclass(frozen=True) class ReceivedMessage: priority: int protocol: int target_id: int sender_id: int message_counter: int payload: bytes class Subscription: def __init__( self, *, gateway: "Gateway", protocol: int | None, sender_id: int | None, target_id: int | None, queue_size: int, ) -> None: self.gateway = gateway self.protocol = protocol self.sender_id = sender_id self.target_id = target_id self.queue: asyncio.Queue[ReceivedMessage] = asyncio.Queue( maxsize=queue_size, ) self.dropped_messages = 0 def matches(self, message: ReceivedMessage) -> bool: return ( (self.protocol is None or self.protocol == message.protocol) and (self.sender_id is None or self.sender_id == message.sender_id) and (self.target_id is None or self.target_id == message.target_id) ) async def receive(self) -> ReceivedMessage: return await self.queue.get() def close(self) -> None: self.gateway.unsubscribe(self) async def __aenter__(self) -> "Subscription": return self async def __aexit__(self, exc_type, exc_value, traceback) -> None: self.close() class Gateway: def __init__(self, channel: str): self._message_counter = 0 self._bus = can.Bus( interface="socketcan", channel=channel, ignore_config=True, fd=False, receive_own_messages=False, ) self._notifier: can.Notifier | None = None self._subscriptions: set[Subscription] = set() @staticmethod def encode_arbitration_id( *, priority: int = 3, protocol: int, target_id: int, sender_id: int, message_counter: int, packet_index: int, is_first_packet: bool, is_last_packet: bool, ) -> int: if not isinstance(priority, int): raise TypeError("Priority must be an integer") if priority < 0 or priority > 3: raise ValueError("Priority must be between 0 and 3") if not isinstance(target_id, int): raise TypeError("Target ID must be an integer") if target_id < 0 or target_id > 127: raise ValueError("Target ID must be between 0 and 127") if not isinstance(sender_id, int): raise TypeError("Sender ID must be an integer") if sender_id < 1 or sender_id > 127: raise ValueError("Sender ID must be between 1 and 127") if not isinstance(protocol, int): raise TypeError("Protocol must be an integer") if protocol < 0 or protocol > 7: raise ValueError("Protocol must be between 0 and 7") if not isinstance(message_counter, int): raise TypeError("Transfer counter must be an integer") if message_counter < 0 or message_counter > 7: raise ValueError("Transfer counter must be between 0 and 7") if not isinstance(packet_index, int): raise TypeError("Fragment index must be an integer") if packet_index < 0 or packet_index > 31: raise ValueError("Fragment index must be between 0 and 31") if not isinstance(is_first_packet, bool): raise TypeError("is_first_packet must be a bool") if not isinstance(is_last_packet, bool): raise TypeError("is_last_packet must be a bool") if is_first_packet and packet_index != 0: raise ValueError("The first packet must have index 0") return ( (priority << 27) # 2 bits: 28–27 | (protocol << 24) # 3 bits: 26–24 | (target_id << 17) # 7 bits: 23–17 | (sender_id << 10) # 7 bits: 16–10 | (message_counter << 7) # 3 bits: 9–7 | (packet_index << 2) # 5 bits: 6–2 | (int(is_first_packet) << 1) # 1 bit: 1 | int(is_last_packet) # 1 bit: 0 ) async def send_message( self, target_id: int, sender_id: int, protocol: int, payload: bytes, priority: int = 3, ) -> None: if not isinstance(payload, bytes): raise TypeError("Payload must be bytes") if not payload: raise ValueError("Payload must not be empty") packets: list[can.Message] = [] for i in range(0, len(payload), 8): data = payload[i : i + 8] arbitration_id = self.encode_arbitration_id( priority=priority, target_id=target_id, sender_id=sender_id, protocol=protocol, message_counter=self._message_counter, packet_index=(i // 8) % 32, is_first_packet=i == 0, is_last_packet=i + 8 >= len(payload), ) packets.append( can.Message( arbitration_id=arbitration_id, is_extended_id=True, is_fd=False, data=data, check=True, ) ) self._message_counter = (self._message_counter + 1) % 8 for packet in packets: self._bus.send(packet, timeout=1.0) def subscribe( self, *, protocol: int | None = None, sender_id: int | None = None, target_id: int | None = None, queue_size: int = 100, ) -> Subscription: if not isinstance(queue_size, int): raise TypeError("Queue size must be an integer") if queue_size < 1: raise ValueError("Queue size must be at least 1") subscription = Subscription( gateway=self, protocol=protocol, sender_id=sender_id, target_id=target_id, queue_size=queue_size, ) self._subscriptions.add(subscription) return subscription def unsubscribe(self, subscription: Subscription) -> None: self._subscriptions.discard(subscription) def _publish_message(self, message: ReceivedMessage) -> None: for subscription in self._subscriptions: if not subscription.matches(message): continue try: subscription.queue.put_nowait(message) except asyncio.QueueFull: # Explicit policy: drop this new message for this subscriber. subscription.dropped_messages += 1 def shutdown(self) -> None: if self._notifier is not None: self._notifier.stop() self._notifier = None self._bus.shutdown() def start_receiving(self) -> None: if self._notifier is not None: return self._notifier = can.Notifier( self._bus, [self._receive_packet], loop=asyncio.get_running_loop(), ) def _receive_packet(self, packet: can.Message) -> None: if ( not packet.is_extended_id or packet.is_remote_frame or packet.is_error_frame or packet.is_fd ): return arbitration_id = packet.arbitration_id sender_id = (arbitration_id >> 10) & 0x7F if sender_id == 0: return # Reserved: only target ID 0 means broadcast. packet_index = (arbitration_id >> 2) & 0x1F is_first_packet = bool(arbitration_id & 2) is_last_packet = bool(arbitration_id & 1) if is_first_packet and packet_index != 0: return # Log every valid received packet, including individual fragments. print( f"RECV: priority={(arbitration_id >> 27) & 0x03} " f"protocol={(arbitration_id >> 24) & 0x07} " f"target_id={(arbitration_id >> 17) & 0x7F} " f"sender_id={sender_id} " f"message_counter={(arbitration_id >> 7) & 0x07} " f"packet_index={packet_index} " f"is_first_packet={int(is_first_packet)} " f"is_last_packet={int(is_last_packet)} " f"payload=[{bytes(packet.data).hex(' ').upper()}]" ) # For now, handle complete single-packet messages such as heartbeats. # Multi-packet messages need reassembly before publication. if not (is_first_packet and is_last_packet): return self._publish_message(ReceivedMessage( priority=(arbitration_id >> 27) & 0x03, protocol=(arbitration_id >> 24) & 0x07, target_id=(arbitration_id >> 17) & 0x7F, sender_id=sender_id, message_counter=(arbitration_id >> 7) & 0x07, payload=bytes(packet.data), ))