NODEDC_MISSION_CORE/plugins/vesc/runtime/serial.py

"""Exclusive serial ownership and OS attachment generation checks."""

import fcntl
import hashlib
import os
from pathlib import Path
import re
import select
import stat
import termios
import time
from dataclasses import dataclass

from .protocol import Decoder, request, test_packet, current_packet, speed_packet, hall_packet


def device_id(value):
    return "vesc_" + hashlib.sha256(value.encode()).hexdigest()[:32]


@dataclass(frozen=True)
class Attachment:
    usb: str
    address: str
    tty: str
    speed: str

    @property
    def binding(self):
        return self.usb + ":" + self.address

    @property
    def id(self):
        return device_id("provisional:" + self.binding)


def attachment_at(path):
    """Read one physical USB generation; never walk sibling devices or drivers."""
    if not re.fullmatch(r"[0-9]+-[0-9]+(?:\.[0-9]+)*", path.name): return None
    try:
        if ((path / "idVendor").read_text().strip() != "0483"
                or (path / "idProduct").read_text().strip() != "5740"
                or (path / "product").read_text().strip() != "ChibiOS/RT Virtual COM Port"):
            return None
        address = (path / "devnum").read_text().strip()
        tty = [p.name for p in path.glob(path.name + ":*/tty/ttyACM*")
               if re.fullmatch(r"ttyACM[0-9]+", p.name)]
        speed = (path / "speed").read_text().strip() + " Мбит/с"
        if len(tty) != 1 or address != (path / "devnum").read_text().strip(): return None
        return Attachment(path.name, address, tty[0], speed)
    except (OSError, ValueError): return None


def check_attachment(attachment, root=Path("/sys/bus/usb/devices")):
    if (not re.fullmatch(r"[0-9]+-[0-9]+(?:\.[0-9]+)*", attachment.usb)
            or attachment_at(root / attachment.usb) != attachment):
        raise OSError("USB attachment changed")


def discover(root=Path("/sys/bus/usb/devices")):
    found = [attachment_at(path) for path in sorted(root.iterdir())]
    return [item for item in found if item is not None][:128]


class Link:
    def __init__(self, attachment):
        self.attachment = attachment
        self.fd = -1
        self.decoder = Decoder()
        self.hall_result = None
        self.hall_pending = False
        self.check()
        fd = os.open("/dev/" + attachment.tty, os.O_RDWR | os.O_NOCTTY | os.O_NONBLOCK | os.O_NOFOLLOW)
        try:
            info = os.fstat(fd)
            if not stat.S_ISCHR(info.st_mode) or os.major(info.st_rdev) != 166:
                raise ValueError("Not a CDC ACM device")
            fcntl.flock(fd, fcntl.LOCK_EX | fcntl.LOCK_NB)
            fcntl.ioctl(fd, termios.TIOCEXCL)
            settings = termios.tcgetattr(fd)
            settings[0] = settings[1] = settings[3] = 0
            settings[2] = termios.CLOCAL | termios.CREAD | termios.CS8
            settings[4] = settings[5] = termios.B115200
            settings[6][termios.VMIN] = settings[6][termios.VTIME] = 0
            termios.tcsetattr(fd, termios.TCSANOW, settings)
            self.check()
            self.fd = fd
        except BaseException:
            os.close(fd)
            raise

    def check(self):
        check_attachment(self.attachment)

    def close(self):
        if self.fd >= 0:
            os.close(self.fd)
            self.fd = -1

    def query(self, command, timeout=2):
        return self._exchange(request(command), command, timeout)

    def _exchange(self, payload, command, timeout):
        self.check()
        if not self.hall_pending:
            self.decoder = Decoder()
            termios.tcflush(self.fd, termios.TCIFLUSH)
        deadline = time.monotonic() + min(timeout, 8 if command == 62 else 2)
        sent = 0
        while sent < len(payload):
            if not select.select([], [self.fd], [], max(0, deadline - time.monotonic()))[1]:
                raise TimeoutError("Serial write timeout")
            sent += os.write(self.fd, payload[sent:])
        total = 0
        while time.monotonic() < deadline:
            if not select.select([self.fd], [], [], max(0, deadline - time.monotonic()))[0]:
                break
            raw = os.read(self.fd, 4096)
            if not raw:
                raise OSError("Serial device disconnected")
            total += len(raw)
            if total > 32768:
                raise ValueError("Unexpected serial traffic")
            answer = None
            for packet in self.decoder.feed(raw):
                if self.hall_pending and packet[0] == 28:
                    self.hall_result = packet
                if packet[0] == command:
                    answer = packet
            if answer is not None:
                self.check()
                return answer
        raise TimeoutError("Controller did not reply")

    def test_command(self, action):
        self._test_write(test_packet(action))

    def test_current(self, current_a):
        self._test_write(current_packet(current_a))

    def test_speed(self, erpm):
        self._test_write(speed_packet(erpm))

    def set_temporary_limits(self, config):
        from .temporary_limits import packet
        if self._exchange(packet(config), 48, 2) != bytes([48]):
            raise ValueError("Invalid volatile limits ACK")

    def detect_hall(self):
        if self.hall_pending: raise ValueError("Hall detection already started")
        self.decoder = Decoder()
        self.hall_result = None
        self.hall_pending = True
        self._test_write(hall_packet())

    def _test_write(self, payload):
        self.check()
        deadline = time.monotonic() + 0.04
        sent = 0
        while sent < len(payload):
            if not select.select([], [self.fd], [], max(0, deadline - time.monotonic()))[1]:
                raise TimeoutError("Test command write timeout")
            sent += os.write(self.fd, payload[sent:])