FPv2: log all present ECU addresses

release/files_common

@@ -101,6 +101,7 @@ selfdrive/car/interfaces.py
 selfdrive/car/vin.py
 selfdrive/car/disable_ecu.py
 selfdrive/car/fw_versions.py
+selfdrive/car/ecu_addrs.py
 selfdrive/car/isotp_parallel_query.py
 selfdrive/car/tests/__init__.py
 selfdrive/car/tests/test_car_interfaces.py

selfdrive/car/car_helpers.py

@@ -8,7 +8,7 @@
 from selfdrive.car.interfaces import get_interface_attr
 from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
 from selfdrive.car.vin import get_vin, VIN_UNKNOWN
-from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car
+from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car, get_present_ecus
 from system.swaglog import cloudlog
 import cereal.messaging as messaging
 from selfdrive.car import gen_empty_fingerprint
@@ -79,6 +79,7 @@ def _get_interface_names() -> Dict[str, List[str]]:
 def fingerprint(logcan, sendcan):
   fixed_fingerprint = os.environ.get('FINGERPRINT', "")
   skip_fw_query = os.environ.get('SKIP_FW_QUERY', False)
+  ecu_responses = set()
 
   if not fixed_fingerprint and not skip_fw_query:
     # Vin query only reliably works thorugh OBDII
@@ -97,6 +98,7 @@ def fingerprint(logcan, sendcan):
     else:
       cloudlog.warning("Getting VIN & FW versions")
       _, vin = get_vin(logcan, sendcan, bus)
+      ecu_responses = get_present_ecus(logcan, sendcan)
       car_fw = get_fw_versions(logcan, sendcan)
 
     exact_fw_match, fw_candidates = match_fw_to_car(car_fw)
@@ -163,8 +165,8 @@ def fingerprint(logcan, sendcan):
     car_fingerprint = fixed_fingerprint
     source = car.CarParams.FingerprintSource.fixed
 
-  cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint,
-                 source=source, fuzzy=not exact_match, fw_count=len(car_fw))
+  cloudlog.event("fingerprinted", car_fingerprint=car_fingerprint, source=source, fuzzy=not exact_match,
+                 fw_count=len(car_fw), ecu_responses=ecu_responses, error=True)
   return car_fingerprint, finger, vin, car_fw, source, exact_match
 
 

selfdrive/car/ecu_addrs.py

@@ -0,0 +1,90 @@
+#!/usr/bin/env python3
+import capnp
+import time
+import traceback
+from typing import Optional, Set, Tuple
+
+import cereal.messaging as messaging
+from panda.python.uds import SERVICE_TYPE
+from selfdrive.car import make_can_msg
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from system.swaglog import cloudlog
+
+
+def make_tester_present_msg(addr, bus, subaddr=None):
+  dat = [0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0]
+  if subaddr is not None:
+    dat.insert(0, subaddr)
+
+  dat.extend([0x0] * (8 - len(dat)))
+  return make_can_msg(addr, bytes(dat), bus)
+
+
+def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader, subaddr: Optional[int] = None) -> bool:
+  # ISO-TP messages are always padded to 8 bytes
+  # tester present response is always a single frame
+  dat_offset = 1 if subaddr is not None else 0
+  if len(msg.dat) == 8 and 1 <= msg.dat[dat_offset] <= 7:
+    # success response
+    if msg.dat[dat_offset + 1] == (SERVICE_TYPE.TESTER_PRESENT + 0x40):
+      return True
+    # error response
+    if msg.dat[dat_offset + 1] == 0x7F and msg.dat[dat_offset + 2] == SERVICE_TYPE.TESTER_PRESENT:
+      return True
+  return False
+
+
+def get_all_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, bus: int, timeout: float = 1, debug: bool = True) -> Set[Tuple[int, Optional[int], int]]:
+  addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)]
+  queries: Set[Tuple[int, Optional[int], int]] = {(addr, None, bus) for addr in addr_list}
+  responses = queries
+  return get_ecu_addrs(logcan, sendcan, queries, responses, timeout=timeout, debug=debug)
+
+
+def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, queries: Set[Tuple[int, Optional[int], int]],
+                  responses: Set[Tuple[int, Optional[int], int]], timeout: float = 1, debug: bool = False) -> Set[Tuple[int, Optional[int], int]]:
+  ecu_responses: Set[Tuple[int, Optional[int], int]] = set()  # set((addr, subaddr, bus),)
+  try:
+    msgs = [make_tester_present_msg(addr, bus, subaddr) for addr, subaddr, bus in queries]
+
+    messaging.drain_sock_raw(logcan)
+    sendcan.send(can_list_to_can_capnp(msgs, msgtype='sendcan'))
+    start_time = time.monotonic()
+    while time.monotonic() - start_time < timeout:
+      can_packets = messaging.drain_sock(logcan, wait_for_one=True)
+      for packet in can_packets:
+        for msg in packet.can:
+          subaddr = None if (msg.address, None, msg.src) in responses else msg.dat[0]
+          if (msg.address, subaddr, msg.src) in responses and is_tester_present_response(msg, subaddr):
+            if debug:
+              print(f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}")
+              if (msg.address, subaddr, msg.src) in ecu_responses:
+                print(f"Duplicate ECU address: {hex(msg.address)}")
+            ecu_responses.add((msg.address, subaddr, msg.src))
+  except Exception:
+    cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}")
+  return ecu_responses
+
+
+if __name__ == "__main__":
+  import argparse
+
+  parser = argparse.ArgumentParser(description='Get addresses of all ECUs')
+  parser.add_argument('--debug', action='store_true')
+  args = parser.parse_args()
+
+  logcan = messaging.sub_sock('can')
+  sendcan = messaging.pub_sock('sendcan')
+
+  time.sleep(1.0)
+
+  print("Getting ECU addresses ...")
+  ecu_addrs = get_all_ecu_addrs(logcan, sendcan, 1, debug=args.debug)
+
+  print()
+  print("Found ECUs on addresses:")
+  for addr, subaddr, bus in ecu_addrs:
+    msg = f"  0x{hex(addr)}"
+    if subaddr is not None:
+      msg += f" (sub-address: 0x{hex(subaddr)})"
+    print(msg)

selfdrive/car/fw_versions.py

@@ -3,18 +3,20 @@
 import traceback
 from collections import defaultdict
 from dataclasses import dataclass, field
-from typing import Any, List
+from typing import Any, List, Optional, Set, Tuple
 from tqdm import tqdm
 
 import panda.python.uds as uds
 from cereal import car
+from selfdrive.car.ecu_addrs import get_ecu_addrs
 from selfdrive.car.interfaces import get_interface_attr
 from selfdrive.car.fingerprints import FW_VERSIONS
 from selfdrive.car.isotp_parallel_query import IsoTpParallelQuery
 from selfdrive.car.toyota.values import CAR as TOYOTA
 from system.swaglog import cloudlog
 
 Ecu = car.CarParams.Ecu
+ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
 
 
 def p16(val):
@@ -261,7 +263,6 @@ def match_fw_to_car_exact(fw_versions_dict):
       ecu_type = ecu[0]
       addr = ecu[1:]
       found_version = fw_versions_dict.get(addr, None)
-      ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
       if ecu_type == Ecu.esp and candidate in (TOYOTA.RAV4, TOYOTA.COROLLA, TOYOTA.HIGHLANDER, TOYOTA.SIENNA, TOYOTA.LEXUS_IS) and found_version is None:
         continue
 
@@ -299,11 +300,46 @@ def match_fw_to_car(fw_versions, allow_fuzzy=True):
   return exact_match, matches
 
 
+def get_present_ecus(logcan, sendcan):
+  queries = list()
+  parallel_queries = list()
+  responses = set()
+  versions = get_interface_attr('FW_VERSIONS', ignore_none=True)
+
+  for r in REQUESTS:
+    if r.brand not in versions:
+      continue
+
+    for brand_versions in versions[r.brand].values():
+      for ecu_type, addr, sub_addr in brand_versions:
+        # Only query ecus in whitelist if whitelist is not empty
+        if len(r.whitelist_ecus) == 0 or ecu_type in r.whitelist_ecus:
+          a = (addr, sub_addr, r.bus)
+          # Build set of queries
+          if sub_addr is None:
+            if a not in parallel_queries:
+              parallel_queries.append(a)
+          else:  # subaddresses must be queried one by one
+            if [a] not in queries:
+              queries.append([a])
+
+          # Build set of expected responses to filter
+          response_addr = uds.get_rx_addr_for_tx_addr(addr, r.rx_offset)
+          responses.add((response_addr, sub_addr, r.bus))
+
+  queries.insert(0, parallel_queries)
+
+  ecu_responses: Set[Tuple[int, Optional[int], int]] = set()
+  for query in queries:
+    ecu_responses.update(get_ecu_addrs(logcan, sendcan, set(query), responses, timeout=0.1))
+  return ecu_responses
+
+
 def get_fw_versions(logcan, sendcan, extra=None, timeout=0.1, debug=False, progress=False):
   ecu_types = {}
 
   # Extract ECU addresses to query from fingerprints
-  # ECUs using a subadress need be queried one by one, the rest can be done in parallel
+  # ECUs using a subaddress need be queried one by one, the rest can be done in parallel
   addrs = []
   parallel_addrs = []