sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

openpilot/selfdrive/pandad/.gitignore

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

openpilot/selfdrive/pandad/__init__.py

@@ -0,0 +1,3 @@
+from openpilot.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

openpilot/selfdrive/pandad/main.cc

@@ -0,0 +1,22 @@
+#include <cassert>
+
+#include "selfdrive/pandad/pandad.h"
+#include "common/swaglog.h"
+#include "common/util.h"
+#include "common/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::string serial = (argc > 1) ? argv[1] : "";
+  pandad_main_thread(serial);
+  return 0;
+}

openpilot/selfdrive/pandad/panda.cc

@@ -0,0 +1,290 @@
+#include "selfdrive/pandad/panda.h"
+
+#include <unistd.h>
+
+#include <cassert>
+#include <stdexcept>
+#include <vector>
+
+#include "openpilot/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) {
+  handle = std::make_unique<PandaSpiHandle>(serial);
+  LOGW("connected to %s over SPI", serial.c_str());
+
+  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() {
+  return PandaSpiHandle::list();
+}
+
+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_sp) {
+  handle->control_write(0xdf, alternative_experience, safety_param_sp);
+}
+
+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/board/obj/") + 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_mads) {
+  handle->control_write(0xf3, engaged, engaged_mads);
+}
+
+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 >= 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;
+    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;
+    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;
+}

openpilot/selfdrive/pandad/panda.h

@@ -0,0 +1,98 @@
+#pragma once
+
+#include <cstdint>
+#include <ctime>
+#include <functional>
+#include <list>
+#include <memory>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include "openpilot/cereal/gen/cpp/car.capnp.h"
+#include "openpilot/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<PandaSpiHandle> handle;
+
+public:
+  Panda(std::string serial);
+
+  cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
+
+  bool connected();
+  bool comms_healthy();
+  std::string hw_serial();
+
+  // Static functions
+  static std::vector<std::string> list();
+
+  // 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_sp=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_mads);
+  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() {}
+  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);
+};

openpilot/selfdrive/pandad/panda_comms.h

@@ -0,0 +1,52 @@
+#pragma once
+
+#include <atomic>
+#include <cstdint>
+#include <mutex>
+#include <string>
+#include <vector>
+
+
+#define TIMEOUT 0
+#define SPI_BUF_SIZE 2048
+
+
+class PandaSpiHandle {
+public:
+  std::string hw_serial;
+  std::atomic<bool> connected = true;
+  std::atomic<bool> comms_healthy = true;
+
+  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;
+
+  struct __attribute__((packed)) spi_header {
+    uint8_t sync;
+    uint8_t endpoint;
+    uint16_t tx_len;
+    uint16_t max_rx_len;
+  };
+
+  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(struct spi_ioc_transfer &t);
+
+  spi_header header;
+  uint32_t xfer_count = 0;
+};

openpilot/selfdrive/pandad/panda_safety.cc

@@ -0,0 +1,84 @@
+#include "selfdrive/pandad/pandad.h"
+#include "openpilot/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 CarParamsSP", 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;
+    panda_->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_) {
+    const uint16_t safety_param = obd_multiplexing_requested ? 0U : 1U;
+    panda_->set_safety_model(cereal::CarParams::SafetyModel::ELM327, safety_param);
+    prev_obd_multiplexing_ = obd_multiplexing_requested;
+    params_.putBool("ObdMultiplexingChanged", true);
+  }
+}
+
+// TODO-SP: 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("CarParamsSP")};
+}
+
+// TODO-SP: Use structs instead of vector
+void PandaSafety::setSafetyMode(const std::vector<std::string> &params_string) {
+  AlignedBuffer aligned_buf;
+  AlignedBuffer aligned_buf_sp;
+
+  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_sp(aligned_buf_sp.align(params_string[1].data(), params_string[1].size()));
+  cereal::CarParamsSP::Reader car_params_sp = cmsg_sp.getRoot<cereal::CarParamsSP>();
+
+  auto safety_configs = car_params.getSafetyConfigs();
+  uint16_t alternative_experience = car_params.getAlternativeExperience();
+  uint16_t safety_param_sp = car_params_sp.getSafetyParam();
+
+  cereal::CarParams::SafetyModel safety_model = safety_configs[0].getSafetyModel();
+  uint16_t safety_param = safety_configs[0].getSafetyParam();
+
+  LOGW("setting safety model: %d, param: %d, alternative experience: %d, param_sp: %d", (int)safety_model, safety_param, alternative_experience, safety_param_sp);
+  panda_->set_alternative_experience(alternative_experience, safety_param_sp);
+  panda_->set_safety_model(safety_model, safety_param);
+}
+
+bool PandaSafety::getOffroadMode() {
+  auto offroad_mode = params_.getBool("OffroadMode");
+  return offroad_mode;
+}

openpilot/selfdrive/pandad/pandad.cc

@@ -0,0 +1,468 @@
+#include "selfdrive/pandad/pandad.h"
+
+#include <array>
+#include <bitset>
+#include <cassert>
+#include <cerrno>
+#include <memory>
+#include <thread>
+#include <utility>
+
+#include "openpilot/cereal/gen/cpp/car.capnp.h"
+#include "openpilot/cereal/messaging/messaging.h"
+#include "openpilot/cereal/services.h"
+#include "common/ratekeeper.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "common/util.h"
+#include "common/hardware/hw.h"
+
+#define MAX_IR_PANDA_VAL 50
+#define CUTOFF_IL 400
+#define SATURATE_IL 1000
+
+ExitHandler do_exit;
+
+bool check_connected(Panda *panda) {
+  if (!panda->connected()) {
+    do_exit = true;
+    return false;
+  }
+  return true;
+}
+
+bool process_mads_heartbeat(SubMaster *sm) {
+  const auto &mads = (*sm)["selfdriveStateSP"].getSelfdriveStateSP().getMads();
+  return sm->allAliveAndValid({"selfdriveStateSP"}) && mads.getEnabled();
+}
+
+Panda *connect(std::string serial) {
+  std::unique_ptr<Panda> panda;
+  try {
+    panda = std::make_unique<Panda>(serial);
+  } 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(Panda *panda, 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_connected(panda)) {
+    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) {
+      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(Panda *panda, PubMaster *pm) {
+  static std::vector<can_frame> raw_can_data;
+  {
+    raw_can_data.clear();
+    bool 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);
+  ps.setSoundOutputLevel(health.sound_output_level_pkt);
+  ps.setControlsAllowedLateral(health.controls_allowed_lateral_pkt);
+  ps.setControlsAllowedLongitudinal(health.controls_allowed_longitudinal_pkt);
+}
+
+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, Panda *panda, bool is_onroad, bool spoofing_started, bool always_offroad) {
+  // build msg
+  MessageBuilder msg;
+  auto evt = msg.initEvent();
+  auto pss = evt.initPandaStates(1);
+
+  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;
+  }
+
+  if (spoofing_started) {
+    health.ignition_line_pkt = 1;
+  }
+
+  bool ignition_local = ((health.ignition_line_pkt != 0) || (health.ignition_can_pkt != 0)) && !always_offroad;
+
+  // 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);
+  }
+
+  bool power_save_desired = !ignition_local;
+  if (health.power_save_enabled_pkt != power_save_desired) {
+    panda->set_power_saving(power_save_desired);
+  }
+
+  // set safety mode to NO_OUTPUT when car is off or we're not onroad. ELM327 is an alternative if we want to leverage athenad/connect
+  bool should_close_relay = !ignition_local || !is_onroad;
+  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[0];
+  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], can_health[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) {
+  auto health_opt = panda->get_state();
+  if (!health_opt) {
+    return;
+  }
+
+  // build msg
+  MessageBuilder msg;
+  auto evt = msg.initEvent();
+  evt.setValid(panda->comms_healthy());
+
+  auto ps = evt.initPeripheralState();
+  ps.setPandaType(panda->hw_type);
+
+  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(Panda *panda, PubMaster *pm, bool engaged, bool engaged_mads, bool is_onroad, bool spoofing_started, bool always_offroad) {
+  auto ignition_opt = send_panda_states(pm, panda, 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()) {
+    if (!panda->comms_healthy()) {
+      LOGE("Reconnecting, communication to panda not healthy");
+      do_exit = true;
+    }
+  }
+
+  panda->send_heartbeat(engaged, engaged_mads);
+}
+
+void process_peripheral_state(Panda *panda, PubMaster *pm, bool no_fan_control, bool is_onroad) {
+  static Params params;
+  static SubMaster sm({"deviceState", "driverCameraState"});
+
+  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;
+  static bool not_car = false;
+  static bool not_car_checked = 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);
+
+  {
+    sm.update(0);
+    if (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.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;
+    }
+
+    // turn off IR leds if body
+    if (!not_car_checked && is_onroad) {
+      std::string cp_bytes = params.get("CarParams");
+      if (cp_bytes.size() > 0) {
+        AlignedBuffer aligned_buf;
+        capnp::FlatArrayMessageReader cmsg(aligned_buf.align(cp_bytes.data(), cp_bytes.size()));
+        cereal::CarParams::Reader CP = cmsg.getRoot<cereal::CarParams>();
+        not_car = CP.getNotCar();
+        not_car_checked = true;
+      }
+    }
+    if (not_car) {
+      ir_pwr = 0;
+    }
+
+    if (ir_pwr != prev_ir_pwr || sm.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(Panda *panda) {
+  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 helper thread for event-driven sendcan.
+  std::thread send_thread(can_send_thread, panda, fake_send);
+
+  RateKeeper rk("pandad", 100);
+  SubMaster sm({"selfdriveState", "deviceState", "selfdriveStateSP"});
+  PubMaster pm({"can", "pandaStates", "peripheralState"});
+  PandaSafety panda_safety(panda);
+  bool engaged = false;
+  bool engaged_mads = false;
+  bool is_onroad = false;
+  bool always_offroad = false;
+
+  // Main loop: receive CAN first, then process lower priority panda and peripheral state.
+  while (!do_exit && check_connected(panda)) {
+    can_recv(panda, &pm);
+
+    // Process peripheral state at 20 Hz
+    if (rk.frame() % 5 == 0) {
+      process_peripheral_state(panda, &pm, no_fan_control, is_onroad);
+    }
+
+    // Process panda state at 10 Hz
+    if (rk.frame() % 10 == 0) {
+      sm.update(0);
+      engaged = sm.allAliveAndValid({"selfdriveState"}) && sm["selfdriveState"].getSelfdriveState().getEnabled();
+      if (sm.updated("deviceState")) {
+        is_onroad = sm["deviceState"].getDeviceState().getStarted();
+      }
+      engaged_mads = process_mads_heartbeat(&sm);
+      always_offroad = panda_safety.getOffroadMode();
+      process_panda_state(panda, &pm, engaged, engaged_mads, is_onroad, spoofing_started, always_offroad);
+      panda_safety.configureSafetyMode(is_onroad);
+    }
+
+    // Send out peripheralState at 2Hz
+    if (rk.frame() % 50 == 0) {
+      send_peripheral_state(panda, &pm);
+    }
+
+    // Forward logs from panda to cloudlog if available
+    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) {
+    if (panda->connected()) {
+      panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
+    }
+  }
+
+  send_thread.join();
+}
+
+void pandad_main_thread(std::string serial) {
+  if (serial.empty()) {
+    auto serials = Panda::list();
+
+    if (serials.empty()) {
+      LOGW("no pandas found, exiting");
+      return;
+    }
+    serial = serials[0];
+  }
+
+  LOGW("connecting to panda: %s", serial.c_str());
+
+  Panda *panda = nullptr;
+  while (!do_exit) {
+    panda = connect(serial);
+    if (panda) break;
+    util::sleep_for(100);
+  }
+
+  if (!do_exit) {
+    LOGW("connected to panda");
+    pandad_run(panda);
+  }
+
+  delete panda;
+}

openpilot/selfdrive/pandad/pandad.h

@@ -0,0 +1,35 @@
+#pragma once
+
+#include <string>
+
+#include "common/params.h"
+#include "selfdrive/pandad/panda.h"
+
+void pandad_main_thread(std::string serial);
+
+// 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(Panda *panda) : panda_(panda) {}
+  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;
+  Panda *panda_;
+  Params params_;
+};

openpilot/selfdrive/pandad/pandad.py

@@ -0,0 +1,145 @@
+#!/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, McuType, FW_PATH
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.common.hardware import HARDWARE
+from openpilot.common.swaglog import cloudlog
+
+from openpilot.sunnypilot.selfdrive.pandad.rivian_long_flasher import flash_rivian_long
+
+
+def get_expected_signature() -> bytes:
+  fn = os.path.join(FW_PATH, McuType.H7.config.app_fn)
+  return Panda.get_signature_from_firmware(fn)
+
+def flash_panda(panda_serial: str):
+  panda = Panda(panda_serial)
+
+  # skip flashing if the detected panda is not supported
+  if panda.get_type() not in Panda.SUPPORTED_DEVICES:
+    cloudlog.warning(f"Panda {panda_serial} is not supported (hw_type: {panda.get_type()}), skipping flash...")
+    panda.close()
+    return
+
+  fw_signature = get_expected_signature()
+  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
+
+  panda.close()
+
+
+def check_panda_support(panda_serials: list[str]) -> list[str]:
+  spi_serials = set(Panda.spi_list())
+  for serial in panda_serials:
+    if serial in spi_serials:
+      return [serial]
+
+  for serial in panda_serials:
+    panda = Panda(serial)
+    is_internal = panda.is_internal()
+    panda.close()
+    if is_internal:
+      return [serial]
+
+  return []
+
+
+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)
+
+  # check health for lost heartbeat
+  try:
+    for s in Panda.list():
+      with Panda(s) as p:
+        health = p.health()
+        if p.is_internal() and health["heartbeat_lost"]:
+          Params().put_bool("PandaHeartbeatLost", True, block=True)
+          cloudlog.event("heartbeat lost", deviceState=health)
+  except Exception:
+    cloudlog.exception("pandad.uncaught_exception")
+
+  count = 0
+  while not do_exit:
+    try:
+      cloudlog.event("pandad.flash_and_connect", count=count)
+      if (count % 2) == 0:
+        HARDWARE.reset_internal_panda()
+      else:
+        HARDWARE.recover_internal_panda()
+      count += 1
+
+      # Flash all Pandas in DFU mode
+      for serial in PandaDFU.list():
+        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):
+        # custom flasher for xnor's Rivian Longitudinal Upgrade Kit
+        flash_rivian_long(panda_serials)
+        # find the internal supported panda (e.g. skip external Black Panda)
+        panda_serials = check_panda_support(panda_serials)
+
+        assert len(panda_serials) == 1
+        cloudlog.info(f"{len(panda_serials)} panda found, connecting - {panda_serials}")
+        flash_panda(panda_serials[0])
+
+        # run real pandad
+        os.environ['MANAGER_DAEMON'] = 'pandad'
+        process = subprocess.Popen(["./pandad"], cwd=os.path.join(BASEDIR, "openpilot/selfdrive/pandad"))
+        process.wait()
+    # 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")
+    except PandaProtocolMismatch:
+      cloudlog.exception("pandad.protocol_mismatch")
+    except Exception:
+      cloudlog.exception("pandad.uncaught_exception")
+
+
+if __name__ == "__main__":
+  main()

openpilot/selfdrive/pandad/pandad_api_impl.py

@@ -0,0 +1,88 @@
+import time
+from openpilot.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

openpilot/selfdrive/pandad/spi.cc

@@ -0,0 +1,408 @@
+#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) {
+  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;
+}

openpilot/selfdrive/pandad/tests/test_pandad.py

@@ -0,0 +1,81 @@
+import os
+import pytest
+import time
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import log
+from openpilot.common.gpio import gpio_set, gpio_init
+from panda import Panda, PandaDFU
+from openpilot.system.manager.process_config import managed_processes
+from openpilot.common.hardware import HARDWARE
+from openpilot.common.hardware.tici.pins import GPIO
+
+HERE = os.path.dirname(os.path.realpath(__file__))
+
+
+@pytest.mark.tici
+class TestPandad:
+  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(10)
+      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 _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())
+    HARDWARE.reset_internal_panda()
+
+  def test_in_dfu(self):
+    HARDWARE.recover_internal_panda()
+    self._run_test()
+
+  def test_in_bootstub(self):
+    with Panda() as p:
+      p.reset(enter_bootstub=True)
+      assert p.bootstub
+    self._run_test()
+
+  def test_in_reset(self):
+    gpio_init(GPIO.STM_RST_N, True)
+    gpio_set(GPIO.STM_RST_N, 1)
+    assert not Panda.list()
+    self._run_test()
+
+  def test_release_to_devel_bootstub(self):
+    st = time.monotonic()
+    self._flash_bootstub(None)
+    print("flash done", time.monotonic() - st)
+    self._run_test()
+
+  def test_recover_from_bad_bootstub(self):
+    self._go_to_dfu()
+    with PandaDFU(None) as pd:
+      pd._handle.program(pd.get_mcu_type().config.bootstub_address, b"\x00"*100)
+    HARDWARE.reset_internal_panda()
+    assert not Panda.list()
+    assert not PandaDFU.list()
+
+    self._run_test()

openpilot/selfdrive/pandad/tests/test_pandad_canprotocol.cc

@@ -0,0 +1,135 @@
+#define CATCH_CONFIG_MAIN
+#define CATCH_CONFIG_ENABLE_BENCHMARKING
+
+#include <climits>
+
+#include "catch2/catch.hpp"
+#include "openpilot/cereal/messaging/messaging.h"
+#include "common/util.h"
+#include "selfdrive/pandad/panda.h"
+
+struct PandaTest : public Panda {
+  PandaTest(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(int can_list_size_, cereal::PandaState::PandaType hw_type_) : can_list_size(can_list_size_), Panda() {
+  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));
+    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 can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
+  PandaTest test(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 can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
+  PandaTest test(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);
+  }
+}

openpilot/selfdrive/pandad/tests/test_pandad_loopback.py

@@ -0,0 +1,116 @@
+import os
+import copy
+import random
+import time
+import pytest
+from collections import defaultdict
+from pprint import pprint
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal import log
+from opendbc.car.structs import car
+from opendbc.car.can_definitions import CanData
+from openpilot.common.utils import retry
+from openpilot.common.params import Params
+from openpilot.common.timeout import Timeout
+from openpilot.selfdrive.pandad import can_list_to_can_capnp
+from openpilot.selfdrive.test.helpers import with_processes
+
+
+def publish_device_state(pm, started):
+  msg = messaging.new_message('deviceState')
+  msg.deviceState.started = started
+  pm.send('deviceState', msg)
+
+@retry(attempts=3)
+def setup_pandad():
+  params = Params()
+  params.clear_all()
+
+  pm = messaging.PubMaster(['deviceState'])
+  sm = messaging.SubMaster(['pandaStates'])
+  publish_device_state(pm, False)
+  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']):
+      publish_device_state(pm, False)
+      sm.update(100)
+
+  # 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]
+
+  params.put_bool("FirmwareQueryDone", True, block=True)
+  params.put_bool("ControlsReady", True, block=True)
+  params.put("CarParams", cp.to_bytes(), block=True)
+
+  publish_device_state(pm, True)
+  with Timeout(90, "pandad didn't set safety mode"):
+    while any(ps.safetyModel != car.CarParams.SafetyModel.allOutput for ps in sm['pandaStates']):
+      publish_device_state(pm, True)
+      sm.update(100)
+
+def send_random_can_messages(sendcan, count):
+  sent_msgs = defaultdict(set)
+  for _ in range(count):
+    to_send = []
+    for __ in range(random.randrange(20)):
+      bus = random.choice(range(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):
+    setup_pandad()
+
+    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))
+
+      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"

openpilot/selfdrive/pandad/tests/test_pandad_spi.py

@@ -0,0 +1,107 @@
+import os
+import time
+import numpy as np
+import pytest
+import random
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal.services import SERVICE_LIST
+from openpilot.common.timeout import Timeout
+from openpilot.selfdrive.test.helpers import with_processes
+from openpilot.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()
+
+    sendcan = messaging.pub_sock('sendcan')
+    socks = {s: messaging.sub_sock(s, conflate=False, timeout=100) for s in ('can', 'pandaStates', 'peripheralState')}
+    readiness_services = {'pandaStates', 'peripheralState'}
+    ready = set()
+    with Timeout(2, "pandad services didn't become ready"):
+      while not readiness_services <= ready:
+        for service, sock in socks.items():
+          if messaging.drain_sock_raw(sock):
+            ready.add(service)
+        time.sleep(0.01)
+
+    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 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