IQ.Pilot Release Commit @ bec7652

iqpilot/selfdrive/pandad/.gitignore

@@ -0,0 +1,3 @@
+pandad
+pandad_api_impl.cpp
+tests/test_pandad_usbprotocol

iqpilot/selfdrive/pandad/SConscript

@@ -0,0 +1,13 @@
+Import('env', 'common', 'messaging')
+
+import panda as panda_package
+
+env.Append(CXXFLAGS=['-DPANDA_FW_PATH=\\"%s\\"' % panda_package.FW_PATH])
+
+libs = ['usb-1.0', common, messaging, 'pthread']
+panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc'])
+
+env.Program('pandad', ['main.cc', 'pandad.cc', 'panda_safety.cc'], LIBS=[panda] + libs)
+
+if GetOption('extras'):
+  env.Program('tests/test_pandad_usbprotocol', ['tests/test_pandad_usbprotocol.cc'], LIBS=[panda] + libs)

iqpilot/selfdrive/pandad/__init__.py

@@ -0,0 +1,3 @@
+from iqpilot.selfdrive.pandad.pandad_api_impl import can_list_to_can_capnp, can_capnp_to_list
+assert can_list_to_can_capnp
+assert can_capnp_to_list

iqpilot/selfdrive/pandad/main.cc

@@ -0,0 +1,22 @@
+#include <cassert>
+
+#include "selfdrive/pandad/pandad.h"
+#include "common/swaglog.h"
+#include "common/util.h"
+#include "system/hardware/hw.h"
+
+int main(int argc, char *argv[]) {
+  LOGW("starting pandad");
+
+  if (!Hardware::PC()) {
+    int err;
+    err = util::set_realtime_priority(54);
+    assert(err == 0);
+    err = util::set_core_affinity({3});
+    assert(err == 0);
+  }
+
+  std::vector<std::string> serials(argv + 1, argv + argc);
+  pandad_main_thread(serials);
+  return 0;
+}

iqpilot/selfdrive/pandad/panda.cc

@@ -0,0 +1,312 @@
+#include "selfdrive/pandad/panda.h"
+
+#include <unistd.h>
+
+#include <cassert>
+#include <stdexcept>
+#include <vector>
+
+#include "cereal/messaging/messaging.h"
+#include "common/swaglog.h"
+#include "common/util.h"
+
+const bool PANDAD_MAXOUT = getenv("PANDAD_MAXOUT") != nullptr;
+
+Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) {
+  // try USB first, then SPI
+  try {
+    handle = std::make_unique<PandaUsbHandle>(serial);
+    LOGW("connected to %s over USB", serial.c_str());
+  } catch (std::exception &e) {
+#ifndef __APPLE__
+    handle = std::make_unique<PandaSpiHandle>(serial);
+    LOGW("connected to %s over SPI", serial.c_str());
+#else
+    throw e;
+#endif
+  }
+
+  hw_type = get_hw_type();
+  can_reset_communications();
+}
+
+bool Panda::connected() {
+  return handle->connected;
+}
+
+bool Panda::comms_healthy() {
+  return handle->comms_healthy;
+}
+
+std::string Panda::hw_serial() {
+  return handle->hw_serial;
+}
+
+std::vector<std::string> Panda::list(bool usb_only) {
+  std::vector<std::string> serials = PandaUsbHandle::list();
+
+#ifndef __APPLE__
+  if (!usb_only) {
+    for (const auto &s : PandaSpiHandle::list()) {
+      if (std::find(serials.begin(), serials.end(), s) == serials.end()) {
+        serials.push_back(s);
+      }
+    }
+  }
+#endif
+
+  return serials;
+}
+
+void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) {
+  handle->control_write(0xdc, (uint16_t)safety_model, safety_param);
+}
+
+void Panda::set_alternative_experience(uint16_t alternative_experience, uint16_t safety_param_iq) {
+  handle->control_write(0xdf, alternative_experience, safety_param_iq);
+}
+
+std::string Panda::serial_read(int port_number) {
+  std::string ret;
+  char buffer[USBPACKET_MAX_SIZE] = {};
+
+  while (true) {
+    int bytes_read = handle->control_read(0xe0, port_number, 0, (unsigned char *)buffer, USBPACKET_MAX_SIZE);
+    if (bytes_read <= 0) {
+      break;
+    }
+    ret.append(buffer, bytes_read);
+  }
+
+  return ret;
+}
+
+void Panda::set_uart_baud(int uart, int rate) {
+  handle->control_write(0xe4, uart, int(rate / 300));
+}
+
+cereal::PandaState::PandaType Panda::get_hw_type() {
+  unsigned char hw_query[1] = {0};
+
+  handle->control_read(0xc1, 0, 0, hw_query, 1);
+  return (cereal::PandaState::PandaType)(hw_query[0]);
+}
+
+void Panda::set_fan_speed(uint16_t fan_speed) {
+  handle->control_write(0xb1, fan_speed, 0);
+}
+
+uint16_t Panda::get_fan_speed() {
+  uint16_t fan_speed_rpm = 0;
+  handle->control_read(0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm));
+  return fan_speed_rpm;
+}
+
+void Panda::set_ir_pwr(uint16_t ir_pwr) {
+  handle->control_write(0xb0, ir_pwr, 0);
+}
+
+std::optional<health_t> Panda::get_state() {
+  health_t health {0};
+  int err = handle->control_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health));
+  return err >= 0 ? std::make_optional(health) : std::nullopt;
+}
+
+std::optional<can_health_t> Panda::get_can_state(uint16_t can_number) {
+  can_health_t can_health {0};
+  int err = handle->control_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health));
+  return err >= 0 ? std::make_optional(can_health) : std::nullopt;
+}
+
+void Panda::set_loopback(bool loopback) {
+  handle->control_write(0xe5, loopback, 0);
+}
+
+std::optional<std::vector<uint8_t>> Panda::get_firmware_version() {
+  std::vector<uint8_t> fw_sig_buf(128);
+  int read_1 = handle->control_read(0xd3, 0, 0, &fw_sig_buf[0], 64);
+  int read_2 = handle->control_read(0xd4, 0, 0, &fw_sig_buf[64], 64);
+  return ((read_1 == 64) && (read_2 == 64)) ? std::make_optional(fw_sig_buf) : std::nullopt;
+}
+
+std::optional<std::string> Panda::get_serial() {
+  char serial_buf[17] = {'\0'};
+  int err = handle->control_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16);
+  return err >= 0 ? std::make_optional(serial_buf) : std::nullopt;
+}
+
+bool Panda::up_to_date() {
+  if (auto fw_sig = get_firmware_version()) {
+    for (auto fn : { "panda.bin.signed", "panda_h7.bin.signed" }) {
+      auto content = util::read_file(std::string(PANDA_FW_PATH) + fn);
+      if (content.size() >= fw_sig->size() &&
+          memcmp(content.data() + content.size() - fw_sig->size(), fw_sig->data(), fw_sig->size()) == 0) {
+        return true;
+      }
+    }
+  }
+  return false;
+}
+
+void Panda::set_power_saving(bool power_saving) {
+  handle->control_write(0xe7, power_saving, 0);
+}
+
+void Panda::enable_deepsleep() {
+  handle->control_write(0xfb, 0, 0);
+}
+
+void Panda::send_heartbeat(bool engaged, bool engaged_aol) {
+  handle->control_write(0xf3, engaged, engaged_aol);
+}
+
+void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {
+  handle->control_write(0xde, bus, (speed * 10));
+}
+
+void Panda::set_can_fd_auto(uint16_t bus, bool enabled) {
+  handle->control_write(0xe8, bus, enabled);
+}
+
+void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) {
+  handle->control_write(0xf9, bus, (speed * 10));
+}
+
+void Panda::set_canfd_non_iso(uint16_t bus, bool non_iso) {
+  handle->control_write(0xfc, bus, non_iso);
+}
+
+static uint8_t len_to_dlc(uint8_t len) {
+  if (len <= 8) {
+    return len;
+  }
+  if (len <= 24) {
+    return 8 + ((len - 8) / 4) + ((len % 4) ? 1 : 0);
+  } else {
+    return 11 + (len / 16) + ((len % 16) ? 1 : 0);
+  }
+}
+
+void Panda::pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
+                            std::function<void(uint8_t *, size_t)> write_func) {
+  int32_t pos = 0;
+  uint8_t send_buf[2 * USB_TX_SOFT_LIMIT];
+
+  for (const auto &cmsg : can_data_list) {
+    // check if the message is intended for this panda
+    uint8_t bus = cmsg.getSrc();
+    if (bus < bus_offset || bus >= (bus_offset + PANDA_BUS_OFFSET)) {
+      continue;
+    }
+    auto can_data = cmsg.getDat();
+    uint8_t data_len_code = len_to_dlc(can_data.size());
+    assert(can_data.size() <= 64);
+    assert(can_data.size() == dlc_to_len[data_len_code]);
+
+    can_header header = {};
+    header.addr = cmsg.getAddress();
+    header.extended = (cmsg.getAddress() >= 0x800) ? 1 : 0;
+    header.data_len_code = data_len_code;
+    header.bus = bus - bus_offset;
+    header.checksum = 0;
+
+    memcpy(&send_buf[pos], (uint8_t *)&header, sizeof(can_header));
+    memcpy(&send_buf[pos + sizeof(can_header)], (uint8_t *)can_data.begin(), can_data.size());
+    uint32_t msg_size = sizeof(can_header) + can_data.size();
+
+    // set checksum
+    ((can_header *) &send_buf[pos])->checksum = calculate_checksum(&send_buf[pos], msg_size);
+
+    pos += msg_size;
+
+    if (pos >= USB_TX_SOFT_LIMIT) {
+      write_func(send_buf, pos);
+      pos = 0;
+    }
+  }
+
+  // send remaining packets
+  if (pos > 0) write_func(send_buf, pos);
+}
+
+void Panda::can_send(const capnp::List<cereal::CanData>::Reader &can_data_list) {
+  pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) {
+    handle->bulk_write(3, data, size, 5);
+  });
+}
+
+bool Panda::can_receive(std::vector<can_frame>& out_vec) {
+  // Check if enough space left in buffer to store RECV_SIZE data
+  assert(receive_buffer_size + RECV_SIZE <= sizeof(receive_buffer));
+
+  int recv = handle->bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE);
+  if (!comms_healthy()) {
+    return false;
+  }
+
+  if (PANDAD_MAXOUT) {
+    static uint8_t junk[RECV_SIZE];
+    handle->bulk_read(0xab, junk, RECV_SIZE - recv);
+  }
+
+  bool ret = true;
+  if (recv > 0) {
+    receive_buffer_size += recv;
+    ret = unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec);
+  }
+  return ret;
+}
+
+void Panda::can_reset_communications() {
+  handle->control_write(0xc0, 0, 0);
+}
+
+bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec) {
+  int pos = 0;
+
+  while (pos <= size - sizeof(can_header)) {
+    can_header header;
+    memcpy(&header, &data[pos], sizeof(can_header));
+
+    const uint8_t data_len = dlc_to_len[header.data_len_code];
+    if (pos + sizeof(can_header) + data_len > size) {
+      // we don't have all the data for this message yet
+      break;
+    }
+
+    if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) {
+      LOGE("Panda CAN checksum failed");
+      size = 0;
+      can_reset_communications();
+      return false;
+    }
+
+    can_frame &canData = out_vec.emplace_back();
+    canData.address = header.addr;
+    canData.src = header.bus + bus_offset;
+    if (header.rejected) {
+      canData.src += CAN_REJECTED_BUS_OFFSET;
+    }
+    if (header.returned) {
+      canData.src += CAN_RETURNED_BUS_OFFSET;
+    }
+
+    canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len);
+
+    pos += sizeof(can_header) + data_len;
+  }
+
+  // move the overflowing data to the beginning of the buffer for the next round
+  memmove(data, &data[pos], size - pos);
+  size -= pos;
+
+  return true;
+}
+
+uint8_t Panda::calculate_checksum(uint8_t *data, uint32_t len) {
+  uint8_t checksum = 0U;
+  for (uint32_t i = 0U; i < len; i++) {
+    checksum ^= data[i];
+  }
+  return checksum;
+}

iqpilot/selfdrive/pandad/panda.h

@@ -0,0 +1,99 @@
+#pragma once
+
+#include <cstdint>
+#include <ctime>
+#include <functional>
+#include <list>
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include "cereal/gen/cpp/car.capnp.h"
+#include "cereal/gen/cpp/log.capnp.h"
+#include "panda/board/health.h"
+#include "panda/board/can.h"
+#include "selfdrive/pandad/panda_comms.h"
+
+#define USB_TX_SOFT_LIMIT   (0x100U)
+#define USBPACKET_MAX_SIZE  (0x40)
+
+#define RECV_SIZE (0x4000U)
+
+#define CAN_REJECTED_BUS_OFFSET   0xC0U
+#define CAN_RETURNED_BUS_OFFSET 0x80U
+
+#define PANDA_BUS_OFFSET 4
+
+struct __attribute__((packed)) can_header {
+  uint8_t reserved : 1;
+  uint8_t bus : 3;
+  uint8_t data_len_code : 4;
+  uint8_t rejected : 1;
+  uint8_t returned : 1;
+  uint8_t extended : 1;
+  uint32_t addr : 29;
+  uint8_t checksum : 8;
+};
+
+struct can_frame {
+  long address;
+  std::string dat;
+  long src;
+};
+
+
+class Panda {
+private:
+  std::unique_ptr<PandaCommsHandle> handle;
+
+public:
+  Panda(std::string serial="", uint32_t bus_offset=0);
+
+  cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
+  const uint32_t bus_offset;
+
+  bool connected();
+  bool comms_healthy();
+  std::string hw_serial();
+
+  // Static functions
+  static std::vector<std::string> list(bool usb_only=false);
+
+  // Panda functionality
+  cereal::PandaState::PandaType get_hw_type();
+  void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U);
+  void set_alternative_experience(uint16_t alternative_experience, uint16_t safety_param_iq=0U);
+  std::string serial_read(int port_number = 0);
+  void set_uart_baud(int uart, int rate);
+  void set_fan_speed(uint16_t fan_speed);
+  uint16_t get_fan_speed();
+  void set_ir_pwr(uint16_t ir_pwr);
+  std::optional<health_t> get_state();
+  std::optional<can_health_t> get_can_state(uint16_t can_number);
+  void set_loopback(bool loopback);
+  std::optional<std::vector<uint8_t>> get_firmware_version();
+  bool up_to_date();
+  std::optional<std::string> get_serial();
+  void set_power_saving(bool power_saving);
+  void enable_deepsleep();
+  void send_heartbeat(bool engaged, bool engaged_aol);
+  void set_can_speed_kbps(uint16_t bus, uint16_t speed);
+  void set_can_fd_auto(uint16_t bus, bool enabled);
+  void set_data_speed_kbps(uint16_t bus, uint16_t speed);
+  void set_canfd_non_iso(uint16_t bus, bool non_iso);
+  void can_send(const capnp::List<cereal::CanData>::Reader &can_data_list);
+  bool can_receive(std::vector<can_frame>& out_vec);
+  void can_reset_communications();
+
+protected:
+  // for unit tests
+  uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64];
+  uint32_t receive_buffer_size = 0;
+
+  Panda(uint32_t bus_offset) : bus_offset(bus_offset) {}
+  void pack_can_buffer(const capnp::List<cereal::CanData>::Reader &can_data_list,
+                         std::function<void(uint8_t *, size_t)> write_func);
+  bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector<can_frame> &out_vec);
+  uint8_t calculate_checksum(uint8_t *data, uint32_t len);
+};

iqpilot/selfdrive/pandad/panda_comms.cc

@@ -0,0 +1,227 @@
+#include "selfdrive/pandad/panda.h"
+
+#include <cassert>
+#include <stdexcept>
+#include <memory>
+
+#include "common/swaglog.h"
+
+static libusb_context *init_usb_ctx() {
+  libusb_context *context = nullptr;
+  int err = libusb_init(&context);
+  if (err != 0) {
+    LOGE("libusb initialization error");
+    return nullptr;
+  }
+
+#if LIBUSB_API_VERSION >= 0x01000106
+  libusb_set_option(context, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO);
+#else
+  libusb_set_debug(context, 3);
+#endif
+  return context;
+}
+
+PandaUsbHandle::PandaUsbHandle(std::string serial) : PandaCommsHandle(serial) {
+  // init libusb
+  ssize_t num_devices;
+  libusb_device **dev_list = NULL;
+  int err = 0;
+  ctx = init_usb_ctx();
+  if (!ctx) { goto fail; }
+
+  // connect by serial
+  num_devices = libusb_get_device_list(ctx, &dev_list);
+  if (num_devices < 0) { goto fail; }
+  for (size_t i = 0; i < num_devices; ++i) {
+    libusb_device_descriptor desc;
+    libusb_get_device_descriptor(dev_list[i], &desc);
+    if (desc.idVendor == 0x3801 && desc.idProduct == 0xddcc) {
+      int ret = libusb_open(dev_list[i], &dev_handle);
+      if (dev_handle == NULL || ret < 0) { goto fail; }
+
+      unsigned char desc_serial[26] = { 0 };
+      ret = libusb_get_string_descriptor_ascii(dev_handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
+      if (ret < 0) { goto fail; }
+
+      hw_serial = std::string((char *)desc_serial, ret);
+      if (serial.empty() || serial == hw_serial) {
+        break;
+      }
+      libusb_close(dev_handle);
+      dev_handle = NULL;
+    }
+  }
+  if (dev_handle == NULL) goto fail;
+  libusb_free_device_list(dev_list, 1);
+  dev_list = nullptr;
+
+  if (libusb_kernel_driver_active(dev_handle, 0) == 1) {
+    libusb_detach_kernel_driver(dev_handle, 0);
+  }
+
+  err = libusb_set_configuration(dev_handle, 1);
+  if (err != 0) { goto fail; }
+
+  err = libusb_claim_interface(dev_handle, 0);
+  if (err != 0) { goto fail; }
+
+  return;
+
+fail:
+  if (dev_list != NULL) {
+    libusb_free_device_list(dev_list, 1);
+  }
+  cleanup();
+  throw std::runtime_error("Error connecting to panda");
+}
+
+PandaUsbHandle::~PandaUsbHandle() {
+  std::lock_guard lk(hw_lock);
+  cleanup();
+  connected = false;
+}
+
+void PandaUsbHandle::cleanup() {
+  if (dev_handle) {
+    libusb_release_interface(dev_handle, 0);
+    libusb_close(dev_handle);
+  }
+
+  if (ctx) {
+    libusb_exit(ctx);
+  }
+}
+
+std::vector<std::string> PandaUsbHandle::list() {
+  static std::unique_ptr<libusb_context, decltype(&libusb_exit)> context(init_usb_ctx(), libusb_exit);
+  // init libusb
+  ssize_t num_devices;
+  libusb_device **dev_list = NULL;
+  std::vector<std::string> serials;
+  if (!context) { return serials; }
+
+  num_devices = libusb_get_device_list(context.get(), &dev_list);
+  if (num_devices < 0) {
+    LOGE("libusb can't get device list");
+    goto finish;
+  }
+  for (size_t i = 0; i < num_devices; ++i) {
+    libusb_device *device = dev_list[i];
+    libusb_device_descriptor desc;
+    libusb_get_device_descriptor(device, &desc);
+    if (desc.idVendor == 0x3801 && desc.idProduct == 0xddcc) {
+      libusb_device_handle *handle = NULL;
+      int ret = libusb_open(device, &handle);
+      if (ret < 0) { goto finish; }
+
+      unsigned char desc_serial[26] = { 0 };
+      ret = libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, desc_serial, std::size(desc_serial));
+      libusb_close(handle);
+      if (ret < 0) { goto finish; }
+
+      serials.push_back(std::string((char *)desc_serial, ret));
+    }
+  }
+
+finish:
+  if (dev_list != NULL) {
+    libusb_free_device_list(dev_list, 1);
+  }
+  return serials;
+}
+
+void PandaUsbHandle::handle_usb_issue(int err, const char func[]) {
+  LOGE_100("usb error %d \"%s\" in %s", err, libusb_strerror((enum libusb_error)err), func);
+  if (err == LIBUSB_ERROR_NO_DEVICE) {
+    LOGE("lost connection");
+    connected = false;
+  }
+  // TODO: check other errors, is simply retrying okay?
+}
+
+int PandaUsbHandle::control_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) {
+  int err;
+  const uint8_t bmRequestType = LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
+
+  if (!connected) {
+    return LIBUSB_ERROR_NO_DEVICE;
+  }
+
+  std::lock_guard lk(hw_lock);
+  do {
+    err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout);
+    if (err < 0) handle_usb_issue(err, __func__);
+  } while (err < 0 && connected);
+
+  return err;
+}
+
+int PandaUsbHandle::control_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout) {
+  int err;
+  const uint8_t bmRequestType = LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE;
+
+  if (!connected) {
+    return LIBUSB_ERROR_NO_DEVICE;
+  }
+
+  std::lock_guard lk(hw_lock);
+  do {
+    err = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, data, wLength, timeout);
+    if (err < 0) handle_usb_issue(err, __func__);
+  } while (err < 0 && connected);
+
+  return err;
+}
+
+int PandaUsbHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
+  int err;
+  int transferred = 0;
+
+  if (!connected) {
+    return 0;
+  }
+
+  std::lock_guard lk(hw_lock);
+  do {
+    // Try sending can messages. If the receive buffer on the panda is full it will NAK
+    // and libusb will try again. After 5ms, it will time out. We will drop the messages.
+    err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout);
+
+    if (err == LIBUSB_ERROR_TIMEOUT) {
+      LOGW("Transmit buffer full");
+      break;
+    } else if (err != 0 || length != transferred) {
+      handle_usb_issue(err, __func__);
+    }
+  } while (err != 0 && connected);
+
+  return transferred;
+}
+
+int PandaUsbHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
+  int err;
+  int transferred = 0;
+
+  if (!connected) {
+    return 0;
+  }
+
+  std::lock_guard lk(hw_lock);
+
+  do {
+    err = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout);
+
+    if (err == LIBUSB_ERROR_TIMEOUT) {
+      break; // timeout is okay to exit, recv still happened
+    } else if (err == LIBUSB_ERROR_OVERFLOW) {
+      comms_healthy = false;
+      LOGE_100("overflow got 0x%x", transferred);
+    } else if (err != 0) {
+      handle_usb_issue(err, __func__);
+    }
+
+  } while (err != 0 && connected);
+
+  return transferred;
+}

iqpilot/selfdrive/pandad/panda_comms.h

@@ -0,0 +1,93 @@
+#pragma once
+
+#include <atomic>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#ifndef __APPLE__
+#include <linux/spi/spidev.h>
+#endif
+
+#include <libusb-1.0/libusb.h>
+
+
+#define TIMEOUT 0
+#define SPI_BUF_SIZE 2048
+
+
+// comms base class
+class PandaCommsHandle {
+public:
+  PandaCommsHandle(std::string serial) {}
+  virtual ~PandaCommsHandle() {}
+  virtual void cleanup() = 0;
+
+  std::string hw_serial;
+  std::atomic<bool> connected = true;
+  std::atomic<bool> comms_healthy = true;
+  static std::vector<std::string> list();
+
+  // HW communication
+  virtual int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT) = 0;
+  virtual int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT) = 0;
+  virtual int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0;
+  virtual int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT) = 0;
+};
+
+class PandaUsbHandle : public PandaCommsHandle {
+public:
+  PandaUsbHandle(std::string serial);
+  ~PandaUsbHandle();
+  int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT);
+  int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT);
+  int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
+  int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
+  void cleanup();
+
+  static std::vector<std::string> list();
+
+private:
+  libusb_context *ctx = NULL;
+  libusb_device_handle *dev_handle = NULL;
+  std::recursive_mutex hw_lock;
+  void handle_usb_issue(int err, const char func[]);
+};
+
+#ifndef __APPLE__
+struct __attribute__((packed)) spi_header {
+  uint8_t sync;
+  uint8_t endpoint;
+  uint16_t tx_len;
+  uint16_t max_rx_len;
+};
+
+class PandaSpiHandle : public PandaCommsHandle {
+public:
+  PandaSpiHandle(std::string serial);
+  ~PandaSpiHandle();
+  int control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout=TIMEOUT);
+  int control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout=TIMEOUT);
+  int bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
+  int bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
+  void cleanup();
+
+  static std::vector<std::string> list();
+
+private:
+  int spi_fd = -1;
+  uint8_t tx_buf[SPI_BUF_SIZE];
+  uint8_t rx_buf[SPI_BUF_SIZE];
+  inline static std::recursive_mutex hw_lock;
+
+  int wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length);
+  int bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout);
+  int spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
+  int spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout);
+  int lltransfer(spi_ioc_transfer &t);
+
+  spi_header header;
+  uint32_t xfer_count = 0;
+};
+#endif

iqpilot/selfdrive/pandad/panda_safety.cc

@@ -0,0 +1,95 @@
+#include "selfdrive/pandad/pandad.h"
+#include "cereal/messaging/messaging.h"
+#include "common/swaglog.h"
+
+void PandaSafety::configureSafetyMode(bool is_onroad) {
+  if (is_onroad && !safety_configured_) {
+    updateMultiplexingMode();
+
+    auto car_params = fetchCarParams();
+    if (!car_params.empty()) {
+      LOGW("got %lu bytes CarParams", car_params[0].size());
+      LOGW("got %lu bytes IQCarParams", car_params[1].size());
+      setSafetyMode(car_params);
+      safety_configured_ = true;
+    }
+  } else if (!is_onroad) {
+    initialized_ = false;
+    safety_configured_ = false;
+    log_once_ = false;
+  }
+}
+
+void PandaSafety::updateMultiplexingMode() {
+  // Initialize to ELM327 without OBD multiplexing for initial fingerprinting
+  if (!initialized_) {
+    prev_obd_multiplexing_ = false;
+    for (int i = 0; i < pandas_.size(); ++i) {
+      pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
+    }
+    initialized_ = true;
+  }
+
+  // Switch between multiplexing modes based on the OBD multiplexing request
+  bool obd_multiplexing_requested = params_.getBool("ObdMultiplexingEnabled");
+  if (obd_multiplexing_requested != prev_obd_multiplexing_) {
+    for (int i = 0; i < pandas_.size(); ++i) {
+      const uint16_t safety_param = (i > 0 || !obd_multiplexing_requested) ? 1U : 0U;
+      pandas_[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
+    }
+    prev_obd_multiplexing_ = obd_multiplexing_requested;
+    params_.putBool("ObdMultiplexingChanged", true);
+  }
+}
+
+// TODO-IQ: Use structs instead of vector
+std::vector<std::string> PandaSafety::fetchCarParams() {
+  if (!params_.getBool("FirmwareQueryDone")) {
+    return {};
+  }
+
+  if (!log_once_) {
+    LOGW("Finished FW query, Waiting for params to set safety model");
+    log_once_ = true;
+  }
+
+  if (!params_.getBool("ControlsReady")) {
+    return {};
+  }
+  return {params_.get("CarParams"), params_.get("IQCarParams")};
+}
+
+// TODO-IQ: Use structs instead of vector
+void PandaSafety::setSafetyMode(const std::vector<std::string> &params_string) {
+  AlignedBuffer aligned_buf;
+  AlignedBuffer aligned_buf_iq;
+
+  capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params_string[0].data(), params_string[0].size()));
+  cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+
+  capnp::FlatArrayMessageReader cmsg_iq(aligned_buf_iq.align(params_string[1].data(), params_string[1].size()));
+  cereal::IQCarParams::Reader car_params_iq = cmsg_iq.getRoot<cereal::IQCarParams>();
+
+  auto safety_configs = car_params.getSafetyConfigs();
+  uint16_t alternative_experience = car_params.getAlternativeExperience();
+  uint16_t safety_param_iq = car_params_iq.getIqSafetyFlags();
+
+  for (int i = 0; i < pandas_.size(); ++i) {
+    // Default to SILENT safety model if not specified
+    cereal::CarParams::SafetyModel safety_model = cereal::CarParams::SafetyModel::SILENT;
+    uint16_t safety_param = 0U;
+    if (i < safety_configs.size()) {
+      safety_model = safety_configs[i].getSafetyModel();
+      safety_param = safety_configs[i].getSafetyParam();
+    }
+
+    LOGW("Panda %d: setting safety model: %d, param: %d, alternative experience: %d, param_iq: %d", i, (int)safety_model, safety_param, alternative_experience, safety_param_iq);
+    pandas_[i]->set_alternative_experience(alternative_experience, safety_param_iq);
+    pandas_[i]->set_safety_model(safety_model, safety_param);
+  }
+}
+
+bool PandaSafety::getOffroadMode() {
+  auto offroad_mode = params_.getBool("IQAlwaysOffroad");
+  return offroad_mode;
+}

iqpilot/selfdrive/pandad/pandad.cc

@@ -0,0 +1,613 @@
+#include "selfdrive/pandad/pandad.h"
+
+#include <algorithm>
+#include <array>
+#include <bitset>
+#include <cassert>
+#include <cerrno>
+#include <memory>
+#include <thread>
+#include <utility>
+
+#include "cereal/gen/cpp/car.capnp.h"
+#include "cereal/messaging/messaging.h"
+#include "cereal/services.h"
+#include "common/ratekeeper.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+#include "system/hardware/hw.h"
+
+// -- Multi-panda conventions --
+// Ordering:
+// - The internal panda will always be the first panda
+// - Consecutive pandas will be sorted based on panda type, and then serial number
+// Connecting:
+// - If a panda connection is dropped, pandad will reconnect to all pandas
+// - If a panda is added, we will only reconnect when we are offroad
+// CAN buses:
+// - Each panda will have its block of 4 buses. E.g.: the second panda will use
+//   bus numbers 4, 5, 6 and 7
+// - The internal panda will always be used for accessing the OBD2 port,
+//   and thus firmware queries
+// Safety:
+// - SafetyConfig is a list, which is mapped to the connected pandas
+// - If there are more pandas connected than there are SafetyConfigs,
+//   the excess pandas will remain in "silent" or "noOutput" mode
+// Ignition:
+// - If any of the ignition sources in any panda is high, ignition is high
+
+#define MAX_IR_PANDA_VAL 50
+#define CUTOFF_IL 400
+#define SATURATE_IL 1000
+
+#define ALT_EXP_AOL_DISENGAGE_LATERAL_ON_BRAKE 2048
+
+ExitHandler do_exit;
+
+bool check_all_connected(const std::vector<Panda *> &pandas) {
+  for (const auto& panda : pandas) {
+    if (!panda->connected()) {
+      do_exit = true;
+      return false;
+    }
+  }
+  return true;
+}
+
+std::vector<const char*> filter_available_services(const std::vector<const char*> &requested, const char *context) {
+  std::vector<const char*> available;
+  available.reserve(requested.size());
+
+  for (const char *name : requested) {
+    if (services.count(name) > 0) {
+      available.push_back(name);
+    } else {
+      LOGW("%s: service '%s' not found in cereal services map, disabling dependent logic", context, name);
+    }
+  }
+
+  return available;
+}
+
+bool process_aol_heartbeat(SubMaster *sm, bool has_iq_state_service, bool has_car_params_service) {
+  if (!has_iq_state_service || !has_car_params_service) {
+    return false;
+  }
+
+  const int &alt_exp = (*sm)["carParams"].getCarParams().getAlternativeExperience();
+  const bool disengage_lateral_on_brake = (alt_exp & ALT_EXP_AOL_DISENGAGE_LATERAL_ON_BRAKE) != 0;
+
+  const bool iq_state_alive = sm->allAliveAndValid({"iqState"});
+  if (!iq_state_alive) {
+    return false;
+  }
+
+  const auto &aol = (*sm)["iqState"].getIqState().getAol();
+  const bool heartbeat_type = disengage_lateral_on_brake ? aol.getActive() : aol.getEnabled();
+
+  const bool engaged = iq_state_alive && heartbeat_type;
+
+  return engaged;
+}
+
+Panda *connect(std::string serial="", uint32_t index=0) {
+  std::unique_ptr<Panda> panda;
+  try {
+    panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_OFFSET));
+  } catch (std::exception &e) {
+    return nullptr;
+  }
+
+  // common panda config
+  if (getenv("BOARDD_LOOPBACK")) {
+    panda->set_loopback(true);
+  }
+  //panda->enable_deepsleep();
+
+  for (int i = 0; i < PANDA_CAN_CNT; i++) {
+    panda->set_can_fd_auto(i, true);
+  }
+
+  bool is_supported_panda = std::find(SUPPORTED_PANDA_TYPES.begin(), SUPPORTED_PANDA_TYPES.end(), panda->hw_type) != SUPPORTED_PANDA_TYPES.end();
+
+  if (!is_supported_panda) {
+    LOGW("panda %s is not supported (hw_type: %i), skipping firmware check...", panda->hw_serial().c_str(), static_cast<uint16_t>(panda->hw_type));
+    return panda.release();
+  }
+
+  if (!panda->up_to_date() && !getenv("BOARDD_SKIP_FW_CHECK")) {
+    throw std::runtime_error("Panda firmware out of date. Run pandad.py to update.");
+  }
+
+  return panda.release();
+}
+
+void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
+  util::set_thread_name("pandad_can_send");
+
+  AlignedBuffer aligned_buf;
+  std::unique_ptr<Context> context(Context::create());
+  std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "sendcan", "127.0.0.1", false, true, services.at("sendcan").queue_size));
+  assert(subscriber != NULL);
+  subscriber->setTimeout(100);
+
+  // run as fast as messages come in
+  while (!do_exit && check_all_connected(pandas)) {
+    std::unique_ptr<Message> msg(subscriber->receive());
+    if (!msg) {
+      continue;
+    }
+
+    capnp::FlatArrayMessageReader cmsg(aligned_buf.align(msg.get()));
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+    // Don't send if older than 1 second
+    if ((nanos_since_boot() - event.getLogMonoTime() < 1e9) && !fake_send) {
+      for (const auto& panda : pandas) {
+        LOGT("sending sendcan to panda: %s", (panda->hw_serial()).c_str());
+        panda->can_send(event.getSendcan());
+        LOGT("sendcan sent to panda: %s", (panda->hw_serial()).c_str());
+      }
+    } else {
+      LOGE("sendcan too old to send: %" PRIu64 ", %" PRIu64, nanos_since_boot(), event.getLogMonoTime());
+    }
+  }
+}
+
+void can_recv(std::vector<Panda *> &pandas, PubMaster *pm) {
+  static std::vector<can_frame> raw_can_data;
+  {
+    bool comms_healthy = true;
+    raw_can_data.clear();
+    for (const auto& panda : pandas) {
+      comms_healthy &= panda->can_receive(raw_can_data);
+    }
+
+    MessageBuilder msg;
+    auto evt = msg.initEvent();
+    evt.setValid(comms_healthy);
+    auto canData = evt.initCan(raw_can_data.size());
+    for (size_t i = 0; i < raw_can_data.size(); ++i) {
+      canData[i].setAddress(raw_can_data[i].address);
+      canData[i].setDat(kj::arrayPtr((uint8_t*)raw_can_data[i].dat.data(), raw_can_data[i].dat.size()));
+      canData[i].setSrc(raw_can_data[i].src);
+    }
+    pm->send("can", msg);
+  }
+}
+
+void fill_panda_state(cereal::PandaState::Builder &ps, cereal::PandaState::PandaType hw_type, const health_t &health) {
+  ps.setVoltage(health.voltage_pkt);
+  ps.setCurrent(health.current_pkt);
+  ps.setUptime(health.uptime_pkt);
+  ps.setSafetyTxBlocked(health.safety_tx_blocked_pkt);
+  ps.setSafetyRxInvalid(health.safety_rx_invalid_pkt);
+  ps.setIgnitionLine(health.ignition_line_pkt);
+  ps.setIgnitionCan(health.ignition_can_pkt);
+  ps.setControlsAllowed(health.controls_allowed_pkt);
+  ps.setTxBufferOverflow(health.tx_buffer_overflow_pkt);
+  ps.setRxBufferOverflow(health.rx_buffer_overflow_pkt);
+  ps.setPandaType(hw_type);
+  ps.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_mode_pkt));
+  ps.setSafetyParam(health.safety_param_pkt);
+  ps.setFaultStatus(cereal::PandaState::FaultStatus(health.fault_status_pkt));
+  ps.setPowerSaveEnabled((bool)(health.power_save_enabled_pkt));
+  ps.setHeartbeatLost((bool)(health.heartbeat_lost_pkt));
+  ps.setAlternativeExperience(health.alternative_experience_pkt);
+  ps.setHarnessStatus(cereal::PandaState::HarnessStatus(health.car_harness_status_pkt));
+  ps.setInterruptLoad(health.interrupt_load_pkt);
+  ps.setFanPower(health.fan_power);
+  ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid_pkt));
+  ps.setSpiErrorCount(health.spi_error_count_pkt);
+  ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f);
+  ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f);
+}
+
+void fill_panda_can_state(cereal::PandaState::PandaCanState::Builder &cs, const can_health_t &can_health) {
+  cs.setBusOff((bool)can_health.bus_off);
+  cs.setBusOffCnt(can_health.bus_off_cnt);
+  cs.setErrorWarning((bool)can_health.error_warning);
+  cs.setErrorPassive((bool)can_health.error_passive);
+  cs.setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error));
+  cs.setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error));
+  cs.setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error));
+  cs.setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error));
+  cs.setReceiveErrorCnt(can_health.receive_error_cnt);
+  cs.setTransmitErrorCnt(can_health.transmit_error_cnt);
+  cs.setTotalErrorCnt(can_health.total_error_cnt);
+  cs.setTotalTxLostCnt(can_health.total_tx_lost_cnt);
+  cs.setTotalRxLostCnt(can_health.total_rx_lost_cnt);
+  cs.setTotalTxCnt(can_health.total_tx_cnt);
+  cs.setTotalRxCnt(can_health.total_rx_cnt);
+  cs.setTotalFwdCnt(can_health.total_fwd_cnt);
+  cs.setCanSpeed(can_health.can_speed);
+  cs.setCanDataSpeed(can_health.can_data_speed);
+  cs.setCanfdEnabled(can_health.canfd_enabled);
+  cs.setBrsEnabled(can_health.brs_enabled);
+  cs.setCanfdNonIso(can_health.canfd_non_iso);
+  cs.setIrq0CallRate(can_health.irq0_call_rate);
+  cs.setIrq1CallRate(can_health.irq1_call_rate);
+  cs.setIrq2CallRate(can_health.irq2_call_rate);
+  cs.setCanCoreResetCnt(can_health.can_core_reset_cnt);
+}
+
+std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool is_onroad, bool spoofing_started, bool always_offroad) {
+  bool ignition_local = false;
+  const uint32_t pandas_cnt = pandas.size();
+  static Params params;
+  const bool allow_offroad_external_can_tx = params.getBool("Konn3ktAllowOffroadExternalCanTx");
+
+  // build msg
+  MessageBuilder msg;
+  auto evt = msg.initEvent();
+  auto pss = evt.initPandaStates(pandas_cnt);
+
+  std::vector<health_t> pandaStates;
+  pandaStates.reserve(pandas_cnt);
+
+  std::vector<std::array<can_health_t, PANDA_CAN_CNT>> pandaCanStates;
+  pandaCanStates.reserve(pandas_cnt);
+
+  const bool red_panda_comma_three = (pandas.size() == 2) &&
+                                     (pandas[0]->hw_type == cereal::PandaState::PandaType::DOS) &&
+                                     (pandas[1]->hw_type == cereal::PandaState::PandaType::RED_PANDA);
+
+  for (const auto& panda : pandas){
+    auto health_opt = panda->get_state();
+    if (!health_opt) {
+      return std::nullopt;
+    }
+
+    health_t health = *health_opt;
+
+    std::array<can_health_t, PANDA_CAN_CNT> can_health{};
+    for (uint32_t i = 0; i < PANDA_CAN_CNT; i++) {
+      auto can_health_opt = panda->get_can_state(i);
+      if (!can_health_opt) {
+        return std::nullopt;
+      }
+      can_health[i] = *can_health_opt;
+    }
+    pandaCanStates.push_back(can_health);
+
+    if (spoofing_started) {
+      health.ignition_line_pkt = 1;
+    }
+
+    // on comma three setups with a red panda, the dos can
+    // get false positive ignitions due to the harness box
+    // without a harness connector, so ignore it
+    if (red_panda_comma_three && (panda->hw_type == cereal::PandaState::PandaType::DOS)) {
+      health.ignition_line_pkt = 0;
+    }
+
+    // Keep physical ignition detection independent from always_offroad mode.
+    // always_offroad should affect safety/onroad state, not force panda low-power.
+    ignition_local |= ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0));
+
+    pandaStates.push_back(health);
+  }
+
+  for (uint32_t i = 0; i < pandas_cnt; i++) {
+    auto panda = pandas[i];
+    const auto &health = pandaStates[i];
+
+    // Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
+    if (health.safety_mode_pkt == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
+      panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+    }
+
+    const bool allow_external_tx_now = allow_offroad_external_can_tx && !is_onroad;
+
+    bool power_save_desired = allow_external_tx_now ? false : !ignition_local;
+    if (health.power_save_enabled_pkt != power_save_desired) {
+      panda->set_power_saving(power_save_desired);
+    }
+
+    if (allow_external_tx_now && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::ALL_OUTPUT))) {
+      panda->set_safety_model(cereal::CarParams::SafetyModel::ALL_OUTPUT);
+    }
+
+    bool should_close_relay = (!ignition_local || !is_onroad) && !allow_external_tx_now;
+    if (should_close_relay && (health.safety_mode_pkt != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
+      panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+    }
+
+    if (!panda->comms_healthy()) {
+      evt.setValid(false);
+    }
+
+    auto ps = pss[i];
+    fill_panda_state(ps, panda->hw_type, health);
+
+    auto cs = std::array{ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
+    for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) {
+      fill_panda_can_state(cs[j], pandaCanStates[i][j]);
+    }
+
+    // Convert faults bitset to capnp list
+    std::bitset<sizeof(health.faults_pkt) * 8> fault_bits(health.faults_pkt);
+    auto faults = ps.initFaults(fault_bits.count());
+
+    size_t j = 0;
+    for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
+         f <= size_t(cereal::PandaState::FaultType::HEARTBEAT_LOOP_WATCHDOG); f++) {
+      if (fault_bits.test(f)) {
+        faults.set(j, cereal::PandaState::FaultType(f));
+        j++;
+      }
+    }
+  }
+
+  pm->send("pandaStates", msg);
+  return ignition_local;
+}
+
+void send_peripheral_state(Panda *panda, PubMaster *pm) {
+  // build msg
+  MessageBuilder msg;
+  auto evt = msg.initEvent();
+  evt.setValid(panda->comms_healthy());
+
+  auto ps = evt.initPeripheralState();
+  ps.setPandaType(panda->hw_type);
+
+  double read_time = millis_since_boot();
+  ps.setVoltage(Hardware::get_voltage());
+  ps.setCurrent(Hardware::get_current());
+  read_time = millis_since_boot() - read_time;
+  if (read_time > 50) {
+    LOGW("reading hwmon took %lfms", read_time);
+  }
+
+  // fall back to panda's voltage and current measurement
+  if (ps.getVoltage() == 0 && ps.getCurrent() == 0) {
+    auto health_opt = panda->get_state();
+    if (health_opt) {
+      health_t health = *health_opt;
+      ps.setVoltage(health.voltage_pkt);
+      ps.setCurrent(health.current_pkt);
+    }
+  }
+
+  uint16_t fan_speed_rpm = panda->get_fan_speed();
+  ps.setFanSpeedRpm(fan_speed_rpm);
+
+  pm->send("peripheralState", msg);
+}
+
+void process_panda_state(std::vector<Panda *> &pandas, PubMaster *pm, bool engaged, bool engaged_aol, bool is_onroad, bool spoofing_started, bool always_offroad) {
+  std::vector<std::string> connected_serials;
+  for (Panda *p : pandas) {
+    connected_serials.push_back(p->hw_serial());
+  }
+
+  {
+    auto ignition_opt = send_panda_states(pm, pandas, is_onroad, spoofing_started, always_offroad);
+    if (!ignition_opt) {
+      LOGE("Failed to get ignition_opt");
+      return;
+    }
+
+    // check if we should have pandad reconnect
+    if (!ignition_opt.value()) {
+      bool comms_healthy = true;
+      for (const auto &panda : pandas) {
+        comms_healthy &= panda->comms_healthy();
+      }
+
+      if (!comms_healthy) {
+        LOGE("Reconnecting, communication to pandas not healthy");
+        do_exit = true;
+
+      } else {
+        // check for new pandas
+        for (std::string &s : Panda::list(true)) {
+          if (!std::count(connected_serials.begin(), connected_serials.end(), s)) {
+            LOGW("Reconnecting to new panda: %s", s.c_str());
+            do_exit = true;
+            break;
+          }
+        }
+      }
+    }
+
+    for (const auto &panda : pandas) {
+      panda->send_heartbeat(engaged, engaged_aol);
+    }
+  }
+}
+
+void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control) {
+  static Params params;
+  static const bool has_device_state = services.count("deviceState") > 0;
+  static const bool has_driver_camera_state = services.count("driverCameraState") > 0;
+  static std::vector<const char*> sm_services = filter_available_services({"deviceState", "driverCameraState"}, "pandad peripheral");
+  static std::unique_ptr<SubMaster> sm = sm_services.empty() ? nullptr : std::make_unique<SubMaster>(sm_services);
+
+  static uint64_t last_driver_camera_t = 0;
+  static uint16_t prev_fan_speed = 999;
+  static int ir_pwr = 0;
+  static int prev_ir_pwr = 999;
+  static uint32_t prev_frame_id = UINT32_MAX;
+  static bool driver_view = false;
+
+  // TODO: can we merge these?
+  static FirstOrderFilter integ_lines_filter(0, 30.0, 0.05);
+  static FirstOrderFilter integ_lines_filter_driver_view(0, 5.0, 0.05);
+
+  {
+    if (sm != nullptr) {
+      sm->update(0);
+    }
+
+    if (sm != nullptr && has_device_state && sm->updated("deviceState") && !no_fan_control) {
+      // Fan speed
+      uint16_t fan_speed = (*sm)["deviceState"].getDeviceState().getFanSpeedPercentDesired();
+      if (fan_speed != prev_fan_speed || sm->frame % 100 == 0) {
+        panda->set_fan_speed(fan_speed);
+        prev_fan_speed = fan_speed;
+      }
+    }
+
+    if (sm != nullptr && has_driver_camera_state && sm->updated("driverCameraState")) {
+      auto event = (*sm)["driverCameraState"];
+      int cur_integ_lines = event.getDriverCameraState().getIntegLines();
+
+      // reset the filter when camerad restarts
+      if (event.getDriverCameraState().getFrameId() < prev_frame_id) {
+        integ_lines_filter.reset(0);
+        integ_lines_filter_driver_view.reset(0);
+        driver_view = params.getBool("IsDriverViewEnabled");
+      }
+      prev_frame_id = event.getDriverCameraState().getFrameId();
+
+      cur_integ_lines = (driver_view ? integ_lines_filter_driver_view : integ_lines_filter).update(cur_integ_lines);
+      last_driver_camera_t = event.getLogMonoTime();
+
+      if (cur_integ_lines <= CUTOFF_IL) {
+        ir_pwr = 0;
+      } else if (cur_integ_lines > SATURATE_IL) {
+        ir_pwr = 100;
+      } else {
+        ir_pwr = 100 * (cur_integ_lines - CUTOFF_IL) / (SATURATE_IL - CUTOFF_IL);
+      }
+    }
+
+    // Disable IR on input timeout
+    if (nanos_since_boot() - last_driver_camera_t > 1e9) {
+      ir_pwr = 0;
+    }
+
+    int frame = sm != nullptr ? sm->frame : 0;
+    if (ir_pwr != prev_ir_pwr || frame % 100 == 0) {
+      int16_t ir_panda = util::map_val(ir_pwr, 0, 100, 0, MAX_IR_PANDA_VAL);
+      panda->set_ir_pwr(ir_panda);
+      Hardware::set_ir_power(ir_pwr);
+      prev_ir_pwr = ir_pwr;
+    }
+  }
+}
+
+void pandad_run(std::vector<Panda *> &pandas) {
+  const bool no_fan_control = getenv("NO_FAN_CONTROL") != nullptr;
+  const bool spoofing_started = getenv("STARTED") != nullptr;
+  const bool fake_send = getenv("FAKESEND") != nullptr;
+
+  // Start the CAN send thread
+  std::thread send_thread(can_send_thread, pandas, fake_send);
+
+  Params params;
+  RateKeeper rk("pandad", 100);
+  const bool has_car_params_service = services.count("carParams") > 0;
+  const bool has_selfdrive_state_service = services.count("selfdriveState") > 0;
+  const bool has_iq_state_service = services.count("iqState") > 0;
+  std::vector<const char*> sm_services = filter_available_services({"selfdriveState", "carParams", "iqState"}, "pandad main");
+  std::unique_ptr<SubMaster> sm = sm_services.empty() ? nullptr : std::make_unique<SubMaster>(sm_services);
+  if (!has_iq_state_service) {
+    LOGW("iqState service not found in cereal services map; steering guidance heartbeat disabled");
+  }
+  PubMaster pm({"can", "pandaStates", "peripheralState"});
+  PandaSafety panda_safety(pandas);
+  Panda *peripheral_panda = pandas[0];
+  bool engaged = false;
+  bool engaged_aol = false;
+  bool is_onroad = false;
+  bool always_offroad = false;
+
+  // Main loop: receive CAN data and process states
+  while (!do_exit && check_all_connected(pandas)) {
+    can_recv(pandas, &pm);
+
+    // Process peripheral state at 20 Hz
+    if (rk.frame() % 5 == 0) {
+      process_peripheral_state(peripheral_panda, &pm, no_fan_control);
+    }
+
+    // Process panda state at 10 Hz
+    if (rk.frame() % 10 == 0) {
+      if (sm != nullptr) {
+        sm->update(0);
+      }
+      engaged = (sm != nullptr) && has_selfdrive_state_service &&
+                sm->allAliveAndValid({"selfdriveState"}) && (*sm)["selfdriveState"].getSelfdriveState().getEnabled();
+      engaged_aol = (sm != nullptr) ? process_aol_heartbeat(sm.get(), has_iq_state_service, has_car_params_service) : false;
+      is_onroad = params.getBool("IsOnroad");
+      always_offroad = panda_safety.getOffroadMode();
+      process_panda_state(pandas, &pm, engaged, engaged_aol, is_onroad, spoofing_started, always_offroad);
+      panda_safety.configureSafetyMode(is_onroad);
+    }
+
+    // Send out peripheralState at 2Hz
+    if (rk.frame() % 50 == 0) {
+      send_peripheral_state(peripheral_panda, &pm);
+    }
+
+    // Forward logs from pandas to cloudlog if available
+    for (auto *panda : pandas) {
+      std::string log = panda->serial_read();
+      if (!log.empty()) {
+        if (log.find("Register 0x") != std::string::npos) {
+          // Log register divergent faults as errors
+          LOGE("%s", log.c_str());
+        } else {
+          LOGD("%s", log.c_str());
+        }
+      }
+    }
+
+    rk.keepTime();
+  }
+
+  // Close relay on exit to prevent a fault
+  if (is_onroad && !engaged) {
+    for (auto &p : pandas) {
+      if (p->connected()) {
+        p->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+      }
+    }
+  }
+
+  send_thread.join();
+}
+
+void pandad_main_thread(std::vector<std::string> serials) {
+  if (serials.size() == 0) {
+    serials = Panda::list();
+
+    if (serials.size() == 0) {
+      LOGW("no pandas found, exiting");
+      return;
+    }
+  }
+
+  std::string serials_str;
+  for (int i = 0; i < serials.size(); i++) {
+    serials_str += serials[i];
+    if (i < serials.size() - 1) serials_str += ", ";
+  }
+  LOGW("connecting to pandas: %s", serials_str.c_str());
+
+  // connect to all provided serials
+  std::vector<Panda *> pandas;
+  for (int i = 0; i < serials.size() && !do_exit; /**/) {
+    Panda *p = connect(serials[i], i);
+    if (!p) {
+      util::sleep_for(100);
+      continue;
+    }
+
+    pandas.push_back(p);
+    ++i;
+  }
+
+  if (!do_exit) {
+    LOGW("connected to all pandas");
+    pandad_run(pandas);
+  }
+
+  for (Panda *panda : pandas) {
+    delete panda;
+  }
+}

iqpilot/selfdrive/pandad/pandad.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "common/params.h"
+#include "selfdrive/pandad/panda.h"
+
+void pandad_main_thread(std::vector<std::string> serials);
+
+// deprecated devices
+static const std::vector<cereal::PandaState::PandaType> SUPPORTED_PANDA_TYPES = {
+  cereal::PandaState::PandaType::RED_PANDA,
+  cereal::PandaState::PandaType::TRES,
+  cereal::PandaState::PandaType::CUATRO,
+};
+
+
+class PandaSafety {
+public:
+  PandaSafety(const std::vector<Panda *> &pandas) : pandas_(pandas) {}
+  void configureSafetyMode(bool is_onroad);
+  bool getOffroadMode();
+
+private:
+  void updateMultiplexingMode();
+  std::vector<std::string> fetchCarParams();
+  void setSafetyMode(const std::vector<std::string> &params_string);
+
+  bool initialized_ = false;
+  bool log_once_ = false;
+  bool safety_configured_ = false;
+  bool prev_obd_multiplexing_ = false;
+  std::vector<Panda *> pandas_;
+  Params params_;
+};

iqpilot/selfdrive/pandad/pandad.py

@@ -0,0 +1,197 @@
+#!/usr/bin/env python3
+# simple pandad wrapper that updates the panda first
+import os
+import usb1
+import time
+import signal
+import subprocess
+
+from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
+from iqpilot.common.basedir import BASEDIR
+from iqpilot.common.params import Params
+from iqpilot.system.hardware import HARDWARE
+from iqpilot.common.swaglog import cloudlog
+
+
+def get_expected_signature(panda: Panda) -> bytes:
+  try:
+    fn = os.path.join(FW_PATH, panda.get_mcu_type().config.app_fn)
+    return Panda.get_signature_from_firmware(fn)
+  except Exception:
+    cloudlog.exception("Error computing expected signature")
+    return b""
+
+def flash_panda(panda_serial: str) -> Panda:
+  try:
+    panda = Panda(panda_serial)
+  except PandaProtocolMismatch:
+    cloudlog.warning("detected protocol mismatch, reflashing panda")
+    HARDWARE.recover_internal_panda()
+    raise
+
+  # skip flashing if the detected panda is not supported
+  supported_panda = check_panda_support(panda)
+  if not supported_panda:
+    cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {panda.get_type()}), skipping flash...")
+    return panda
+
+  fw_signature = get_expected_signature(panda)
+  internal_panda = panda.is_internal()
+
+  panda_version = "bootstub" if panda.bootstub else panda.get_version()
+  panda_signature = b"" if panda.bootstub else panda.get_signature()
+  cloudlog.warning(f"Panda {panda_serial} connected, version: {panda_version}, signature {panda_signature.hex()[:16]}, expected {fw_signature.hex()[:16]}")
+
+  if panda.bootstub or panda_signature != fw_signature:
+    cloudlog.info("Panda firmware out of date, update required")
+    panda.flash()
+    cloudlog.info("Done flashing")
+
+  if panda.bootstub:
+    bootstub_version = panda.get_version()
+    cloudlog.info(f"Flashed firmware not booting, flashing development bootloader. {bootstub_version=}, {internal_panda=}")
+    if internal_panda:
+      HARDWARE.recover_internal_panda()
+    panda.recover(reset=(not internal_panda))
+    cloudlog.info("Done flashing bootstub")
+
+  if panda.bootstub:
+    cloudlog.info("Panda still not booting, exiting")
+    raise AssertionError
+
+  panda_signature = panda.get_signature()
+  if panda_signature != fw_signature:
+    cloudlog.info("Version mismatch after flashing, exiting")
+    raise AssertionError
+
+  return panda
+
+
+def check_panda_support(panda) -> bool:
+  hw_type = panda.get_type()
+  if hw_type in Panda.SUPPORTED_DEVICES:
+    return True
+  if hw_type == Panda.HW_TYPE_UNKNOWN and (os.environ.get('LITE') == '1' or os.path.exists('/tmp/lite_hw')):
+    return True
+  return False
+
+
+def main() -> None:
+  # signal pandad to close the relay and exit
+  def signal_handler(signum, frame):
+    cloudlog.info(f"Caught signal {signum}, exiting")
+    nonlocal do_exit
+    do_exit = True
+    if process is not None:
+      process.send_signal(signal.SIGINT)
+
+  process = None
+  do_exit = False
+  signal.signal(signal.SIGINT, signal_handler)
+
+  count = 0
+  first_run = True
+  params = Params()
+  no_internal_panda_count = 0
+
+  while not do_exit:
+    try:
+      count += 1
+      cloudlog.event("pandad.flash_and_connect", count=count)
+      params.remove("PandaSignatures")
+
+      # Handle missing internal panda
+      if time.monotonic() < 60.:
+        no_internal_panda_count = 0
+      if no_internal_panda_count > 0:
+        if no_internal_panda_count == 3:
+          cloudlog.info("No pandas found, putting internal panda into DFU")
+          HARDWARE.recover_internal_panda()
+        else:
+          cloudlog.info("No pandas found, resetting internal panda")
+          HARDWARE.reset_internal_panda()
+        time.sleep(3)  # wait to come back up
+
+      # Flash all Pandas in DFU mode
+      dfu_serials = PandaDFU.list()
+      if len(dfu_serials) > 0:
+        for serial in dfu_serials:
+          cloudlog.info(f"Panda in DFU mode found, flashing recovery {serial}")
+          PandaDFU(serial).recover()
+        time.sleep(1)
+
+      panda_serials = Panda.list()
+      if len(panda_serials) == 0:
+        no_internal_panda_count += 1
+        continue
+
+      cloudlog.info(f"{len(panda_serials)} panda(s) found, connecting - {panda_serials}")
+
+      # Flash pandas
+      pandas: list[Panda] = []
+      for serial in panda_serials:
+        pandas.append(flash_panda(serial))
+
+      # Ensure internal panda is present if expected
+      internal_pandas = [panda for panda in pandas if panda.is_internal()]
+      if HARDWARE.has_internal_panda() and len(internal_pandas) == 0:
+        cloudlog.error("Internal panda is missing, trying again")
+        no_internal_panda_count += 1
+        continue
+      no_internal_panda_count = 0
+
+      # sort pandas to have deterministic order
+      # * the internal one is always first
+      # * then sort by hardware type
+      # * as a last resort, sort by serial number
+      pandas.sort(key=lambda x: (not x.is_internal(), x.get_type(), x.get_usb_serial()))
+      panda_serials = [p.get_usb_serial() for p in pandas]
+
+      # log panda fw versions
+      params.put("PandaSignatures", b','.join(p.get_signature() for p in pandas))
+
+      for panda in pandas:
+        # skip health check if the detected panda is not supported
+        supported_panda = check_panda_support(panda)
+        if not supported_panda:
+          cloudlog.warning(f"Panda {panda.get_usb_serial()} is not supported (hw_type: {panda.get_type()}), skipping health check...")
+          continue
+
+        # check health for lost heartbeat
+        health = panda.health()
+        if health["heartbeat_lost"]:
+          params.put_bool("PandaHeartbeatLost", True)
+          cloudlog.event("heartbeat lost", deviceState=health, serial=panda.get_usb_serial())
+        if health["som_reset_triggered"]:
+          params.put_bool("PandaSomResetTriggered", True)
+          cloudlog.event("panda.som_reset_triggered", health=health, serial=panda.get_usb_serial())
+
+        if first_run:
+          # reset panda to ensure we're in a good state
+          cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
+          panda.reset(reconnect=True)
+
+      for p in pandas:
+        p.close()
+    # TODO: wrap all panda exceptions in a base panda exception
+    except (usb1.USBErrorNoDevice, usb1.USBErrorPipe):
+      # a panda was disconnected while setting everything up. let's try again
+      cloudlog.exception("Panda USB exception while setting up")
+      continue
+    except PandaProtocolMismatch:
+      cloudlog.exception("pandad.protocol_mismatch")
+      continue
+    except Exception:
+      cloudlog.exception("pandad.uncaught_exception")
+      continue
+
+    first_run = False
+
+    # run pandad with all connected serials as arguments
+    os.environ['MANAGER_DAEMON'] = 'pandad'
+    process = subprocess.Popen(["./pandad", *panda_serials], cwd=os.path.join(BASEDIR, "iqpilot/selfdrive/pandad"))
+    process.wait()
+
+
+if __name__ == "__main__":
+  main()

iqpilot/selfdrive/pandad/pandad_api_impl.py

@@ -0,0 +1,88 @@
+import time
+from iqpilot.cereal import log
+
+NO_TRAVERSAL_LIMIT = 2**64 - 1
+
+# Cache schema fields for faster access (avoids string lookup on each field access)
+_cached_reader_fields = None  # (address_field, dat_field, src_field) for reading
+_cached_writer_fields = None  # (address_field, dat_field, src_field) for writing
+
+
+def _get_reader_fields(schema):
+  """Get cached schema field objects for reading."""
+  global _cached_reader_fields
+  if _cached_reader_fields is None:
+    fields = schema.fields
+    _cached_reader_fields = (fields['address'], fields['dat'], fields['src'])
+  return _cached_reader_fields
+
+
+def _get_writer_fields(schema):
+  """Get cached schema field objects for writing."""
+  global _cached_writer_fields
+  if _cached_writer_fields is None:
+    fields = schema.fields
+    _cached_writer_fields = (fields['address'], fields['dat'], fields['src'])
+  return _cached_writer_fields
+
+
+def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
+  """Convert list of CAN messages to Cap'n Proto serialized bytes.
+
+  Args:
+    can_msgs: List of tuples [(address, data_bytes, src), ...]
+    msgtype: 'can' or 'sendcan'
+    valid: Whether the event is valid
+
+  Returns:
+    Cap'n Proto serialized bytes
+  """
+  global _cached_writer_fields
+
+  dat = log.Event.new_message(valid=valid, logMonoTime=int(time.monotonic() * 1e9))
+  can_data = dat.init(msgtype, len(can_msgs))
+
+  # Cache schema fields on first call
+  if _cached_writer_fields is None and len(can_msgs) > 0:
+    _cached_writer_fields = _get_writer_fields(can_data[0].schema)
+
+  if _cached_writer_fields is not None:
+    addr_f, dat_f, src_f = _cached_writer_fields
+    for i, msg in enumerate(can_msgs):
+      f = can_data[i]
+      f._set_by_field(addr_f, msg[0])
+      f._set_by_field(dat_f, msg[1])
+      f._set_by_field(src_f, msg[2])
+
+  return dat.to_bytes()
+
+
+def can_capnp_to_list(strings, msgtype='can'):
+  """Convert Cap'n Proto serialized bytes to list of CAN messages.
+
+  Args:
+    strings: Tuple/list of serialized Cap'n Proto bytes
+    msgtype: 'can' or 'sendcan'
+
+  Returns:
+    List of tuples [(nanos, [(address, data, src), ...]), ...]
+  """
+  global _cached_reader_fields
+  result = []
+
+  for s in strings:
+    with log.Event.from_bytes(s, traversal_limit_in_words=NO_TRAVERSAL_LIMIT) as event:
+      frames = getattr(event, msgtype)
+
+      # Cache schema fields on first frame for faster access
+      if _cached_reader_fields is None and len(frames) > 0:
+        _cached_reader_fields = _get_reader_fields(frames[0].schema)
+
+      if _cached_reader_fields is not None:
+        addr_f, dat_f, src_f = _cached_reader_fields
+        frame_list = [(f._get_by_field(addr_f), f._get_by_field(dat_f), f._get_by_field(src_f)) for f in frames]
+      else:
+        frame_list = []
+
+      result.append((event.logMonoTime, frame_list))
+  return result

iqpilot/selfdrive/pandad/spi.cc

@@ -0,0 +1,410 @@
+#ifndef __APPLE__
+#include <sys/file.h>
+#include <sys/ioctl.h>
+#include <linux/spi/spidev.h>
+
+#include <cassert>
+#include <cmath>
+#include <cstring>
+#include <iomanip>
+#include <sstream>
+
+#include "common/util.h"
+#include "common/timing.h"
+#include "common/swaglog.h"
+#include "panda/board/comms_definitions.h"
+#include "selfdrive/pandad/panda_comms.h"
+
+
+#define SPI_SYNC 0x5AU
+#define SPI_HACK 0x79U
+#define SPI_DACK 0x85U
+#define SPI_NACK 0x1FU
+#define SPI_CHECKSUM_START 0xABU
+
+
+enum SpiError {
+  NACK = -2,
+  ACK_TIMEOUT = -3,
+};
+
+const unsigned int SPI_ACK_TIMEOUT = 500; // milliseconds
+const std::string SPI_DEVICE = "/dev/spidev0.0";
+
+class LockEx {
+public:
+  LockEx(int fd, std::recursive_mutex &m) : fd(fd), m(m) {
+    m.lock();
+    flock(fd, LOCK_EX);
+  }
+
+  ~LockEx() {
+    flock(fd, LOCK_UN);
+    m.unlock();
+  }
+
+private:
+  int fd;
+  std::recursive_mutex &m;
+};
+
+#define SPILOG(fn, fmt, ...) do {  \
+      fn(fmt, ## __VA_ARGS__);     \
+      fn("  %d / 0x%x / %d / %d / tx: %s", \
+         xfer_count, header.endpoint, header.tx_len, header.max_rx_len, \
+         util::hexdump(tx_buf, std::min((int)header.tx_len, 8)).c_str()); \
+      } while (0)
+
+PandaSpiHandle::PandaSpiHandle(std::string serial) : PandaCommsHandle(serial) {
+  int ret;
+  const int uid_len = 12;
+  uint8_t uid[uid_len] = {0};
+
+  uint32_t spi_mode = SPI_MODE_0;
+  uint8_t spi_bits_per_word = 8;
+
+  // 50MHz is the max of the 845. note that some older
+  // revs of the comma three may not support this speed
+  uint32_t spi_speed = 50000000;
+  try {
+    if (!util::file_exists(SPI_DEVICE)) {
+      throw std::runtime_error("Error connecting to panda: SPI device not found");
+    }
+
+    spi_fd = open(SPI_DEVICE.c_str(), O_RDWR);
+    if (spi_fd < 0) {
+      LOGE("failed opening SPI device %d", spi_fd);
+      throw std::runtime_error("Error connecting to panda: failed to open SPI device");
+    }
+
+    // SPI settings
+    util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode, "failed setting SPI mode");
+    util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed, "failed setting SPI speed");
+    util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word, "failed setting SPI bits per word");
+
+    // get hw UID/serial
+    ret = control_read(0xc3, 0, 0, uid, uid_len, 100);
+    if (ret == uid_len) {
+      std::stringstream stream;
+      for (int i = 0; i < uid_len; i++) {
+        stream << std::hex << std::setw(2) << std::setfill('0') << int(uid[i]);
+      }
+      hw_serial = stream.str();
+    } else {
+      LOGD("failed to get serial %d", ret);
+      throw std::runtime_error("Error connecting to panda: failed to get serial");
+    }
+
+    if (!serial.empty() && (serial != hw_serial)) {
+      throw std::runtime_error("Error connecting to panda: serial mismatch");
+    }
+
+  } catch (...) {
+    cleanup();
+    throw;
+  }
+  return;
+}
+
+PandaSpiHandle::~PandaSpiHandle() {
+  std::lock_guard lk(hw_lock);
+  cleanup();
+}
+
+void PandaSpiHandle::cleanup() {
+  if (spi_fd != -1) {
+    close(spi_fd);
+    spi_fd = -1;
+  }
+}
+
+
+
+int PandaSpiHandle::control_write(uint8_t request, uint16_t param1, uint16_t param2, unsigned int timeout) {
+  ControlPacket_t packet = {
+    .request = request,
+    .param1 = param1,
+    .param2 = param2,
+    .length = 0
+  };
+  return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), NULL, 0, timeout);
+}
+
+int PandaSpiHandle::control_read(uint8_t request, uint16_t param1, uint16_t param2, unsigned char *data, uint16_t length, unsigned int timeout) {
+  ControlPacket_t packet = {
+    .request = request,
+    .param1 = param1,
+    .param2 = param2,
+    .length = length
+  };
+  return spi_transfer_retry(0, (uint8_t *) &packet, sizeof(packet), data, length, timeout);
+}
+
+int PandaSpiHandle::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
+  return bulk_transfer(endpoint, data, length, NULL, 0, timeout);
+}
+int PandaSpiHandle::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
+  return bulk_transfer(endpoint, NULL, 0, data, length, timeout);
+}
+
+int PandaSpiHandle::bulk_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t rx_len, unsigned int timeout) {
+  const int xfer_size = SPI_BUF_SIZE - 0x40;
+
+  int ret = 0;
+  uint16_t length = (tx_data != NULL) ? tx_len : rx_len;
+  for (int i = 0; i < (int)std::ceil((float)length / xfer_size); i++) {
+    int d;
+    if (tx_data != NULL) {
+      int len = std::min(xfer_size, tx_len - (xfer_size * i));
+      d = spi_transfer_retry(endpoint, tx_data + (xfer_size * i), len, NULL, 0, timeout);
+    } else {
+      uint16_t to_read = std::min(xfer_size, rx_len - ret);
+      d = spi_transfer_retry(endpoint, NULL, 0, rx_data + (xfer_size * i), to_read, timeout);
+    }
+
+    if (d < 0) {
+      SPILOG(LOGE, "SPI: bulk transfer failed with %d", d);
+      comms_healthy = false;
+      return d;
+    }
+
+    ret += d;
+    if ((rx_data != NULL) && d < xfer_size) {
+      break;
+    }
+  }
+
+  return ret;
+}
+
+std::vector<std::string> PandaSpiHandle::list() {
+  try {
+    PandaSpiHandle sh("");
+    return {sh.hw_serial};
+  } catch (std::exception &e) {
+    // no panda on SPI
+  }
+  return {};
+}
+
+void add_checksum(uint8_t *data, int data_len) {
+  data[data_len] = SPI_CHECKSUM_START;
+  for (int i=0; i < data_len; i++) {
+    data[data_len] ^= data[i];
+  }
+}
+
+bool check_checksum(uint8_t *data, int data_len) {
+  uint8_t checksum = SPI_CHECKSUM_START;
+  for (uint16_t i = 0U; i < data_len; i++) {
+    checksum ^= data[i];
+  }
+  return checksum == 0U;
+}
+
+
+int PandaSpiHandle::spi_transfer_retry(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) {
+  int ret;
+  int nack_count = 0;
+  int timeout_count = 0;
+  bool timed_out = false;
+  double start_time = millis_since_boot();
+
+  do {
+    ret = spi_transfer(endpoint, tx_data, tx_len, rx_data, max_rx_len, timeout);
+
+    if (ret < 0) {
+      timed_out = (timeout != 0) && (timeout_count > 5);
+      timeout_count += ret == SpiError::ACK_TIMEOUT;
+
+      // give other threads a chance to run
+      std::this_thread::yield();
+
+      if (ret == SpiError::NACK) {
+        // prevent busy waiting while the panda is NACK'ing
+        // due to full TX buffers
+        nack_count += 1;
+        if (nack_count > 3) {
+          SPILOG(LOGD, "NACK sleep %d", nack_count);
+          usleep(std::clamp(nack_count*10, 200, 2000));
+        }
+      }
+    }
+  } while (ret < 0 && connected && !timed_out);
+
+  if (ret < 0) {
+    SPILOG(LOGE, "transfer failed, after %d tries, %.2fms", timeout_count, millis_since_boot() - start_time);
+  }
+
+  return ret;
+}
+
+int PandaSpiHandle::wait_for_ack(uint8_t ack, uint8_t tx, unsigned int timeout, unsigned int length) {
+  double start_millis = millis_since_boot();
+  if (timeout == 0) {
+    timeout = SPI_ACK_TIMEOUT;
+  }
+  timeout = std::clamp(timeout, 20U, SPI_ACK_TIMEOUT);
+
+  spi_ioc_transfer transfer = {
+    .tx_buf = (uint64_t)tx_buf,
+    .rx_buf = (uint64_t)rx_buf,
+    .len = length,
+  };
+  memset(tx_buf, tx, length);
+
+  while (true) {
+    int ret = lltransfer(transfer);
+    if (ret < 0) {
+      SPILOG(LOGE, "SPI: failed to send ACK request");
+      return ret;
+    }
+
+    if (rx_buf[0] == ack) {
+      break;
+    } else if (rx_buf[0] == SPI_NACK) {
+      SPILOG(LOGD, "SPI: got NACK, waiting for 0x%x", ack);
+      return SpiError::NACK;
+    }
+
+    // handle timeout
+    if (millis_since_boot() - start_millis > timeout) {
+      SPILOG(LOGW, "SPI: timed out waiting for ACK, waiting for 0x%x", ack);
+      return SpiError::ACK_TIMEOUT;
+    }
+  }
+
+  return 0;
+}
+
+int PandaSpiHandle::lltransfer(spi_ioc_transfer &t) {
+  static const double err_prob = std::stod(util::getenv("SPI_ERR_PROB", "-1"));
+
+  if (err_prob > 0) {
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob) {
+      printf("transfer len error\n");
+      t.len = rand() % SPI_BUF_SIZE;
+    }
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.tx_buf != (uint64_t)NULL) {
+      printf("corrupting TX\n");
+      for (int i = 0; i < t.len; i++) {
+        if ((static_cast<double>(rand()) / RAND_MAX) > 0.9) {
+          ((uint8_t*)t.tx_buf)[i] = (uint8_t)(rand() % 256);
+        }
+      }
+    }
+  }
+
+  int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &t);
+
+  if (err_prob > 0) {
+    if ((static_cast<double>(rand()) / RAND_MAX) < err_prob && t.rx_buf != (uint64_t)NULL) {
+      printf("corrupting RX\n");
+      for (int i = 0; i < t.len; i++) {
+        if ((static_cast<double>(rand()) / RAND_MAX) > 0.9) {
+          ((uint8_t*)t.rx_buf)[i] = (uint8_t)(rand() % 256);
+        }
+      }
+    }
+  }
+
+  return ret;
+}
+
+int PandaSpiHandle::spi_transfer(uint8_t endpoint, uint8_t *tx_data, uint16_t tx_len, uint8_t *rx_data, uint16_t max_rx_len, unsigned int timeout) {
+  int ret;
+  uint16_t rx_data_len;
+  LockEx lock(spi_fd, hw_lock);
+
+  // needs to be less, since we need to have space for the checksum
+  assert(tx_len < SPI_BUF_SIZE);
+  assert(max_rx_len < SPI_BUF_SIZE);
+
+  xfer_count++;
+  header = {
+    .sync = SPI_SYNC,
+    .endpoint = endpoint,
+    .tx_len = tx_len,
+    .max_rx_len = max_rx_len
+  };
+
+  spi_ioc_transfer transfer = {
+    .tx_buf = (uint64_t)tx_buf,
+    .rx_buf = (uint64_t)rx_buf
+  };
+
+  // Send header
+  memcpy(tx_buf, &header, sizeof(header));
+  add_checksum(tx_buf, sizeof(header));
+  transfer.len = sizeof(header) + 1;
+  ret = lltransfer(transfer);
+  if (ret < 0) {
+    SPILOG(LOGE, "SPI: failed to send header");
+    goto fail;
+  }
+
+  // Wait for (N)ACK
+  ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // Send data
+  if (tx_data != NULL) {
+    memcpy(tx_buf, tx_data, tx_len);
+  }
+  add_checksum(tx_buf, tx_len);
+  transfer.len = tx_len + 1;
+  ret = lltransfer(transfer);
+  if (ret < 0) {
+    SPILOG(LOGE, "SPI: failed to send data");
+    goto fail;
+  }
+
+  // Wait for (N)ACK
+  ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3);
+  if (ret < 0) {
+    goto fail;
+  }
+
+  // Read data
+  rx_data_len = *(uint16_t *)(rx_buf+1);
+  if (rx_data_len >= SPI_BUF_SIZE) {
+    SPILOG(LOGE, "SPI: RX data len larger than buf size %d", rx_data_len);
+    goto fail;
+  }
+
+  transfer.len = rx_data_len + 1;
+  transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1);
+  ret = lltransfer(transfer);
+  if (ret < 0) {
+    SPILOG(LOGE, "SPI: failed to read rx data");
+    goto fail;
+  }
+  if (!check_checksum(rx_buf, rx_data_len + 4)) {
+    SPILOG(LOGE, "SPI: bad checksum");
+    goto fail;
+  }
+
+  if (rx_data != NULL) {
+    memcpy(rx_data, rx_buf + 3, rx_data_len);
+  }
+
+  return rx_data_len;
+
+fail:
+  // ensure slave is in a consistent state
+  // and ready for the next transfer
+  int nack_cnt = 0;
+  while (nack_cnt < 3) {
+    if (wait_for_ack(SPI_NACK, 0x14, 1, SPI_BUF_SIZE/2) == 0) {
+      nack_cnt += 1;
+    } else {
+      nack_cnt = 0;
+    }
+  }
+
+  if (ret >= 0) ret = -1;
+  return ret;
+}
+#endif

iqpilot/selfdrive/pandad/tests/test_pandad.py

@@ -0,0 +1,114 @@
+import os
+import pytest
+import time
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import log
+from iqpilot.common.gpio import gpio_set, gpio_init
+from panda import Panda, PandaDFU
+from iqpilot.system.manager.process_config import managed_processes
+from iqpilot.system.hardware import HARDWARE
+from iqpilot.system.hardware.tici.pins import GPIO
+
+HERE = os.path.dirname(os.path.realpath(__file__))
+
+
+@pytest.mark.tici
+class TestPandad:
+
+  def setup_method(self):
+    # ensure panda is up
+    if len(Panda.list()) == 0:
+      self._run_test(60)
+
+  def teardown_method(self):
+    managed_processes['pandad'].stop()
+
+  def _run_test(self, timeout=30) -> float:
+    st = time.monotonic()
+    sm = messaging.SubMaster(['pandaStates'])
+
+    managed_processes['pandad'].start()
+    while (time.monotonic() - st) < timeout:
+      sm.update(100)
+      if len(sm['pandaStates']) and sm['pandaStates'][0].pandaType != log.PandaState.PandaType.unknown:
+        break
+    dt = time.monotonic() - st
+    managed_processes['pandad'].stop()
+
+    if len(sm['pandaStates']) == 0 or sm['pandaStates'][0].pandaType == log.PandaState.PandaType.unknown:
+      raise Exception("pandad failed to start")
+
+    return dt
+
+  def _go_to_dfu(self):
+    HARDWARE.recover_internal_panda()
+    assert Panda.wait_for_dfu(None, 10)
+
+  def _assert_no_panda(self):
+    assert not Panda.wait_for_dfu(None, 3)
+    assert not Panda.wait_for_panda(None, 3)
+
+  def _flash_bootstub(self, fn):
+    self._go_to_dfu()
+    pd = PandaDFU(None)
+    if fn is None:
+      fn = os.path.join(HERE, pd.get_mcu_type().config.bootstub_fn)
+    with open(fn, "rb") as f:
+      pd.program_bootstub(f.read())
+    pd.reset()
+    HARDWARE.reset_internal_panda()
+
+  def test_in_dfu(self):
+    HARDWARE.recover_internal_panda()
+    self._run_test(60)
+
+  def test_in_bootstub(self):
+    with Panda() as p:
+      p.reset(enter_bootstub=True)
+      assert p.bootstub
+    self._run_test()
+
+  def test_internal_panda_reset(self):
+    gpio_init(GPIO.STM_RST_N, True)
+    gpio_set(GPIO.STM_RST_N, 1)
+    time.sleep(0.5)
+    assert all(not Panda(s).is_internal() for s in Panda.list())
+    self._run_test()
+
+    assert any(Panda(s).is_internal() for s in Panda.list())
+
+  def test_best_case_startup_time(self):
+    # run once so we're up to date
+    self._run_test(60)
+
+    ts = []
+    for _ in range(10):
+      # should be nearly instant this time
+      dt = self._run_test(5)
+      ts.append(dt)
+
+    # 5s for USB (due to enumeration)
+    # - 0.2s pandad -> pandad
+    # - plus some buffer
+    print("startup times", ts, sum(ts) / len(ts))
+    assert 0.1 < (sum(ts)/len(ts)) < 0.7
+
+  def test_old_spi_protocol(self):
+    # flash firmware with old SPI protocol
+    self._flash_bootstub(os.path.join(HERE, "bootstub.panda_h7_spiv0.bin"))
+    self._run_test(45)
+
+  def test_release_to_devel_bootstub(self):
+    self._flash_bootstub(None)
+    self._run_test(45)
+
+  def test_recover_from_bad_bootstub(self):
+    self._go_to_dfu()
+    with PandaDFU(None) as pd:
+      pd.program_bootstub(b"\x00"*1024)
+      pd.reset()
+    HARDWARE.reset_internal_panda()
+    self._assert_no_panda()
+
+    self._run_test(60)

iqpilot/selfdrive/pandad/tests/test_pandad_loopback.py

@@ -0,0 +1,113 @@
+import os
+import copy
+import random
+import time
+import pytest
+from collections import defaultdict
+from pprint import pprint
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal import car, log
+from iqdbc.car.can_definitions import CanData
+from iqpilot.common.utils import retry
+from iqpilot.common.params import Params
+from iqpilot.common.timeout import Timeout
+from iqpilot.selfdrive.pandad import can_list_to_can_capnp
+from iqpilot.system.hardware import TICI
+from iqpilot.selfdrive.test.helpers import with_processes
+
+
+@retry(attempts=3)
+def setup_pandad(num_pandas):
+  params = Params()
+  params.clear_all()
+  params.put_bool("IsOnroad", False)
+
+  sm = messaging.SubMaster(['pandaStates'])
+  with Timeout(90, "pandad didn't start"):
+    while sm.recv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \
+        any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']):
+      sm.update(1000)
+
+  found_pandas = len(sm['pandaStates'])
+  assert num_pandas == found_pandas, "connected pandas ({found_pandas}) doesn't match expected panda count ({num_pandas}). \
+                                      connect another panda for multipanda tests."
+
+  # pandad safety setting relies on these params
+  cp = car.CarParams.new_message()
+
+  safety_config = car.CarParams.SafetyConfig.new_message()
+  safety_config.safetyModel = car.CarParams.SafetyModel.allOutput
+  cp.safetyConfigs = [safety_config]*num_pandas
+
+  params.put_bool("IsOnroad", True)
+  params.put_bool("FirmwareQueryDone", True)
+  params.put_bool("ControlsReady", True)
+  params.put("CarParams", cp.to_bytes())
+
+  with Timeout(90, "pandad didn't set safety mode"):
+    while any(ps.safetyModel != car.CarParams.SafetyModel.allOutput for ps in sm['pandaStates']):
+      sm.update(1000)
+
+def send_random_can_messages(sendcan, count, num_pandas=1):
+  sent_msgs = defaultdict(set)
+  for _ in range(count):
+    to_send = []
+    for __ in range(random.randrange(20)):
+      bus = random.choice([b for b in range(3*num_pandas) if b % 4 != 3])
+      addr = random.randrange(1, 1<<29)
+      dat = bytes(random.getrandbits(8) for _ in range(random.randrange(1, 9)))
+      if (addr, dat) in sent_msgs[bus]:
+        continue
+      sent_msgs[bus].add((addr, dat))
+      to_send.append(CanData(addr, dat, bus))
+    sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
+  return sent_msgs
+
+
+@pytest.mark.tici
+class TestBoarddLoopback:
+  @classmethod
+  def setup_class(cls):
+    os.environ['STARTED'] = '1'
+    os.environ['BOARDD_LOOPBACK'] = '1'
+
+  @with_processes(['pandad'])
+  def test_loopback(self):
+    num_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1
+    setup_pandad(num_pandas)
+
+    sendcan = messaging.pub_sock('sendcan')
+    can = messaging.sub_sock('can', conflate=False, timeout=100)
+    sm = messaging.SubMaster(['pandaStates'])
+    time.sleep(1)
+
+    n = 200
+    for i in range(n):
+      print(f"pandad loopback {i}/{n}")
+
+      sent_msgs = send_random_can_messages(sendcan, random.randrange(20, 100), num_pandas)
+
+      sent_loopback = copy.deepcopy(sent_msgs)
+      sent_loopback.update({k+128: copy.deepcopy(v) for k, v in sent_msgs.items()})
+      sent_total = {k: len(v) for k, v in sent_loopback.items()}
+      for _ in range(100 * 5):
+        sm.update(0)
+        recvd = messaging.drain_sock(can, wait_for_one=True)
+        for msg in recvd:
+          for m in msg.can:
+            key = (m.address, m.dat)
+            assert key in sent_loopback[m.src], f"got unexpected msg: {m.src=} {m.address=} {m.dat=}"
+            sent_loopback[m.src].discard(key)
+
+        if all(len(v) == 0 for v in sent_loopback.values()):
+          break
+
+      # if a set isn't empty, messages got dropped
+      pprint(sent_msgs)
+      pprint(sent_loopback)
+      print({k: len(x) for k, x in sent_loopback.items()})
+      print(sum([len(x) for x in sent_loopback.values()]))
+      pprint(sm['pandaStates'])  # may drop messages due to RX buffer overflow
+      for bus in sent_loopback.keys():
+        assert not len(sent_loopback[bus]), f"loop {i}: bus {bus} missing {len(sent_loopback[bus])} out of {sent_total[bus]} messages"

iqpilot/selfdrive/pandad/tests/test_pandad_spi.py

@@ -0,0 +1,102 @@
+import os
+import time
+import numpy as np
+import pytest
+import random
+
+import iqpilot.cereal.messaging as messaging
+from iqpilot.cereal.services import SERVICE_LIST
+from iqpilot.selfdrive.test.helpers import with_processes
+from iqpilot.selfdrive.pandad.tests.test_pandad_loopback import setup_pandad, send_random_can_messages
+
+JUNGLE_SPAM = "JUNGLE_SPAM" in os.environ
+
+@pytest.mark.tici
+class TestBoarddSpi:
+  @classmethod
+  def setup_class(cls):
+    os.environ['STARTED'] = '1'
+    os.environ['SPI_ERR_PROB'] = '0.001'
+    if not JUNGLE_SPAM:
+      os.environ['BOARDD_LOOPBACK'] = '1'
+
+  @with_processes(['pandad'])
+  def test_spi_corruption(self, subtests):
+    setup_pandad(1)
+
+    sendcan = messaging.pub_sock('sendcan')
+    socks = {s: messaging.sub_sock(s, conflate=False, timeout=100) for s in ('can', 'pandaStates', 'peripheralState')}
+    time.sleep(2)
+    for s in socks.values():
+      messaging.drain_sock_raw(s)
+
+    total_recv_count = 0
+    total_sent_count = 0
+    sent_msgs = {bus: list() for bus in range(3)}
+
+    st = time.monotonic()
+    ts = {s: list() for s in socks.keys()}
+    for _ in range(int(os.getenv("TEST_TIME", "20"))):
+      # send some CAN messages
+      if not JUNGLE_SPAM:
+        sent = send_random_can_messages(sendcan, random.randrange(2, 20))
+        for k, v in sent.items():
+          sent_msgs[k].extend(list(v))
+          total_sent_count += len(v)
+
+      for service, sock in socks.items():
+        for m in messaging.drain_sock(sock):
+          ts[service].append(m.logMonoTime)
+
+          # sanity check for corruption
+          assert m.valid or (service == "can")
+          if service == "can":
+            for msg in m.can:
+              if JUNGLE_SPAM:
+                # PandaJungle.set_generated_can(True)
+                i = msg.address - 0x200
+                assert msg.address >= 0x200
+                assert msg.src == (i%3)
+                assert msg.dat == b"\xff"*(i%8)
+                total_recv_count += 1
+                continue
+
+              if msg.src > 4:
+                continue
+              key = (msg.address, msg.dat)
+              assert key in sent_msgs[msg.src], f"got unexpected msg: {msg.src=} {msg.address=} {msg.dat=}"
+              # TODO: enable this
+              #sent_msgs[msg.src].remove(key)
+              total_recv_count += 1
+          elif service == "pandaStates":
+            assert len(m.pandaStates) == 1
+            ps = m.pandaStates[0]
+            assert ps.uptime < 1000
+            assert ps.pandaType == "tres"
+            assert ps.ignitionLine
+            assert not ps.ignitionCan
+            assert 4000 < ps.voltage < 14000
+          elif service == "peripheralState":
+            ps = m.peripheralState
+            assert ps.pandaType == "tres"
+            assert 4000 < ps.voltage < 14000
+            assert 50 < ps.current < 1000
+            assert ps.fanSpeedRpm < 10000
+
+      time.sleep(0.5)
+    et = time.monotonic() - st
+
+    print("\n======== timing report ========")
+    for service, times in ts.items():
+      dts = np.diff(times)/1e6
+      print(service.ljust(17), f"{np.mean(dts):7.2f} {np.min(dts):7.2f} {np.max(dts):7.2f}")
+      with subtests.test(msg="timing check", service=service):
+        edt = 1e3 / SERVICE_LIST[service].frequency
+        assert edt*0.9 < np.mean(dts) < edt*1.1
+        assert np.max(dts) < edt*8
+        assert np.min(dts) < edt
+        assert len(dts) >= ((et-0.5)*SERVICE_LIST[service].frequency*0.8)
+
+    with subtests.test(msg="CAN traffic"):
+      print(f"Sent {total_sent_count} CAN messages, got {total_recv_count} back. {total_recv_count/(total_sent_count+1e-4):.2%} received")
+      assert total_recv_count > 20

iqpilot/selfdrive/pandad/tests/test_pandad_usbprotocol.cc

@@ -0,0 +1,135 @@
+#define CATCH_CONFIG_MAIN
+#define CATCH_CONFIG_ENABLE_BENCHMARKING
+
+#include "catch2/catch.hpp"
+#include "cereal/messaging/messaging.h"
+#include "common/util.h"
+#include "selfdrive/pandad/panda.h"
+
+struct PandaTest : public Panda {
+  PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type);
+  void test_can_send();
+  void test_can_recv(uint32_t chunk_size = 0);
+  void test_chunked_can_recv();
+
+  std::map<int, std::string> test_data;
+  int can_list_size = 0;
+  int total_pakets_size = 0;
+  MessageBuilder msg;
+  capnp::List<cereal::CanData>::Reader can_data_list;
+};
+
+PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState::PandaType hw_type) : can_list_size(can_list_size), Panda(bus_offset_) {
+  this->hw_type = hw_type;
+  int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 8);
+  // prepare test data
+  for (int i = 0; i < data_limit; ++i) {
+    std::random_device rd;
+    std::independent_bits_engine<std::default_random_engine, CHAR_BIT, unsigned char> rbe(rd());
+
+    int data_len = dlc_to_len[i];
+    std::string bytes(data_len, '\0');
+    std::generate(bytes.begin(), bytes.end(), std::ref(rbe));
+    test_data[data_len] = bytes;
+  }
+
+  // generate can messages for this panda
+  auto can_list = msg.initEvent().initSendcan(can_list_size);
+  for (uint8_t i = 0; i < can_list_size; ++i) {
+    auto can = can_list[i];
+    uint32_t id = util::random_int(0, std::size(dlc_to_len) - 1);
+    const std::string &dat = test_data[dlc_to_len[id]];
+    can.setAddress(i);
+    can.setSrc(util::random_int(0, 2) + bus_offset);
+    can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size()));
+    total_pakets_size += sizeof(can_header) + dat.size();
+  }
+
+  can_data_list = can_list.asReader();
+  INFO("test " << can_list_size << " packets, total size " << total_pakets_size);
+}
+
+void PandaTest::test_can_send() {
+  std::vector<uint8_t> unpacked_data;
+  this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) {
+    unpacked_data.insert(unpacked_data.end(), chunk, &chunk[size]);
+  });
+  REQUIRE(unpacked_data.size() == total_pakets_size);
+
+  int cnt = 0;
+  INFO("test can message integrity");
+  for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) {
+    can_header header;
+    memcpy(&header, &unpacked_data[pos], sizeof(can_header));
+    const uint8_t data_len = dlc_to_len[header.data_len_code];
+    pckt_len = sizeof(can_header) + data_len;
+
+    REQUIRE(header.addr == cnt);
+    REQUIRE(test_data.find(data_len) != test_data.end());
+    const std::string &dat = test_data[data_len];
+    REQUIRE(memcmp(dat.data(), &unpacked_data[pos + sizeof(can_header)], dat.size()) == 0);
+    ++cnt;
+  }
+  REQUIRE(cnt == can_list_size);
+}
+
+void PandaTest::test_can_recv(uint32_t rx_chunk_size) {
+  std::vector<can_frame> frames;
+  this->pack_can_buffer(can_data_list, [&](uint8_t *data, uint32_t size) {
+    if (rx_chunk_size == 0) {
+      REQUIRE(this->unpack_can_buffer(data, size, frames));
+    } else {
+      this->receive_buffer_size = 0;
+      uint32_t pos = 0;
+
+      while (pos < size) {
+        uint32_t chunk_size = std::min(rx_chunk_size, size - pos);
+        memcpy(&this->receive_buffer[this->receive_buffer_size], &data[pos], chunk_size);
+        this->receive_buffer_size += chunk_size;
+        pos += chunk_size;
+
+        REQUIRE(this->unpack_can_buffer(this->receive_buffer, this->receive_buffer_size, frames));
+      }
+    }
+  });
+
+  REQUIRE(frames.size() == can_list_size);
+  for (int i = 0; i < frames.size(); ++i) {
+    REQUIRE(frames[i].address == i);
+    REQUIRE(test_data.find(frames[i].dat.size()) != test_data.end());
+    const std::string &dat = test_data[frames[i].dat.size()];
+    REQUIRE(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0);
+  }
+}
+
+TEST_CASE("send/recv CAN 2.0 packets") {
+  auto bus_offset = GENERATE(0, 4);
+  auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
+  PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::DOS);
+
+  SECTION("can_send") {
+    test.test_can_send();
+  }
+  SECTION("can_receive") {
+    test.test_can_recv();
+  }
+  SECTION("chunked_can_receive") {
+    test.test_can_recv(0x40);
+  }
+}
+
+TEST_CASE("send/recv CAN FD packets") {
+  auto bus_offset = GENERATE(0, 4);
+  auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
+  PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::RED_PANDA);
+
+  SECTION("can_send") {
+    test.test_can_send();
+  }
+  SECTION("can_receive") {
+    test.test_can_recv();
+  }
+  SECTION("chunked_can_receive") {
+    test.test_can_recv(0x40);
+  }
+}