Panda abstraction layer in boardd (#1919)

* start on panda abstraction layer

* handle bulk transfer in library

* Safety model abstraction

* Abstract hw type into library

* Handle disconnect

* RTC stuff

* Fan control

* Health

* Panda fw version

* Read serial

* Power saving

* Power mode

* Cleanup pigeon thread init

* Rename safety setter variable name

* Remove comment

* Unused global cleanup

* malloc -> new

* whitespace

* Use std::thread

* Use std::thread for safety setter

* Cleanup igntion_last global

* Heartbeat

* logd

* More global cleanup

* Put back sleep

* ir pwr

* Always broadcast health

* init struct with zeroes

* Fix eon build

* fix race condition

* fix Adeebs comments

* abstract can send and receive
old-commit-hash: 80acb32825

release/files_common

@@ -88,6 +88,8 @@ selfdrive/boardd/boardd.py
 selfdrive/boardd/boardd_api_impl.pyx
 selfdrive/boardd/boardd_setup.py
 selfdrive/boardd/can_list_to_can_capnp.cc
+selfdrive/boardd/panda.cc
+selfdrive/boardd/panda.h
 
 selfdrive/car/__init__.py
 selfdrive/car/car_helpers.py

selfdrive/boardd/SConscript

@@ -1,9 +1,8 @@
 Import('env', 'common', 'cereal', 'messaging', 'cython_dependencies')
 
-env.Program('boardd.cc', LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
+env.Program('boardd', ['boardd.cc', 'panda.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
 env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
 
 env.Command(['boardd_api_impl.so', 'boardd_api_impl.cpp'],
             cython_dependencies + ['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/panda.cc

@@ -0,0 +1,314 @@
+#include <stdexcept>
+#include <cassert>
+#include <iostream>
+
+#include "common/swaglog.h"
+
+#include "panda.h"
+
+Panda::Panda(){
+  int err;
+
+  err = pthread_mutex_init(&usb_lock, NULL);
+  if (err != 0) { goto fail; }
+
+  // init libusb
+  err = libusb_init(&ctx);
+  if (err != 0) { goto fail; }
+
+#if LIBUSB_API_VERSION >= 0x01000106
+  libusb_set_option(ctx, LIBUSB_OPTION_LOG_LEVEL, LIBUSB_LOG_LEVEL_INFO);
+#else
+  libusb_set_debug(ctx, 3);
+#endif
+
+  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; }
+
+  hw_type = get_hw_type();
+  is_pigeon =
+    (hw_type == cereal::HealthData::HwType::GREY_PANDA) ||
+    (hw_type == cereal::HealthData::HwType::BLACK_PANDA) ||
+    (hw_type == cereal::HealthData::HwType::UNO);
+  has_rtc = (hw_type == cereal::HealthData::HwType::UNO);
+
+  return;
+
+fail:
+  cleanup();
+  throw std::runtime_error("Error connecting to panda");
+}
+
+Panda::~Panda(){
+  pthread_mutex_lock(&usb_lock);
+  cleanup();
+  connected = false;
+  pthread_mutex_unlock(&usb_lock);
+}
+
+void Panda::cleanup(){
+  if (dev_handle){
+    libusb_release_interface(dev_handle, 0);
+    libusb_close(dev_handle);
+  }
+
+  if (ctx) {
+    libusb_exit(ctx);
+  }
+}
+
+void Panda::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 Panda::usb_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;
+
+  pthread_mutex_lock(&usb_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);
+
+  pthread_mutex_unlock(&usb_lock);
+
+  return err;
+}
+
+int Panda::usb_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;
+
+  pthread_mutex_lock(&usb_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);
+  pthread_mutex_unlock(&usb_lock);
+
+  return err;
+}
+
+int Panda::usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
+  int err;
+  int transferred = 0;
+
+  pthread_mutex_lock(&usb_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);
+
+  pthread_mutex_unlock(&usb_lock);
+  return transferred;
+}
+
+int Panda::usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) {
+  int err;
+  int transferred = 0;
+
+  pthread_mutex_lock(&usb_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) {
+      LOGE_100("overflow got 0x%x", transferred);
+    } else if (err != 0) {
+      handle_usb_issue(err, __func__);
+    }
+
+  } while(err != 0 && connected);
+
+  pthread_mutex_unlock(&usb_lock);
+
+  return transferred;
+}
+
+void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param){
+  usb_write(0xdc, (uint16_t)safety_model, safety_param);
+}
+
+cereal::HealthData::HwType Panda::get_hw_type() {
+  unsigned char hw_query[1] = {0};
+
+  usb_read(0xc1, 0, 0, hw_query, 1);
+  return (cereal::HealthData::HwType)(hw_query[0]);
+}
+
+void Panda::set_rtc(struct tm sys_time){
+  // tm struct has year defined as years since 1900
+  usb_write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0);
+  usb_write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0);
+  usb_write(0xa3, (uint16_t)sys_time.tm_mday, 0);
+  // usb_write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0);
+  usb_write(0xa5, (uint16_t)sys_time.tm_hour, 0);
+  usb_write(0xa6, (uint16_t)sys_time.tm_min, 0);
+  usb_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};
+
+  usb_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){
+  usb_write(0xb1, fan_speed, 0);
+}
+
+uint16_t Panda::get_fan_speed(){
+  uint16_t fan_speed_rpm = 0;
+  usb_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) {
+  usb_write(0xb0, ir_pwr, 0);
+}
+
+health_t Panda::get_health(){
+  health_t health {0};
+  usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health));
+  return health;
+}
+
+void Panda::set_loopback(bool loopback){
+  usb_write(0xe5, loopback, 0);
+}
+
+const char* Panda::get_firmware_version(){
+  const char* fw_sig_buf = new char[128]();
+
+  int read_1 = usb_read(0xd3, 0, 0, (unsigned char*)fw_sig_buf, 64);
+  int read_2 = usb_read(0xd4, 0, 0, (unsigned char*)fw_sig_buf + 64, 64);
+
+  if ((read_1 == 64) && (read_2 == 64)) {
+    return fw_sig_buf;
+  }
+
+  delete[] fw_sig_buf;
+  return NULL;
+}
+
+const char* Panda::get_serial(){
+  const char* serial_buf = new char[16]();
+
+  int err = usb_read(0xd0, 0, 0, (unsigned char*)serial_buf, 16);
+
+  if (err >= 0) {
+    return serial_buf;
+  }
+
+  delete[] serial_buf;
+  return NULL;
+
+}
+
+void Panda::set_power_saving(bool power_saving){
+  usb_write(0xe7, power_saving, 0);
+}
+
+void Panda::set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode){
+  usb_write(0xe6, (uint16_t)power_mode, 0);
+}
+
+void Panda::send_heartbeat(){
+  usb_write(0xf3, 1, 0);
+}
+
+void Panda::can_send(capnp::List<cereal::CanData>::Reader can_data_list){
+  int msg_count = can_data_list.size();
+
+  uint32_t *send = new uint32_t[msg_count*0x10]();
+
+  for (int i = 0; i < msg_count; i++) {
+    auto cmsg = can_data_list[i];
+    if (cmsg.getAddress() >= 0x800) { // extended
+      send[i*4] = (cmsg.getAddress() << 3) | 5;
+    } else { // normal
+      send[i*4] = (cmsg.getAddress() << 21) | 1;
+    }
+    auto can_data = cmsg.getDat();
+    assert(can_data.size() <= 8);
+    send[i*4+1] = can_data.size() | (cmsg.getSrc() << 4);
+    memcpy(&send[i*4+2], can_data.begin(), can_data.size());
+  }
+
+  usb_bulk_write(3, (unsigned char*)send, msg_count*0x10, 5);
+
+  delete[] send;
+}
+
+int Panda::can_receive(cereal::Event::Builder &event){
+  uint32_t data[RECV_SIZE/4];
+  int recv = usb_bulk_read(0x81, (unsigned char*)data, RECV_SIZE);
+
+  // return if length is 0
+  if (recv <= 0) {
+    return 0;
+  } else if (recv == RECV_SIZE) {
+    LOGW("Receive buffer full");
+  }
+
+  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);
+  }
+
+  return recv;
+}

selfdrive/boardd/panda.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <ctime>
+#include <cstdint>
+#include <pthread.h>
+
+#include <libusb-1.0/libusb.h>
+
+#include "cereal/gen/cpp/car.capnp.h"
+#include "cereal/gen/cpp/log.capnp.h"
+
+// double the FIFO size
+#define RECV_SIZE (0x1000)
+#define TIMEOUT 0
+
+// copied from panda/board/main.c
+struct __attribute__((packed)) health_t {
+  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;
+};
+
+class Panda {
+ private:
+  libusb_context *ctx = NULL;
+  libusb_device_handle *dev_handle = NULL;
+  pthread_mutex_t usb_lock;
+  void handle_usb_issue(int err, const char func[]);
+  void cleanup();
+
+ public:
+  Panda();
+  ~Panda();
+
+  bool connected = true;
+  cereal::HealthData::HwType hw_type = cereal::HealthData::HwType::UNKNOWN;
+  bool is_pigeon = false;
+  bool has_rtc = false;
+
+  // HW communication
+  int usb_write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout=TIMEOUT);
+  int usb_read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned char *data, uint16_t wLength, unsigned int timeout=TIMEOUT);
+  int usb_bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
+  int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
+
+  // Panda functionality
+  cereal::HealthData::HwType get_hw_type();
+  void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0);
+  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);
+  health_t get_health();
+  void set_loopback(bool loopback);
+  const char* get_firmware_version();
+  const char* get_serial();
+  void set_power_saving(bool power_saving);
+  void set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode);
+  void send_heartbeat();
+  void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
+  int can_receive(cereal::Event::Builder &event);
+
+};

selfdrive/boardd/tests/test_boardd_loopback.py

@@ -8,6 +8,7 @@ from functools import wraps
 import cereal.messaging as messaging
 from cereal import car
 from common.basedir import PARAMS
+from common.android import ANDROID
 from common.params import Params
 from common.spinner import Spinner
 from panda import Panda
@@ -35,7 +36,7 @@ os.environ['PARAMS_PATH'] = PARAMS
 @with_processes(['boardd'])
 def test_boardd_loopback():
   # wait for boardd to init
-  spinner = Spinner()
+  spinner = Spinner(noop=(not ANDROID))
   time.sleep(2)
 
   # boardd blocks on CarVin and CarParams