diff --git a/release/files_common b/release/files_common index 352514acb49b55..c5be569c156ec3 100644 --- a/release/files_common +++ b/release/files_common @@ -105,6 +105,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 diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py index 7863f0d882318c..d8e6e26b7bef9d 100644 --- a/selfdrive/car/car_helpers.py +++ b/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 selfdrive.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 diff --git a/selfdrive/car/ecu_addrs.py b/selfdrive/car/ecu_addrs.py new file mode 100755 index 00000000000000..267701509a6926 --- /dev/null +++ b/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) diff --git a/selfdrive/car/fw_versions.py b/selfdrive/car/fw_versions.py index 4f649481c6263a..4549fae86923f1 100755 --- a/selfdrive/car/fw_versions.py +++ b/selfdrive/car/fw_versions.py @@ -3,11 +3,12 @@ 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 @@ -15,6 +16,7 @@ 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): @@ -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 = []