openpilot v0.9.6 release

date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6

selfdrive/boardd/.gitignore

@@ -0,0 +1,3 @@
+boardd
+boardd_api_impl.cpp
+tests/test_boardd_usbprotocol

selfdrive/boardd/SConscript

@@ -0,0 +1,11 @@
+Import('env', 'envCython', 'common', 'cereal', 'messaging')
+
+libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
+panda = env.Library('panda', ['panda.cc', 'panda_comms.cc', 'spi.cc'])
+
+env.Program('boardd', ['main.cc', 'boardd.cc'], LIBS=[panda] + libs)
+env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
+
+envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
+if GetOption('extras'):
+  env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc'], LIBS=[panda] + libs)

selfdrive/boardd/boardd.cc

@@ -0,0 +1,641 @@
+#include "selfdrive/boardd/boardd.h"
+
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <bitset>
+#include <cassert>
+#include <cerrno>
+#include <chrono>
+#include <future>
+#include <memory>
+#include <thread>
+
+#include "cereal/gen/cpp/car.capnp.h"
+#include "cereal/messaging/messaging.h"
+#include "common/params.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, boardd will reconnect to all pandas
+// - If a panda is added, we will only reconnect when we are offroad
+// CAN buses:
+// - Each panda will have it's 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_POWER 0.5f
+#define MIN_IR_POWER 0.0f
+#define CUTOFF_IL 400
+#define SATURATE_IL 1000
+using namespace std::chrono_literals;
+
+std::atomic<bool> ignition(false);
+
+ExitHandler do_exit;
+
+static std::string get_time_str(const struct tm &time) {
+  char s[30] = {'\0'};
+  std::strftime(s, std::size(s), "%Y-%m-%d %H:%M:%S", &time);
+  return s;
+}
+
+bool check_all_connected(const std::vector<Panda *> &pandas) {
+  for (const auto& panda : pandas) {
+    if (!panda->connected()) {
+      do_exit = true;
+      return false;
+    }
+  }
+  return true;
+}
+
+enum class SyncTimeDir { TO_PANDA, FROM_PANDA };
+
+void sync_time(Panda *panda, SyncTimeDir dir) {
+  if (!panda->has_rtc) return;
+
+  setenv("TZ", "UTC", 1);
+  struct tm sys_time = util::get_time();
+  struct tm rtc_time = panda->get_rtc();
+
+  if (dir == SyncTimeDir::TO_PANDA) {
+    if (util::time_valid(sys_time)) {
+      // Write time to RTC if it looks reasonable
+      double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
+      if (std::abs(seconds) > 1.1) {
+        panda->set_rtc(sys_time);
+        LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s",
+              seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
+      }
+    }
+  } else if (dir == SyncTimeDir::FROM_PANDA) {
+    LOGW("System time: %s, RTC time: %s", get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
+
+    if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) {
+      const struct timeval tv = {mktime(&rtc_time), 0};
+      settimeofday(&tv, 0);
+      LOGE("System time wrong, setting from RTC.");
+    }
+  }
+}
+
+bool safety_setter_thread(std::vector<Panda *> pandas) {
+  LOGD("Starting safety setter thread");
+
+  Params p;
+
+  // there should be at least one panda connected
+  if (pandas.size() == 0) {
+    return false;
+  }
+
+  // initialize to ELM327 without OBD multiplexing for fingerprinting
+  bool obd_multiplexing_enabled = false;
+  for (int i = 0; i < pandas.size(); i++) {
+    pandas[i]->set_safety_model(cereal::CarParams::SafetyModel::ELM327, 1U);
+  }
+
+  // openpilot can switch between multiplexing modes for different FW queries
+  while (true) {
+    if (do_exit || !check_all_connected(pandas) || !ignition) {
+      return false;
+    }
+
+    bool obd_multiplexing_requested = p.getBool("ObdMultiplexingEnabled");
+    if (obd_multiplexing_requested != obd_multiplexing_enabled) {
+      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);
+      }
+      obd_multiplexing_enabled = obd_multiplexing_requested;
+      p.putBool("ObdMultiplexingChanged", true);
+    }
+
+    if (p.getBool("FirmwareQueryDone")) {
+      LOGW("finished FW query");
+      break;
+    }
+    util::sleep_for(20);
+  }
+
+  std::string params;
+  LOGW("waiting for params to set safety model");
+  while (true) {
+    if (do_exit || !check_all_connected(pandas) || !ignition) {
+      return false;
+    }
+
+    if (p.getBool("ControlsReady")) {
+      params = p.get("CarParams");
+      if (params.size() > 0) break;
+    }
+    util::sleep_for(100);
+  }
+  LOGW("got %lu bytes CarParams", params.size());
+
+  AlignedBuffer aligned_buf;
+  capnp::FlatArrayMessageReader cmsg(aligned_buf.align(params.data(), params.size()));
+  cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+  cereal::CarParams::SafetyModel safety_model;
+  uint16_t safety_param;
+
+  auto safety_configs = car_params.getSafetyConfigs();
+  uint16_t alternative_experience = car_params.getAlternativeExperience();
+  for (uint32_t i = 0; i < pandas.size(); i++) {
+    auto panda = pandas[i];
+
+    if (safety_configs.size() > i) {
+      safety_model = safety_configs[i].getSafetyModel();
+      safety_param = safety_configs[i].getSafetyParam();
+    } else {
+      // If no safety mode is specified, default to silent
+      safety_model = cereal::CarParams::SafetyModel::SILENT;
+      safety_param = 0U;
+    }
+
+    LOGW("panda %d: setting safety model: %d, param: %d, alternative experience: %d", i, (int)safety_model, safety_param, alternative_experience);
+    panda->set_alternative_experience(alternative_experience);
+    panda->set_safety_model(safety_model, safety_param);
+  }
+
+  return true;
+}
+
+Panda *connect(std::string serial="", uint32_t index=0) {
+  std::unique_ptr<Panda> panda;
+  try {
+    panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_CNT));
+  } catch (std::exception &e) {
+    return nullptr;
+  }
+
+  // common panda config
+  if (getenv("BOARDD_LOOPBACK")) {
+    panda->set_loopback(true);
+  }
+  //panda->enable_deepsleep();
+
+  if (!panda->up_to_date() && !getenv("BOARDD_SKIP_FW_CHECK")) {
+    throw std::runtime_error("Panda firmware out of date. Run pandad.py to update.");
+  }
+
+  sync_time(panda.get(), SyncTimeDir::FROM_PANDA);
+  return panda.release();
+}
+
+void can_send_thread(std::vector<Panda *> pandas, bool fake_send) {
+  util::set_thread_name("boardd_can_send");
+
+  AlignedBuffer aligned_buf;
+  std::unique_ptr<Context> context(Context::create());
+  std::unique_ptr<SubSocket> subscriber(SubSocket::create(context.get(), "sendcan"));
+  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) {
+      if (errno == EINTR) {
+        do_exit = true;
+      }
+      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_thread(std::vector<Panda *> pandas) {
+  util::set_thread_name("boardd_can_recv");
+
+  PubMaster pm({"can"});
+
+  // run at 100Hz
+  RateKeeper rk("boardd_can_recv", 100);
+  std::vector<can_frame> raw_can_data;
+
+  while (!do_exit && check_all_connected(pandas)) {
+    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 (uint i = 0; i<raw_can_data.size(); i++) {
+      canData[i].setAddress(raw_can_data[i].address);
+      canData[i].setBusTime(raw_can_data[i].busTime);
+      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);
+
+    rk.keepTime();
+  }
+}
+
+std::optional<bool> send_panda_states(PubMaster *pm, const std::vector<Panda *> &pandas, bool spoofing_started) {
+  bool ignition_local = false;
+  const uint32_t pandas_cnt = pandas.size();
+
+  // 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;
+    }
+
+    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);
+    }
+
+    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. ELM327 is an alternative if we want to leverage athenad/connect
+    if (!ignition_local && (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];
+    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.setGmlanSendErrs(health.gmlan_send_errs_pkt);
+    ps.setPandaType(panda->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);
+    ps.setFanPower(health.fan_power);
+    ps.setFanStallCount(health.fan_stall_count);
+    ps.setSafetyRxChecksInvalid((bool)(health.safety_rx_checks_invalid));
+    ps.setSpiChecksumErrorCount(health.spi_checksum_error_count);
+    ps.setSbu1Voltage(health.sbu1_voltage_mV / 1000.0f);
+    ps.setSbu2Voltage(health.sbu2_voltage_mV / 1000.0f);
+
+    std::array<cereal::PandaState::PandaCanState::Builder, PANDA_CAN_CNT> cs = {ps.initCanState0(), ps.initCanState1(), ps.initCanState2()};
+
+    for (uint32_t j = 0; j < PANDA_CAN_CNT; j++) {
+      const auto &can_health = pandaCanStates[i][j];
+      cs[j].setBusOff((bool)can_health.bus_off);
+      cs[j].setBusOffCnt(can_health.bus_off_cnt);
+      cs[j].setErrorWarning((bool)can_health.error_warning);
+      cs[j].setErrorPassive((bool)can_health.error_passive);
+      cs[j].setLastError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_error));
+      cs[j].setLastStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_stored_error));
+      cs[j].setLastDataError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_error));
+      cs[j].setLastDataStoredError(cereal::PandaState::PandaCanState::LecErrorCode(can_health.last_data_stored_error));
+      cs[j].setReceiveErrorCnt(can_health.receive_error_cnt);
+      cs[j].setTransmitErrorCnt(can_health.transmit_error_cnt);
+      cs[j].setTotalErrorCnt(can_health.total_error_cnt);
+      cs[j].setTotalTxLostCnt(can_health.total_tx_lost_cnt);
+      cs[j].setTotalRxLostCnt(can_health.total_rx_lost_cnt);
+      cs[j].setTotalTxCnt(can_health.total_tx_cnt);
+      cs[j].setTotalRxCnt(can_health.total_rx_cnt);
+      cs[j].setTotalFwdCnt(can_health.total_fwd_cnt);
+      cs[j].setCanSpeed(can_health.can_speed);
+      cs[j].setCanDataSpeed(can_health.can_data_speed);
+      cs[j].setCanfdEnabled(can_health.canfd_enabled);
+      cs[j].setBrsEnabled(can_health.brs_enabled);
+      cs[j].setCanfdNonIso(can_health.canfd_non_iso);
+      cs[j].setIrq0CallRate(can_health.irq0_call_rate);
+      cs[j].setIrq1CallRate(can_health.irq1_call_rate);
+      cs[j].setIrq2CallRate(can_health.irq2_call_rate);
+      cs[j].setCanCoreResetCnt(can_health.can_core_reset_cnt);
+    }
+
+    // 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(PubMaster *pm, Panda *panda) {
+  // 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);
+  }
+
+  uint16_t fan_speed_rpm = panda->get_fan_speed();
+  ps.setFanSpeedRpm(fan_speed_rpm);
+
+  pm->send("peripheralState", msg);
+}
+
+void panda_state_thread(std::vector<Panda *> pandas, bool spoofing_started) {
+  util::set_thread_name("boardd_panda_state");
+
+  Params params;
+  SubMaster sm({"controlsState"});
+  PubMaster pm({"pandaStates", "peripheralState"});
+
+  Panda *peripheral_panda = pandas[0];
+  bool is_onroad = false;
+  bool is_onroad_last = false;
+  std::future<bool> safety_future;
+
+  std::vector<std::string> connected_serials;
+  for (Panda *p : pandas) {
+    connected_serials.push_back(p->hw_serial());
+  }
+
+  LOGD("start panda state thread");
+
+  // run at 10hz
+  RateKeeper rk("panda_state_thread", 10);
+
+  while (!do_exit && check_all_connected(pandas)) {
+    // send out peripheralState at 2Hz
+    if (sm.frame % 5 == 0) {
+      send_peripheral_state(&pm, peripheral_panda);
+    }
+
+    auto ignition_opt = send_panda_states(&pm, pandas, spoofing_started);
+
+    if (!ignition_opt) {
+      LOGE("Failed to get ignition_opt");
+      rk.keepTime();
+      continue;
+    }
+
+    ignition = *ignition_opt;
+
+    // check if we should have pandad reconnect
+    if (!ignition) {
+      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;
+          }
+        }
+      }
+
+      if (do_exit) {
+        break;
+      }
+    }
+
+    is_onroad = params.getBool("IsOnroad");
+
+    // set new safety on onroad transition, after params are cleared
+    if (is_onroad && !is_onroad_last) {
+      if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) {
+        safety_future = std::async(std::launch::async, safety_setter_thread, pandas);
+      } else {
+        LOGW("Safety setter thread already running");
+      }
+    }
+
+    is_onroad_last = is_onroad;
+
+    sm.update(0);
+    const bool engaged = sm.allAliveAndValid({"controlsState"}) && sm["controlsState"].getControlsState().getEnabled();
+
+    for (const auto &panda : pandas) {
+      panda->send_heartbeat(engaged);
+    }
+
+    rk.keepTime();
+  }
+}
+
+
+void peripheral_control_thread(Panda *panda, bool no_fan_control) {
+  util::set_thread_name("boardd_peripheral_control");
+
+  SubMaster sm({"deviceState", "driverCameraState"});
+
+  uint64_t last_driver_camera_t = 0;
+  uint16_t prev_fan_speed = 999;
+  uint16_t ir_pwr = 0;
+  uint16_t prev_ir_pwr = 999;
+
+  FirstOrderFilter integ_lines_filter(0, 30.0, 0.05);
+
+  while (!do_exit && panda->connected()) {
+    sm.update(1000);
+
+    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();
+
+      cur_integ_lines = integ_lines_filter.update(cur_integ_lines);
+      last_driver_camera_t = event.getLogMonoTime();
+
+      if (cur_integ_lines <= CUTOFF_IL) {
+        ir_pwr = 100.0 * MIN_IR_POWER;
+      } else if (cur_integ_lines > SATURATE_IL) {
+        ir_pwr = 100.0 * MAX_IR_POWER;
+      } else {
+        ir_pwr = 100.0 * (MIN_IR_POWER + ((cur_integ_lines - CUTOFF_IL) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_IL - CUTOFF_IL)));
+      }
+    }
+
+    // Disable IR on input timeout
+    if (nanos_since_boot() - last_driver_camera_t > 1e9) {
+      ir_pwr = 0;
+    }
+
+    if (ir_pwr != prev_ir_pwr || sm.frame % 100 == 0 || ir_pwr >= 50.0) {
+      panda->set_ir_pwr(ir_pwr);
+      prev_ir_pwr = ir_pwr;
+    }
+
+    // Write to rtc once per minute when no ignition present
+    if (!ignition && (sm.frame % 120 == 1)) {
+      sync_time(panda, SyncTimeDir::TO_PANDA);
+    }
+  }
+}
+
+void boardd_main_thread(std::vector<std::string> serials) {
+  LOGW("launching boardd");
+
+  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");
+
+    std::vector<std::thread> threads;
+
+    threads.emplace_back(panda_state_thread, pandas, getenv("STARTED") != nullptr);
+    threads.emplace_back(peripheral_control_thread, pandas[0], getenv("NO_FAN_CONTROL") != nullptr);
+
+    threads.emplace_back(can_send_thread, pandas, getenv("FAKESEND") != nullptr);
+    threads.emplace_back(can_recv_thread, pandas);
+
+    for (auto &t : threads) t.join();
+  }
+
+  for (Panda *panda : pandas) {
+    delete panda;
+  }
+}

selfdrive/boardd/boardd.h

@@ -0,0 +1,9 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "selfdrive/boardd/panda.h"
+
+bool safety_setter_thread(std::vector<Panda *> pandas);
+void boardd_main_thread(std::vector<std::string> serials);

selfdrive/boardd/boardd.py

@@ -0,0 +1,10 @@
+# Cython, now uses scons to build
+from openpilot.selfdrive.boardd.boardd_api_impl import can_list_to_can_capnp
+assert can_list_to_can_capnp
+
+def can_capnp_to_can_list(can, src_filter=None):
+  ret = []
+  for msg in can:
+    if src_filter is None or msg.src in src_filter:
+      ret.append((msg.address, msg.busTime, msg.dat, msg.src))
+  return ret

selfdrive/boardd/boardd_api_impl.pyx

@@ -0,0 +1,30 @@
+# distutils: language = c++
+# cython: language_level=3
+from libcpp.vector cimport vector
+from libcpp.string cimport string
+from libcpp cimport bool
+
+cdef extern from "panda.h":
+  cdef struct can_frame:
+    long address
+    string dat
+    long busTime
+    long src
+
+cdef extern from "can_list_to_can_capnp.cc":
+  void can_list_to_can_capnp_cpp(const vector[can_frame] &can_list, string &out, bool sendCan, bool valid)
+
+def can_list_to_can_capnp(can_msgs, msgtype='can', valid=True):
+  cdef vector[can_frame] can_list
+  can_list.reserve(len(can_msgs))
+
+  cdef can_frame f
+  for can_msg in can_msgs:
+    f.address = can_msg[0]
+    f.busTime = can_msg[1]
+    f.dat = can_msg[2]
+    f.src = can_msg[3]
+    can_list.push_back(f)
+  cdef string out
+  can_list_to_can_capnp_cpp(can_list, out, msgtype == 'sendcan', valid)
+  return out

selfdrive/boardd/can_list_to_can_capnp.cc

@@ -0,0 +1,21 @@
+#include "cereal/messaging/messaging.h"
+#include "selfdrive/boardd/panda.h"
+
+void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan, bool valid) {
+  MessageBuilder msg;
+  auto event = msg.initEvent(valid);
+
+  auto canData = sendCan ? event.initSendcan(can_list.size()) : event.initCan(can_list.size());
+  int j = 0;
+  for (auto it = can_list.begin(); it != can_list.end(); it++, j++) {
+    auto c = canData[j];
+    c.setAddress(it->address);
+    c.setBusTime(it->busTime);
+    c.setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size()));
+    c.setSrc(it->src);
+  }
+  const uint64_t msg_size = capnp::computeSerializedSizeInWords(msg) * sizeof(capnp::word);
+  out.resize(msg_size);
+  kj::ArrayOutputStream output_stream(kj::ArrayPtr<capnp::byte>((unsigned char *)out.data(), msg_size));
+  capnp::writeMessage(output_stream, msg);
+}

selfdrive/boardd/main.cc

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

selfdrive/boardd/panda.cc

@@ -0,0 +1,322 @@
+#include "selfdrive/boardd/panda.h"
+
+#include <unistd.h>
+
+#include <cassert>
+#include <stdexcept>
+#include <vector>
+
+#include "cereal/messaging/messaging.h"
+#include "common/swaglog.h"
+#include "common/util.h"
+
+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();
+  has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) ||
+            (hw_type == cereal::PandaState::PandaType::DOS) ||
+            (hw_type == cereal::PandaState::PandaType::TRES);
+
+  can_reset_communications();
+
+  return;
+}
+
+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 (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) {
+  handle->control_write(0xdf, alternative_experience, 0);
+}
+
+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_rtc(struct tm sys_time) {
+  // tm struct has year defined as years since 1900
+  handle->control_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0);
+  handle->control_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0);
+  handle->control_write(0xa3, (uint16_t)sys_time.tm_mday, 0);
+  // handle->control_write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0);
+  handle->control_write(0xa5, (uint16_t)sys_time.tm_hour, 0);
+  handle->control_write(0xa6, (uint16_t)sys_time.tm_min, 0);
+  handle->control_write(0xa7, (uint16_t)sys_time.tm_sec, 0);
+}
+
+struct tm Panda::get_rtc() {
+  struct __attribute__((packed)) timestamp_t {
+    uint16_t year; // Starts at 0
+    uint8_t month;
+    uint8_t day;
+    uint8_t weekday;
+    uint8_t hour;
+    uint8_t minute;
+    uint8_t second;
+  } rtc_time = {0};
+
+  handle->control_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time));
+
+  struct tm new_time = { 0 };
+  new_time.tm_year = rtc_time.year - 1900; // tm struct has year defined as years since 1900
+  new_time.tm_mon  = rtc_time.month - 1;
+  new_time.tm_mday = rtc_time.day;
+  new_time.tm_hour = rtc_time.hour;
+  new_time.tm_min  = rtc_time.minute;
+  new_time.tm_sec  = rtc_time.second;
+
+  return new_time;
+}
+
+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) {
+  handle->control_write(0xf3, engaged, 0);
+}
+
+void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) {
+  handle->control_write(0xde, bus, (speed * 10));
+}
+
+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 (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_CNT)) {
+      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(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 (recv == RECV_SIZE) {
+    LOGW("Panda receive buffer full");
+  }
+  receive_buffer_size += recv;
+
+  return (recv <= 0) ? true : unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec);
+}
+
+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;
+    }
+
+    can_frame &canData = out_vec.emplace_back();
+    canData.busTime = 0;
+    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;
+    }
+
+    if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) {
+      LOGE("Panda CAN checksum failed");
+      size = 0;
+      return false;
+    }
+
+    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;
+}

selfdrive/boardd/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 "cereal/gen/cpp/car.capnp.h"
+#include "cereal/gen/cpp/log.capnp.h"
+#include "panda/board/health.h"
+#include "panda/board/can_definitions.h"
+#include "selfdrive/boardd/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
+
+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 busTime;
+  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;
+  bool has_rtc = false;
+  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);
+  void set_rtc(struct tm sys_time);
+  struct tm get_rtc();
+  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);
+  void set_can_speed_kbps(uint16_t bus, uint16_t speed);
+  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(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);
+};

selfdrive/boardd/panda_comms.cc

@@ -0,0 +1,227 @@
+#include "selfdrive/boardd/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 == 0xbbaa && 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 == 0xbbaa && 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).c_str());
+    }
+  }
+
+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;
+}

selfdrive/boardd/panda_comms.h

@@ -0,0 +1,82 @@
+#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__
+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);
+};
+#endif

selfdrive/boardd/pandad.py

@@ -0,0 +1,185 @@
+#!/usr/bin/env python3
+# simple boardd wrapper that updates the panda first
+import os
+import usb1
+import time
+import subprocess
+from typing import List, NoReturn
+from functools import cmp_to_key
+
+from panda import Panda, PandaDFU, PandaProtocolMismatch, FW_PATH
+from openpilot.common.basedir import BASEDIR
+from openpilot.common.params import Params
+from openpilot.selfdrive.boardd.set_time import set_time
+from openpilot.system.hardware import HARDWARE
+from openpilot.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
+
+  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 panda_sort_cmp(a: Panda, b: Panda):
+  a_type = a.get_type()
+  b_type = b.get_type()
+
+  # make sure the internal one is always first
+  if a.is_internal() and not b.is_internal():
+    return -1
+  if not a.is_internal() and b.is_internal():
+    return 1
+
+  # sort by hardware type
+  if a_type != b_type:
+    return a_type < b_type
+
+  # last resort: sort by serial number
+  return a.get_usb_serial() < b.get_usb_serial()
+
+
+def main() -> NoReturn:
+  count = 0
+  first_run = True
+  params = Params()
+  no_internal_panda_count = 0
+
+  while True:
+    try:
+      count += 1
+      cloudlog.event("pandad.flash_and_connect", count=count)
+      params.remove("PandaSignatures")
+
+      # Handle missing internal panda
+      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
+      pandas.sort(key=cmp_to_key(panda_sort_cmp))
+      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:
+        # 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:
+          if panda.is_internal():
+            # update time from RTC
+            set_time(cloudlog)
+
+          # reset panda to ensure we're in a good state
+          cloudlog.info(f"Resetting panda {panda.get_usb_serial()}")
+          if panda.is_internal():
+            HARDWARE.reset_internal_panda()
+          else:
+            panda.reset(reconnect=False)
+
+      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 boardd with all connected serials as arguments
+    os.environ['MANAGER_DAEMON'] = 'boardd'
+    os.chdir(os.path.join(BASEDIR, "selfdrive/boardd"))
+    subprocess.run(["./boardd", *panda_serials], check=True)
+
+if __name__ == "__main__":
+  main()

selfdrive/boardd/set_time.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python3
+import os
+import datetime
+from panda import Panda
+
+from openpilot.common.time import MIN_DATE
+
+def set_time(logger):
+  sys_time = datetime.datetime.today()
+  if sys_time > MIN_DATE:
+    logger.info("System time valid")
+    return
+
+  try:
+    ps = Panda.list()
+    if len(ps) == 0:
+      logger.error("Failed to set time, no pandas found")
+      return
+
+    for s in ps:
+      with Panda(serial=s) as p:
+        if not p.is_internal():
+          continue
+
+        # Set system time from panda RTC time
+        panda_time = p.get_datetime()
+        if panda_time > MIN_DATE:
+          logger.info(f"adjusting time from '{sys_time}' to '{panda_time}'")
+          os.system(f"TZ=UTC date -s '{panda_time}'")
+        break
+  except Exception:
+    logger.exception("Failed to fetch time from panda")
+
+if __name__ == "__main__":
+  import logging
+  logging.basicConfig(level=logging.DEBUG)
+
+  set_time(logging)

selfdrive/boardd/spi.cc

@@ -0,0 +1,377 @@
+#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/boardd/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,
+};
+
+struct __attribute__((packed)) spi_header {
+  uint8_t sync;
+  uint8_t endpoint;
+  uint16_t tx_len;
+  uint16_t max_rx_len;
+};
+
+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;
+};
+
+
+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;
+
+  if (!util::file_exists(SPI_DEVICE)) {
+    goto fail;
+  }
+
+  spi_fd = open(SPI_DEVICE.c_str(), O_RDWR);
+  if (spi_fd < 0) {
+    LOGE("failed opening SPI device %d", spi_fd);
+    goto fail;
+  }
+
+  // SPI settings
+  ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MODE, &spi_mode);
+  if (ret < 0) {
+    LOGE("failed setting SPI mode %d", ret);
+    goto fail;
+  }
+
+  ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_MAX_SPEED_HZ, &spi_speed);
+  if (ret < 0) {
+    LOGE("failed setting SPI speed");
+    goto fail;
+  }
+
+  ret = util::safe_ioctl(spi_fd, SPI_IOC_WR_BITS_PER_WORD, &spi_bits_per_word);
+  if (ret < 0) {
+    LOGE("failed setting SPI bits per word");
+    goto fail;
+  }
+
+  // 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);
+    goto fail;
+  }
+
+  if (!serial.empty() && (serial != hw_serial)) {
+    goto fail;
+  }
+
+  return;
+
+fail:
+  cleanup();
+  throw std::runtime_error("Error connecting to panda");
+}
+
+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) {
+      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) {
+          usleep(std::clamp(nack_count*10, 200, 2000));
+        }
+      }
+    }
+  } while (ret < 0 && connected && !timed_out);
+
+  if (ret < 0) {
+    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, 100U, SPI_ACK_TIMEOUT);
+
+  spi_ioc_transfer transfer = {
+    .tx_buf = (uint64_t)tx_buf,
+    .rx_buf = (uint64_t)rx_buf,
+    .len = length
+  };
+  tx_buf[0] = tx;
+
+  while (true) {
+    int ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+    if (ret < 0) {
+      LOGE("SPI: failed to send ACK request");
+      return ret;
+    }
+
+    if (rx_buf[0] == ack) {
+      break;
+    } else if (rx_buf[0] == SPI_NACK) {
+      LOGD("SPI: got NACK");
+      return SpiError::NACK;
+    }
+
+    // handle timeout
+    if (millis_since_boot() - start_millis > timeout) {
+      LOGD("SPI: timed out waiting for ACK");
+      return SpiError::ACK_TIMEOUT;
+    }
+  }
+
+  return 0;
+}
+
+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);
+
+  spi_header 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 = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+  if (ret < 0) {
+    LOGE("SPI: failed to send header");
+    goto transfer_fail;
+  }
+
+  // Wait for (N)ACK
+  ret = wait_for_ack(SPI_HACK, 0x11, timeout, 1);
+  if (ret < 0) {
+    goto transfer_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 = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+  if (ret < 0) {
+    LOGE("SPI: failed to send data");
+    goto transfer_fail;
+  }
+
+  // Wait for (N)ACK
+  ret = wait_for_ack(SPI_DACK, 0x13, timeout, 3);
+  if (ret < 0) {
+    goto transfer_fail;
+  }
+
+  // Read data
+  rx_data_len = *(uint16_t *)(rx_buf+1);
+  if (rx_data_len >= SPI_BUF_SIZE) {
+    LOGE("SPI: RX data len larger than buf size %d", rx_data_len);
+    goto transfer_fail;
+  }
+
+  transfer.len = rx_data_len + 1;
+  transfer.rx_buf = (uint64_t)(rx_buf + 2 + 1);
+  ret = util::safe_ioctl(spi_fd, SPI_IOC_MESSAGE(1), &transfer);
+  if (ret < 0) {
+    LOGE("SPI: failed to read rx data");
+    goto transfer_fail;
+  }
+  if (!check_checksum(rx_buf, rx_data_len + 4)) {
+    LOGE("SPI: bad checksum");
+    goto transfer_fail;
+  }
+
+  if (rx_data != NULL) {
+    memcpy(rx_data, rx_buf + 3, rx_data_len);
+  }
+
+  return rx_data_len;
+
+transfer_fail:
+  return ret;
+}
+#endif

selfdrive/boardd/tests/test_boardd_loopback.py

@@ -0,0 +1,102 @@
+#!/usr/bin/env python3
+import os
+import copy
+import random
+import time
+import unittest
+from collections import defaultdict
+from pprint import pprint
+
+import cereal.messaging as messaging
+from cereal import car, log
+from openpilot.common.params import Params
+from openpilot.common.timeout import Timeout
+from openpilot.selfdrive.boardd.boardd import can_list_to_can_capnp
+from openpilot.selfdrive.car import make_can_msg
+from openpilot.system.hardware import TICI
+from openpilot.selfdrive.test.helpers import phone_only, with_processes
+
+
+class TestBoardd(unittest.TestCase):
+
+  @classmethod
+  def setUpClass(cls):
+    os.environ['STARTED'] = '1'
+    os.environ['BOARDD_LOOPBACK'] = '1'
+
+  @phone_only
+  @with_processes(['pandad'])
+  def test_loopback(self):
+    params = Params()
+    params.put_bool("IsOnroad", False)
+
+    with Timeout(90, "boardd didn't start"):
+      sm = messaging.SubMaster(['pandaStates'])
+      while sm.rcv_frame['pandaStates'] < 1 or len(sm['pandaStates']) == 0 or \
+          any(ps.pandaType == log.PandaState.PandaType.unknown for ps in sm['pandaStates']):
+        sm.update(1000)
+
+    num_pandas = len(sm['pandaStates'])
+    expected_pandas = 2 if TICI and "SINGLE_PANDA" not in os.environ else 1
+    self.assertEqual(num_pandas, expected_pandas, "connected pandas ({num_pandas}) doesn't match expected panda count ({expected_pandas}). \
+                                                   connect another panda for multipanda tests.")
+
+    # boardd 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())
+
+    sendcan = messaging.pub_sock('sendcan')
+    can = messaging.sub_sock('can', conflate=False, timeout=100)
+    sm = messaging.SubMaster(['pandaStates'])
+    time.sleep(0.5)
+
+    n = 200
+    for i in range(n):
+      print(f"boardd loopback {i}/{n}")
+
+      sent_msgs = defaultdict(set)
+      for _ in range(random.randrange(20, 100)):
+        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)))
+          sent_msgs[bus].add((addr, dat))
+          to_send.append(make_can_msg(addr, dat, bus))
+        sendcan.send(can_list_to_can_capnp(to_send, msgtype='sendcan'))
+
+      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"
+
+
+if __name__ == "__main__":
+  unittest.main()