diff --git a/panda b/panda index 6f95a096e6..8d0d148681 160000 --- a/panda +++ b/panda @@ -1 +1 @@ -Subproject commit 6f95a096e6beb254786759003a38e6e5c4f2c10e +Subproject commit 8d0d148681163929260abf5742701cbf503abb22 diff --git a/release/files_common b/release/files_common index 3fc354396a..2706d2e8bf 100644 --- a/release/files_common +++ b/release/files_common @@ -92,6 +92,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/usbdevice.cc +selfdrive/boardd/usbdevice.h selfdrive/boardd/pigeon.cc selfdrive/boardd/pigeon.h selfdrive/boardd/set_time.py diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript index 07ded56e1b..c2142b7b27 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', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs) +env.Program('boardd', ['boardd.cc', 'panda.cc', 'usbdevice.cc', 'pigeon.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', 'usbdevice.cc'], LIBS=libs) diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index 0ca943de2e..a65bff63af 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -185,7 +185,7 @@ bool safety_setter_thread(std::vector pandas) { Panda *usb_connect(std::string serial="", uint32_t index=0) { std::unique_ptr panda; try { - panda = std::make_unique(serial, (index * PANDA_BUS_CNT)); + panda = std::make_unique(serial, (index * BUS_CNT)); } catch (std::exception &e) { return nullptr; } diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 6850ad41b2..90c31dd34f 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -1,10 +1,7 @@ #include "selfdrive/boardd/panda.h" -#include - #include #include -#include #include "cereal/messaging/messaging.h" #include "panda/board/dlc_to_len.h" @@ -12,263 +9,44 @@ #include "selfdrive/common/swaglog.h" #include "selfdrive/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) { - libusb_open(dev_list[i], &dev_handle); - if (dev_handle == NULL) { goto fail; } - - unsigned char desc_serial[26] = { 0 }; - int 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 (!open(serial)) { + throw std::runtime_error("Error connecting to panda"); } - 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; } - hw_type = get_hw_type(); - assert((hw_type != cereal::PandaState::PandaType::WHITE_PANDA) && (hw_type != cereal::PandaState::PandaType::GREY_PANDA)); has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) || (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; } -void Panda::cleanup() { - if (dev_handle) { - libusb_release_interface(dev_handle, 0); - libusb_close(dev_handle); - } - - if (ctx) { - libusb_exit(ctx); - } -} - -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; - libusb_open(device, &handle); - unsigned char desc_serial[26] = { 0 }; - int 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; -} +Panda::~Panda() {} void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param) { - usb_write(0xdc, (uint16_t)safety_model, safety_param); + write(0xdc, (uint16_t)safety_model, safety_param); } void Panda::set_unsafe_mode(uint16_t unsafe_mode) { - usb_write(0xdf, unsafe_mode, 0); + write(0xdf, unsafe_mode, 0); } cereal::PandaState::PandaType Panda::get_hw_type() { unsigned char hw_query[1] = {0}; - usb_read(0xc1, 0, 0, hw_query, 1); + 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); + write(0xa1, (uint16_t)(1900 + sys_time.tm_year), 0); + write(0xa2, (uint16_t)(1 + sys_time.tm_mon), 0); + write(0xa3, (uint16_t)sys_time.tm_mday, 0); + // write(0xa4, (uint16_t)(1 + sys_time.tm_wday), 0); + write(0xa5, (uint16_t)sys_time.tm_hour, 0); + write(0xa6, (uint16_t)sys_time.tm_min, 0); + write(0xa7, (uint16_t)sys_time.tm_sec, 0); } struct tm Panda::get_rtc() { @@ -282,7 +60,7 @@ struct tm Panda::get_rtc() { uint8_t second; } rtc_time = {0}; - usb_read(0xa0, 0, 0, (unsigned char*)&rtc_time, sizeof(rtc_time)); + 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 @@ -296,60 +74,60 @@ struct tm Panda::get_rtc() { } void Panda::set_fan_speed(uint16_t fan_speed) { - usb_write(0xb1, fan_speed, 0); + 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)); + 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); + write(0xb0, ir_pwr, 0); } health_t Panda::get_state() { health_t health {0}; - usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); + read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health)); return health; } void Panda::set_loopback(bool loopback) { - usb_write(0xe5, loopback, 0); + 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 = read(0xd3, 0, 0, &fw_sig_buf[0], 64); + int read_2 = 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 = 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); + write(0xe7, power_saving, 0); } void Panda::set_usb_power_mode(cereal::PeripheralState::UsbPowerMode power_mode) { - usb_write(0xe6, (uint16_t)power_mode, 0); + write(0xe6, (uint16_t)power_mode, 0); } void Panda::send_heartbeat(bool engaged) { - usb_write(0xf3, engaged, 0); + write(0xf3, engaged, 0); } void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { - usb_write(0xde, bus, (speed * 10)); + write(0xde, bus, (speed * 10)); } void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) { - usb_write(0xf9, bus, (speed * 10)); + write(0xf9, bus, (speed * 10)); } static uint8_t len_to_dlc(uint8_t len) { @@ -382,7 +160,7 @@ void Panda::pack_can_buffer(const capnp::List::Reader &can_data 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)) { + if (bus < bus_offset || bus >= (bus_offset + BUS_CNT)) { continue; } auto can_data = cmsg.getDat(); @@ -410,13 +188,13 @@ 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); + 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); + int recv = bulk_read(0x81, (uint8_t*)data, RECV_SIZE); if (!comms_healthy) { return false; } @@ -435,7 +213,7 @@ bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &o comms_healthy = false; return false; } - int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i)); + int chunk_len = std::min((int)USBPACKET_MAX_SIZE, (size - i)); recv_buf.insert(recv_buf.end(), &data[i + 1], &data[i + chunk_len]); } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index 1a18a7f15a..ad93131eed 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -1,26 +1,15 @@ #pragma once -#include -#include -#include #include -#include -#include #include #include -#include - -#include "cereal/gen/cpp/car.capnp.h" #include "cereal/gen/cpp/log.capnp.h" +#include "selfdrive/boardd/usbdevice.h" -#define TIMEOUT 0 -#define PANDA_BUS_CNT 4 -#define RECV_SIZE (0x4000U) -#define USB_TX_SOFT_LIMIT (0x100U) -#define USBPACKET_MAX_SIZE (0x40) -#define CANPACKET_HEAD_SIZE 5U -#define CANPACKET_MAX_SIZE 72U +#define RECV_SIZE (MAX_EP1_CHUNK_PER_BULK_TRANSFER + 128U) +#define USB_TX_SOFT_LIMIT (USBPACKET_MAX_SIZE * 4) +#define CANPACKET_MAX_SIZE (CANPACKET_HEAD_SIZE + CANPACKET_DATA_SIZE_MAX) #define CANPACKET_REJECTED (0xC0U) #define CANPACKET_RETURNED (0x80U) @@ -64,35 +53,15 @@ struct can_frame { long src; }; -class Panda { - private: - libusb_context *ctx = NULL; - libusb_device_handle *dev_handle = NULL; - std::mutex usb_lock; - std::vector recv_buf; - void handle_usb_issue(int err, const char func[]); - void cleanup(); - - public: +class Panda : public USBDevice { +public: Panda(std::string serial="", uint32_t bus_offset=0); ~Panda(); - std::string usb_serial; - std::atomic connected = true; - std::atomic comms_healthy = true; cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; bool has_rtc = false; const uint32_t bus_offset; - // 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, int safety_param=0); @@ -120,4 +89,7 @@ protected: void pack_can_buffer(const capnp::List::Reader &can_data_list, std::function write_func); bool unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec); + +private: + std::vector recv_buf; }; diff --git a/selfdrive/boardd/pigeon.cc b/selfdrive/boardd/pigeon.cc index 912f4b03e7..6edf84b555 100644 --- a/selfdrive/boardd/pigeon.cc +++ b/selfdrive/boardd/pigeon.cc @@ -186,8 +186,8 @@ void PandaPigeon::connect(Panda * p) { } void PandaPigeon::set_baud(int baud) { - panda->usb_write(0xe2, 1, 0); - panda->usb_write(0xe4, 1, baud/300); + panda->write(0xe2, 1, 0); + panda->write(0xe4, 1, baud/300); } void PandaPigeon::send(const std::string &s) { @@ -200,7 +200,7 @@ void PandaPigeon::send(const std::string &s) { int ll = std::min(0x20, len-i); memcpy(&a[1], &dat[i], ll); - panda->usb_bulk_write(2, a, ll+1); + panda->bulk_write(2, a, ll+1); } } @@ -209,7 +209,7 @@ std::string PandaPigeon::receive() { r.reserve(0x1000 + 0x40); unsigned char dat[0x40]; while (r.length() < 0x1000) { - int len = panda->usb_read(0xe0, 1, 0, dat, sizeof(dat)); + int len = panda->read(0xe0, 1, 0, dat, sizeof(dat)); if (len <= 0) break; r.append((char*)dat, len); } @@ -218,7 +218,7 @@ std::string PandaPigeon::receive() { } void PandaPigeon::set_power(bool power) { - panda->usb_write(0xd9, power, 0); + panda->write(0xd9, power, 0); } PandaPigeon::~PandaPigeon() { diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc index 6a13cbd71f..d2f7443f25 100644 --- a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc +++ b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc @@ -64,7 +64,7 @@ void PandaTest::test_can_send() { for (int i = 0, counter = 0; i < size; i += USBPACKET_MAX_SIZE, counter++) { REQUIRE(chunk[i] == counter); - const int len = std::min(USBPACKET_MAX_SIZE, size_left); + const int len = std::min((int)USBPACKET_MAX_SIZE, size_left); unpacked_data.insert(unpacked_data.end(), &chunk[i + 1], &chunk[i + len]); size_left -= len; } diff --git a/selfdrive/boardd/usbdevice.cc b/selfdrive/boardd/usbdevice.cc new file mode 100644 index 0000000000..014fa88b51 --- /dev/null +++ b/selfdrive/boardd/usbdevice.cc @@ -0,0 +1,140 @@ +#include "selfdrive/boardd/usbdevice.h" + +#include +#include + +#include "selfdrive/common/swaglog.h" + +namespace { + +libusb_context *init_usb_ctx() { + libusb_context *context = nullptr; + int err = libusb_init(&context); + if (err != 0) { + LOGE("libusb initialization error %d", err); + 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; +} + +struct DeviceIterator { + DeviceIterator(libusb_context *ctx) { + ssize_t num_devices = libusb_get_device_list(ctx, &dev_list); + for (ssize_t i = 0; i < num_devices; ++i) { + libusb_device_descriptor desc = {}; + int ret = libusb_get_device_descriptor(dev_list[i], &desc); + if (ret < 0 || desc.idVendor != USB_VID || desc.idProduct != USB_PID) continue; + + libusb_device_handle *handle = nullptr; + if (libusb_open(dev_list[i], &handle) == 0) { + unsigned char serial[256] = {'\0'}; + libusb_get_string_descriptor_ascii(handle, desc.iSerialNumber, serial, std::size(serial) - 1); + devices[(const char *)serial] = dev_list[i]; + libusb_close(handle); + } + } + } + + ~DeviceIterator() { + if (dev_list) libusb_free_device_list(dev_list, 1); + } + + std::map::iterator begin() { return devices.begin(); } + std::map::iterator end() { return devices.end(); } + std::map devices; + libusb_device **dev_list = nullptr; +}; + +} // namespace + +bool USBDevice::open(const std::string &serial) { + ctx = init_usb_ctx(); + if (!ctx) return false; + + for (const auto &[s, device] : DeviceIterator(ctx)) { + if (serial.empty() || serial == s) { + usb_serial = s; + libusb_open(device, &dev_handle); + break; + } + } + if (!dev_handle) return false; + + if (libusb_kernel_driver_active(dev_handle, 0) == 1) { + libusb_detach_kernel_driver(dev_handle, 0); + } + + if (libusb_set_configuration(dev_handle, 1) != 0 || + libusb_claim_interface(dev_handle, 0) != 0) { + return false; + } + + return true; +} + +USBDevice::~USBDevice() { + if (dev_handle) { + libusb_release_interface(dev_handle, 0); + libusb_close(dev_handle); + } + if (ctx) libusb_exit(ctx); +} + +std::vector USBDevice::list() { + std::vector serials; + if (libusb_context *ctx = init_usb_ctx()) { + for (auto it : DeviceIterator(ctx)) { + serials.push_back(it.first); + } + libusb_exit(ctx); + } + return serials; +} + +int USBDevice::control_transfer(libusb_endpoint_direction dir, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout) { + std::lock_guard lk(usb_lock); + + int ret = LIBUSB_ERROR_NO_DEVICE; + const uint8_t bmRequestType = dir | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_RECIPIENT_DEVICE; + while (connected) { + ret = libusb_control_transfer(dev_handle, bmRequestType, bRequest, wValue, wIndex, NULL, 0, timeout); + if (ret >= 0) break; + + LOGE_100("usb control_transfer error %d, \"%s\"", ret, libusb_strerror((enum libusb_error)ret)); + if (ret == LIBUSB_ERROR_NO_DEVICE) { + LOGE("lost connection"); + connected = false; + } + // TODO: check other errors, is simply retrying okay? + } + return ret; +} + +int USBDevice::bulk_transfer(uint8_t endpoint, uint8_t *data, int length, unsigned int timeout) { + std::lock_guard lk(usb_lock); + + int transferred = 0; + while (connected) { + int ret = libusb_bulk_transfer(dev_handle, endpoint, data, length, &transferred, timeout); + if (ret == 0 && length == transferred) break; + + if (ret != 0) { + LOGE_100("usb control_transfer error %d, \"%s\"", ret, libusb_strerror((enum libusb_error)ret)); + if (ret == LIBUSB_ERROR_NO_DEVICE) { + connected = false; + } else if (ret == LIBUSB_ERROR_OVERFLOW) { + comms_healthy = false; + } else if (ret == LIBUSB_ERROR_TIMEOUT && (endpoint & LIBUSB_ENDPOINT_IN)) { + // timeout is okay to exit, recv still happened + LOGW("bulk_transfer timeout"); + break; + } + } + }; + return transferred; +} diff --git a/selfdrive/boardd/usbdevice.h b/selfdrive/boardd/usbdevice.h new file mode 100644 index 0000000000..1b94358b56 --- /dev/null +++ b/selfdrive/boardd/usbdevice.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include +#include + +#include +#include "panda/board/panda.h" + +#define TIMEOUT 0 + +class USBDevice { +public: + USBDevice() = default; + ~USBDevice(); + bool open(const std::string &serial); + static std::vector list(); + + inline int write(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout = TIMEOUT) { + return control_transfer(LIBUSB_ENDPOINT_OUT, bRequest, wValue, wIndex, timeout); + } + inline int read(uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint8_t *data, uint16_t wLength, unsigned int timeout = TIMEOUT) { + return control_transfer(LIBUSB_ENDPOINT_IN, bRequest, wValue, wIndex, timeout); + } + inline int bulk_write(uint8_t endpoint, uint8_t *data, int length, unsigned int timeout = TIMEOUT) { + return bulk_transfer(LIBUSB_ENDPOINT_OUT | endpoint, data, length, timeout); + } + inline int bulk_read(uint8_t endpoint, uint8_t *data, int length, unsigned int timeout = TIMEOUT) { + return bulk_transfer(LIBUSB_ENDPOINT_IN | endpoint, data, length, timeout); + } + + std::atomic comms_healthy = true; + std::atomic connected = true; + std::string usb_serial; + +private: + int control_transfer(libusb_endpoint_direction dir, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, unsigned int timeout = TIMEOUT); + int bulk_transfer(uint8_t endpoint, uint8_t *data, int length, unsigned int timeout = TIMEOUT); + + std::mutex usb_lock; + libusb_context *ctx = nullptr; + libusb_device_handle *dev_handle = nullptr; +};