Добавить порт SETProtocol v2 для TMS320F2812
This commit is contained in:
173
python/setprotocol/can.py
Normal file
173
python/setprotocol/can.py
Normal file
@@ -0,0 +1,173 @@
|
||||
"""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-сегмента")
|
||||
Reference in New Issue
Block a user