BMW_E8x_EPS/adapters/slcan_adapter.py

"""Generic SLCAN (Lawicel) driver — for the CAN-IO board's USB bridge mode.

Unlike the CANdapter (see candapter.py), this targets a real, unmodified
SLCAN implementation: 't'/'T' frame prefixes, no extra un-counted bytes
after the data field. Written for firmware/src/usb_bridge.cpp on the
CAN-IO board, but works with any standard SLCAN device.

On top of plain SLCAN, the CAN-IO board adds a digital-IO channel on the
same serial link that does NOT depend on the CAN bus being alive: '!' lines
report IN/OUT state, '@' lines set outputs. See io_state / set_output /
request_io below, and usb_bridge.h for the firmware side.
"""
from __future__ import annotations

import dataclasses
import time
from typing import Optional

import serial

from .candapter import BITRATE_CODES, CanFrame


@dataclasses.dataclass
class IoState:
    inputs: int          # bit0 = IN1 ... bit3 = IN4
    outputs: int         # bit0 = OUT1, bit1 = OUT2
    uptime_s: int
    recv_time: float

    def input_on(self, index: int) -> bool:
        return bool(self.inputs & (1 << index))

    def output_on(self, index: int) -> bool:
        return bool(self.outputs & (1 << index))


class SlcanAdapter:
    def __init__(self, port: str, bitrate: int, control_baud: int = 115200):
        if bitrate not in BITRATE_CODES:
            raise ValueError(f"Unsupported bitrate {bitrate}; choose one of {sorted(BITRATE_CODES)}")
        self._ser = serial.Serial(port, control_baud, timeout=0.1)
        self._buf = bytearray()
        self.io_state: Optional[IoState] = None
        self._send_command("C")  # close channel in case it was left open
        self._send_command(f"S{BITRATE_CODES[bitrate]}")
        self._send_command("O")

    def _send_command(self, cmd: str) -> None:
        self._ser.reset_input_buffer()
        self._ser.write((cmd + "\r").encode("ascii"))
        time.sleep(0.05)
        self._ser.read(self._ser.in_waiting or 1)  # drain ACK/BELL/text reply

    def close(self) -> None:
        try:
            self._send_command("C")
        finally:
            self._ser.close()

    def __enter__(self) -> "SlcanAdapter":
        return self

    def __exit__(self, *exc) -> None:
        self.close()

    def send_frame(self, arbitration_id: int, data: bytes, is_extended: bool = False) -> None:
        self._ser.write(self._frame_line(arbitration_id, data, is_extended).encode("ascii"))

    def send_frames(self, frames) -> None:
        """Write several frames in one go - one syscall per batch instead of
        per frame, which is the difference between holding a 10ms cycle and
        not when a few dozen IDs come due together."""
        if not frames:
            return
        blob = "".join(self._frame_line(aid, data, ext) for aid, data, ext in frames)
        self._ser.write(blob.encode("ascii"))

    @staticmethod
    def _frame_line(arbitration_id: int, data: bytes, is_extended: bool = False) -> str:
        if is_extended:
            return f"T{arbitration_id:08X}{len(data):X}{data.hex().upper()}\r"
        return f"t{arbitration_id:03X}{len(data):X}{data.hex().upper()}\r"

    # ---- CAN-independent digital IO (CAN-IO board only) -------------------

    def set_output(self, index: int, on: bool) -> None:
        """Drive OUT<index+1>. Takes effect even with a dead/one-node CAN bus."""
        self._ser.write(f"@S{index}{1 if on else 0}\r".encode("ascii"))

    def toggle_output(self, index: int) -> None:
        self._ser.write(f"@T{index}\r".encode("ascii"))

    def request_io(self) -> None:
        """Ask for an immediate IO report; the reply updates self.io_state
        the next time read_frame() runs."""
        self._ser.write(b"@G\r")

    def read_frame(self, timeout: Optional[float] = None) -> Optional[CanFrame]:
        """Read one frame, or None if nothing complete arrived within timeout.

        Also consumes '!' IO report lines as they go by, updating io_state -
        so callers that poll this in a loop get IO tracking for free.
        """
        deadline = None if timeout is None else time.monotonic() + timeout
        while True:
            if deadline is not None and time.monotonic() >= deadline:
                return None

            nl = self._buf.find(b"\r")
            if nl == -1:
                # Only take what's already buffered: a plain read(64) blocks
                # until 64 bytes arrive or the port's own timeout expires,
                # which would stall a caller that asked for a few ms and
                # wreck the transmit cadence of anything sharing this thread.
                waiting = self._ser.in_waiting
                if waiting:
                    self._buf.extend(self._ser.read(waiting))
                elif deadline is None:
                    self._buf.extend(self._ser.read(1))
                else:
                    time.sleep(0.0005)
                continue

            line = bytes(self._buf[:nl])
            del self._buf[: nl + 1]

            # ACK (0x06) / BELL (0x07) replies carry no terminator of their
            # own, so they end up glued to the front of whatever line comes
            # next - strip them before looking at the prefix.
            line = line.lstrip(b"\x06\x07")

            if not line:
                continue
            if line[0:1] == b"!":
                self._parse_io(line)
                continue
            if chr(line[0]) not in ("t", "T"):
                continue  # ignore stray ACK/BELL bytes mixed into the stream

            frame = self._parse_frame(line)
            if frame is not None:
                return frame

    def _parse_io(self, line: bytes) -> None:
        text = line.decode("ascii", errors="replace")
        try:
            self.io_state = IoState(
                inputs=int(text[1:3], 16),
                outputs=int(text[3:5], 16),
                uptime_s=int(text[5:13], 16),
                recv_time=time.time(),
            )
        except (ValueError, IndexError):
            pass

    @staticmethod
    def _parse_frame(line: bytes) -> Optional[CanFrame]:
        text = line.decode("ascii", errors="replace")
        is_extended = text[0] == "T"
        try:
            if is_extended:
                id_hex, rest = text[1:9], text[9:]
            else:
                id_hex, rest = text[1:4], text[4:]

            dlc = int(rest[0], 16)
            data = bytes.fromhex(rest[1 : 1 + dlc * 2])
            return CanFrame(
                arbitration_id=int(id_hex, 16),
                is_extended=is_extended,
                data=data,
                timestamp_ms=None,
                recv_time=time.time(),
            )
        except (ValueError, IndexError):
            return None