fix(sensing): UDP relay for multi-node node_id firmware clobber

pyproject.toml

@@ -282,7 +282,7 @@ addopts = [
     "--cov-branch",
     "-v",
 ]
-testpaths = ["tests"]
+testpaths = ["tests", "scripts"]
 python_files = ["test_*.py", "*_test.py"]
 python_classes = ["Test*", "Describe*", "When*"]
 python_functions = ["test_*", "it_*", "should_*"]

scripts/csi_node_id_relay.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+"""
+UDP relay that rewrites byte 4 (node_id) of CSI/vitals packets based on source IP.
+
+Workaround for firmware bug (issues #374/#386): the WiFi stack clobbers
+g_nvs_config.node_id between main and csi_collector_init, so all CSI packets ship
+with node_id=1 regardless of NVS. We patch byte 4 here using a source-IP map.
+
+Listens on 0.0.0.0:5005 (where ESP32s send) and forwards rewritten packets to
+127.0.0.1:<dest_port> (where sensing-server listens with --udp-port).
+
+Usage: csi_node_id_relay.py [--listen-port 5005] [--dest-port 5099]
+"""
+import argparse
+import socket
+import sys
+
+# Source IP -> node_id mapping. Update as new ESPs join.
+IP_TO_NODE_ID = {
+    "192.168.13.222": 1,   # ESP32-S3 #1, MAC 10:b4:1d:ea:eb:a0
+    "192.168.3.246":  2,   # ESP32-S3 #2, MAC b8:f8:62:f9:d5:58
+}
+
+# Magics whose byte 4 is node_id (LE). 0xC5110001 CSI, 0xC5110002 vitals,
+# 0xC5110004 wasm-output (ADR-040). All three put node_id at offset 4.
+NODE_ID_MAGICS = {
+    bytes.fromhex("010011c5"),
+    bytes.fromhex("020011c5"),
+    bytes.fromhex("040011c5"),
+}
+
+def rewrite_packet(data: bytes, src_ip: str,
+                   ip_map: dict | None = None,
+                   magics: set | None = None) -> bytes:
+    """Return packet with byte[4] (node_id) corrected for src_ip, or data unchanged."""
+    if ip_map is None:
+        ip_map = IP_TO_NODE_ID
+    if magics is None:
+        magics = NODE_ID_MAGICS
+    nid = ip_map.get(src_ip)
+    if not (isinstance(nid, int) and 0 <= nid <= 255):
+        return data
+    if len(data) >= 5 and data[:4] in magics and data[4] != nid:
+        return bytes([data[0], data[1], data[2], data[3], nid]) + data[5:]
+    return data
+
+
+def main():
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--listen-port", type=int, default=5005)
+    ap.add_argument("--dest-port",   type=int, default=5099)
+    ap.add_argument("--dest-host",   default="127.0.0.1")
+    args = ap.parse_args()
+
+    rx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    rx.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+    rx.bind(("0.0.0.0", args.listen_port))
+    tx = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+    dest = (args.dest_host, args.dest_port)
+
+    print(f"relay: 0.0.0.0:{args.listen_port} -> {args.dest_host}:{args.dest_port}", flush=True)
+    print(f"map:   {IP_TO_NODE_ID}", flush=True)
+
+    counts = {}
+    rewrites = 0
+    total_forwarded = 0
+    while True:
+        try:
+            data, addr = rx.recvfrom(4096)
+        except KeyboardInterrupt:
+            break
+        src_ip = addr[0]
+        rewritten = rewrite_packet(data, src_ip)
+        if rewritten is not data:
+            data = rewritten
+            rewrites += 1
+        tx.sendto(data, dest)
+        counts[src_ip] = counts.get(src_ip, 0) + 1
+        total_forwarded += 1
+        if total_forwarded % 200 == 0:
+            print(f"forwarded: {counts}  rewrites={rewrites}", flush=True)
+
+if __name__ == "__main__":
+    main()

scripts/test_csi_node_id_relay.py

@@ -0,0 +1,103 @@
+"""
+Tests for csi_node_id_relay.py — verifies node_id rewrite logic.
+
+Covers fix for issues #374 / #386:
+  All ESP32 nodes ship node_id=1 due to firmware clobber bug.
+  Relay must rewrite byte[4] to the correct node_id based on source IP.
+"""
+import sys
+import os
+sys.path.insert(0, os.path.dirname(__file__))
+
+from csi_node_id_relay import rewrite_packet, IP_TO_NODE_ID, NODE_ID_MAGICS
+
+# CSI magic (0xC5110001 LE)
+CSI_MAGIC = bytes.fromhex("010011c5")
+# Vitals magic (0xC5110002 LE)
+VITALS_MAGIC = bytes.fromhex("020011c5")
+
+IP_NODE1 = "192.168.13.222"
+IP_NODE2 = "192.168.3.246"
+IP_UNKNOWN = "10.0.0.99"
+
+
+def make_packet(magic: bytes, node_id: int, payload_len: int = 20) -> bytes:
+    return magic + bytes([node_id]) + bytes(payload_len)
+
+
+# ── node_id rewrite ───────────────────────────────────────────────────────────
+
+def test_node1_ip_rewrites_to_1_when_clobbered():
+    pkt = make_packet(CSI_MAGIC, node_id=1, payload_len=20)  # already correct
+    result = rewrite_packet(pkt, IP_NODE1)
+    assert result[4] == 1
+    assert result is pkt  # no copy when already correct
+
+def test_node2_ip_rewrites_from_1_to_2():
+    # firmware clobber: node 2 sends node_id=1
+    pkt = make_packet(CSI_MAGIC, node_id=1, payload_len=20)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result[4] == 2, f"expected node_id=2, got {result[4]}"
+
+def test_node2_vitals_rewritten_too():
+    pkt = make_packet(VITALS_MAGIC, node_id=1, payload_len=10)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result[4] == 2
+
+def test_payload_unchanged_after_rewrite():
+    payload = bytes(range(20))
+    pkt = CSI_MAGIC + bytes([1]) + payload
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result[5:] == payload, "payload must not be modified"
+
+def test_magic_preserved_after_rewrite():
+    pkt = make_packet(CSI_MAGIC, node_id=1)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result[:4] == CSI_MAGIC
+
+# ── passthrough cases ─────────────────────────────────────────────────────────
+
+def test_unknown_ip_passes_through_unchanged():
+    pkt = make_packet(CSI_MAGIC, node_id=1)
+    result = rewrite_packet(pkt, IP_UNKNOWN)
+    assert result is pkt  # identity — not copied
+
+def test_wrong_magic_passes_through_unchanged():
+    bad_magic = bytes([0xDE, 0xAD, 0xBE, 0xEF])
+    pkt = bad_magic + bytes([1]) + bytes(10)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result is pkt
+
+def test_packet_too_short_passes_through_unchanged():
+    pkt = CSI_MAGIC  # only 4 bytes, no node_id byte
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result is pkt
+
+def test_already_correct_node_id_not_copied():
+    # node 2 already has node_id=2 (won't happen in practice but must not corrupt)
+    pkt = make_packet(CSI_MAGIC, node_id=2)
+    result = rewrite_packet(pkt, IP_NODE2)
+    assert result is pkt  # no copy needed
+
+# ── all three magics are covered ──────────────────────────────────────────────
+
+def test_all_three_magics_are_rewritten():
+    wasm_magic = bytes.fromhex("040011c5")  # ADR-040: 0xC5110004
+    for magic in [CSI_MAGIC, VITALS_MAGIC, wasm_magic]:
+        pkt = magic + bytes([1]) + bytes(10)
+        result = rewrite_packet(pkt, IP_NODE2)
+        assert result[4] == 2, f"magic {magic.hex()} not rewritten"
+
+# ── ip map contents ───────────────────────────────────────────────────────────
+
+def test_both_nodes_in_default_map():
+    assert IP_NODE1 in IP_TO_NODE_ID
+    assert IP_NODE2 in IP_TO_NODE_ID
+    assert IP_TO_NODE_ID[IP_NODE1] == 1
+    assert IP_TO_NODE_ID[IP_NODE2] == 2
+
+def test_custom_ip_map():
+    custom_map = {"10.0.0.1": 3}
+    pkt = make_packet(CSI_MAGIC, node_id=1)
+    result = rewrite_packet(pkt, "10.0.0.1", ip_map=custom_map)
+    assert result[4] == 3