IQ.Pilot Release Commit @ f2a861c

iqpilot/tools/cabana/streams/socketcanstream.cc

@@ -0,0 +1,148 @@
+#include "tools/cabana/streams/socketcanstream.h"
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include <cstdio>
+#include <filesystem>
+#include <fstream>
+
+#include <QFormLayout>
+#include <QHBoxLayout>
+#include <QMessageBox>
+#include <QPushButton>
+
+SocketCanStream::SocketCanStream(QObject *parent, SocketCanStreamConfig config_) : config(config_), LiveStream(parent) {
+  if (!available()) {
+    throw std::runtime_error("SocketCAN not available");
+  }
+
+  fprintf(stderr, "Connecting to SocketCAN device %s\n", config.device.c_str());
+  if (!connect()) {
+    throw std::runtime_error("Failed to connect to SocketCAN device");
+  }
+}
+
+SocketCanStream::~SocketCanStream() {
+  stop();
+  if (sock_fd >= 0) {
+    ::close(sock_fd);
+    sock_fd = -1;
+  }
+}
+
+bool SocketCanStream::available() {
+  int fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+  if (fd < 0) return false;
+  ::close(fd);
+  return true;
+}
+
+bool SocketCanStream::connect() {
+  sock_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+  if (sock_fd < 0) {
+    fprintf(stderr, "Failed to create CAN socket\n");
+    return false;
+  }
+
+  // Enable CAN-FD
+  int fd_enable = 1;
+  setsockopt(sock_fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &fd_enable, sizeof(fd_enable));
+
+  struct ifreq ifr = {};
+  strncpy(ifr.ifr_name, config.device.c_str(), IFNAMSIZ - 1);
+  if (ioctl(sock_fd, SIOCGIFINDEX, &ifr) < 0) {
+    fprintf(stderr, "Failed to get interface index for %s\n", config.device.c_str());
+    ::close(sock_fd);
+    sock_fd = -1;
+    return false;
+  }
+
+  struct sockaddr_can addr = {};
+  addr.can_family = AF_CAN;
+  addr.can_ifindex = ifr.ifr_ifindex;
+  if (bind(sock_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+    fprintf(stderr, "Failed to bind CAN socket\n");
+    ::close(sock_fd);
+    sock_fd = -1;
+    return false;
+  }
+
+  // Set read timeout so the thread can check for interruption
+  struct timeval tv = {.tv_sec = 0, .tv_usec = 100000};  // 100ms
+  setsockopt(sock_fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv));
+
+  return true;
+}
+
+void SocketCanStream::streamThread() {
+  struct canfd_frame frame;
+
+  while (!exit_) {
+    ssize_t nbytes = read(sock_fd, &frame, sizeof(frame));
+    if (nbytes <= 0) continue;
+
+    uint8_t len = (nbytes == CAN_MTU) ? frame.len : frame.len;  // works for both CAN and CAN-FD
+
+    MessageBuilder msg;
+    auto evt = msg.initEvent();
+    auto canData = evt.initCan(1);
+    canData[0].setAddress(frame.can_id & CAN_EFF_MASK);
+    canData[0].setSrc(0);
+    canData[0].setDat(kj::arrayPtr(frame.data, len));
+
+    handleEvent(capnp::messageToFlatArray(msg));
+  }
+}
+
+OpenSocketCanWidget::OpenSocketCanWidget(QWidget *parent) : AbstractOpenStreamWidget(parent) {
+  QVBoxLayout *main_layout = new QVBoxLayout(this);
+  main_layout->addStretch(1);
+
+  QFormLayout *form_layout = new QFormLayout();
+
+  QHBoxLayout *device_layout = new QHBoxLayout();
+  device_edit = new QComboBox();
+  device_edit->setFixedWidth(300);
+  device_layout->addWidget(device_edit);
+
+  QPushButton *refresh = new QPushButton(tr("Refresh"));
+  refresh->setFixedWidth(100);
+  device_layout->addWidget(refresh);
+  form_layout->addRow(tr("Device"), device_layout);
+  main_layout->addLayout(form_layout);
+
+  main_layout->addStretch(1);
+
+  QObject::connect(refresh, &QPushButton::clicked, this, &OpenSocketCanWidget::refreshDevices);
+  QObject::connect(device_edit, &QComboBox::currentTextChanged, this, [=]{ config.device = device_edit->currentText().toStdString(); });
+
+  // Populate devices
+  refreshDevices();
+}
+
+void OpenSocketCanWidget::refreshDevices() {
+  device_edit->clear();
+  // Scan /sys/class/net/ for CAN interfaces (type 280 = ARPHRD_CAN)
+  std::error_code ec;
+  for (const auto &entry : std::filesystem::directory_iterator("/sys/class/net", ec)) {
+    std::ifstream type_file(entry.path() / "type");
+    int type = 0;
+    if (type_file >> type && type == 280) {
+      device_edit->addItem(QString::fromStdString(entry.path().filename().string()));
+    }
+  }
+}
+
+AbstractStream *OpenSocketCanWidget::open() {
+  try {
+    return new SocketCanStream(qApp, config);
+  } catch (std::exception &e) {
+    QMessageBox::warning(nullptr, tr("Warning"), tr("Failed to connect to SocketCAN device: '%1'").arg(e.what()));
+    return nullptr;
+  }
+}