380 lines
18 KiB
Python
380 lines
18 KiB
Python
#!/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).
|
|
|
|
Обратный канал: rx шлёт tx heartbeat, поэтому на стороне отправителя видно,
|
|
отвечает ли приёмник и не ушёл ли он сам в failsafe.
|
|
|
|
Устройство выбирается по USB product string (CART-TX / CART-RX) и подтверждается
|
|
handshake-кадром IDENT (роль + версия протокола). Для интернета используй общий
|
|
ключ (--key) — каждый UDP-пакет аутентифицируется HMAC-SHA256; без ключа трафик
|
|
идёт в открытом виде (только для доверенной локальной сети).
|
|
|
|
Параметры можно задавать и через env (CART_BRIDGE_ROLE/PEER/BIND/DEVICE/KEY/
|
|
LOG_LEVEL) — так настраиваются systemd-службы (см. deploy/).
|
|
|
|
Запуск (из каталога прошивки):
|
|
.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 os
|
|
import select
|
|
import socket
|
|
import struct
|
|
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.50 # нет UDP дольше -> rx начинает слать failsafe
|
|
HELLO_TIMEOUT_S = 1.0
|
|
SEQ_RESET_AFTER = 20 # столько подряд «несвежих» пакетов = перезапуск источника
|
|
|
|
# Обратный канал «жив ли приёмник»: rx шлёт heartbeat в сторону tx (на адрес
|
|
# источника последнего CONTROL), tx по ним показывает, отвечает ли приёмник и
|
|
# не ушёл ли он сам в failsafe. Это PC-only служебный трафик (к STM отношения не
|
|
# имеет), поэтому живёт здесь, а не в зеркале протокола link_proto.
|
|
HEARTBEAT_MAGIC = b"\xCB\x01"
|
|
HEARTBEAT_PERIOD_S = 0.2 # как часто rx шлёт heartbeat
|
|
HEARTBEAT_TIMEOUT_S = 1.0 # нет heartbeat дольше -> tx считает приёмник недоступным
|
|
HEARTBEAT_STATUS_S = 5.0 # период печати статуса приёмника на tx
|
|
HB_FLAG_FAILSAFE = 0x01
|
|
|
|
log = logging.getLogger("bridge")
|
|
|
|
|
|
def pack_hb(failsafe: bool, last_seq: int) -> bytes:
|
|
flags = HB_FLAG_FAILSAFE if failsafe else 0
|
|
return HEARTBEAT_MAGIC + struct.pack("<BI", flags, last_seq & 0xFFFFFFFF)
|
|
|
|
|
|
def parse_hb(payload: bytes):
|
|
"""-> dict(failsafe, last_seq) либо None, если это не heartbeat."""
|
|
if len(payload) != len(HEARTBEAT_MAGIC) + 5 or payload[:2] != HEARTBEAT_MAGIC:
|
|
return None
|
|
flags, last_seq = struct.unpack_from("<BI", payload, 2)
|
|
return {"failsafe": bool(flags & HB_FLAG_FAILSAFE), "last_seq": last_seq}
|
|
|
|
|
|
_ANSI = {"green": "\033[1;32m", "red": "\033[1;31m", "yellow": "\033[1;33m", "reset": "\033[0m"}
|
|
|
|
|
|
def render_link_status(alive: bool, failsafe: bool, color: bool):
|
|
"""Живая одна строка для оператора: зелёная «СВЯЗЬ ЕСТЬ» / красная «СВЯЗИ
|
|
НЕТ», обновляется на месте (возврат каретки)."""
|
|
if not alive:
|
|
text, c = "СВЯЗИ НЕТ", "red"
|
|
elif failsafe:
|
|
text, c = "СВЯЗЬ ЕСТЬ (приёмник в failsafe)", "yellow"
|
|
else:
|
|
text, c = "СВЯЗЬ ЕСТЬ", "green"
|
|
body = "● " + text
|
|
if color:
|
|
body = _ANSI[c] + body + _ANSI["reset"]
|
|
sys.stdout.write("\r\033[2K" + body) # \033[2K — очистить строку
|
|
sys.stdout.flush()
|
|
|
|
|
|
# ---------------- выбор и 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
|
|
rx_alive = False # получаем ли heartbeat от приёмника
|
|
rx_failsafe = False # сообщил ли приёмник, что сам в failsafe
|
|
last_hb = 0.0
|
|
last_status = 0.0
|
|
# В терминале — живая цветная строка; под systemd (journald, не TTY) — обычные
|
|
# события по изменению состояния.
|
|
tty = sys.stdout.isatty()
|
|
color = tty and os.environ.get("NO_COLOR") is None
|
|
while True:
|
|
# read(1) блокирует до первого байта, затем забираем всё доступное —
|
|
# кадр уходит в сеть сразу, без ожидания таймаута чтения (иначе каждый
|
|
# кадр задерживался бы до таймаута порта).
|
|
data = ser.read(1)
|
|
if ser.in_waiting:
|
|
data += ser.read(ser.in_waiting)
|
|
for ftype, payload in parser.feed(data):
|
|
if ftype == lp.CONTROL:
|
|
sock.sendto(seal(payload, key), peer)
|
|
sent += 1
|
|
elif ftype == lp.LOG:
|
|
log.info("STM-TX: %s", payload.decode("ascii", "replace"))
|
|
|
|
now = time.monotonic()
|
|
prev_alive, prev_fs = rx_alive, rx_failsafe
|
|
# обратный канал: heartbeat от приёмника (приходит на тот же сокет)
|
|
while select.select([sock], [], [], 0)[0]:
|
|
try:
|
|
datagram, _ = sock.recvfrom(2048)
|
|
except BlockingIOError:
|
|
break
|
|
hb = parse_hb(unseal(datagram, key) or b"")
|
|
if hb is None:
|
|
continue
|
|
last_hb = now
|
|
rx_alive = True
|
|
rx_failsafe = hb["failsafe"]
|
|
if rx_alive and now - last_hb > HEARTBEAT_TIMEOUT_S:
|
|
rx_alive, rx_failsafe = False, False
|
|
|
|
if tty:
|
|
render_link_status(rx_alive, rx_failsafe, color) # живая строка
|
|
else:
|
|
if rx_alive != prev_alive:
|
|
(log.info if rx_alive else log.warning)(
|
|
"приёмник отвечает" if rx_alive else "приёмник НЕ отвечает")
|
|
elif rx_alive and rx_failsafe != prev_fs:
|
|
(log.warning if rx_failsafe else log.info)(
|
|
"приёмник в failsafe: наш поток не доходит" if rx_failsafe
|
|
else "приёмник принимает поток")
|
|
if now - last_status >= HEARTBEAT_STATUS_S:
|
|
last_status = now
|
|
log.info("приёмник: %s (отправлено %d)",
|
|
"отвечает" if rx_alive else "НЕ отвечает", sent)
|
|
|
|
|
|
# ---------------- роль 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.setblocking(False)
|
|
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
|
|
last_hb_sent = 0.0
|
|
fs_seq = 0
|
|
in_failsafe = False
|
|
rejects = 0 # подряд отброшенных по seq -> признак перезапуска источника
|
|
tx_addr = None # куда слать heartbeat (источник последнего CONTROL)
|
|
|
|
def accept(datagram, addr):
|
|
"""Проверить пакет и обновить состояние; вернуть payload принятого
|
|
свежего кадра либо None."""
|
|
nonlocal last_seq, last_net, in_failsafe, rejects, tx_addr
|
|
if allowed_host and addr[0] != allowed_host:
|
|
log.warning("пакет с неожиданного адреса %s отброшен", addr[0])
|
|
return None
|
|
payload = unseal(datagram, key)
|
|
if not payload or len(payload) != lp.CONTROL_LEN:
|
|
log.warning("битый/неаутентичный пакет отброшен")
|
|
return None
|
|
tx_addr = addr # аутентичный CONTROL -> знаем обратный адрес для heartbeat
|
|
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 not fresh:
|
|
return None # устаревший/повторный — anti-replay
|
|
rejects = 0
|
|
last_seq = c["seq"]
|
|
last_net = time.monotonic()
|
|
if in_failsafe:
|
|
log.info("связь восстановлена")
|
|
in_failsafe = False
|
|
return payload
|
|
|
|
while True:
|
|
# Ждём пакет до 20 мс (пауза цикла), затем БЕЗ блокировки выгребаем всю
|
|
# накопившуюся очередь сокета и шлём на STM только самый свежий кадр
|
|
# (latest-wins): так задержка не растёт, даже если пакеты пришли пачкой.
|
|
newest = None
|
|
ready = select.select([sock], [], [], lp.FRAME_PERIOD_MS / 2000.0)[0]
|
|
while ready:
|
|
try:
|
|
datagram, addr = sock.recvfrom(2048)
|
|
except BlockingIOError:
|
|
break
|
|
p = accept(datagram, addr)
|
|
if p is not None:
|
|
newest = p
|
|
ready = select.select([sock], [], [], 0)[0]
|
|
if newest is not None:
|
|
ser.write(lp.build_frame(lp.CONTROL, newest))
|
|
|
|
# сетевой 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
|
|
|
|
# обратный heartbeat к tx: «приёмник жив», с признаком собственного failsafe
|
|
if tx_addr is not None and now - last_hb_sent >= HEARTBEAT_PERIOD_S:
|
|
sock.sendto(seal(pack_hb(in_failsafe, last_seq or 0), key), tx_addr)
|
|
last_hb_sent = now
|
|
|
|
# неблокирующий дренаж входящих LOG от STM-RX
|
|
if ser.in_waiting:
|
|
for ftype, payload in parser.feed(ser.read(ser.in_waiting)):
|
|
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():
|
|
# Значения по умолчанию берутся из env (CART_BRIDGE_*), чтобы systemd-юнит
|
|
# мог задавать всё через EnvironmentFile; аргументы CLI их переопределяют.
|
|
env = os.environ.get
|
|
ap = argparse.ArgumentParser(description="мост пульт<->тележка через сеть")
|
|
ap.add_argument("--role", default=env("CART_BRIDGE_ROLE"), choices=("tx", "rx"))
|
|
ap.add_argument("--peer", default=env("CART_BRIDGE_PEER"),
|
|
help="tx: куда слать HOST:PORT; rx: разрешённый источник (опц.)")
|
|
ap.add_argument("--bind", default=env("CART_BRIDGE_BIND", "0.0.0.0:5005"),
|
|
help="rx: локальный UDP HOST:PORT")
|
|
ap.add_argument("--device", default=env("CART_BRIDGE_DEVICE"),
|
|
help="путь к порту или подстрока product/serial (иначе по роли)")
|
|
ap.add_argument("--key", default=env("CART_BRIDGE_KEY"),
|
|
help="общий ключ для HMAC-аутентификации пакетов (для интернета)")
|
|
ap.add_argument("--log-level", default=env("CART_BRIDGE_LOG_LEVEL", "INFO"))
|
|
args = ap.parse_args()
|
|
|
|
logging.basicConfig(level=args.log_level.upper(),
|
|
format="%(asctime)s %(levelname)s %(message)s")
|
|
if args.role not in ("tx", "rx"):
|
|
raise SystemExit("нужна роль: --role tx|rx (или CART_BRIDGE_ROLE)")
|
|
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("остановлено")
|
|
finally:
|
|
if sys.stdout.isatty():
|
|
sys.stdout.write("\n") # увести живую строку от приглашения shell
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|