radar_system/device_firmware/cart_firmware/tools/bridge.py

#!/usr/bin/env python3
"""Сетевой мост «пульт -> тележка»: один симметричный сервер, роль задаётся
флагом. Два экземпляра переносят управление с STM-передатчика на STM-приёмник
через локальную сеть/интернет.

    [пульт] ADC-> STM(CART-TX) --USB--> bridge --role tx --UDP--> bridge --role rx --USB--> STM(CART-RX) --SBUS-> драйвер

Роль tx: читает кадры CONTROL из USB от STM-TX и шлёт их по UDP на peer.
Роль rx: принимает UDP, пишет CONTROL в USB на STM-RX; при тишине из сети шлёт
         явную нейтраль с флагом failsafe (главный предохранитель — на самом STM).

Устройство выбирается по USB product string (CART-TX / CART-RX) и подтверждается
handshake-кадром IDENT (роль + версия протокола). Для интернета используй общий
ключ (--key) — каждый UDP-пакет аутентифицируется HMAC-SHA256; без ключа трафик
идёт в открытом виде (только для доверенной локальной сети).

Запуск (из каталога прошивки):
    .venv/bin/python tools/bridge.py --role tx --peer 10.0.0.2:5005
    .venv/bin/python tools/bridge.py --role rx --bind 0.0.0.0:5005
"""
import argparse
import hmac
import logging
import socket
import sys
import time
from hashlib import sha256

import serial
from serial.tools import list_ports

import link_proto as lp

HMAC_LEN = 8          # усечённый HMAC-SHA256 на пакет
NET_TIMEOUT_S = 0.10  # нет UDP дольше -> rx начинает слать failsafe
HELLO_TIMEOUT_S = 1.0
SEQ_RESET_AFTER = 20  # столько подряд «несвежих» пакетов = перезапуск источника

log = logging.getLogger("bridge")


# ---------------- выбор и handshake USB-устройства ----------------

def find_port(role: str, selector: str | None) -> str:
    """Найти последовательный порт по роли (product string) или явному
    селектору (путь /dev/..., подстрока product или серийник)."""
    if selector and ("/" in selector or selector.startswith("COM")):
        return selector  # явный путь к устройству
    want_product = {"tx": "CART-TX", "rx": "CART-RX"}[role]
    matches = []
    for p in list_ports.comports():
        hay = " ".join(filter(None, (p.product, p.serial_number, p.description))).upper()
        if selector:
            if selector.upper() in hay:
                matches.append(p)
        elif want_product in hay:
            matches.append(p)
    if not matches:
        raise RuntimeError(f"USB-устройство роли {role} ({want_product}) не найдено")
    if len(matches) > 1:
        names = ", ".join(f"{m.device}({m.product})" for m in matches)
        raise RuntimeError(f"найдено несколько кандидатов роли {role}: {names}; уточни --device")
    return matches[0].device


def handshake(ser: serial.Serial, role: str):
    """Послать HELLO, дождаться IDENT, проверить роль и версию протокола."""
    want_role = {"tx": lp.ROLE_TX, "rx": lp.ROLE_RX}[role]
    parser = lp.FrameParser()
    ser.reset_input_buffer()
    ser.write(lp.build_frame(lp.HELLO, b""))
    deadline = time.monotonic() + HELLO_TIMEOUT_S
    while time.monotonic() < deadline:
        for ftype, payload in parser.feed(ser.read(256)):
            if ftype != lp.IDENT:
                continue
            ident = lp.unpack_ident(payload)
            if ident["proto_ver"] != lp.VERSION:
                raise RuntimeError(
                    f"версия протокола устройства {ident['proto_ver']} != {lp.VERSION}")
            if ident["role"] != want_role:
                raise RuntimeError(
                    f"устройство представилось ролью {lp.role_name(ident['role'])}, ожидалась {role}")
            log.info("handshake OK: роль %s, fw 0x%04x, uid %s",
                     role, ident["fw_ver"], ident["uid"].hex())
            return
    raise RuntimeError("нет ответа IDENT (не та прошивка/порт?)")


def open_device(role: str, selector: str | None) -> serial.Serial:
    port = find_port(role, selector)
    ser = serial.Serial(port, baudrate=115200, timeout=0.05)
    handshake(ser, role)
    return ser


# ---------------- аутентификация пакетов ----------------

def seal(payload: bytes, key: bytes | None) -> bytes:
    if not key:
        return payload
    return payload + hmac.new(key, payload, sha256).digest()[:HMAC_LEN]


def unseal(datagram: bytes, key: bytes | None) -> bytes | None:
    """Проверить HMAC и вернуть payload, либо None если пакет невалиден."""
    if not key:
        return datagram
    if len(datagram) < HMAC_LEN:
        return None
    payload, tag = datagram[:-HMAC_LEN], datagram[-HMAC_LEN:]
    want = hmac.new(key, payload, sha256).digest()[:HMAC_LEN]
    return payload if hmac.compare_digest(tag, want) else None


def seq_newer(a: int, b: int) -> bool:
    """Арифметика последовательных номеров с переполнением uint32."""
    return 0 < ((a - b) & 0xFFFFFFFF) < 0x80000000


# ---------------- роль TX: USB -> сеть ----------------

def run_tx(args, key):
    peer = parse_hostport(args.peer, "peer")
    ser = open_device("tx", args.device)
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    parser = lp.FrameParser()
    log.info("tx: %s -> UDP %s:%d", ser.port, *peer)
    sent = 0
    while True:
        for ftype, payload in parser.feed(ser.read(256)):
            if ftype == lp.CONTROL:
                sock.sendto(seal(payload, key), peer)
                sent += 1
                if sent % 100 == 0:
                    c = lp.unpack_control(payload)
                    log.debug("tx seq=%d ch2=%d ch4=%d", c["seq"], c["ch"][1], c["ch"][3])
            elif ftype == lp.LOG:
                log.info("STM-TX: %s", payload.decode("ascii", "replace"))


# ---------------- роль RX: сеть -> USB ----------------

def run_rx(args, key):
    bind = parse_hostport(args.bind, "bind")
    # для rx --peer — фильтр источника по хосту (порт источника эфемерный)
    allowed_host = args.peer.split(":")[0] if args.peer else None
    ser = open_device("rx", args.device)
    sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    sock.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
    sock.bind(bind)
    sock.settimeout(lp.FRAME_PERIOD_MS / 1000.0)
    parser = lp.FrameParser()
    log.info("rx: UDP %s:%d -> %s", bind[0], bind[1], ser.port)

    last_seq = None
    last_net = time.monotonic()
    last_fs = 0.0
    fs_seq = 0
    in_failsafe = False
    rejects = 0  # подряд отброшенных по seq -> признак перезапуска источника

    while True:
        try:
            datagram, addr = sock.recvfrom(2048)
            if allowed_host and addr[0] != allowed_host:
                log.warning("пакет с неожиданного адреса %s отброшен", addr[0])
                datagram = None
            payload = unseal(datagram, key) if datagram else None
            if payload and len(payload) == lp.CONTROL_LEN:
                c = lp.unpack_control(payload)
                # Принять свежий seq ИЛИ новую сессию: источник (STM-TX) после
                # ребута начинает seq заново с 0, и без сброса last_seq поток
                # завис бы в failsafe навсегда. Новую сессию распознаём по пачке
                # подряд «несвежих» пакетов (устойчиво к одиночным reorder/dup).
                fresh = last_seq is None or seq_newer(c["seq"], last_seq)
                if not fresh:
                    rejects += 1
                    if rejects >= SEQ_RESET_AFTER:
                        log.warning("seq откатился (%d -> %d): новая сессия", last_seq, c["seq"])
                        fresh = True
                if fresh:
                    rejects = 0
                    last_seq = c["seq"]
                    last_net = time.monotonic()
                    if in_failsafe:
                        log.info("связь восстановлена")
                        in_failsafe = False
                    ser.write(lp.build_frame(lp.CONTROL, payload))
            elif datagram is not None:
                log.warning("битый/неаутентичный пакет отброшен")
        except socket.timeout:
            pass

        # сетевой failsafe: тишина -> активная нейтраль на STM в темпе 20 Гц
        now = time.monotonic()
        if now - last_net >= NET_TIMEOUT_S and now - last_fs >= lp.FRAME_PERIOD_MS / 1000.0:
            if not in_failsafe:
                log.warning("потеря сети -> failsafe (нейтраль)")
                in_failsafe = True
                last_seq = None  # рестарт источника при простое примем сразу
            ser.write(lp.build_frame(lp.CONTROL, lp.neutral_control(fs_seq, failsafe=True)))
            fs_seq = (fs_seq + 1) & 0xFFFFFFFF
            last_fs = now

        # дренируем входящие LOG/IDENT от STM-RX
        for ftype, payload in parser.feed(ser.read(256)):
            if ftype == lp.LOG:
                log.info("STM-RX: %s", payload.decode("ascii", "replace"))


# ---------------- общее ----------------

def parse_hostport(s: str, what: str):
    if ":" not in s:
        raise SystemExit(f"--{what} ожидает HOST:PORT, получено {s!r}")
    host, port = s.rsplit(":", 1)
    return (host, int(port))


def main():
    ap = argparse.ArgumentParser(description="мост пульт<->тележка через сеть")
    ap.add_argument("--role", required=True, choices=("tx", "rx"))
    ap.add_argument("--peer", help="tx: куда слать HOST:PORT; rx: разрешённый источник (опц.)")
    ap.add_argument("--bind", default="0.0.0.0:5005", help="rx: локальный UDP HOST:PORT")
    ap.add_argument("--device", help="путь к порту или подстрока product/serial (иначе по роли)")
    ap.add_argument("--key", help="общий ключ для HMAC-аутентификации пакетов (для интернета)")
    ap.add_argument("--log-level", default="INFO")
    args = ap.parse_args()

    logging.basicConfig(level=args.log_level.upper(),
                        format="%(asctime)s %(levelname)s %(message)s")
    key = args.key.encode() if args.key else None
    if args.role == "tx" and not args.peer:
        raise SystemExit("--peer обязателен для роли tx")
    if not key:
        log.warning("без --key: трафик не аутентифицирован, только для доверенной сети")

    try:
        (run_tx if args.role == "tx" else run_rx)(args, key)
    except KeyboardInterrupt:
        log.info("остановлено")


if __name__ == "__main__":
    main()