diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 00000000000..598ac699f2f --- /dev/null +++ b/.gitattributes @@ -0,0 +1 @@ +board/debug/can_replay_data.bin binary diff --git a/board/debug/can_replay.h b/board/debug/can_replay.h new file mode 100644 index 00000000000..eb187b1b77c --- /dev/null +++ b/board/debug/can_replay.h @@ -0,0 +1,98 @@ +#pragma once + +#include "board/debug/can_replay_data.h" + +#define CAN_REPLAY_INTERVAL_US 10000U + +bool can_replay_enabled = false; +uint32_t can_replay_event_idx = 0U; +uint32_t can_replay_msg_idx = 0U; +uint32_t can_replay_data_pos = CAN_REPLAY_EVENT_COUNT; +uint32_t can_replay_next_frame_ts = 0U; + +static void can_replay_reset(void) { + can_replay_event_idx = 0U; + can_replay_msg_idx = 0U; + can_replay_data_pos = CAN_REPLAY_EVENT_COUNT; + can_replay_next_frame_ts = microsecond_timer_get(); +} + +void can_replay_set_enabled(bool enabled) { + can_replay_enabled = enabled; + can_replay_reset(); + + if (enabled) { + can_clear(&can_rx_q); + set_power_save_state(false); + } +} + +uint32_t can_replay_status(uint8_t *resp) { + resp[0] = can_replay_enabled ? 1U : 0U; + WORD_TO_BYTE_ARRAY(&resp[1], can_replay_event_idx); + WORD_TO_BYTE_ARRAY(&resp[5], CAN_REPLAY_EVENT_COUNT); + WORD_TO_BYTE_ARRAY(&resp[9], CAN_REPLAY_MSG_COUNT); + return 13U; +} + +static void can_replay_push_msg(void) { + uint8_t dict_idx = can_replay_data[can_replay_data_pos]; + can_replay_data_pos += 1U; + + uint16_t address = can_replay_dict_addr[dict_idx]; + uint8_t bus_len = can_replay_dict_bus_len[dict_idx]; + uint8_t len = bus_len & 0xFU; + uint8_t bus = (bus_len >> 4U) & 0x7U; + + CANPacket_t to_push = {0}; + to_push.fd = 0U; + to_push.returned = 0U; + to_push.rejected = 0U; + to_push.extended = 0U; + to_push.addr = address; + to_push.bus = bus; + to_push.data_len_code = len; + (void)memcpy(to_push.data, &can_replay_data[can_replay_data_pos], len); + can_replay_data_pos += len; + can_set_checksum(&to_push); + + if (bus < PANDA_CAN_CNT) { + uint8_t can_number = CAN_NUM_FROM_BUS_NUM(bus); + can_health[can_number].total_rx_cnt += 1U; + } + + safety_rx_invalid += safety_rx_hook(&to_push) ? 0U : 1U; + ignition_can_hook(&to_push); + led_set(LED_BLUE, true); + rx_buffer_overflow += can_push(&can_rx_q, &to_push) ? 0U : 1U; +} + +static void can_replay_push_frame(void) { + uint8_t msg_count = can_replay_data[can_replay_event_idx]; + + for (uint8_t i = 0U; i < msg_count; i++) { + can_replay_push_msg(); + can_replay_msg_idx += 1U; + } + + can_replay_event_idx += 1U; + if (can_replay_event_idx >= CAN_REPLAY_EVENT_COUNT) { + can_replay_event_idx = 0U; + can_replay_msg_idx = 0U; + can_replay_data_pos = CAN_REPLAY_EVENT_COUNT; + } +} + +void can_replay_tick(void) { + COMPILE_TIME_ASSERT(sizeof(can_replay_data) == CAN_REPLAY_DATA_SIZE); + + if (!can_replay_enabled) can_replay_set_enabled(true); + if (can_replay_enabled) { + uint32_t now = microsecond_timer_get(); + while ((int32_t)(now - can_replay_next_frame_ts) >= 0) { + can_replay_push_frame(); + can_replay_next_frame_ts += CAN_REPLAY_INTERVAL_US; + now = microsecond_timer_get(); + } + } +} diff --git a/board/debug/can_replay_data.bin b/board/debug/can_replay_data.bin new file mode 100644 index 00000000000..ac3c070a40c Binary files /dev/null and b/board/debug/can_replay_data.bin differ diff --git a/board/debug/can_replay_data.h b/board/debug/can_replay_data.h new file mode 100644 index 00000000000..2b2b86e48bc --- /dev/null +++ b/board/debug/can_replay_data.h @@ -0,0 +1,56 @@ +// Generated by gen_can_replay_data.py; do not edit by hand. +// Route: 77611a1fac303767/2020-03-24--09-50-38/3 +// Car fingerprint: TOYOTA_COROLLA_TSS2 +// First 300 CAN events include all bus <= 2 traffic; later events keep parser-used traffic. + +#pragma once + +#define CAN_REPLAY_EVENT_COUNT 6000U +#define CAN_REPLAY_MSG_COUNT 70430U +#define CAN_REPLAY_DATA_SIZE 634691U +#define CAN_REPLAY_MAX_MSGS_PER_EVENT 51U +#define CAN_REPLAY_DICT_COUNT 166U + +static const uint16_t can_replay_dict_addr[CAN_REPLAY_DICT_COUNT] = { + 0x0220U, 0x0025U, 0x0199U, 0x0232U, 0x019aU, 0x032cU, 0x0024U, 0x0260U, 0x019bU, 0x019cU, 0x019dU, 0x019eU, + 0x02e4U, 0x019fU, 0x03bbU, 0x01a0U, 0x01a1U, 0x01a2U, 0x01a3U, 0x01b4U, 0x01b5U, 0x01b6U, 0x01b7U, 0x01b8U, + 0x01b9U, 0x01baU, 0x01bbU, 0x00aaU, 0x03d0U, 0x0343U, 0x00b4U, 0x0228U, 0x03d1U, 0x03d2U, 0x0226U, 0x0610U, + 0x03d3U, 0x0622U, 0x0361U, 0x0191U, 0x01c4U, 0x01d3U, 0x02c1U, 0x00baU, 0x01aaU, 0x02e7U, 0x0128U, 0x0160U, + 0x0161U, 0x01d0U, 0x036dU, 0x0489U, 0x0262U, 0x048aU, 0x06efU, 0x0283U, 0x01d2U, 0x0320U, 0x032aU, 0x02e6U, + 0x0072U, 0x0399U, 0x0180U, 0x0181U, 0x0182U, 0x0183U, 0x0344U, 0x0184U, 0x0185U, 0x0186U, 0x0187U, 0x0188U, + 0x0189U, 0x0240U, 0x0241U, 0x018aU, 0x018bU, 0x0242U, 0x0243U, 0x018cU, 0x018dU, 0x0244U, 0x0245U, 0x018eU, + 0x018fU, 0x0246U, 0x0247U, 0x0190U, 0x03fcU, 0x0191U, 0x0248U, 0x0192U, 0x0193U, 0x0194U, 0x0195U, 0x0196U, + 0x0197U, 0x0198U, 0x0365U, 0x02d8U, 0x0412U, 0x0123U, 0x04ffU, 0x0338U, 0x0371U, 0x033eU, 0x063bU, 0x0366U, + 0x033dU, 0x0620U, 0x03d0U, 0x0434U, 0x03f9U, 0x03b7U, 0x0614U, 0x02f9U, 0x06d1U, 0x0367U, 0x0623U, 0x0624U, + 0x0611U, 0x048cU, 0x048dU, 0x048eU, 0x03f6U, 0x048fU, 0x06f3U, 0x0450U, 0x0389U, 0x03a5U, 0x03eaU, 0x0435U, + 0x0411U, 0x0671U, 0x0440U, 0x0441U, 0x0494U, 0x0130U, 0x0605U, 0x03b1U, 0x0423U, 0x0638U, 0x0382U, 0x03b0U, + 0x048bU, 0x0420U, 0x04d5U, 0x0380U, 0x02fcU, 0x03bcU, 0x0414U, 0x045aU, 0x04d3U, 0x02fdU, 0x0384U, 0x0386U, + 0x03e6U, 0x03e7U, 0x03e8U, 0x03e9U, 0x03a6U, 0x03a7U, 0x06faU, 0x06fbU, 0x06fcU, 0x06fdU, +}; + +static const uint8_t can_replay_dict_bus_len[CAN_REPLAY_DICT_COUNT] = { + 0x04U, 0x08U, 0x18U, 0x06U, 0x18U, 0x08U, 0x08U, 0x08U, 0x18U, 0x18U, 0x18U, 0x18U, 0x25U, 0x18U, 0x08U, 0x17U, + 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x08U, 0x18U, 0x28U, 0x08U, 0x04U, + 0x18U, 0x18U, 0x08U, 0x08U, 0x18U, 0x08U, 0x08U, 0x28U, 0x08U, 0x08U, 0x08U, 0x04U, 0x06U, 0x28U, 0x16U, 0x18U, + 0x17U, 0x08U, 0x28U, 0x28U, 0x08U, 0x28U, 0x28U, 0x27U, 0x08U, 0x08U, 0x02U, 0x28U, 0x05U, 0x08U, 0x18U, 0x18U, + 0x18U, 0x18U, 0x28U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, + 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x08U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, 0x18U, + 0x18U, 0x18U, 0x27U, 0x08U, 0x28U, 0x17U, 0x08U, 0x08U, 0x28U, 0x27U, 0x08U, 0x27U, 0x02U, 0x08U, 0x01U, 0x08U, + 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, 0x22U, 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, 0x18U, + 0x08U, 0x08U, 0x08U, 0x08U, 0x28U, 0x08U, 0x18U, 0x18U, 0x28U, 0x17U, 0x08U, 0x08U, 0x01U, 0x08U, 0x08U, 0x08U, + 0x28U, 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, 0x28U, 0x08U, 0x28U, 0x28U, 0x06U, 0x06U, 0x05U, 0x07U, 0x08U, 0x08U, + 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, 0x08U, +}; + +extern const uint8_t can_replay_data[CAN_REPLAY_DATA_SIZE]; +__asm__( + ".section .rodata.can_replay_data,\"a\",%progbits\n" + ".balign 4\n" + ".global can_replay_data\n" + ".type can_replay_data, %object\n" + "can_replay_data:\n" + ".incbin \"board/debug/can_replay_data.bin\"\n" + ".size can_replay_data, 634691\n" + ".balign 4\n" + ".previous\n" +); diff --git a/board/debug/gen_can_replay_data.py b/board/debug/gen_can_replay_data.py new file mode 100755 index 00000000000..5032efbc8bf --- /dev/null +++ b/board/debug/gen_can_replay_data.py @@ -0,0 +1,171 @@ +#!/usr/bin/env python3 +import argparse +import textwrap +from pathlib import Path + +from opendbc.car.car_helpers import interfaces +from opendbc.car.fingerprints import MIGRATION +from openpilot.tools.lib.logreader import LogReader + + +DEFAULT_ROUTE = "77611a1fac303767/2020-03-24--09-50-38/3" +DEFAULT_PREFIX_EVENTS = 300 + +# Live fingerprinting enables Toyota BSM when 0x3f6 is present on bus 0. +# The logged CarParams for this old route has enableBsm unset, so the parser +# derived from the log alone does not keep it. +EXTRA_KEEP_ADDRS = { + (0, 0x3F6), +} + + +def parser_used_addrs(CI, RI): + used = set() + for cp in CI.can_parsers.values(): + used |= {(cp.bus, addr) for addr in cp.addresses} + if getattr(RI, "rcp", None) is not None: + used |= {(RI.rcp.bus, addr) for addr in RI.rcp.addresses} + return used + + +def load_events_and_used_addrs(lr, CP): + CI = interfaces[CP.carFingerprint](CP) + RI = interfaces[CP.carFingerprint].RadarInterface(CP) + + events = [] + for msg in lr: + if msg.which() != "can": + continue + + can_msgs = [(int(can.address), bytes(can.dat), int(can.src)) for can in msg.can if int(can.src) <= 2] + event = (msg.logMonoTime, can_msgs) + events.append(event) + CI.update([event]) + RI.update([event]) + + return events, parser_used_addrs(CI, RI) + + +def build_payload(route, prefix_events): + lr = LogReader(route) + CP = lr.first("carParams").as_builder() + CP.carFingerprint = str(MIGRATION.get(CP.carFingerprint, CP.carFingerprint)) + events, used = load_events_and_used_addrs(lr, CP) + seen = {(bus, address) for _, event in events for address, _, bus in event} + used |= EXTRA_KEEP_ADDRS & seen + + counts = bytearray() + records = bytearray() + dictionary = [] + dictionary_idxs = {} + msg_count = 0 + max_count = 0 + + for event_idx, (_, event) in enumerate(events): + can_msgs = [] + for address, dat, bus in event: + if (event_idx >= prefix_events) and ((bus, address) not in used): + continue + assert address < 0x800, f"extended address not supported: {address:#x}" + assert len(dat) <= 8, f"CAN-FD data not supported: {len(dat)}" + can_msgs.append((address, dat, bus)) + + assert len(can_msgs) <= 255 + counts.append(len(can_msgs)) + max_count = max(max_count, len(can_msgs)) + msg_count += len(can_msgs) + + for address, dat, bus in can_msgs: + key = (address, bus, len(dat)) + if key not in dictionary_idxs: + assert len(dictionary) < 256 + dictionary_idxs[key] = len(dictionary) + dictionary.append(key) + + records.append(dictionary_idxs[key]) + records += dat + + payload = bytes(counts + records) + return payload, len(counts), msg_count, max_count, CP.carFingerprint, dictionary + + +def format_u16_array(vals): + lines = [] + for i in range(0, len(vals), 12): + chunk = vals[i:i + 12] + lines.append(" " + ", ".join(f"0x{v:04x}U" for v in chunk) + ",") + return "\n".join(lines) + + +def format_u8_array(vals): + lines = [] + for i in range(0, len(vals), 16): + chunk = vals[i:i + 16] + lines.append(" " + ", ".join(f"0x{v:02x}U" for v in chunk) + ",") + return "\n".join(lines) + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--route", default=DEFAULT_ROUTE) + parser.add_argument("--prefix-events", type=int, default=DEFAULT_PREFIX_EVENTS) + parser.add_argument("--output", type=Path, default=Path(__file__).with_name("can_replay_data.h")) + parser.add_argument("--bin-output", type=Path, default=Path(__file__).with_name("can_replay_data.bin")) + args = parser.parse_args() + + payload, event_count, msg_count, max_count, fingerprint, dictionary = build_payload(args.route, args.prefix_events) + dict_addrs = [address for address, _, _ in dictionary] + dict_bus_lens = [(bus << 4) | length for _, bus, length in dictionary] + + header = f"""\ +// Generated by {Path(__file__).name}; do not edit by hand. +// Route: {args.route} +// Car fingerprint: {fingerprint} +// First {args.prefix_events} CAN events include all bus <= 2 traffic; later events keep parser-used traffic. + +#pragma once + +#define CAN_REPLAY_EVENT_COUNT {event_count}U +#define CAN_REPLAY_MSG_COUNT {msg_count}U +#define CAN_REPLAY_DATA_SIZE {len(payload)}U +#define CAN_REPLAY_MAX_MSGS_PER_EVENT {max_count}U +#define CAN_REPLAY_DICT_COUNT {len(dictionary)}U + +static const uint16_t can_replay_dict_addr[CAN_REPLAY_DICT_COUNT] = {{ +{format_u16_array(dict_addrs)} +}}; + +static const uint8_t can_replay_dict_bus_len[CAN_REPLAY_DICT_COUNT] = {{ +{format_u8_array(dict_bus_lens)} +}}; + +extern const uint8_t can_replay_data[CAN_REPLAY_DATA_SIZE]; +__asm__( + ".section .rodata.can_replay_data,\\"a\\",%progbits\\n" + ".balign 4\\n" + ".global can_replay_data\\n" + ".type can_replay_data, %object\\n" + "can_replay_data:\\n" + ".incbin \\"board/debug/can_replay_data.bin\\"\\n" + ".size can_replay_data, {len(payload)}\\n" + ".balign 4\\n" + ".previous\\n" +); +""" + args.bin_output.write_bytes(payload) + args.output.write_text(header) + print(textwrap.dedent(f"""\ + wrote {args.output} + wrote {args.bin_output} + route: {args.route} + fingerprint: {fingerprint} + events: {event_count} + messages: {msg_count} + max messages/event: {max_count} + dictionary entries: {len(dictionary)} + payload bytes: {len(payload)} + """).strip()) + + +if __name__ == "__main__": + main() diff --git a/board/main.c b/board/main.c index e3e98074798..d4f244c77b4 100644 --- a/board/main.c +++ b/board/main.c @@ -23,8 +23,17 @@ #include "board/obj/gitversion.h" #include "board/can_comms.h" +#ifdef ALLOW_DEBUG +#include "board/debug/can_replay.h" +#endif #include "board/main_comms.h" +#ifdef ALLOW_DEBUG +#define CAN_REPLAY_TICK() can_replay_tick() +#else +#define CAN_REPLAY_TICK() ((void)0) +#endif + // ********************* Serial debugging ********************* @@ -341,6 +350,7 @@ int main(void) { // LED should keep on blinking all the time while (true) { + CAN_REPLAY_TICK(); #ifdef ALLOW_DEBUG if (stop_mode_requested) { enter_stop_mode(); @@ -352,15 +362,19 @@ int main(void) { #endif // useful for debugging, fade breaks = panda is overloaded for (uint32_t fade = 0U; fade < MAX_LED_FADE; fade += 1U) { + CAN_REPLAY_TICK(); led_set(LED_RED, true); delay(fade >> 4); + CAN_REPLAY_TICK(); led_set(LED_RED, false); delay((MAX_LED_FADE - fade) >> 4); } for (uint32_t fade = MAX_LED_FADE; fade > 0U; fade -= 1U) { + CAN_REPLAY_TICK(); led_set(LED_RED, true); delay(fade >> 4); + CAN_REPLAY_TICK(); led_set(LED_RED, false); delay((MAX_LED_FADE - fade) >> 4); } diff --git a/board/main_comms.h b/board/main_comms.h index 8229a47ab9a..8230dab7428 100644 --- a/board/main_comms.h +++ b/board/main_comms.h @@ -321,6 +321,19 @@ int comms_control_handler(ControlPacket_t *req, uint8_t *resp) { UNUSED(ret); } break; + #ifdef ALLOW_DEBUG + // **** 0xfb: DEBUG: set/get embedded CAN replay mode + case 0xfb: + if (req->param1 == 0U) { + can_replay_set_enabled(false); + } else if (req->param1 == 1U) { + can_replay_set_enabled(true); + } else if (req->param1 == 2U) { + resp_len = can_replay_status(resp); + } else { + } + break; + #endif // **** 0xfc: set CAN FD non-ISO mode case 0xfc: if (req->param1 < PANDA_CAN_CNT) { diff --git a/python/__init__.py b/python/__init__.py index 4c954b23950..97edf0504bc 100644 --- a/python/__init__.py +++ b/python/__init__.py @@ -686,6 +686,19 @@ def set_canfd_non_iso(self, bus, non_iso): def set_canfd_auto(self, bus, auto): self._handle.controlWrite(Panda.REQUEST_OUT, 0xe8, bus, int(auto), b'') + def set_can_replay(self, enable): + self._handle.controlWrite(Panda.REQUEST_OUT, 0xfb, int(enable), 0, b'') + + def get_can_replay_status(self): + dat = self._handle.controlRead(Panda.REQUEST_IN, 0xfb, 2, 0, 13) + enabled, event_idx, event_count, msg_count = struct.unpack("