templates/python/setprotocol/can.py

"""Classic-CAN segmentation for canonical SETProtocol v2 frames."""

from __future__ import annotations

from dataclasses import dataclass

from .core import (
    CRC_SIZE,
    FRAME_MAX,
    HEADER_SIZE,
    SOF,
    FrameFlag,
    SetProtocolError,
)

CAN_ID_MASK = 0x1FFFFFFF
CAN_ID_PREFIX = 0x12
CAN_ID_PREFIX_MASK = 0x1F000000
PCI_FIRST = 0x10
PCI_CONSECUTIVE = 0x20
PCI_FLOW_CONTROL = 0x30
PCI_TYPE_MASK = 0xF0
PCI_VALUE_MASK = 0x0F
FIRST_DATA_SIZE = 5
CONSECUTIVE_DATA_SIZE = 7
REASSEMBLY_TIMEOUT_MS = 500


@dataclass(frozen=True)
class CanAddress:
    destination: int
    source: int
    priority: int = 0
    channel: int = 0

    def __post_init__(self) -> None:
        if not 0 <= self.destination <= 0xFF:
            raise SetProtocolError("CAN destination вне диапазона u8")
        if not 0 <= self.source <= 0xFF:
            raise SetProtocolError("CAN source вне диапазона u8")
        if self.priority not in (0, 1):
            raise SetProtocolError("CAN priority должен быть 0 или 1")
        if not 0 <= self.channel <= 0x7F:
            raise SetProtocolError("CAN channel вне диапазона 0..127")

    def pack(self) -> int:
        return (
            (CAN_ID_PREFIX << 24)
            | (self.destination << 16)
            | (self.source << 8)
            | (self.priority << 7)
            | self.channel
        )

    @classmethod
    def unpack(cls, can_id: int) -> "CanAddress":
        if not 0 <= can_id <= CAN_ID_MASK:
            raise SetProtocolError("CAN ID вне 29-битного диапазона")
        if can_id & CAN_ID_PREFIX_MASK != CAN_ID_PREFIX << 24:
            raise SetProtocolError("CAN ID не принадлежит SETProtocol v2")
        return cls(
            destination=(can_id >> 16) & 0xFF,
            source=(can_id >> 8) & 0xFF,
            priority=(can_id >> 7) & 1,
            channel=can_id & 0x7F,
        )


@dataclass(frozen=True)
class CanFrame:
    can_id: int
    data: bytes

    def __post_init__(self) -> None:
        CanAddress.unpack(self.can_id)
        if not 1 <= len(self.data) <= 8:
            raise SetProtocolError("classic CAN data должен содержать 1..8 байт")


def segment(packet: bytes, address: CanAddress) -> list[CanFrame]:
    """Split one complete encoded SETP frame into classic-CAN frames."""
    if not HEADER_SIZE + CRC_SIZE <= len(packet) <= FRAME_MAX:
        raise SetProtocolError("неверная длина SETP-пакета для CAN")
    can_id = address.pack()
    result = [
        CanFrame(
            can_id,
            bytes((PCI_FIRST,))
            + len(packet).to_bytes(2, "little")
            + packet[:FIRST_DATA_SIZE],
        )
    ]
    sequence = 1
    for offset in range(FIRST_DATA_SIZE, len(packet), CONSECUTIVE_DATA_SIZE):
        chunk = packet[offset : offset + CONSECUTIVE_DATA_SIZE]
        result.append(CanFrame(can_id, bytes((PCI_CONSECUTIVE | sequence,)) + chunk))
        sequence = (sequence + 1) & PCI_VALUE_MASK
    return result


class CanReassembler:
    """Reassemble one SETP packet from one active source/channel."""

    def __init__(self, timeout_ms: int = REASSEMBLY_TIMEOUT_MS) -> None:
        if timeout_ms <= 0:
            raise ValueError("timeout_ms должен быть положительным")
        self.timeout_ms = timeout_ms
        self.reset()

    def reset(self) -> None:
        self._buffer = bytearray()
        self._expected_length = 0
        self._can_id: int | None = None
        self._deadline_ms = 0
        self._next_sequence = 1

    def feed(self, frame: CanFrame, now_ms: int) -> bytes | None:
        if self._can_id is not None and now_ms >= self._deadline_ms:
            self.reset()
            raise SetProtocolError("таймаут сборки SETProtocol CAN")
        pci = frame.data[0] & PCI_TYPE_MASK
        if pci == PCI_FIRST:
            if len(frame.data) != 8:
                raise SetProtocolError("первый CAN-сегмент должен иметь DLC 8")
            total = int.from_bytes(frame.data[1:3], "little")
            if not HEADER_SIZE + CRC_SIZE <= total <= FRAME_MAX:
                raise SetProtocolError("неверная полная длина SETP CAN")
            self._buffer = bytearray(frame.data[3:])
            self._expected_length = total
            self._can_id = frame.can_id
            self._deadline_ms = now_ms + self.timeout_ms
            self._next_sequence = 1
            return None
        if pci == PCI_CONSECUTIVE:
            sequence = frame.data[0] & PCI_VALUE_MASK
            if (
                self._can_id is None
                or frame.can_id != self._can_id
                or sequence != self._next_sequence
                or len(frame.data) < 2
            ):
                self.reset()
                raise SetProtocolError("нарушена последовательность SETP CAN")
            remaining = self._expected_length - len(self._buffer)
            chunk = frame.data[1:]
            if len(chunk) > remaining:
                self.reset()
                raise SetProtocolError("CAN-сегмент длиннее остатка SETP-пакета")
            self._buffer.extend(chunk)
            self._next_sequence = (self._next_sequence + 1) & PCI_VALUE_MASK
            self._deadline_ms = now_ms + self.timeout_ms
            if len(self._buffer) != self._expected_length:
                return None
            packet = bytes(self._buffer)
            can_id = self._can_id
            self.reset()
            assert can_id is not None
            address = CanAddress.unpack(can_id)
            if packet[:2] != SOF:
                raise SetProtocolError("SETP CAN packet не содержит SOF")
            source = int.from_bytes(packet[6:8], "little")
            destination = int.from_bytes(packet[8:10], "little")
            priority = int(bool(packet[3] & int(FrameFlag.PRIORITY)))
            if (
                source != address.source
                or destination != address.destination
                or priority != address.priority
            ):
                raise SetProtocolError("SETP header не совпадает с CAN ID")
            return packet
        if pci == PCI_FLOW_CONTROL:
            return None
        raise SetProtocolError("неизвестный тип SETP CAN-сегмента")