boardd: new class USBDevice (#23015)
* USBDevice * merge master * rebase master * add to fiels_common * inline functions * namespace * init err * remove nested if * cleanup * bump panda for branch * change includes * bump panda branch * change name * bump master panda Co-authored-by: Igor Biletskyy <briskspirit@users.noreply.github.com> Co-authored-by: Igor Biletskyy <bs@privacy.im>pull/23258/head
Dean Lee
GitHub
parent
e49480185a
commit
4061f50da3
2
panda
2
panda
|
|
@ -1 +1 @@
|
|||
Subproject commit 6f95a096e6beb254786759003a38e6e5c4f2c10e
|
||||
Subproject commit 8d0d148681163929260abf5742701cbf503abb22
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -185,7 +185,7 @@ bool safety_setter_thread(std::vector<Panda *> pandas) {
|
|||
Panda *usb_connect(std::string serial="", uint32_t index=0) {
|
||||
std::unique_ptr<Panda> panda;
|
||||
try {
|
||||
panda = std::make_unique<Panda>(serial, (index * PANDA_BUS_CNT));
|
||||
panda = std::make_unique<Panda>(serial, (index * BUS_CNT));
|
||||
} catch (std::exception &e) {
|
||||
return nullptr;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,10 +1,7 @@
|
|||
#include "selfdrive/boardd/panda.h"
|
||||
|
||||
#include <unistd.h>
|
||||
|
||||
#include <cassert>
|
||||
#include <stdexcept>
|
||||
#include <vector>
|
||||
|
||||
#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<std::string> Panda::list() {
|
||||
// init libusb
|
||||
ssize_t num_devices;
|
||||
libusb_context *context = NULL;
|
||||
libusb_device **dev_list = NULL;
|
||||
std::vector<std::string> 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<std::vector<uint8_t>> Panda::get_firmware_version() {
|
||||
std::vector<uint8_t> 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<std::string> 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<cereal::CanData>::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<cereal::CanData>::Reader &can_data
|
|||
|
||||
void Panda::can_send(capnp::List<cereal::CanData>::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<can_frame>& 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<can_frame> &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]);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -1,26 +1,15 @@
|
|||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <cstdint>
|
||||
#include <ctime>
|
||||
#include <functional>
|
||||
#include <list>
|
||||
#include <mutex>
|
||||
#include <optional>
|
||||
#include <vector>
|
||||
|
||||
#include <libusb-1.0/libusb.h>
|
||||
|
||||
#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<uint8_t> 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<bool> connected = true;
|
||||
std::atomic<bool> 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<std::string> 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<cereal::CanData>::Reader &can_data_list,
|
||||
std::function<void(uint8_t *, size_t)> write_func);
|
||||
bool unpack_can_buffer(uint8_t *data, int size, std::vector<can_frame> &out_vec);
|
||||
|
||||
private:
|
||||
std::vector<uint8_t> recv_buf;
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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() {
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,140 @@
|
|||
#include "selfdrive/boardd/usbdevice.h"
|
||||
|
||||
#include <map>
|
||||
#include <memory>
|
||||
|
||||
#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<std::string, libusb_device *>::iterator begin() { return devices.begin(); }
|
||||
std::map<std::string, libusb_device *>::iterator end() { return devices.end(); }
|
||||
std::map<std::string, libusb_device *> 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<std::string> USBDevice::list() {
|
||||
std::vector<std::string> 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;
|
||||
}
|
||||
|
|
@ -0,0 +1,43 @@
|
|||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <mutex>
|
||||
#include <vector>
|
||||
|
||||
#include <libusb-1.0/libusb.h>
|
||||
#include "panda/board/panda.h"
|
||||
|
||||
#define TIMEOUT 0
|
||||
|
||||
class USBDevice {
|
||||
public:
|
||||
USBDevice() = default;
|
||||
~USBDevice();
|
||||
bool open(const std::string &serial);
|
||||
static std::vector<std::string> 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<bool> comms_healthy = true;
|
||||
std::atomic<bool> 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;
|
||||
};
|
||||
Loading…
Reference in New Issue