firmware fingerprinting: order brand requests

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/ecu_addrs.py

@@ -0,0 +1,76 @@
+#!/usr/bin/env python3
+import capnp
+import time
+import traceback
+from typing import Dict, Set
+
+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 selfdrive.swaglog import cloudlog
+
+TESTER_PRESENT_DAT = bytes([0x02, SERVICE_TYPE.TESTER_PRESENT, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0])
+
+
+def is_tester_present_response(msg: capnp.lib.capnp._DynamicStructReader) -> bool:
+  # ISO-TP messages are always padded to 8 bytes
+  # tester present response is always a single frame
+  if len(msg.dat) == 8 and 1 <= msg.dat[0] <= 7:
+    # success response
+    if msg.dat[1] == (SERVICE_TYPE.TESTER_PRESENT + 0x40):
+      return True
+    # error response
+    if msg.dat[1] == 0x7F and msg.dat[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[int]:
+  addr_list = [0x700 + i for i in range(256)] + [0x18da00f1 + (i << 8) for i in range(256)]
+  query_addrs = response_addrs = {addr: bus for addr in addr_list}
+  return get_ecu_addrs(logcan, sendcan, query_addrs, response_addrs, timeout=timeout, debug=debug)
+
+
+def get_ecu_addrs(logcan: messaging.SubSocket, sendcan: messaging.PubSocket, query_addrs: Dict[int, int], response_addrs: Dict[int, int], timeout: float = 1, debug: bool = True) -> Set[int]:
+  ecu_addrs = set()
+  try:
+    msgs = [make_can_msg(addr, TESTER_PRESENT_DAT, bus) for addr, bus in query_addrs.items()]
+
+    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:
+          if msg.address in response_addrs and msg.src == response_addrs[msg.address] and is_tester_present_response(msg):
+            if debug:
+              print(f"CAN-RX: {hex(msg.address)} - 0x{bytes.hex(msg.dat)}")
+              if msg.address in ecu_addrs:
+                print(f"Duplicate ECU address: {hex(msg.address)}")
+            ecu_addrs.add(msg.address)
+  except Exception:
+    cloudlog.warning(f"ECU addr scan exception: {traceback.format_exc()}")
+  return ecu_addrs
+
+
+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 in ecu_addrs:
+    print(f"  {hex(addr)}")

selfdrive/car/fw_versions.py

@@ -8,13 +8,15 @@
 
 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 selfdrive.swaglog import cloudlog
 
 Ecu = car.CarParams.Ecu
+ESSENTIAL_ECUS = [Ecu.engine, Ecu.eps, Ecu.esp, Ecu.fwdRadar, Ecu.fwdCamera, Ecu.vsa]
 
 
 def p16(val):
@@ -259,7 +261,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
 
@@ -296,20 +297,57 @@ def match_fw_to_car(fw_versions, allow_fuzzy=True):
 
   return exact_match, matches
 
+def get_brand_candidates(logcan, sendcan, versions):
+  response_offset_bus_by_brand = dict()
+  for r in REQUESTS:
+    # TODO: some brands have multiple response offsets
+    response_offset_bus_by_brand[r.brand] = (r.rx_offset, r.bus)
+
+  # maps query and response addresses to their request's bus
+  query_addrs_by_brand = defaultdict(dict)
+  response_addrs_by_brand = defaultdict(dict)
+  for brand, brand_versions in versions.items():
+    if brand not in response_offset_bus_by_brand:
+      continue
+
+    for c in brand_versions.values():
+      for ecu, addr, _ in c.keys():
+        # reduce matches with ecus not critical to fingerprinting
+        if ecu not in ESSENTIAL_ECUS:
+          continue
+
+        response_offset, bus = response_offset_bus_by_brand[brand]
+        query_addrs_by_brand[brand][addr] = bus
+        response_addrs_by_brand[brand][addr + response_offset] = bus
+
+  query_addrs = dict(addr for addr_bus in query_addrs_by_brand.values() for addr in addr_bus.items())
+  response_addrs = dict(addr for addr_bus in response_addrs_by_brand.values() for addr in addr_bus.items())
+  ecu_response_addrs = get_ecu_addrs(logcan, sendcan, query_addrs, response_addrs)
+  cloudlog.event("ecu response addrs", ecu_response_addrs=ecu_response_addrs)
+
+  brand_candidates = list()
+  for brand, brand_addrs in response_addrs_by_brand.items():
+    cloudlog.event("candidate brand intersection", brand=brand, intersection=set(brand_addrs).intersection(ecu_response_addrs))
+    if len(set(brand_addrs).intersection(ecu_response_addrs)) >= 4:
+      brand_candidates.append(brand)
+  return brand_candidates
 
 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 = []
 
   versions = get_interface_attr('FW_VERSIONS', ignore_none=True)
   if extra is not None:
     versions.update(extra)
 
+  brand_candidates = get_brand_candidates(logcan, sendcan, versions)
   for brand, brand_versions in versions.items():
+    if brand_candidates and brand not in brand_candidates:
+      continue
     for c in brand_versions.values():
       for ecu_type, addr, sub_addr in c.keys():
         a = (brand, addr, sub_addr)