From aecb6d13e7a605ee25628bdc69086960b2b3c801 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Mon, 4 Aug 2025 16:09:22 -0700 Subject: [PATCH] cabana: add independent panda lib (#35920) * cabana: add separate panda lib * cleanup --- tools/cabana/SConscript | 7 +- tools/cabana/panda.cc | 346 +++++++++++++++++++++++++++++ tools/cabana/panda.h | 95 ++++++++ tools/cabana/streams/pandastream.h | 2 +- 4 files changed, 446 insertions(+), 4 deletions(-) create mode 100644 tools/cabana/panda.cc create mode 100644 tools/cabana/panda.h diff --git a/tools/cabana/SConscript b/tools/cabana/SConscript index 91edfafe00..89be3cceb2 100644 --- a/tools/cabana/SConscript +++ b/tools/cabana/SConscript @@ -15,7 +15,8 @@ else: qt_libs = ['qt_util'] + base_libs cabana_env = qt_env.Clone() -cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, 'panda', 'avutil', 'avcodec', 'avformat', 'bz2', 'zstd', 'curl', 'yuv', 'usb-1.0'] + qt_libs + +cabana_libs = [widgets, cereal, messaging, visionipc, replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'zstd', 'curl', 'yuv', 'usb-1.0'] + qt_libs opendbc_path = '-DOPENDBC_FILE_PATH=\'"%s"\'' % (cabana_env.Dir("../../opendbc/dbc").abspath) cabana_env['CXXFLAGS'] += [opendbc_path] @@ -29,7 +30,7 @@ cabana_lib = cabana_env.Library("cabana_lib", ['mainwin.cc', 'streams/socketcans 'streams/routes.cc', 'dbc/dbc.cc', 'dbc/dbcfile.cc', 'dbc/dbcmanager.cc', 'utils/export.cc', 'utils/util.cc', 'chart/chartswidget.cc', 'chart/chart.cc', 'chart/signalselector.cc', 'chart/tiplabel.cc', 'chart/sparkline.cc', - 'commands.cc', 'messageswidget.cc', 'streamselector.cc', 'settings.cc', + 'commands.cc', 'messageswidget.cc', 'streamselector.cc', 'settings.cc', 'panda.cc', 'cameraview.cc', 'detailwidget.cc', 'tools/findsimilarbits.cc', 'tools/findsignal.cc', 'tools/routeinfo.cc'], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) cabana_env.Program('cabana', ['cabana.cc', cabana_lib, assets], LIBS=cabana_libs, FRAMEWORKS=base_frameworks) @@ -40,4 +41,4 @@ output_json_file = 'tools/cabana/dbc/car_fingerprint_to_dbc.json' generate_dbc = cabana_env.Command('#' + output_json_file, ['dbc/generate_dbc_json.py'], "python3 tools/cabana/dbc/generate_dbc_json.py --out " + output_json_file) -cabana_env.Depends(generate_dbc, ["#common", "#selfdrive/pandad", '#opendbc_repo', "#cereal", "#msgq_repo"]) +cabana_env.Depends(generate_dbc, ["#common", '#opendbc_repo', "#cereal", "#msgq_repo"]) diff --git a/tools/cabana/panda.cc b/tools/cabana/panda.cc new file mode 100644 index 0000000000..0612d67746 --- /dev/null +++ b/tools/cabana/panda.cc @@ -0,0 +1,346 @@ +#include "tools/cabana/panda.h" + +#include +#include +#include +#include +#include + +#include "cereal/messaging/messaging.h" +#include "common/swaglog.h" +#include "common/util.h" + +static libusb_context *init_usb_ctx() { + libusb_context *context = nullptr; + int err = libusb_init(&context); + if (err != 0) { + LOGE("libusb initialization error"); + 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; +} + +Panda::Panda(std::string serial, uint32_t bus_offset) : bus_offset(bus_offset) { + if (!init_usb_connection(serial)) { + throw std::runtime_error("Error connecting to panda"); + } + + LOGW("connected to %s over USB", serial.c_str()); + hw_type = get_hw_type(); + can_reset_communications(); +} + +Panda::~Panda() { + cleanup_usb(); +} + +bool Panda::connected() { + return connected_flag; +} + +bool Panda::comms_healthy() { + return comms_healthy_flag; +} + +std::string Panda::hw_serial() { + return hw_serial_str; +} + +std::vector Panda::list(bool usb_only) { + static std::unique_ptr context(init_usb_ctx(), libusb_exit); + + ssize_t num_devices; + libusb_device **dev_list = NULL; + std::vector serials; + if (!context) { return serials; } + + num_devices = libusb_get_device_list(context.get(), &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 == 0x3801 && 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)); + } + } + +finish: + if (dev_list != NULL) { + libusb_free_device_list(dev_list, 1); + } + return serials; +} + +void Panda::set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param) { + control_write(0xdc, (uint16_t)safety_model, safety_param); +} + + +cereal::PandaState::PandaType Panda::get_hw_type() { + unsigned char hw_query[1] = {0}; + + control_read(0xc1, 0, 0, hw_query, 1); + return (cereal::PandaState::PandaType)(hw_query[0]); +} + + + + +void Panda::send_heartbeat(bool engaged) { + control_write(0xf3, engaged, 0); +} + +void Panda::set_can_speed_kbps(uint16_t bus, uint16_t speed) { + control_write(0xde, bus, (speed * 10)); +} + + +void Panda::set_data_speed_kbps(uint16_t bus, uint16_t speed) { + control_write(0xf9, bus, (speed * 10)); +} + + + +bool Panda::can_receive(std::vector& out_vec) { + // Check if enough space left in buffer to store RECV_SIZE data + assert(receive_buffer_size + RECV_SIZE <= sizeof(receive_buffer)); + + int recv = bulk_read(0x81, &receive_buffer[receive_buffer_size], RECV_SIZE); + if (!comms_healthy()) { + return false; + } + + bool ret = true; + if (recv > 0) { + receive_buffer_size += recv; + ret = unpack_can_buffer(receive_buffer, receive_buffer_size, out_vec); + } + return ret; +} + +void Panda::can_reset_communications() { + control_write(0xc0, 0, 0); +} + +bool Panda::unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec) { + int pos = 0; + + while (pos <= size - sizeof(can_header)) { + can_header header; + memcpy(&header, &data[pos], sizeof(can_header)); + + const uint8_t data_len = dlc_to_len[header.data_len_code]; + if (pos + sizeof(can_header) + data_len > size) { + // we don't have all the data for this message yet + break; + } + + if (calculate_checksum(&data[pos], sizeof(can_header) + data_len) != 0) { + LOGE("Panda CAN checksum failed"); + size = 0; + can_reset_communications(); + return false; + } + + can_frame &canData = out_vec.emplace_back(); + canData.address = header.addr; + canData.src = header.bus + bus_offset; + if (header.rejected) { + canData.src += CAN_REJECTED_BUS_OFFSET; + } + if (header.returned) { + canData.src += CAN_RETURNED_BUS_OFFSET; + } + + canData.dat.assign((char *)&data[pos + sizeof(can_header)], data_len); + + pos += sizeof(can_header) + data_len; + } + + // move the overflowing data to the beginning of the buffer for the next round + memmove(data, &data[pos], size - pos); + size -= pos; + + return true; +} + +uint8_t Panda::calculate_checksum(uint8_t *data, uint32_t len) { + uint8_t checksum = 0U; + for (uint32_t i = 0U; i < len; i++) { + checksum ^= data[i]; + } + return checksum; +} + +// USB implementation methods +bool Panda::init_usb_connection(const std::string& serial) { + ssize_t num_devices; + libusb_device **dev_list = NULL; + int err = 0; + + ctx = init_usb_ctx(); + if (!ctx) { 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 == 0x3801 && 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; } + + hw_serial_str = std::string((char *)desc_serial, ret); + if (serial.empty() || serial == hw_serial_str) { + break; + } + libusb_close(dev_handle); + dev_handle = NULL; + } + } + if (dev_handle == NULL) goto fail; + libusb_free_device_list(dev_list, 1); + + 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 true; + +fail: + if (dev_list != NULL) { + libusb_free_device_list(dev_list, 1); + } + cleanup_usb(); + return false; +} + +void Panda::cleanup_usb() { + if (dev_handle) { + libusb_release_interface(dev_handle, 0); + libusb_close(dev_handle); + dev_handle = nullptr; + } + + if (ctx) { + libusb_exit(ctx); + ctx = nullptr; + } + connected_flag = false; +} + +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_flag = false; + } +} + +int Panda::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_flag) { + return LIBUSB_ERROR_NO_DEVICE; + } + + 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_flag); + + return err; +} + +int Panda::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_flag) { + return LIBUSB_ERROR_NO_DEVICE; + } + + 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_flag); + + return err; +} + +int Panda::bulk_write(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + if (!connected_flag) { + return 0; + } + + do { + 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_flag); + + return transferred; +} + +int Panda::bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout) { + int err; + int transferred = 0; + + if (!connected_flag) { + return 0; + } + + 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_flag = false; + LOGE_100("overflow got 0x%x", transferred); + } else if (err != 0) { + handle_usb_issue(err, __func__); + } + } while (err != 0 && connected_flag); + + return transferred; +} diff --git a/tools/cabana/panda.h b/tools/cabana/panda.h new file mode 100644 index 0000000000..d318c33f4d --- /dev/null +++ b/tools/cabana/panda.h @@ -0,0 +1,95 @@ +#pragma once + +#include +#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 "panda/board/can.h" + +#define USB_TX_SOFT_LIMIT (0x100U) +#define USBPACKET_MAX_SIZE (0x40) +#define RECV_SIZE (0x4000U) +#define TIMEOUT 0 + +#define CAN_REJECTED_BUS_OFFSET 0xC0U +#define CAN_RETURNED_BUS_OFFSET 0x80U + +#define PANDA_BUS_OFFSET 4 + +struct __attribute__((packed)) can_header { + uint8_t reserved : 1; + uint8_t bus : 3; + uint8_t data_len_code : 4; + uint8_t rejected : 1; + uint8_t returned : 1; + uint8_t extended : 1; + uint32_t addr : 29; + uint8_t checksum : 8; +}; + +struct can_frame { + long address; + std::string dat; + long src; +}; + + +class Panda { +public: + Panda(std::string serial="", uint32_t bus_offset=0); + ~Panda(); + + cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN; + const uint32_t bus_offset; + + bool connected(); + bool comms_healthy(); + std::string hw_serial(); + + // Static functions + static std::vector list(bool usb_only=false); + + // Panda functionality + cereal::PandaState::PandaType get_hw_type(); + void set_safety_model(cereal::CarParams::SafetyModel safety_model, uint16_t safety_param=0U); + void send_heartbeat(bool engaged); + void set_can_speed_kbps(uint16_t bus, uint16_t speed); + void set_data_speed_kbps(uint16_t bus, uint16_t speed); + bool can_receive(std::vector& out_vec); + void can_reset_communications(); + +private: + // USB connection members + libusb_context *ctx = nullptr; + libusb_device_handle *dev_handle = nullptr; + std::string hw_serial_str; + std::atomic connected_flag = true; + std::atomic comms_healthy_flag = true; + + // CAN buffer members + uint8_t receive_buffer[RECV_SIZE + sizeof(can_header) + 64]; + uint32_t receive_buffer_size = 0; + + // Internal methods + bool init_usb_connection(const std::string& serial); + void cleanup_usb(); + void handle_usb_issue(int err, const char func[]); + 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); + bool unpack_can_buffer(uint8_t *data, uint32_t &size, std::vector &out_vec); + uint8_t calculate_checksum(uint8_t *data, uint32_t len); +}; diff --git a/tools/cabana/streams/pandastream.h b/tools/cabana/streams/pandastream.h index 826b1aa986..e17ad887fc 100644 --- a/tools/cabana/streams/pandastream.h +++ b/tools/cabana/streams/pandastream.h @@ -7,7 +7,7 @@ #include #include "tools/cabana/streams/livestream.h" -#include "selfdrive/pandad/panda.h" +#include "tools/cabana/panda.h" const uint32_t speeds[] = {10U, 20U, 50U, 100U, 125U, 250U, 500U, 1000U}; const uint32_t data_speeds[] = {10U, 20U, 50U, 100U, 125U, 250U, 500U, 1000U, 2000U, 5000U};