From 5c123f01b4971c456a5161026687e41cfb8b9919 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Fri, 4 Nov 2022 16:36:27 -0700 Subject: [PATCH] boardd: prep for SPI + factor out USB (#26356) * merge origin/spi-panda * just prep * boardd: factor out USB comms * fix those * add to release files * little more old-commit-hash: 06be96cae29bb347da2a9df43976613f3f2cc025 --- release/files_common | 2 + selfdrive/boardd/SConscript | 4 +- selfdrive/boardd/boardd.cc | 18 +- selfdrive/boardd/panda.cc | 284 +++++--------------------------- selfdrive/boardd/panda.h | 45 ++--- selfdrive/boardd/panda_comms.cc | 232 ++++++++++++++++++++++++++ selfdrive/boardd/panda_comms.h | 51 ++++++ 7 files changed, 350 insertions(+), 286 deletions(-) create mode 100644 selfdrive/boardd/panda_comms.cc create mode 100644 selfdrive/boardd/panda_comms.h diff --git a/release/files_common b/release/files_common index e3c417040c..61d16a2088 100644 --- a/release/files_common +++ b/release/files_common @@ -90,6 +90,8 @@ selfdrive/boardd/boardd_api_impl.pyx selfdrive/boardd/can_list_to_can_capnp.cc selfdrive/boardd/panda.cc selfdrive/boardd/panda.h +selfdrive/boardd/panda_comms.h +selfdrive/boardd/panda_comms.cc selfdrive/boardd/set_time.py selfdrive/boardd/pandad.py diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index dcbea03d3c..356b5de663 100644 --- a/selfdrive/boardd/SConscript +++ b/selfdrive/boardd/SConscript @@ -1,9 +1,9 @@ Import('env', 'envCython', 'common', 'cereal', 'messaging') libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'] -env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc'], LIBS=libs) +env.Program('boardd', ['main.cc', 'boardd.cc', 'panda.cc', 'panda_comms.cc'], LIBS=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('test'): - env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc'], LIBS=libs) + env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc', 'panda_comms.cc'], LIBS=libs) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 2d613b68ce..5496902252 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -19,8 +19,6 @@ #include #include -#include - #include "cereal/gen/cpp/car.capnp.h" #include "cereal/messaging/messaging.h" #include "common/params.h" @@ -67,7 +65,7 @@ static std::string get_time_str(const struct tm &time) { bool check_all_connected(const std::vector &pandas) { for (const auto& panda : pandas) { - if (!panda->connected) { + if (!panda->connected()) { do_exit = true; return false; } @@ -184,7 +182,7 @@ bool safety_setter_thread(std::vector pandas) { return true; } -Panda *usb_connect(std::string serial="", uint32_t index=0) { +Panda *connect(std::string serial="", uint32_t index=0) { std::unique_ptr panda; try { panda = std::make_unique(serial, (index * PANDA_BUS_CNT)); @@ -227,9 +225,9 @@ void can_send_thread(std::vector pandas, bool fake_send) { //Dont 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->usb_serial).c_str()); + LOGT("sending sendcan to panda: %s", (panda->hw_serial).c_str()); panda->can_send(event.getSendcan()); - LOGT("sendcan sent to panda: %s", (panda->usb_serial).c_str()); + LOGT("sendcan sent to panda: %s", (panda->hw_serial).c_str()); } } } @@ -357,7 +355,7 @@ std::optional send_panda_states(PubMaster *pm, const std::vector } #endif - if (!panda->comms_healthy) { + if (!panda->comms_healthy()) { evt.setValid(false); } @@ -433,7 +431,7 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) { // build msg MessageBuilder msg; auto evt = msg.initEvent(); - evt.setValid(panda->comms_healthy); + evt.setValid(panda->comms_healthy()); auto ps = evt.initPeripheralState(); ps.setPandaType(panda->hw_type); @@ -526,7 +524,7 @@ void peripheral_control_thread(Panda *panda, bool no_fan_control) { FirstOrderFilter integ_lines_filter(0, 30.0, 0.05); - while (!do_exit && panda->connected) { + while (!do_exit && panda->connected()) { cnt++; sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update? @@ -595,7 +593,7 @@ void boardd_main_thread(std::vector serials) { // connect to all provided serials std::vector pandas; for (int i = 0; i < serials.size() && !do_exit; /**/) { - Panda *p = usb_connect(serials[i], i); + Panda *p = connect(serials[i], i); if (!p) { // send empty pandaState & peripheralState and try again send_empty_panda_state(&pm); diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 329ce91c44..e68558632e 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -4,75 +4,15 @@ #include #include -#include #include "cereal/messaging/messaging.h" #include "panda/board/dlc_to_len.h" -#include "common/gpio.h" #include "common/swaglog.h" #include "common/util.h" -static int init_usb_ctx(libusb_context **context) { - assert(context != nullptr); - - int err = libusb_init(context); - if (err != 0) { - LOGE("libusb initialization error"); - return err; - } - -#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 err; -} - - Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { - // init libusb - ssize_t num_devices; - libusb_device **dev_list = NULL; - int err = init_usb_ctx(&ctx); - if (err != 0) { 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; } - - usb_serial = std::string((char *)desc_serial, ret).c_str(); - if (serial.empty() || serial == usb_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; } + // TODO: support SPI here one day... + handle = std::make_unique(serial); hw_type = get_hw_type(); @@ -83,194 +23,44 @@ Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { (hw_type == cereal::PandaState::PandaType::DOS); return; - -fail: - if (dev_list != NULL) { - libusb_free_device_list(dev_list, 1); - } - cleanup(); - throw std::runtime_error("Error connecting to panda"); } -Panda::~Panda() { - std::lock_guard lk(usb_lock); - cleanup(); - connected = false; +bool Panda::connected() { + return handle->connected; } -void Panda::cleanup() { - if (dev_handle) { - libusb_release_interface(dev_handle, 0); - libusb_close(dev_handle); - } - - if (ctx) { - libusb_exit(ctx); - } +bool Panda::comms_healthy() { + return handle->comms_healthy; } std::vector Panda::list() { - // init libusb - ssize_t num_devices; - libusb_context *context = NULL; - libusb_device **dev_list = NULL; - std::vector serials; - - int err = init_usb_ctx(&context); - if (err != 0) { return serials; } - - num_devices = libusb_get_device_list(context, &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); - } - if (context) { - libusb_exit(context); - } - return serials; -} - -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; - - if (!connected) { - return LIBUSB_ERROR_NO_DEVICE; - } - - std::lock_guard lk(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); - - 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; - - if (!connected) { - return LIBUSB_ERROR_NO_DEVICE; - } - - std::lock_guard lk(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); - - return err; -} - -int Panda::usb_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(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); - - return transferred; -} - -int Panda::usb_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(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) { - 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; + return PandaUsbHandle::list(); } void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { - usb_write(0xdc, (uint16_t)safety_model, safety_param); + handle->control_write(0xdc, (uint16_t)safety_model, safety_param); } void Panda::set_alternative_experience(uint16_t alternative_experience) { - usb_write(0xdf, alternative_experience, 0); + handle->control_write(0xdf, alternative_experience, 0); } cereal::PandaState::PandaType Panda::get_hw_type() { unsigned char hw_query[1] = {0}; - usb_read(0xc1, 0, 0, hw_query, 1); + 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 - 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); + 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() { @@ -284,7 +74,7 @@ struct tm Panda::get_rtc() { uint8_t second; } rtc_time = {0}; - usb_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time)); + 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 @@ -298,70 +88,70 @@ struct tm Panda::get_rtc() { } void Panda::set_fan_speed(uint16_t fan_speed) { - usb_write(0xb1, fan_speed, 0); + handle->control_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)); + 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) { - usb_write(0xb0, ir_pwr, 0); + handle->control_write(0xb0, ir_pwr, 0); } std::optional Panda::get_state() { health_t health {0}; - int err = usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); + int err = handle->control_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); return err >= 0 ? std::make_optional(health) : std::nullopt; } std::optional Panda::get_can_state(uint16_t can_number) { can_health_t can_health {0}; - int err = usb_read(0xc2, can_number, 0, (unsigned char*)&can_health, sizeof(can_health)); + 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) { - usb_write(0xe5, loopback, 0); + handle->control_write(0xe5, loopback, 0); } std::optional> Panda::get_firmware_version() { std::vector fw_sig_buf(128); - int read_1 = usb_read(0xd3, 0, 0, &fw_sig_buf[0], 64); - int read_2 = usb_read(0xd4, 0, 0, &fw_sig_buf[64], 64); + 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 Panda::get_serial() { char serial_buf[17] = {'\0'}; - int err = usb_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16); + int err = handle->control_read(0xd0, 0, 0, (uint8_t*)serial_buf, 16); return err >= 0 ? std::make_optional(serial_buf) : std::nullopt; } void Panda::set_power_saving(bool power_saving) { - usb_write(0xe7, power_saving, 0); + handle->control_write(0xe7, power_saving, 0); } void Panda::enable_deepsleep() { - usb_write(0xfb, 0, 0); + handle->control_write(0xfb, 0, 0); } void Panda::send_heartbeat(bool engaged) { - usb_write(0xf3, engaged, 0); + handle->control_write(0xf3, engaged, 0); } void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { - usb_write(0xde, bus, (speed * 10)); + handle->control_write(0xde, bus, (speed * 10)); } void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) { - usb_write(0xf9, bus, (speed * 10)); + handle->control_write(0xf9, bus, (speed * 10)); } void Panda::set_canfd_non_iso(uint16_t bus, bool non_iso) { - usb_write(0xfc, bus, non_iso); + handle->control_write(0xfc, bus, non_iso); } static uint8_t len_to_dlc(uint8_t len) { @@ -422,14 +212,14 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data void Panda::can_send(capnp::List::Reader can_data_list) { pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) { - usb_bulk_write(3, data, size, 5); + handle->bulk_write(3, data, size, 5); }); } bool Panda::can_receive(std::vector& out_vec) { uint8_t data[RECV_SIZE]; - int recv = usb_bulk_read(0x81, (uint8_t*)data, RECV_SIZE); - if (!comms_healthy) { + int recv = handle->bulk_read(0x81, (uint8_t*)data, RECV_SIZE); + if (!comms_healthy()) { return false; } if (recv == RECV_SIZE) { @@ -444,7 +234,7 @@ bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &o for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) { if (data[i] != i / USBPACKET_MAX_SIZE) { LOGE("CAN: MALFORMED USB RECV PACKET"); - comms_healthy = false; + handle->comms_healthy = false; return false; } int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i)); diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index c7a0e7a6c1..5b3cbb9a3e 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -1,26 +1,26 @@ #pragma once -#include #include #include #include #include -#include +#include #include #include -#include - #include "cereal/gen/cpp/car.capnp.h" #include "cereal/gen/cpp/log.capnp.h" #include "panda/board/health.h" +#include "selfdrive/boardd/panda_comms.h" + -#define TIMEOUT 0 #define PANDA_CAN_CNT 3 #define PANDA_BUS_CNT 4 -#define RECV_SIZE (0x4000U) + #define USB_TX_SOFT_LIMIT (0x100U) #define USBPACKET_MAX_SIZE (0x40) + +#define RECV_SIZE (0x4000U) #define CANPACKET_HEAD_SIZE 5U #define CANPACKET_MAX_SIZE 72U #define CANPACKET_REJECTED (0xC0U) @@ -37,41 +37,32 @@ struct __attribute__((packed)) can_header { }; struct can_frame { - long address; - std::string dat; - long busTime; - long src; + long address; + std::string dat; + long busTime; + long src; }; + class Panda { - private: - libusb_context *ctx = NULL; - libusb_device_handle *dev_handle = NULL; - std::mutex usb_lock; +private: + std::unique_ptr handle; std::vector recv_buf; - void handle_usb_issue(int err, const char func[]); - void cleanup(); - public: +public: Panda(std::string serial="", uint32_t bus_offset=0); - ~Panda(); - std::string usb_serial; - std::atomic connected = true; - std::atomic comms_healthy = true; + std::string hw_serial; cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; bool has_rtc = false; const uint32_t bus_offset; + bool connected(); + bool comms_healthy(); + // Static functions static std::vector list(); - // 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::PandaState::PandaType get_hw_type(); void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); diff --git a/selfdrive/boardd/panda_comms.cc b/selfdrive/boardd/panda_comms.cc new file mode 100644 index 0000000000..e73cb69318 --- /dev/null +++ b/selfdrive/boardd/panda_comms.cc @@ -0,0 +1,232 @@ +#include "selfdrive/boardd/panda.h" + +#include +#include + +#include "common/swaglog.h" + +static int init_usb_ctx(libusb_context **context) { + assert(context != nullptr); + + int err = libusb_init(context); + if (err != 0) { + LOGE("libusb initialization error"); + return err; + } + +#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 err; +} + +PandaUsbHandle::PandaUsbHandle(std::string serial) : PandaCommsHandle(serial) { + // init libusb + ssize_t num_devices; + libusb_device **dev_list = NULL; + int err = init_usb_ctx(&ctx); + if (err != 0) { 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; } + + auto 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 PandaUsbHandle::list() { + // init libusb + ssize_t num_devices; + libusb_context *context = NULL; + libusb_device **dev_list = NULL; + std::vector serials; + + int err = init_usb_ctx(&context); + if (err != 0) { return serials; } + + num_devices = libusb_get_device_list(context, &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); + } + if (context) { + libusb_exit(context); + } + 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; +} diff --git a/selfdrive/boardd/panda_comms.h b/selfdrive/boardd/panda_comms.h new file mode 100644 index 0000000000..c5143b16b3 --- /dev/null +++ b/selfdrive/boardd/panda_comms.h @@ -0,0 +1,51 @@ +#pragma once + +#include +#include +#include +#include + +#include + +#define TIMEOUT 0 + + +// comms base class +class PandaCommsHandle { +public: + PandaCommsHandle(std::string serial) {}; + virtual ~PandaCommsHandle() {}; + virtual void cleanup() = 0; + + std::atomic connected = true; + std::atomic comms_healthy = true; + static std::vector 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; + +protected: + std::mutex hw_lock; +}; + +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 list(); + +private: + libusb_context *ctx = NULL; + libusb_device_handle *dev_handle = NULL; + std::vector recv_buf; + void handle_usb_issue(int err, const char func[]); +};