IsoTpParallelQuery: process all functional responses (#25939)

* stash

* process all functional addrs (stash)

* clean up

* rm

* simplify

* let user pass in tx for rx addrs

* revert panda

* simplify

* comment

order

* need to go by rx_addr now

* Revert "need to go by rx_addr now"

This reverts commit 1197ecfbc5b9e5df20b523a0623f644cd8cae1ef.

* stash

* should also work

* this seems pretty clean

* not used

* properly use

* comment

* some fixes

* some fixes

* send consecutive frames on physical addrs

* bump panda

* looks better

* setup_only

* Revert VIN changes

* rev

* bump panda to master

* Update selfdrive/car/isotp_parallel_query.py
old-commit-hash: bea960675f2ae7d2d9e3c95d8c79fde6b61d93d0

selfdrive/car/isotp_parallel_query.py

@@ -9,24 +9,21 @@ from panda.python.uds import CanClient, IsoTpMessage, FUNCTIONAL_ADDRS, get_rx_a
 
 
 class IsoTpParallelQuery:
-  def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addr=False, debug=False, response_pending_timeout=10):
+  def __init__(self, sendcan, logcan, bus, addrs, request, response, response_offset=0x8, functional_addrs=None, debug=False, response_pending_timeout=10):
     self.sendcan = sendcan
     self.logcan = logcan
     self.bus = bus
     self.request = request
     self.response = response
+    self.functional_addrs = functional_addrs or []
     self.debug = debug
-    self.functional_addr = functional_addr
     self.response_pending_timeout = response_pending_timeout
 
-    self.real_addrs = []
-    for a in addrs:
-      if isinstance(a, tuple):
-        self.real_addrs.append(a)
-      else:
-        self.real_addrs.append((a, None))
+    real_addrs = [a if isinstance(a, tuple) else (a, None) for a in addrs]
+    for tx_addr, _ in real_addrs:
+      assert tx_addr not in FUNCTIONAL_ADDRS, f"Functional address should be defined in functional_addrs: {hex(tx_addr)}"
 
-    self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in self.real_addrs}
+    self.msg_addrs = {tx_addr: get_rx_addr_for_tx_addr(tx_addr[0], rx_offset=response_offset) for tx_addr in real_addrs}
     self.msg_buffer = defaultdict(list)
 
   def rx(self):
@@ -35,13 +32,8 @@ class IsoTpParallelQuery:
 
     for packet in can_packets:
       for msg in packet.can:
-        if msg.src == self.bus:
-          if self.functional_addr:
-            if (0x7E8 <= msg.address <= 0x7EF) or (0x18DAF100 <= msg.address <= 0x18DAF1FF):
-              fn_addr = next(a for a in FUNCTIONAL_ADDRS if msg.address - a <= 32)
-              self.msg_buffer[fn_addr].append((msg.address, msg.busTime, msg.dat, msg.src))
-          elif msg.address in self.msg_addrs.values():
-            self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src))
+        if msg.src == self.bus and msg.address in self.msg_addrs.values():
+          self.msg_buffer[msg.address].append((msg.address, msg.busTime, msg.dat, msg.src))
 
   def _can_tx(self, tx_addr, dat, bus):
     """Helper function to send single message"""
@@ -71,6 +63,13 @@ class IsoTpParallelQuery:
     messaging.drain_sock(self.logcan)
     self.msg_buffer = defaultdict(list)
 
+  def _create_isotp_msg(self, tx_addr, sub_addr, rx_addr):
+    can_client = CanClient(self._can_tx, partial(self._can_rx, rx_addr, sub_addr=sub_addr), tx_addr, rx_addr,
+                           self.bus, sub_addr=sub_addr, debug=self.debug)
+
+    max_len = 8 if sub_addr is None else 7
+    return IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug)
+
   def get_data(self, timeout, total_timeout=60.):
     self._drain_rx()
 
@@ -79,22 +78,19 @@ class IsoTpParallelQuery:
     request_counter = {}
     request_done = {}
     for tx_addr, rx_addr in self.msg_addrs.items():
-      # rx_addr not set when using functional tx addr
-      id_addr = rx_addr or tx_addr[0]
-      sub_addr = tx_addr[1]
-
-      can_client = CanClient(self._can_tx, partial(self._can_rx, id_addr, sub_addr=sub_addr), tx_addr[0], rx_addr,
-                             self.bus, sub_addr=sub_addr, debug=self.debug)
-
-      max_len = 8 if sub_addr is None else 7
-
-      msg = IsoTpMessage(can_client, timeout=0, max_len=max_len, debug=self.debug)
-      msg.send(self.request[0])
-
-      msgs[tx_addr] = msg
+      msgs[tx_addr] = self._create_isotp_msg(*tx_addr, rx_addr)
       request_counter[tx_addr] = 0
       request_done[tx_addr] = False
 
+    # Send first request to functional addrs, subsequent responses are handled on physical addrs
+    if len(self.functional_addrs):
+      for addr in self.functional_addrs:
+        self._create_isotp_msg(addr, None, -1).send(self.request[0])
+
+    # If querying functional addrs, set up physical IsoTpMessages to send consecutive frames
+    for msg in msgs.values():
+      msg.send(self.request[0], setup_only=len(self.functional_addrs) > 0)
+
     results = {}
     start_time = time.monotonic()
     response_timeouts = {tx_addr: start_time + timeout for tx_addr in self.msg_addrs}