selfdrive/boardd

selfdrive/boardd/.gitignore

@@ -0,0 +1,2 @@
+boardd
+boardd_api_impl.cpp

selfdrive/boardd/SConscript

@@ -0,0 +1,9 @@
+Import('env', 'common', 'messaging')
+
+env.Program('boardd.cc', LIBS=['usb-1.0', common, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
+env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
+
+env.Command(['boardd_api_impl.so'],
+  ['libcan_list_to_can_capnp.a', 'boardd_api_impl.pyx', 'boardd_setup.py'],
+  "cd selfdrive/boardd && python3 boardd_setup.py build_ext --inplace")
+

selfdrive/boardd/boardd.cc

@@ -0,0 +1,932 @@
+#include <stdio.h>
+#include <time.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sched.h>
+#include <string.h>
+#include <sys/cdefs.h>
+#include <sys/types.h>
+#include <sys/time.h>
+#include <sys/resource.h>
+
+#include <assert.h>
+#include <pthread.h>
+
+#include <libusb-1.0/libusb.h>
+
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+#include "cereal/gen/cpp/car.capnp.h"
+
+#include "common/util.h"
+#include "common/messaging.h"
+#include "common/params.h"
+#include "common/swaglog.h"
+#include "common/timing.h"
+#include "messaging.hpp"
+
+#include <algorithm>
+
+// double the FIFO size
+#define RECV_SIZE (0x1000)
+#define TIMEOUT 0
+
+#define MAX_IR_POWER 0.5f
+#define MIN_IR_POWER 0.0f
+#define CUTOFF_GAIN 0.015625f  // iso400
+#define SATURATE_GAIN 0.0625f  // iso1600
+#define NIBBLE_TO_HEX(n) ((n) < 10 ? (n) + '0' : ((n) - 10) + 'a')
+#define VOLTAGE_K 0.091  // LPF gain for 5s tau (dt/tau / (dt/tau + 1))
+
+namespace {
+
+volatile sig_atomic_t do_exit = 0;
+
+struct __attribute__((packed)) timestamp_t {
+    uint16_t year;
+    uint8_t month;
+    uint8_t day;
+    uint8_t weekday;
+    uint8_t hour;
+    uint8_t minute;
+    uint8_t second;
+};
+
+libusb_context *ctx = NULL;
+libusb_device_handle *dev_handle;
+pthread_mutex_t usb_lock;
+
+bool spoofing_started = false;
+bool fake_send = false;
+bool loopback_can = false;
+cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
+bool is_pigeon = false;
+const uint32_t NO_IGNITION_CNT_MAX = 2 * 60 * 60 * 30;  // turn off charge after 30 hrs
+const float VBATT_START_CHARGING = 11.5;
+const float VBATT_PAUSE_CHARGING = 11.0;
+float voltage_f = 12.5;  // filtered voltage
+uint32_t no_ignition_cnt = 0;
+bool connected_once = false;
+bool ignition_last = false;
+
+bool safety_setter_thread_initialized = false;
+pthread_t safety_setter_thread_handle;
+
+bool pigeon_thread_initialized = false;
+pthread_t pigeon_thread_handle;
+
+bool pigeon_needs_init;
+
+void pigeon_init();
+void *pigeon_thread(void *crap);
+
+void *safety_setter_thread(void *s) {
+  // diagnostic only is the default, needed for VIN query
+  pthread_mutex_lock(&usb_lock);
+  libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::ELM327), 0, NULL, 0, TIMEOUT);
+  pthread_mutex_unlock(&usb_lock);
+
+  char *value_vin;
+  size_t value_vin_sz = 0;
+
+  // switch to SILENT when CarVin param is read
+  while (1) {
+    if (do_exit) return NULL;
+    const int result = read_db_value(NULL, "CarVin", &value_vin, &value_vin_sz);
+    if (value_vin_sz > 0) {
+      // sanity check VIN format
+      assert(value_vin_sz == 17);
+      break;
+    }
+    usleep(100*1000);
+  }
+  LOGW("got CarVin %s", value_vin);
+
+  // VIN query done, stop listening to OBDII
+  pthread_mutex_lock(&usb_lock);
+  libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT);
+  pthread_mutex_unlock(&usb_lock);
+
+  char *value;
+  size_t value_sz = 0;
+
+  LOGW("waiting for params to set safety model");
+  while (1) {
+    if (do_exit) return NULL;
+
+    const int result = read_db_value(NULL, "CarParams", &value, &value_sz);
+    if (value_sz > 0) break;
+    usleep(100*1000);
+  }
+  LOGW("got %d bytes CarParams", value_sz);
+
+  // format for board, make copy due to alignment issues, will be freed on out of scope
+  auto amsg = kj::heapArray<capnp::word>((value_sz / sizeof(capnp::word)) + 1);
+  memcpy(amsg.begin(), value, value_sz);
+  free(value);
+
+  capnp::FlatArrayMessageReader cmsg(amsg);
+  cereal::CarParams::Reader car_params = cmsg.getRoot<cereal::CarParams>();
+
+  int safety_model = int(car_params.getSafetyModel());
+  auto safety_param = car_params.getSafetyParam();
+  LOGW("setting safety model: %d with param %d", safety_model, safety_param);
+
+  pthread_mutex_lock(&usb_lock);
+
+  // set in the mutex to avoid race
+  safety_setter_thread_initialized = false;
+
+  libusb_control_transfer(dev_handle, 0x40, 0xdc, safety_model, safety_param, NULL, 0, TIMEOUT);
+
+  pthread_mutex_unlock(&usb_lock);
+
+  return NULL;
+}
+
+// must be called before threads or with mutex
+bool usb_connect() {
+  int err, err2;
+  unsigned char hw_query[1] = {0};
+  unsigned char fw_sig_buf[128];
+  unsigned char fw_sig_hex_buf[16];
+  unsigned char serial_buf[16];
+  const char *serial;
+  int serial_sz = 0;
+
+  ignition_last = false;
+
+  dev_handle = libusb_open_device_with_vid_pid(ctx, 0xbbaa, 0xddcc);
+  if (dev_handle == NULL) { goto fail; }
+
+  err = libusb_set_configuration(dev_handle, 1);
+  if (err != 0) { goto fail; }
+
+  err = libusb_claim_interface(dev_handle, 0);
+  if (err != 0) { goto fail; }
+
+  if (loopback_can) {
+    libusb_control_transfer(dev_handle, 0xc0, 0xe5, 1, 0, NULL, 0, TIMEOUT);
+  }
+
+  // get panda fw
+  err = libusb_control_transfer(dev_handle, 0xc0, 0xd3, 0, 0, fw_sig_buf, 64, TIMEOUT);
+  err2 = libusb_control_transfer(dev_handle, 0xc0, 0xd4, 0, 0, fw_sig_buf + 64, 64, TIMEOUT);
+  if ((err == 64) && (err2 == 64)) {
+    printf("FW signature read\n");
+    write_db_value(NULL, "PandaFirmware", (const char *)fw_sig_buf, 128);
+
+    for (size_t i = 0; i < 8; i++){
+      fw_sig_hex_buf[2*i] = NIBBLE_TO_HEX(fw_sig_buf[i] >> 4);
+      fw_sig_hex_buf[2*i+1] = NIBBLE_TO_HEX(fw_sig_buf[i] & 0xF);
+    }
+    write_db_value(NULL, "PandaFirmwareHex", (const char *)fw_sig_hex_buf, 16);
+  }
+  else { goto fail; }
+
+  // get panda serial
+  err = libusb_control_transfer(dev_handle, 0xc0, 0xd0, 0, 0, serial_buf, 16, TIMEOUT);
+
+  if (err > 0) {
+    serial = (const char *)serial_buf;
+    serial_sz = strnlen(serial, err);
+    write_db_value(NULL, "PandaDongleId", serial, serial_sz);
+    printf("panda serial: %.*s\n", serial_sz, serial);
+  }
+  else { goto fail; }
+
+  // power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
+#ifndef __x86_64__
+  if (!connected_once) {
+    libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT);
+  }
+#endif
+  connected_once = true;
+
+  libusb_control_transfer(dev_handle, 0xc0, 0xc1, 0, 0, hw_query, 1, TIMEOUT);
+
+  hw_type = (cereal::HealthData::HwType)(hw_query[0]);
+  is_pigeon = (hw_type == cereal::HealthData::HwType::GREY_PANDA) ||
+              (hw_type == cereal::HealthData::HwType::BLACK_PANDA) ||
+              (hw_type == cereal::HealthData::HwType::UNO);
+  if (is_pigeon) {
+    LOGW("panda with gps detected");
+    pigeon_needs_init = true;
+    if (!pigeon_thread_initialized) {
+      err = pthread_create(&pigeon_thread_handle, NULL, pigeon_thread, NULL);
+      assert(err == 0);
+      pigeon_thread_initialized = true;
+    }
+  }
+
+  if (hw_type == cereal::HealthData::HwType::UNO){
+    // Get time from system
+    time_t rawtime;
+    time(&rawtime);
+
+    struct tm * sys_time = gmtime(&rawtime);
+
+    // Get time from RTC
+    timestamp_t rtc_time;
+    libusb_control_transfer(dev_handle, 0xc0, 0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time), TIMEOUT);
+
+    //printf("System: %d-%d-%d\t%d:%d:%d\n", 1900 + sys_time->tm_year, 1 + sys_time->tm_mon, sys_time->tm_mday, sys_time->tm_hour, sys_time->tm_min, sys_time->tm_sec);
+    //printf("RTC: %d-%d-%d\t%d:%d:%d\n", rtc_time.year, rtc_time.month, rtc_time.day, rtc_time.hour, rtc_time.minute, rtc_time.second);
+
+    // Update system time from RTC if it looks off, and RTC time is good
+    if (1900 + sys_time->tm_year < 2019 && rtc_time.year >= 2019){
+      LOGE("System time wrong, setting from RTC");
+
+      struct tm new_time = { 0 };
+      new_time.tm_year = rtc_time.year - 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;
+
+      setenv("TZ","UTC",1);
+      const struct timeval tv = {mktime(&new_time), 0};
+      settimeofday(&tv, 0);
+    }
+  }
+
+  return true;
+fail:
+  return false;
+}
+
+void usb_retry_connect() {
+  LOG("attempting to connect");
+  while (!usb_connect()) { usleep(100*1000); }
+  LOGW("connected to board");
+}
+
+void 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 == -4) {
+    LOGE("lost connection");
+    usb_retry_connect();
+  }
+  // TODO: check other errors, is simply retrying okay?
+}
+
+void can_recv(PubSocket *publisher) {
+  int err;
+  uint32_t data[RECV_SIZE/4];
+  int recv;
+  uint32_t f1, f2;
+
+  uint64_t start_time = nanos_since_boot();
+
+  // do recv
+  pthread_mutex_lock(&usb_lock);
+
+  do {
+    err = libusb_bulk_transfer(dev_handle, 0x81, (uint8_t*)data, RECV_SIZE, &recv, TIMEOUT);
+    if (err != 0) { handle_usb_issue(err, __func__); }
+    if (err == -8) { LOGE_100("overflow got 0x%x", recv); };
+
+    // timeout is okay to exit, recv still happened
+    if (err == -7) { break; }
+  } while(err != 0);
+
+  pthread_mutex_unlock(&usb_lock);
+
+  // return if length is 0
+  if (recv <= 0) {
+    return;
+  } else if (recv == RECV_SIZE) {
+    LOGW("Receive buffer full");
+  }
+
+  // create message
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(start_time);
+  size_t num_msg = recv / 0x10;
+  auto canData = event.initCan(num_msg);
+
+  // populate message
+  for (int i = 0; i < num_msg; i++) {
+    if (data[i*4] & 4) {
+      // extended
+      canData[i].setAddress(data[i*4] >> 3);
+      //printf("got extended: %x\n", data[i*4] >> 3);
+    } else {
+      // normal
+      canData[i].setAddress(data[i*4] >> 21);
+    }
+    canData[i].setBusTime(data[i*4+1] >> 16);
+    int len = data[i*4+1]&0xF;
+    canData[i].setDat(kj::arrayPtr((uint8_t*)&data[i*4+2], len));
+    canData[i].setSrc((data[i*4+1] >> 4) & 0xff);
+  }
+
+  // send to can
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  publisher->send((char*)bytes.begin(), bytes.size());
+}
+
+void can_health(PubSocket *publisher) {
+  int cnt;
+  int err;
+
+  // copied from panda/board/main.c
+  struct __attribute__((packed)) health {
+    uint32_t uptime;
+    uint32_t voltage;
+    uint32_t current;
+    uint32_t can_rx_errs;
+    uint32_t can_send_errs;
+    uint32_t can_fwd_errs;
+    uint32_t gmlan_send_errs;
+    uint32_t faults;
+    uint8_t ignition_line;
+    uint8_t ignition_can;
+    uint8_t controls_allowed;
+    uint8_t gas_interceptor_detected;
+    uint8_t car_harness_status;
+    uint8_t usb_power_mode;
+    uint8_t safety_model;
+    uint8_t fault_status;
+    uint8_t power_save_enabled;
+  } health;
+
+  // recv from board
+  pthread_mutex_lock(&usb_lock);
+  do {
+    cnt = libusb_control_transfer(dev_handle, 0xc0, 0xd2, 0, 0, (unsigned char*)&health, sizeof(health), TIMEOUT);
+    if (cnt != sizeof(health)) {
+      handle_usb_issue(cnt, __func__);
+    }
+  } while(cnt != sizeof(health));
+  pthread_mutex_unlock(&usb_lock);
+
+  if (spoofing_started) {
+    health.ignition_line = 1;
+  }
+
+  voltage_f = VOLTAGE_K * (health.voltage / 1000.0) + (1.0 - VOLTAGE_K) * voltage_f;  // LPF
+
+  // 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_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
+    pthread_mutex_lock(&usb_lock);
+    libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT);
+    pthread_mutex_unlock(&usb_lock);
+  }
+
+  bool ignition = ((health.ignition_line != 0) || (health.ignition_can != 0));
+
+  if (ignition) {
+    no_ignition_cnt = 0;
+  } else {
+    no_ignition_cnt += 1;
+  }
+
+#ifndef __x86_64__
+  bool cdp_mode = health.usb_power_mode == (uint8_t)(cereal::HealthData::UsbPowerMode::CDP);
+  bool no_ignition_exp = no_ignition_cnt > NO_IGNITION_CNT_MAX;
+  if ((no_ignition_exp || (voltage_f < VBATT_PAUSE_CHARGING)) && cdp_mode && !ignition) {
+    printf("TURN OFF CHARGING!\n");
+    pthread_mutex_lock(&usb_lock);
+    libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CLIENT), 0, NULL, 0, TIMEOUT);
+    pthread_mutex_unlock(&usb_lock);
+    printf("POWER DOWN DEVICE\n");
+    system("service call power 17 i32 0 i32 1");
+  }
+  if (!no_ignition_exp && (voltage_f > VBATT_START_CHARGING) && !cdp_mode) {
+    printf("TURN ON CHARGING!\n");
+    pthread_mutex_lock(&usb_lock);
+    libusb_control_transfer(dev_handle, 0xc0, 0xe6, (uint16_t)(cereal::HealthData::UsbPowerMode::CDP), 0, NULL, 0, TIMEOUT);
+    pthread_mutex_unlock(&usb_lock);
+  }
+  // set power save state enabled when car is off and viceversa when it's on
+  if (ignition && (health.power_save_enabled == 1)) {
+    pthread_mutex_lock(&usb_lock);
+    libusb_control_transfer(dev_handle, 0xc0, 0xe7, 0, 0, NULL, 0, TIMEOUT);
+    pthread_mutex_unlock(&usb_lock);
+  }
+  if (!ignition && (health.power_save_enabled == 0)) {
+    pthread_mutex_lock(&usb_lock);
+    libusb_control_transfer(dev_handle, 0xc0, 0xe7, 1, 0, NULL, 0, TIMEOUT);
+    pthread_mutex_unlock(&usb_lock);
+  }
+  // set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
+  if (!ignition && (health.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
+    pthread_mutex_lock(&usb_lock);
+    libusb_control_transfer(dev_handle, 0x40, 0xdc, (uint16_t)(cereal::CarParams::SafetyModel::NO_OUTPUT), 0, NULL, 0, TIMEOUT);
+    pthread_mutex_unlock(&usb_lock);
+  }
+#endif
+
+  // clear VIN, CarParams, and set new safety on car start
+  if (ignition && !ignition_last) {
+    int result = delete_db_value(NULL, "CarVin");
+    assert((result == 0) || (result == ERR_NO_VALUE));
+    result = delete_db_value(NULL, "CarParams");
+    assert((result == 0) || (result == ERR_NO_VALUE));
+
+    if (!safety_setter_thread_initialized) {
+      err = pthread_create(&safety_setter_thread_handle, NULL, safety_setter_thread, NULL);
+      assert(err == 0);
+      safety_setter_thread_initialized = true;
+    }
+  }
+
+  // Get fan RPM
+  uint16_t fan_speed_rpm = 0;
+
+  pthread_mutex_lock(&usb_lock);
+  int sz = libusb_control_transfer(dev_handle, 0xc0, 0xb2, 0, 0, (unsigned char*)&fan_speed_rpm, sizeof(fan_speed_rpm), TIMEOUT);
+  pthread_mutex_unlock(&usb_lock);
+
+  // Write to rtc once per minute when no ignition present
+  if ((hw_type == cereal::HealthData::HwType::UNO) && !ignition && (no_ignition_cnt % 120 == 1)){
+    // Get time from system
+    time_t rawtime;
+    time(&rawtime);
+
+    struct tm * sys_time = gmtime(&rawtime);
+
+    // Write time to RTC if it looks reasonable
+    if (1900 + sys_time->tm_year >= 2019){
+      pthread_mutex_lock(&usb_lock);
+      libusb_control_transfer(dev_handle, 0x40, 0xa1, (uint16_t)(1900 + sys_time->tm_year), 0, NULL, 0, TIMEOUT);
+      libusb_control_transfer(dev_handle, 0x40, 0xa2, (uint16_t)(1 + sys_time->tm_mon), 0, NULL, 0, TIMEOUT);
+      libusb_control_transfer(dev_handle, 0x40, 0xa3, (uint16_t)sys_time->tm_mday, 0, NULL, 0, TIMEOUT);
+      // libusb_control_transfer(dev_handle, 0x40, 0xa4, (uint16_t)(1 + sys_time->tm_wday), 0, NULL, 0, TIMEOUT);
+      libusb_control_transfer(dev_handle, 0x40, 0xa5, (uint16_t)sys_time->tm_hour, 0, NULL, 0, TIMEOUT);
+      libusb_control_transfer(dev_handle, 0x40, 0xa6, (uint16_t)sys_time->tm_min, 0, NULL, 0, TIMEOUT);
+      libusb_control_transfer(dev_handle, 0x40, 0xa7, (uint16_t)sys_time->tm_sec, 0, NULL, 0, TIMEOUT);
+      pthread_mutex_unlock(&usb_lock);
+    }
+  }
+
+  ignition_last = ignition;
+
+  // create message
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  auto healthData = event.initHealth();
+
+  // set fields
+  healthData.setUptime(health.uptime);
+  healthData.setVoltage(health.voltage);
+  healthData.setCurrent(health.current);
+  healthData.setIgnitionLine(health.ignition_line);
+  healthData.setIgnitionCan(health.ignition_can);
+  healthData.setControlsAllowed(health.controls_allowed);
+  healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
+  healthData.setHasGps(is_pigeon);
+  healthData.setCanRxErrs(health.can_rx_errs);
+  healthData.setCanSendErrs(health.can_send_errs);
+  healthData.setCanFwdErrs(health.can_fwd_errs);
+  healthData.setGmlanSendErrs(health.gmlan_send_errs);
+  healthData.setHwType(hw_type);
+  healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode));
+  healthData.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model));
+  healthData.setFanSpeedRpm(fan_speed_rpm);
+  healthData.setFaultStatus(cereal::HealthData::FaultStatus(health.fault_status));
+  healthData.setPowerSaveEnabled((bool)(health.power_save_enabled));
+
+  // send to health
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  publisher->send((char*)bytes.begin(), bytes.size());
+
+  pthread_mutex_lock(&usb_lock);
+
+  // send heartbeat back to panda
+  libusb_control_transfer(dev_handle, 0x40, 0xf3, 1, 0, NULL, 0, TIMEOUT);
+
+  pthread_mutex_unlock(&usb_lock);
+}
+
+
+void can_send(SubSocket *subscriber) {
+  int err;
+
+  // recv from sendcan
+  Message * msg = subscriber->receive();
+
+  auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
+  memcpy(amsg.begin(), msg->getData(), msg->getSize());
+
+  capnp::FlatArrayMessageReader cmsg(amsg);
+  cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+  if (nanos_since_boot() - event.getLogMonoTime() > 1e9) {
+    //Older than 1 second. Dont send.
+    delete msg;
+    return;
+  }
+  int msg_count = event.getSendcan().size();
+
+  uint32_t *send = (uint32_t*)malloc(msg_count*0x10);
+  memset(send, 0, msg_count*0x10);
+
+  for (int i = 0; i < msg_count; i++) {
+    auto cmsg = event.getSendcan()[i];
+    if (cmsg.getAddress() >= 0x800) {
+      // extended
+      send[i*4] = (cmsg.getAddress() << 3) | 5;
+    } else {
+      // normal
+      send[i*4] = (cmsg.getAddress() << 21) | 1;
+    }
+    assert(cmsg.getDat().size() <= 8);
+    send[i*4+1] = cmsg.getDat().size() | (cmsg.getSrc() << 4);
+    memcpy(&send[i*4+2], cmsg.getDat().begin(), cmsg.getDat().size());
+  }
+
+  // release msg
+  delete msg;
+
+  // send to board
+  int sent;
+  pthread_mutex_lock(&usb_lock);
+
+  if (!fake_send) {
+    do {
+      err = libusb_bulk_transfer(dev_handle, 3, (uint8_t*)send, msg_count*0x10, &sent, TIMEOUT);
+      if (err != 0 || msg_count*0x10 != sent) { handle_usb_issue(err, __func__); }
+    } while(err != 0);
+  }
+
+  pthread_mutex_unlock(&usb_lock);
+
+  // done
+  free(send);
+}
+
+// **** threads ****
+
+void *can_send_thread(void *crap) {
+  LOGD("start send thread");
+
+  // sendcan = 8017
+  Context * context = Context::create();
+  SubSocket * subscriber = SubSocket::create(context, "sendcan");
+  assert(subscriber != NULL);
+
+
+  // drain sendcan to delete any stale messages from previous runs
+  while (true){
+    Message * msg = subscriber->receive(true);
+    if (msg == NULL){
+      break;
+    }
+    delete msg;
+  }
+
+  // run as fast as messages come in
+  while (!do_exit) {
+    can_send(subscriber);
+  }
+  return NULL;
+}
+
+void *can_recv_thread(void *crap) {
+  LOGD("start recv thread");
+
+  // can = 8006
+  Context * c = Context::create();
+  PubSocket * publisher = PubSocket::create(c, "can");
+  assert(publisher != NULL);
+
+  // run at 100hz
+  const uint64_t dt = 10000000ULL;
+  uint64_t next_frame_time = nanos_since_boot() + dt;
+
+  while (!do_exit) {
+    can_recv(publisher);
+
+    uint64_t cur_time = nanos_since_boot();
+    int64_t remaining = next_frame_time - cur_time;
+    if (remaining > 0){
+      useconds_t sleep = remaining / 1000;
+      usleep(sleep);
+    } else {
+      LOGW("missed cycle");
+      next_frame_time = cur_time;
+    }
+
+    next_frame_time += dt;
+  }
+  return NULL;
+}
+
+void *can_health_thread(void *crap) {
+  LOGD("start health thread");
+  // health = 8011
+  Context * c = Context::create();
+  PubSocket * publisher = PubSocket::create(c, "health");
+  assert(publisher != NULL);
+
+  // run at 2hz
+  while (!do_exit) {
+    can_health(publisher);
+    usleep(500*1000);
+  }
+  return NULL;
+}
+
+void *hardware_control_thread(void *crap) {
+  LOGD("start hardware control thread");
+  Context * c = Context::create();
+  SubSocket * thermal_sock = SubSocket::create(c, "thermal");
+  SubSocket * front_frame_sock = SubSocket::create(c, "frontFrame");
+  assert(thermal_sock != NULL);
+  assert(front_frame_sock != NULL);
+
+  Poller * poller = Poller::create({thermal_sock, front_frame_sock});
+
+  // Wait for hardware type to be set.
+  while (hw_type == cereal::HealthData::HwType::UNKNOWN){
+    usleep(100*1000);
+  }
+  // Only control fan speed on UNO
+  if (hw_type != cereal::HealthData::HwType::UNO) return NULL;
+
+
+  uint16_t prev_fan_speed = 999;
+  uint16_t prev_ir_pwr = 999;
+  unsigned int cnt = 0;
+
+  while (!do_exit) {
+    cnt++;
+    for (auto sock : poller->poll(1000)){
+      Message * msg = sock->receive();
+      if (msg == NULL) continue;
+
+      auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
+      memcpy(amsg.begin(), msg->getData(), msg->getSize());
+
+      delete msg;
+
+      capnp::FlatArrayMessageReader cmsg(amsg);
+      cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+      auto type = event.which();
+      if(type == cereal::Event::THERMAL){
+        uint16_t fan_speed = event.getThermal().getFanSpeed();
+        if (fan_speed != prev_fan_speed || cnt % 100 == 0){
+          pthread_mutex_lock(&usb_lock);
+          libusb_control_transfer(dev_handle, 0x40, 0xb1, fan_speed, 0, NULL, 0, TIMEOUT);
+          pthread_mutex_unlock(&usb_lock);
+
+          prev_fan_speed = fan_speed;
+        }
+      } else if (type == cereal::Event::FRONT_FRAME){
+        float cur_front_gain = event.getFrontFrame().getGainFrac();
+        uint16_t ir_pwr;
+          if (cur_front_gain <= CUTOFF_GAIN) {
+            ir_pwr = 100.0 * MIN_IR_POWER;
+          } else if (cur_front_gain > SATURATE_GAIN) {
+            ir_pwr = 100.0 * MAX_IR_POWER;
+          } else {
+            ir_pwr = 100.0 * (MIN_IR_POWER + ((cur_front_gain - CUTOFF_GAIN) * (MAX_IR_POWER - MIN_IR_POWER) / (SATURATE_GAIN - CUTOFF_GAIN)));
+          }
+
+        if (ir_pwr != prev_ir_pwr || cnt % 100 == 0 || ir_pwr >= 50.0){
+          pthread_mutex_lock(&usb_lock);
+          libusb_control_transfer(dev_handle, 0x40, 0xb0, ir_pwr, 0, NULL, 0, TIMEOUT);
+          pthread_mutex_unlock(&usb_lock);
+
+          prev_ir_pwr = ir_pwr;
+        }
+      }
+    }
+  }
+
+  delete poller;
+  delete thermal_sock;
+  delete c;
+
+  return NULL;
+}
+
+#define pigeon_send(x) _pigeon_send(x, sizeof(x)-1)
+
+void hexdump(unsigned char *d, int l) {
+  for (int i = 0; i < l; i++) {
+    if (i!=0 && i%0x10 == 0) printf("\n");
+    printf("%2.2X ", d[i]);
+  }
+  printf("\n");
+}
+
+void _pigeon_send(const char *dat, int len) {
+  int sent;
+  unsigned char a[0x20];
+  int err;
+  a[0] = 1;
+  for (int i=0; i<len; i+=0x20) {
+    int ll = std::min(0x20, len-i);
+    memcpy(&a[1], &dat[i], ll);
+    pthread_mutex_lock(&usb_lock);
+    err = libusb_bulk_transfer(dev_handle, 2, a, ll+1, &sent, TIMEOUT);
+    if (err < 0) { handle_usb_issue(err, __func__); }
+    /*assert(err == 0);
+    assert(sent == ll+1);*/
+    //hexdump(a, ll+1);
+    pthread_mutex_unlock(&usb_lock);
+  }
+}
+
+void pigeon_set_power(int power) {
+  pthread_mutex_lock(&usb_lock);
+  int err = libusb_control_transfer(dev_handle, 0xc0, 0xd9, power, 0, NULL, 0, TIMEOUT);
+  if (err < 0) { handle_usb_issue(err, __func__); }
+  pthread_mutex_unlock(&usb_lock);
+}
+
+void pigeon_set_baud(int baud) {
+  int err;
+  pthread_mutex_lock(&usb_lock);
+  err = libusb_control_transfer(dev_handle, 0xc0, 0xe2, 1, 0, NULL, 0, TIMEOUT);
+  if (err < 0) { handle_usb_issue(err, __func__); }
+  err = libusb_control_transfer(dev_handle, 0xc0, 0xe4, 1, baud/300, NULL, 0, TIMEOUT);
+  if (err < 0) { handle_usb_issue(err, __func__); }
+  pthread_mutex_unlock(&usb_lock);
+}
+
+void pigeon_init() {
+  usleep(1000*1000);
+  LOGW("panda GPS start");
+
+  // power off pigeon
+  pigeon_set_power(0);
+  usleep(100*1000);
+
+  // 9600 baud at init
+  pigeon_set_baud(9600);
+
+  // power on pigeon
+  pigeon_set_power(1);
+  usleep(500*1000);
+
+  // baud rate upping
+  pigeon_send("\x24\x50\x55\x42\x58\x2C\x34\x31\x2C\x31\x2C\x30\x30\x30\x37\x2C\x30\x30\x30\x33\x2C\x34\x36\x30\x38\x30\x30\x2C\x30\x2A\x31\x35\x0D\x0A");
+  usleep(100*1000);
+
+  // set baud rate to 460800
+  pigeon_set_baud(460800);
+  usleep(100*1000);
+
+  // init from ubloxd
+  pigeon_send("\xB5\x62\x06\x00\x14\x00\x03\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x01\x00\x01\x00\x00\x00\x00\x00\x1E\x7F");
+  pigeon_send("\xB5\x62\x06\x3E\x00\x00\x44\xD2");
+  pigeon_send("\xB5\x62\x06\x00\x14\x00\x00\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x19\x35");
+  pigeon_send("\xB5\x62\x06\x00\x14\x00\x01\x00\x00\x00\xC0\x08\x00\x00\x00\x08\x07\x00\x01\x00\x01\x00\x00\x00\x00\x00\xF4\x80");
+  pigeon_send("\xB5\x62\x06\x00\x14\x00\x04\xFF\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x1D\x85");
+  pigeon_send("\xB5\x62\x06\x00\x00\x00\x06\x18");
+  pigeon_send("\xB5\x62\x06\x00\x01\x00\x01\x08\x22");
+  pigeon_send("\xB5\x62\x06\x00\x01\x00\x02\x09\x23");
+  pigeon_send("\xB5\x62\x06\x00\x01\x00\x03\x0A\x24");
+  pigeon_send("\xB5\x62\x06\x08\x06\x00\x64\x00\x01\x00\x00\x00\x79\x10");
+  pigeon_send("\xB5\x62\x06\x24\x24\x00\x05\x00\x04\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x5A\x63");
+  pigeon_send("\xB5\x62\x06\x1E\x14\x00\x00\x00\x00\x00\x01\x03\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x3C\x37");
+  pigeon_send("\xB5\x62\x06\x24\x00\x00\x2A\x84");
+  pigeon_send("\xB5\x62\x06\x23\x00\x00\x29\x81");
+  pigeon_send("\xB5\x62\x06\x1E\x00\x00\x24\x72");
+  pigeon_send("\xB5\x62\x06\x01\x03\x00\x01\x07\x01\x13\x51");
+  pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x15\x01\x22\x70");
+  pigeon_send("\xB5\x62\x06\x01\x03\x00\x02\x13\x01\x20\x6C");
+
+  LOGW("panda GPS on");
+}
+
+static void pigeon_publish_raw(PubSocket *publisher, unsigned char *dat, int alen) {
+  // create message
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  auto ublox_raw = event.initUbloxRaw(alen);
+  memcpy(ublox_raw.begin(), dat, alen);
+
+  // send to ubloxRaw
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  publisher->send((char*)bytes.begin(), bytes.size());
+}
+
+
+void *pigeon_thread(void *crap) {
+  // ubloxRaw = 8042
+  Context * context = Context::create();
+  PubSocket * publisher = PubSocket::create(context, "ubloxRaw");
+  assert(publisher != NULL);
+
+  // run at ~100hz
+  unsigned char dat[0x1000];
+  uint64_t cnt = 0;
+  while (!do_exit) {
+    if (pigeon_needs_init) {
+      pigeon_needs_init = false;
+      pigeon_init();
+    }
+    int alen = 0;
+    while (alen < 0xfc0) {
+      pthread_mutex_lock(&usb_lock);
+      int len = libusb_control_transfer(dev_handle, 0xc0, 0xe0, 1, 0, dat+alen, 0x40, TIMEOUT);
+      if (len < 0) { handle_usb_issue(len, __func__); }
+      pthread_mutex_unlock(&usb_lock);
+      if (len <= 0) break;
+
+      //printf("got %d\n", len);
+      alen += len;
+    }
+    if (alen > 0) {
+      if (dat[0] == (char)0x00){
+        LOGW("received invalid ublox message, resetting panda GPS");
+        pigeon_init();
+      } else {
+        pigeon_publish_raw(publisher, dat, alen);
+      }
+    }
+
+    // 10ms
+    usleep(10*1000);
+    cnt++;
+  }
+
+  return NULL;
+}
+
+}
+
+int main() {
+  int err;
+  LOGW("starting boardd");
+
+  // set process priority
+  err = set_realtime_priority(4);
+  LOG("setpriority returns %d", err);
+
+  // check the environment
+  if (getenv("STARTED")) {
+    spoofing_started = true;
+  }
+
+  if (getenv("FAKESEND")) {
+    fake_send = true;
+  }
+
+  if (getenv("BOARDD_LOOPBACK")){
+    loopback_can = true;
+  }
+
+  // init libusb
+  err = libusb_init(&ctx);
+  assert(err == 0);
+  libusb_set_debug(ctx, 3);
+
+  // connect to the board
+  usb_retry_connect();
+
+
+  // create threads
+  pthread_t can_health_thread_handle;
+  err = pthread_create(&can_health_thread_handle, NULL,
+                       can_health_thread, NULL);
+  assert(err == 0);
+
+  pthread_t can_send_thread_handle;
+  err = pthread_create(&can_send_thread_handle, NULL,
+                       can_send_thread, NULL);
+  assert(err == 0);
+
+  pthread_t can_recv_thread_handle;
+  err = pthread_create(&can_recv_thread_handle, NULL,
+                       can_recv_thread, NULL);
+  assert(err == 0);
+
+  pthread_t hardware_control_thread_handle;
+  err = pthread_create(&hardware_control_thread_handle, NULL,
+                       hardware_control_thread, NULL);
+  assert(err == 0);
+
+  // join threads
+
+  err = pthread_join(can_recv_thread_handle, NULL);
+  assert(err == 0);
+
+  err = pthread_join(can_send_thread_handle, NULL);
+  assert(err == 0);
+
+  err = pthread_join(can_health_thread_handle, NULL);
+  assert(err == 0);
+
+  //while (!do_exit) usleep(1000);
+
+  // destruct libusb
+
+  libusb_close(dev_handle);
+  libusb_exit(ctx);
+}

selfdrive/boardd/boardd.py

@@ -0,0 +1,12 @@
+# pylint: skip-file
+
+# Cython, now uses scons to build
+from 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,26 @@
+# distutils: language = c++
+# cython: language_level=3
+from libcpp.vector cimport vector
+from libcpp.string cimport string
+from libcpp cimport bool
+
+cdef struct can_frame:
+  long address
+  string dat
+  long busTime
+  long src
+
+cdef extern 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
+  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/boardd_setup.py

@@ -0,0 +1,31 @@
+import subprocess
+from distutils.core import Extension, setup
+
+from Cython.Build import cythonize
+
+from common.cython_hacks import BuildExtWithoutPlatformSuffix
+from common.basedir import BASEDIR
+import os
+
+PHONELIBS = os.path.join(BASEDIR, 'phonelibs')
+
+ARCH = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
+ARCH_DIR = 'x64' if ARCH == "x86_64" else 'aarch64'
+
+setup(name='Boardd API Implementation',
+      cmdclass={'build_ext': BuildExtWithoutPlatformSuffix},
+      ext_modules=cythonize(
+        Extension(
+          "boardd_api_impl",
+          libraries=[':libcan_list_to_can_capnp.a', ':libcapnp.a', ':libkj.a'] if ARCH == "x86_64" else [':libcan_list_to_can_capnp.a', 'capnp', 'kj'],
+          library_dirs=[
+            './',
+            PHONELIBS + '/capnp-cpp/' + ARCH_DIR + '/lib/',
+            PHONELIBS + '/capnp-c/' + ARCH_DIR + '/lib/'
+          ],
+          sources=['boardd_api_impl.pyx'],
+          language="c++",
+          extra_compile_args=["-std=c++11"],
+        )
+      )
+)

selfdrive/boardd/can_list_to_can_capnp.cc

@@ -0,0 +1,37 @@
+#include <vector>
+#include <tuple>
+#include <string>
+#include "common/timing.h"
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+#include "cereal/gen/cpp/car.capnp.h"
+
+typedef struct {
+	long address;
+	std::string dat;
+	long busTime;
+	long src;
+} can_frame;
+
+extern "C" {
+
+void can_list_to_can_capnp_cpp(const std::vector<can_frame> &can_list, std::string &out, bool sendCan, bool valid) {
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  event.setValid(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++) {
+    canData[j].setAddress(it->address);
+    canData[j].setBusTime(it->busTime);
+    canData[j].setDat(kj::arrayPtr((uint8_t*)it->dat.data(), it->dat.size()));
+    canData[j].setSrc(it->src);
+  }
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  out.append((const char *)bytes.begin(), bytes.size());
+}
+
+}

selfdrive/boardd/tests/boardd_old.py

@@ -0,0 +1,246 @@
+#!/usr/bin/env python3
+
+# This file is not used by OpenPilot. Only boardd.cc is used.
+# The python version is slower, but has more options for development.
+
+# TODO: merge the extra functionalities of this file (like MOCK) in boardd.c and
+# delete this python version of boardd
+
+import os
+import struct
+import time
+
+import cereal.messaging as messaging
+from common.realtime import Ratekeeper
+from selfdrive.swaglog import cloudlog
+from selfdrive.boardd.boardd import can_capnp_to_can_list
+from cereal import car
+
+SafetyModel = car.CarParams.SafetyModel
+
+# USB is optional
+try:
+  import usb1
+  from usb1 import USBErrorIO, USBErrorOverflow  #pylint: disable=no-name-in-module
+except Exception:
+  pass
+
+# *** serialization functions ***
+def can_list_to_can_capnp(can_msgs, msgtype='can'):
+  dat = messaging.new_message()
+  dat.init(msgtype, len(can_msgs))
+  for i, can_msg in enumerate(can_msgs):
+    if msgtype == 'sendcan':
+      cc = dat.sendcan[i]
+    else:
+      cc = dat.can[i]
+    cc.address = can_msg[0]
+    cc.busTime = can_msg[1]
+    cc.dat = bytes(can_msg[2])
+    cc.src = can_msg[3]
+  return dat
+
+
+# *** can driver ***
+def can_health():
+  while 1:
+    try:
+      dat = handle.controlRead(usb1.TYPE_VENDOR | usb1.RECIPIENT_DEVICE, 0xd2, 0, 0, 0x16)
+      break
+    except (USBErrorIO, USBErrorOverflow):
+      cloudlog.exception("CAN: BAD HEALTH, RETRYING")
+  v, i = struct.unpack("II", dat[0:8])
+  ign_line, ign_can = struct.unpack("BB", dat[20:22])
+  return {"voltage": v, "current": i, "ignition_line": bool(ign_line), "ignition_can": bool(ign_can)}
+
+def __parse_can_buffer(dat):
+  ret = []
+  for j in range(0, len(dat), 0x10):
+    ddat = dat[j:j+0x10]
+    f1, f2 = struct.unpack("II", ddat[0:8])
+    ret.append((f1 >> 21, f2>>16, ddat[8:8+(f2&0xF)], (f2>>4)&0xFF))
+  return ret
+
+def can_send_many(arr):
+  snds = []
+  for addr, _, dat, alt in arr:
+    if addr < 0x800:  # only support 11 bit addr
+      snd = struct.pack("II", ((addr << 21) | 1), len(dat) | (alt << 4)) + dat
+      snd = snd.ljust(0x10, b'\x00')
+      snds.append(snd)
+  while 1:
+    try:
+      handle.bulkWrite(3, b''.join(snds))
+      break
+    except (USBErrorIO, USBErrorOverflow):
+      cloudlog.exception("CAN: BAD SEND MANY, RETRYING")
+
+def can_recv():
+  dat = b""
+  while 1:
+    try:
+      dat = handle.bulkRead(1, 0x10*256)
+      break
+    except (USBErrorIO, USBErrorOverflow):
+      cloudlog.exception("CAN: BAD RECV, RETRYING")
+  return __parse_can_buffer(dat)
+
+def can_init():
+  global handle, context
+  handle = None
+  cloudlog.info("attempting can init")
+
+  context = usb1.USBContext()
+  #context.setDebug(9)
+
+  for device in context.getDeviceList(skip_on_error=True):
+    if device.getVendorID() == 0xbbaa and device.getProductID() == 0xddcc:
+      handle = device.open()
+      handle.claimInterface(0)
+      handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
+
+  if handle is None:
+    cloudlog.warning("CAN NOT FOUND")
+    exit(-1)
+
+  cloudlog.info("got handle")
+  cloudlog.info("can init done")
+
+def boardd_mock_loop():
+  can_init()
+  handle.controlWrite(0x40, 0xdc, SafetyModel.allOutput, 0, b'')
+
+  logcan = messaging.sub_sock('can')
+  sendcan = messaging.pub_sock('sendcan')
+
+  while 1:
+    tsc = messaging.drain_sock(logcan, wait_for_one=True)
+    snds = map(lambda x: can_capnp_to_can_list(x.can), tsc)
+    snd = []
+    for s in snds:
+      snd += s
+    snd = list(filter(lambda x: x[-1] <= 2, snd))
+    snd_0 = len(list(filter(lambda x: x[-1] == 0, snd)))
+    snd_1 = len(list(filter(lambda x: x[-1] == 1, snd)))
+    snd_2 = len(list(filter(lambda x: x[-1] == 2, snd)))
+    can_send_many(snd)
+
+    # recv @ 100hz
+    can_msgs = can_recv()
+    got_0 = len(list(filter(lambda x: x[-1] == 0+0x80, can_msgs)))
+    got_1 = len(list(filter(lambda x: x[-1] == 1+0x80, can_msgs)))
+    got_2 = len(list(filter(lambda x: x[-1] == 2+0x80, can_msgs)))
+    print("sent %3d (%3d/%3d/%3d) got %3d (%3d/%3d/%3d)" %
+      (len(snd), snd_0, snd_1, snd_2, len(can_msgs), got_0, got_1, got_2))
+    m = can_list_to_can_capnp(can_msgs, msgtype='sendcan')
+    sendcan.send(m.to_bytes())
+
+def boardd_test_loop():
+  can_init()
+  cnt = 0
+  while 1:
+    can_send_many([[0xbb,0,"\xaa\xaa\xaa\xaa",0], [0xaa,0,"\xaa\xaa\xaa\xaa"+struct.pack("!I", cnt),1]])
+    #can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",0]])
+    #can_send_many([[0xaa,0,"\xaa\xaa\xaa\xaa",1]])
+    # recv @ 100hz
+    can_msgs = can_recv()
+    print("got %d" % (len(can_msgs)))
+    time.sleep(0.01)
+    cnt += 1
+
+# *** main loop ***
+def boardd_loop(rate=100):
+  rk = Ratekeeper(rate)
+
+  can_init()
+
+  # *** publishes can and health
+  logcan = messaging.pub_sock('can')
+  health_sock = messaging.pub_sock('health')
+
+  # *** subscribes to can send
+  sendcan = messaging.sub_sock('sendcan')
+
+  # drain sendcan to delete any stale messages from previous runs
+  messaging.drain_sock(sendcan)
+
+  while 1:
+    # health packet @ 2hz
+    if (rk.frame % (rate // 2)) == 0:
+      health = can_health()
+      msg = messaging.new_message()
+      msg.init('health')
+
+      # store the health to be logged
+      msg.health.voltage = health['voltage']
+      msg.health.current = health['current']
+      msg.health.ignitionLine = health['ignition_line']
+      msg.health.ignitionCan = health['ignition_can']
+      msg.health.controlsAllowed = True
+
+      health_sock.send(msg.to_bytes())
+
+    # recv @ 100hz
+    can_msgs = can_recv()
+
+    # publish to logger
+    # TODO: refactor for speed
+    if len(can_msgs) > 0:
+      dat = can_list_to_can_capnp(can_msgs).to_bytes()
+      logcan.send(dat)
+
+    # send can if we have a packet
+    tsc = messaging.recv_sock(sendcan)
+    if tsc is not None:
+      can_send_many(can_capnp_to_can_list(tsc.sendcan))
+
+    rk.keep_time()
+
+# *** main loop ***
+def boardd_proxy_loop(rate=100, address="192.168.2.251"):
+  rk = Ratekeeper(rate)
+
+  can_init()
+
+  # *** subscribes can
+  logcan = messaging.sub_sock('can', addr=address)
+  # *** publishes to can send
+  sendcan = messaging.pub_sock('sendcan')
+
+  # drain sendcan to delete any stale messages from previous runs
+  messaging.drain_sock(sendcan)
+
+  while 1:
+    # recv @ 100hz
+    can_msgs = can_recv()
+    #for m in can_msgs:
+    #  print("R: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
+
+    # publish to logger
+    # TODO: refactor for speed
+    if len(can_msgs) > 0:
+      dat = can_list_to_can_capnp(can_msgs, "sendcan")
+      sendcan.send(dat)
+
+    # send can if we have a packet
+    tsc = messaging.recv_sock(logcan)
+    if tsc is not None:
+      cl = can_capnp_to_can_list(tsc.can)
+      #for m in cl:
+      #  print("S: {0} {1}".format(hex(m[0]), str(m[2]).encode("hex")))
+      can_send_many(cl)
+
+    rk.keep_time()
+
+def main(gctx=None):
+  if os.getenv("MOCK") is not None:
+    boardd_mock_loop()
+  elif os.getenv("PROXY") is not None:
+    boardd_proxy_loop()
+  elif os.getenv("BOARDTEST") is not None:
+    boardd_test_loop()
+  else:
+    boardd_loop()
+
+if __name__ == "__main__":
+  main()

selfdrive/boardd/tests/fuzzer.py

@@ -0,0 +1,20 @@
+#!/usr/bin/env python3
+from __future__ import print_function
+import time
+import random
+
+from boardd_old import can_init, can_recv, can_send_many, can_health
+
+if __name__ == "__main__":
+  can_init()
+  while 1:
+    c = random.randint(0, 3)
+    if c == 0:
+      print(can_recv())
+    elif c == 1:
+      print(can_health())
+    elif c == 2:
+      many = [[0x123, 0, "abcdef", 0]] * random.randint(1, 10)
+      can_send_many(many)
+    elif c == 3:
+      time.sleep(random.randint(0, 100) / 1000.0)

selfdrive/boardd/tests/replay_many.py

@@ -0,0 +1,68 @@
+#!/usr/bin/env python3
+import os
+import sys
+import time
+import signal
+import traceback
+from panda import Panda
+from multiprocessing import Pool
+
+jungle = "JUNGLE" in os.environ
+if jungle:
+  from panda_jungle import PandaJungle # pylint: disable=import-error
+
+import cereal.messaging as messaging
+from selfdrive.boardd.boardd import can_capnp_to_can_list
+
+def initializer():
+  """Ignore CTRL+C in the worker process.
+  source: https://stackoverflow.com/a/44869451 """
+  signal.signal(signal.SIGINT, signal.SIG_IGN)
+
+def send_thread(sender_serial):
+  global jungle
+  try:
+    if jungle:
+      sender = PandaJungle(sender_serial)
+    else:
+      sender = Panda(sender_serial)
+      sender.set_safety_mode(Panda.SAFETY_ALLOUTPUT)
+    sender.set_can_loopback(False)
+
+    can_sock = messaging.sub_sock('can')
+
+    while True:
+      # Send messages one bus 0 and 1
+      tsc = messaging.recv_one(can_sock)
+      snd = can_capnp_to_can_list(tsc.can)
+      snd = list(filter(lambda x: x[-1] <= 2, snd))
+      sender.can_send_many(snd)
+
+      # Drain panda message buffer
+      sender.can_recv()
+  except Exception:
+    traceback.print_exc()
+
+if __name__ == "__main__":
+  if jungle:
+    serials = PandaJungle.list()
+  else:
+    serials = Panda.list()
+  num_senders = len(serials)
+
+  if num_senders == 0:
+    print("No senders found. Exiting")
+    sys.exit(1)
+  else:
+    print("%d senders found. Starting broadcast" % num_senders)
+
+  pool = Pool(num_senders, initializer=initializer)
+  pool.map_async(send_thread, serials)
+
+  while True:
+    try:
+      time.sleep(10)
+    except KeyboardInterrupt:
+      pool.terminate()
+      pool.join()
+      raise

selfdrive/boardd/tests/test_boardd_api.py

@@ -0,0 +1,77 @@
+import random
+import numpy as np
+
+import selfdrive.boardd.tests.boardd_old as boardd_old
+import selfdrive.boardd.boardd as boardd
+
+from common.realtime import sec_since_boot
+from cereal import log
+import unittest
+
+
+def generate_random_can_data_list():
+  can_list = []
+  cnt = random.randint(1, 64)
+  for j in range(cnt):
+    can_data = np.random.bytes(random.randint(1, 8))
+    can_list.append([random.randint(0, 128), random.randint(0, 128), can_data, random.randint(0, 128)])
+  return can_list, cnt
+
+
+class TestBoarddApiMethods(unittest.TestCase):
+  def test_correctness(self):
+    for i in range(1000):
+      can_list, _ = generate_random_can_data_list()
+
+      # Sendcan
+      # Old API
+      m_old = boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes()
+      # new API
+      m = boardd.can_list_to_can_capnp(can_list, 'sendcan')
+
+      ev_old = log.Event.from_bytes(m_old)
+      ev = log.Event.from_bytes(m)
+
+      self.assertEqual(ev_old.which(), ev.which())
+      self.assertEqual(len(ev.sendcan), len(ev_old.sendcan))
+      for i in range(len(ev.sendcan)):
+        attrs = ['address', 'busTime', 'dat', 'src']
+        for attr in attrs:
+          self.assertEqual(getattr(ev.sendcan[i], attr, 'new'), getattr(ev_old.sendcan[i], attr, 'old'))
+
+      # Can
+      m_old = boardd_old.can_list_to_can_capnp(can_list, 'can').to_bytes()
+      # new API
+      m = boardd.can_list_to_can_capnp(can_list, 'can')
+
+      ev_old = log.Event.from_bytes(m_old)
+      ev = log.Event.from_bytes(m)
+      self.assertEqual(ev_old.which(), ev.which())
+      self.assertEqual(len(ev.can), len(ev_old.can))
+      for i in range(len(ev.can)):
+        attrs = ['address', 'busTime', 'dat', 'src']
+        for attr in attrs:
+          self.assertEqual(getattr(ev.can[i], attr, 'new'), getattr(ev_old.can[i], attr, 'old'))
+
+  def test_performance(self):
+    can_list, cnt = generate_random_can_data_list()
+    recursions = 1000
+
+    n1 = sec_since_boot()
+    for i in range(recursions):
+      boardd_old.can_list_to_can_capnp(can_list, 'sendcan').to_bytes()
+    n2 = sec_since_boot()
+    elapsed_old = n2 - n1
+
+    # print('Old API, elapsed time: {} secs'.format(elapsed_old))
+    n1 = sec_since_boot()
+    for i in range(recursions):
+      boardd.can_list_to_can_capnp(can_list)
+    n2 = sec_since_boot()
+    elapsed_new = n2 - n1
+    # print('New API, elapsed time: {} secs'.format(elapsed_new))
+    self.assertTrue(elapsed_new < elapsed_old / 2)
+
+
+if __name__ == '__main__':
+    unittest.main()

selfdrive/boardd/tests/test_boardd_loopback.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+"""Run boardd with the BOARDD_LOOPBACK envvar before running this test."""
+
+import os
+import random
+import time
+
+from selfdrive.boardd.boardd import can_list_to_can_capnp
+from cereal.messaging import drain_sock, pub_sock, sub_sock
+
+def get_test_string():
+  return b"test"+os.urandom(10)
+
+BUS = 0
+
+def main():
+  rcv = sub_sock('can') # port 8006
+  snd = pub_sock('sendcan') # port 8017
+  time.sleep(0.3) # wait to bind before send/recv
+
+  for i in range(10):
+    print("Loop %d" % i)
+    at = random.randint(1024, 2000)
+    st = get_test_string()[0:8]
+    snd.send(can_list_to_can_capnp([[at, 0, st, 0]], msgtype='sendcan').to_bytes())
+    time.sleep(0.1)
+    res = drain_sock(rcv, True)
+    assert len(res) == 1
+
+    res = res[0].can
+    assert len(res) == 2
+
+    msg0, msg1 = res
+
+    assert msg0.dat == st
+    assert msg1.dat == st
+
+    assert msg0.address == at
+    assert msg1.address == at
+
+    assert msg0.src == 0x80 | BUS
+    assert msg1.src == BUS
+
+  print("Success")
+
+if __name__ == "__main__":
+  main()