first commit
This commit is contained in:
303
app/can.py
Normal file
303
app/can.py
Normal file
@@ -0,0 +1,303 @@
|
||||
# can.py — SocketCAN OBD-II Responder + Link-Control
|
||||
from __future__ import annotations
|
||||
import logging
|
||||
import threading
|
||||
import time
|
||||
import os
|
||||
import sys
|
||||
import subprocess
|
||||
from typing import Callable, Dict, Optional, List
|
||||
from pyroute2 import IPRoute, NetlinkError
|
||||
import json
|
||||
import can
|
||||
|
||||
OBD_REQ_ID = 0x7DF
|
||||
PID_SPEED = 0x0D
|
||||
PID_RPM = 0x0C
|
||||
|
||||
CAP_NET_ADMIN_BIT = 12 # Linux CAP_NET_ADMIN
|
||||
|
||||
def have_cap_netadmin() -> bool:
|
||||
if os.geteuid() == 0:
|
||||
return True
|
||||
try:
|
||||
with open("/proc/self/status", "r", encoding="utf-8") as f:
|
||||
for line in f:
|
||||
if line.startswith("CapEff:"):
|
||||
mask = int(line.split()[1], 16)
|
||||
return bool(mask & (1 << CAP_NET_ADMIN_BIT))
|
||||
except Exception:
|
||||
pass
|
||||
return False
|
||||
|
||||
def need_caps_message() -> str:
|
||||
exe = os.path.realpath(sys.executable)
|
||||
return (
|
||||
"Keine Berechtigung für 'ip link'.\n\n"
|
||||
"Option A) Als root starten (sudo)\n"
|
||||
"Option B) Capabilities auf das laufende Python setzen:\n"
|
||||
f" sudo setcap cap_net_admin,cap_net_raw=eip \"{exe}\"\n"
|
||||
f" getcap \"{exe}\"\n"
|
||||
)
|
||||
|
||||
def list_can_ifaces() -> List[str]:
|
||||
"""Listet verfügbare CAN/vCAN-Interfaces via `ip -json link`."""
|
||||
try:
|
||||
out = subprocess.check_output(["ip", "-json", "link"], text=True)
|
||||
import json
|
||||
items = json.loads(out)
|
||||
names = [i["ifname"] for i in items if i.get("link_type") in ("can", None) and (i["ifname"].startswith("can") or i["ifname"].startswith("vcan"))]
|
||||
return sorted(set(names))
|
||||
except Exception:
|
||||
# Fallback: /sys/class/net
|
||||
try:
|
||||
import os
|
||||
names = [n for n in os.listdir("/sys/class/net") if n.startswith(("can", "vcan"))]
|
||||
return sorted(set(names))
|
||||
except Exception:
|
||||
return []
|
||||
|
||||
def _link_info(ipr: IPRoute, iface: str):
|
||||
idxs = ipr.link_lookup(ifname=iface)
|
||||
if not idxs:
|
||||
raise RuntimeError(f"Interface '{iface}' nicht gefunden")
|
||||
info = ipr.get_links(idxs[0])[0]
|
||||
kind = "none"
|
||||
for k, v in info.get("attrs", []):
|
||||
if k == "IFLA_LINKINFO":
|
||||
for kk, vv in v.get("attrs", []):
|
||||
if kk == "IFLA_INFO_KIND":
|
||||
kind = str(vv)
|
||||
break
|
||||
return idxs[0], kind, info
|
||||
|
||||
def link_state(iface: str) -> str:
|
||||
try:
|
||||
with IPRoute() as ipr:
|
||||
idx, _, info = _link_info(ipr, iface)
|
||||
st = info.get("state")
|
||||
if isinstance(st, str):
|
||||
return st.upper()
|
||||
for k, v in info.get("attrs", []):
|
||||
if k == "IFLA_OPERSTATE":
|
||||
return str(v).upper()
|
||||
except Exception:
|
||||
pass
|
||||
return "UNKNOWN"
|
||||
|
||||
def link_up(iface: str, bitrate: int = 500000, fd: bool = False, set_params: bool = True) -> None:
|
||||
with IPRoute() as ipr:
|
||||
idx, kind, _ = _link_info(ipr, iface)
|
||||
|
||||
# Wenn bereits UP: nichts tun.
|
||||
try:
|
||||
cur = ipr.get_links(idx)[0]
|
||||
if (cur.get("state") or "").upper() == "UP":
|
||||
return
|
||||
except Exception:
|
||||
pass
|
||||
|
||||
# 1) down (ignoriere "invalid argument" / already down)
|
||||
try:
|
||||
ipr.link("set", index=idx, state="down")
|
||||
except NetlinkError as e:
|
||||
if e.code not in (0, 22):
|
||||
raise RuntimeError(f"Netlink error bei '{iface}' (down): {e.code} {e}") from e
|
||||
|
||||
# 2) optional: Parameter nur bei 'can' – EINVAL ignorieren
|
||||
if set_params and kind == "can":
|
||||
try:
|
||||
ipr.link("set", index=idx, kind="can", data={"bitrate": int(bitrate)})
|
||||
except NetlinkError as e:
|
||||
if e.code != 22: # alles außer EINVAL weiterreichen
|
||||
raise RuntimeError(f"Netlink error bei '{iface}' (bitrate): {e.code} {e}") from e
|
||||
# EINVAL -> Treiber mag Param-Change jetzt nicht -> einfach weitermachen
|
||||
|
||||
# 3) up (hier darf es notfalls knallen)
|
||||
try:
|
||||
ipr.link("set", index=idx, state="up")
|
||||
except NetlinkError as e:
|
||||
raise RuntimeError(f"Netlink error bei '{iface}' (up): {e.code} {e}") from e
|
||||
|
||||
def link_down(iface: str) -> None:
|
||||
with IPRoute() as ipr:
|
||||
idx, _, _ = _link_info(ipr, iface)
|
||||
try:
|
||||
ipr.link("set", index=idx, state="down")
|
||||
except NetlinkError as e:
|
||||
if e.code not in (0, 22): # ignore EINVAL/OK
|
||||
raise RuntimeError(f"Netlink error bei '{iface}' (down): {e.code} {e}") from e
|
||||
|
||||
def link_kind(iface: str) -> str:
|
||||
"""liefert nur das INFO_KIND (z.B. 'can', 'slcan', 'vcan', 'none')"""
|
||||
try:
|
||||
with IPRoute() as ipr:
|
||||
_, kind, _ = _link_info(ipr, iface)
|
||||
return kind
|
||||
except Exception:
|
||||
return "unknown"
|
||||
|
||||
|
||||
class ObdResponder:
|
||||
"""
|
||||
OBD-II Mode-01 PID-Responder über SocketCAN (11-bit).
|
||||
Non-blocking, threadsicher, mit Rebind-Funktion und robustem Reopen,
|
||||
falls das Interface DOWN ist.
|
||||
"""
|
||||
def __init__(
|
||||
self,
|
||||
interface: str,
|
||||
resp_id: int,
|
||||
timeout_ms: int = 200,
|
||||
logger: Optional[logging.Logger] = None,
|
||||
):
|
||||
self.interface = interface
|
||||
self.resp_id = resp_id
|
||||
self.timeout_ms = timeout_ms
|
||||
self.log = logger or logging.getLogger("obdcan")
|
||||
|
||||
# PID-Provider: pid -> callable() -> 8-Byte-Payload
|
||||
self.providers: Dict[int, Callable[[], bytes]] = {}
|
||||
|
||||
# Laufzustand / CAN-Ressourcen
|
||||
self._run = threading.Event()
|
||||
self._run.set()
|
||||
self.bus: Optional[can.BusABC] = None
|
||||
self.reader: Optional[can.BufferedReader] = None
|
||||
self.notifier: Optional[can.Notifier] = None
|
||||
|
||||
# Service-Thread, der bei IF=UP den Bus öffnet und RX abwickelt
|
||||
self._thread = threading.Thread(
|
||||
target=self._service_loop, name="OBD-SVC", daemon=True
|
||||
)
|
||||
self._thread.start()
|
||||
|
||||
# ---------- Lifecycle ----------
|
||||
def _open_bus(self) -> None:
|
||||
# ggf. alte Ressourcen schließen, dann neu öffnen
|
||||
self._close_bus()
|
||||
self._close_bus()
|
||||
self.bus = can.interface.Bus(channel=self.interface, interface="socketcan")
|
||||
self.log.info("OBD responder started on %s (resp_id=0x%03X)", self.interface, self.resp_id)
|
||||
self.log.info(
|
||||
"OBD responder started on %s (resp_id=0x%03X)",
|
||||
self.interface, self.resp_id
|
||||
)
|
||||
|
||||
def _close_bus(self) -> None:
|
||||
try:
|
||||
if self.notifier:
|
||||
self.notifier.stop()
|
||||
except Exception:
|
||||
pass
|
||||
try:
|
||||
if self.bus:
|
||||
self.bus.shutdown()
|
||||
except Exception:
|
||||
pass
|
||||
self.bus = None
|
||||
|
||||
def stop(self) -> None:
|
||||
self._run.clear()
|
||||
try:
|
||||
self._thread.join(timeout=1.0)
|
||||
except RuntimeError:
|
||||
pass
|
||||
self._close_bus()
|
||||
|
||||
def rebind(self, interface: Optional[str] = None, resp_id: Optional[int] = None) -> None:
|
||||
if interface is not None:
|
||||
self.interface = interface
|
||||
if resp_id is not None:
|
||||
self.resp_id = resp_id
|
||||
# Bus schließen; Service-Loop öffnet ihn wieder, sobald IF=UP ist
|
||||
self._close_bus()
|
||||
self.log.info("Rebind requested: %s, resp=0x%03X", self.interface, self.resp_id)
|
||||
|
||||
# ---------- Öffentliche API ----------
|
||||
def register_pid(self, pid: int, provider: Callable[[], bytes]) -> None:
|
||||
self.providers[pid] = provider
|
||||
|
||||
# ---------- Service-Loop (robust gegen 'Network is down') ----------
|
||||
def _service_loop(self) -> None:
|
||||
backoff = 0.5
|
||||
while self._run.is_set():
|
||||
# Bus öffnen, wenn IF up ist
|
||||
if self.bus is None:
|
||||
if link_state(self.interface) == "UP":
|
||||
try:
|
||||
self._open_bus()
|
||||
backoff = 0.5
|
||||
except Exception as e:
|
||||
self.log.warning("Bus open failed: %s", e)
|
||||
time.sleep(backoff)
|
||||
backoff = min(5.0, backoff * 1.7)
|
||||
continue
|
||||
else:
|
||||
time.sleep(0.5)
|
||||
continue
|
||||
|
||||
# RX: eigene Poll‑Schleife statt Notifier
|
||||
try:
|
||||
msg = self.bus.recv(0.05) # blocking short timeout
|
||||
if msg is not None:
|
||||
self._handle(msg)
|
||||
except (can.CanOperationError, OSError):
|
||||
# IF ging down -> Bus schließen und später neu öffnen
|
||||
self.log.info("CAN went DOWN — closing bus, will retry…")
|
||||
self._close_bus()
|
||||
time.sleep(0.5)
|
||||
except Exception as e:
|
||||
self.log.warning("CAN recv error: %s", e)
|
||||
time.sleep(0.1)
|
||||
|
||||
# ---------- Message-Handler ----------
|
||||
def _handle(self, msg: can.Message) -> None:
|
||||
if msg.is_extended_id or msg.arbitration_id != OBD_REQ_ID:
|
||||
return
|
||||
data = bytes(msg.data)
|
||||
if len(data) < 3:
|
||||
return
|
||||
|
||||
# tolerant: 02 01 <pid> ... oder 01 <pid> ...
|
||||
if data[0] == 0x02 and len(data) >= 3:
|
||||
mode, pid = data[1], data[2]
|
||||
else:
|
||||
mode, pid = data[0], (data[1] if len(data) >= 2 else None)
|
||||
|
||||
if mode != 0x01 or pid is None:
|
||||
return
|
||||
|
||||
provider = self.providers.get(pid)
|
||||
if not provider:
|
||||
return
|
||||
|
||||
try:
|
||||
payload = provider()
|
||||
if not isinstance(payload, (bytes, bytearray)) or len(payload) != 8:
|
||||
return
|
||||
out = can.Message(
|
||||
arbitration_id=self.resp_id,
|
||||
is_extended_id=False,
|
||||
data=payload,
|
||||
dlc=8
|
||||
)
|
||||
if self.bus:
|
||||
self.bus.send(out)
|
||||
except can.CanError:
|
||||
self.log.warning("CAN send failed (bus off?)")
|
||||
except Exception as e:
|
||||
self.log.exception("Provider error: %s", e)
|
||||
|
||||
# --- Ende Patch ObdResponder -----------------------------------------------
|
||||
|
||||
# Helfer fürs Formatieren
|
||||
def make_speed_response(speed_kmh: int) -> bytes:
|
||||
A = max(0, min(255, int(speed_kmh)))
|
||||
return bytes([0x03, 0x41, PID_SPEED, A, 0x00, 0x00, 0x00, 0x00])
|
||||
|
||||
def make_rpm_response(rpm: int) -> bytes:
|
||||
raw = max(0, int(rpm)) * 4
|
||||
A = (raw >> 8) & 0xFF
|
||||
B = raw & 0xFF
|
||||
return bytes([0x04, 0x41, PID_RPM, A, B, 0x00, 0x00, 0x00])
|
||||
Reference in New Issue
Block a user