Fix JotPluggler remote streaming

openpilot/cereal/messaging/bridge.cc

@@ -1,4 +1,10 @@
 #include <cassert>
+#include <cerrno>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <filesystem>
+#include <unistd.h>
 
 #include "openpilot/cereal/messaging/msgq_to_zmq.h"
 #include "openpilot/cereal/services.h"
@@ -24,7 +30,7 @@ void msgq_to_zmq(const std::vector<std::string> &endpoints, const std::string &i
   bridge.run(endpoints, ip);
 }
 
-void zmq_to_msgq(const std::vector<std::string> &endpoints, const std::string &ip) {
+void zmq_to_msgq(const std::vector<std::string> &endpoints, const std::string &ip, pid_t parent_pid) {
   auto poller = std::make_unique<BridgeZmqPoller>();
   auto pub_context = std::make_unique<Context>();
   auto sub_context = std::make_unique<BridgeZmqContext>();
@@ -34,14 +40,23 @@ void zmq_to_msgq(const std::vector<std::string> &endpoints, const std::string &i
     auto pub_sock = new PubSocket();
     auto sub_sock = new BridgeZmqSubSocket();
     size_t queue_size = services.at(endpoint).queue_size;
-    pub_sock->connect(pub_context.get(), endpoint, true, queue_size);
-    sub_sock->connect(sub_context.get(), endpoint, ip, false);
+    if (pub_sock->connect(pub_context.get(), endpoint, true, queue_size) != 0) {
+      const int error = errno;
+      dprintf(STDERR_FILENO, "Failed to create MSGQ publisher for [%s]: %s\n", endpoint.c_str(), std::strerror(error));
+      _exit(1);
+    }
+    if (sub_sock->connect(sub_context.get(), endpoint, ip, false) != 0) {
+      const int error = zmq_errno();
+      dprintf(STDERR_FILENO, "Failed to connect ZMQ subscriber for [%s] at [%s]: %s\n",
+              endpoint.c_str(), ip.c_str(), zmq_strerror(error));
+      _exit(1);
+    }
 
     poller->registerSocket(sub_sock);
     sub2pub[sub_sock] = pub_sock;
   }
 
-  while (!do_exit) {
+  while (!do_exit && (parent_pid == 0 || getppid() == parent_pid)) {
     for (auto sub_sock : poller->poll(100)) {
       std::unique_ptr<Message> msg(sub_sock->receive(true));
       if (msg) {
@@ -61,10 +76,13 @@ int main(int argc, char **argv) {
   bool is_zmq_to_msgq = argc > 2;
   std::string ip = is_zmq_to_msgq ? argv[1] : "127.0.0.1";
   std::string whitelist_str = is_zmq_to_msgq ? std::string(argv[2]) : "";
+  const pid_t parent_pid = argc > 3 ? std::atoi(argv[3]) : 0;
   std::vector<std::string> endpoints = get_services(whitelist_str, is_zmq_to_msgq);
 
   if (is_zmq_to_msgq) {
-    zmq_to_msgq(endpoints, ip);
+    zmq_to_msgq(endpoints, ip, parent_pid);
+    std::error_code error;
+    if (argc > 4) std::filesystem::remove_all(argv[4], error);
   } else {
     msgq_to_zmq(endpoints, ip);
   }

openpilot/tools/jotpluggler/.gitignore

@@ -8,3 +8,4 @@ generated_dbcs/.stamp
 generated_dbcs/*.dbc
 layouts/.jotpluggler_autosave/
 reports/
+tests/test_stream_bridge*

openpilot/tools/jotpluggler/SConscript

@@ -1,6 +1,7 @@
 import os
 import subprocess
 import bootstrap_icons
+import ffmpeg
 import imgui
 import libusb
 from opendbc import get_generated_dbcs
@@ -108,7 +109,14 @@ if arch == "Darwin":
 else:
   libs += ["GL", "dl"]
 
-program = jot_env.Program("jotpluggler", jot_env.Glob("*.cc"), LIBS=libs)
-jot_env.Depends(program, generated_dbc_stamp)
-jot_env.Depends(program, car_fingerprint_to_dbc)
-jot_env.Depends(program, event_extractors)
+jot_sources = [source for source in jot_env.Glob("*.cc") if os.path.basename(str(source)) != "main.cc"]
+jot_objects = jot_env.Object(jot_sources)
+jot_env.Depends(jot_objects, [generated_dbc_stamp, car_fingerprint_to_dbc, event_extractors])
+program = jot_env.Program("jotpluggler", ["main.cc"] + jot_objects, LIBS=libs)
+bridge = File("#openpilot/cereal/messaging/bridge")
+stream_test_env = jot_env.Clone()
+if arch == "Darwin":
+  stream_test_env["LINKFLAGS"] += [f"-Wl,-rpath,{ffmpeg.LIB_DIR}"]
+stream_bridge_test = stream_test_env.Program(
+  "tests/test_stream_bridge", ["tests/test_stream_bridge.cc"] + jot_objects, LIBS=libs)
+jot_env.Depends([program, stream_bridge_test], bridge)

openpilot/tools/jotpluggler/runtime.cc

@@ -1,5 +1,6 @@
 #include "tools/jotpluggler/app.h"
 #include "tools/jotpluggler/common.h"
+#include "tools/jotpluggler/stream_bridge.h"
 
 #include "openpilot/cereal/services.h"
 #include "common/timing.h"
@@ -8,17 +9,20 @@
 #include "imgui_impl_opengl3_loader.h"
 #include "implot.h"
 #include "common/yuv.h"
-#include "msgq_repo/msgq/ipc.h"
+#include "msgq_repo/msgq/impl_msgq.h"
 #include "tools/replay/framereader.h"
 
 #include <GLFW/glfw3.h>
 
 #include <chrono>
+#include <cerrno>
 #include <cmath>
 #include <condition_variable>
 #include <cstdio>
 #include <cstdlib>
+#include <cstring>
 #include <deque>
+#include <fstream>
 #include <iomanip>
 #include <iostream>
 #include <mutex>
@@ -36,6 +40,15 @@ namespace {
 
 std::atomic<bool> g_glfw_alive{false};
 const bool kLogCameraTimings = env_flag_enabled("JOTP_CAMERA_TIMINGS");
+std::mutex stream_msgq_setup_mutex;
+void stream_setup_test_checkpoint(const char *name, bool wait_for_release = false) {
+  if (const char *root = std::getenv("JOTP_STREAM_SETUP_TEST"); root != nullptr && std::getenv("JOTP_STREAM_BRIDGE") != nullptr) {
+    std::ofstream(std::filesystem::path(root) / name).put('\n');
+    for (int i = 0; wait_for_release && !std::filesystem::exists(std::filesystem::path(root) / "release") && i < 5000; ++i) {
+      std::this_thread::sleep_for(std::chrono::milliseconds(1));
+    }
+  }
+}
 
 CameraType decoder_camera_type(CameraViewKind view) {
   switch (view) {
@@ -559,29 +572,54 @@ struct StreamPoller::Impl {
   }
 
   void run_cereal_source(StreamAccumulator *accumulator) {
-    if (source.kind == StreamSourceKind::CerealRemote) {
-      setenv("ZMQ", "1", 1);
-    } else {
-      unsetenv("ZMQ");
+    const bool remote = source.kind == StreamSourceKind::CerealRemote;
+    std::vector<std::string> selected_services;
+    for (const auto &[name, _] : services) {
+      if (should_subscribe_stream_service(name)) selected_services.push_back(name);
     }
+    const bool setup_test = std::getenv("JOTP_STREAM_SETUP_TEST") != nullptr && std::getenv("JOTP_STREAM_BRIDGE") != nullptr;
+    std::unique_lock setup_lock(stream_msgq_setup_mutex, std::defer_lock);
+    if (!remote && setup_test && !setup_lock.try_lock()) stream_setup_test_checkpoint("local-blocked");
+    if (!setup_lock.owns_lock()) setup_lock.lock();
+    std::unique_ptr<ScopedMsgqPrefix> private_namespace;
+    StreamBridgeProcess bridge_process;
+    if (remote) {
+      private_namespace = std::make_unique<ScopedMsgqPrefix>();
+      private_namespace->activate();
+      const char *override = std::getenv("JOTP_STREAM_BRIDGE");
+      const std::filesystem::path executable = override != nullptr
+        ? override : repo_root() / "openpilot/cereal/messaging/bridge";
+      bridge_process.start(executable, {source.address, stream_bridge_whitelist(selected_services)}, private_namespace->path());
+    }
+    if (remote) stream_setup_test_checkpoint("remote-held", true);
 
     std::unique_ptr<Context> context(Context::create());
     std::unique_ptr<Poller> poller(Poller::create());
     std::vector<std::unique_ptr<SubSocket>> sockets;
-    sockets.reserve(services.size());
-    for (const auto &[name, info] : services) {
-      if (!should_subscribe_stream_service(name)) continue;
-      std::unique_ptr<SubSocket> socket(
-        SubSocket::create(context.get(), name.c_str(), source.address.c_str(), false, true, info.queue_size));
-      if (socket == nullptr) continue;
+    for (const std::string &name : selected_services) {
+      const service &info = services.at(name);
+      std::unique_ptr<SubSocket> socket(SubSocket::create());
+      if (socket->connect(context.get(), name, kStreamMsgqAddress, false, true, info.queue_size) != 0) {
+        const int error = errno;
+        if (auto *msgq_socket = dynamic_cast<MSGQSubSocket *>(socket.get()); msgq_socket && msgq_socket->getQueue()) {
+          msgq_socket->getQueue()->mmap_p = nullptr;
+          msgq_socket->getQueue()->size = 0;
+        }
+        if (!remote) continue;
+        throw std::runtime_error("Failed to connect cereal service " + name + ": " + std::strerror(error));
+      }
       socket->setTimeout(0);
       poller->registerSocket(socket.get());
       sockets.push_back(std::move(socket));
     }
+    if (private_namespace) private_namespace->restore();
+    setup_lock.unlock();
     if (sockets.empty()) throw std::runtime_error("Failed to connect to any cereal service");
+    if (remote) bridge_process.check_running();
     connected.store(true);
 
     while (running.load()) {
+      if (remote) bridge_process.check_running();
       std::vector<SubSocket *> ready = poller->poll(1);
       for (SubSocket *socket : ready) {
         while (running.load()) {

openpilot/tools/jotpluggler/stream_bridge.cc

@@ -0,0 +1,116 @@
+#include "tools/jotpluggler/stream_bridge.h"
+
+#include <cerrno>
+#include <csignal>
+#include <cstdlib>
+#include <cstring>
+#include <stdexcept>
+#include <thread>
+
+#include <spawn.h>
+#include <sys/wait.h>
+#include <unistd.h>
+
+extern char **environ;
+namespace {
+std::filesystem::path msgq_root() {
+#ifdef __APPLE__
+  return "/tmp";
+#else
+  return "/dev/shm";
+#endif
+}
+pid_t wait_nointr(pid_t pid, int *status, int options) {
+  pid_t result;
+  do { result = waitpid(pid, status, options); } while (result < 0 && errno == EINTR);
+  return result;
+}
+std::string exit_error(const std::string &executable, int status) {
+  if (WIFEXITED(status)) return executable + " exited with status " + std::to_string(WEXITSTATUS(status));
+  if (WIFSIGNALED(status)) return executable + " terminated by signal " + std::to_string(WTERMSIG(status));
+  return executable + " exited unexpectedly";
+}
+}  // namespace
+std::string stream_bridge_whitelist(const std::vector<std::string> &service_names) {
+  std::string whitelist;
+  for (const std::string &name : service_names) whitelist += "/\"" + name + "\"/";
+  return whitelist;
+}
+ScopedMsgqPrefix::ScopedMsgqPrefix() {
+  std::string path_template = (msgq_root() / ("msgq_jotp_" + std::to_string(getpid()) + "_XXXXXX")).string();
+  char *created = mkdtemp(path_template.data());
+  if (created == nullptr) throw std::runtime_error("Failed to create a private MSGQ namespace: " + std::string(std::strerror(errno)));
+  path_ = created;
+  prefix_ = path_.filename().string().substr(std::strlen("msgq_"));
+}
+ScopedMsgqPrefix::~ScopedMsgqPrefix() { restore(); std::error_code error; std::filesystem::remove_all(path_, error); }
+void ScopedMsgqPrefix::activate() {
+  if (active_) throw std::runtime_error("MSGQ namespace is already active");
+  previous_prefix_.reset();
+  if (const char *prefix = std::getenv("OPENPILOT_PREFIX")) previous_prefix_ = prefix;
+  if (setenv("OPENPILOT_PREFIX", prefix_.c_str(), 1) != 0) throw std::runtime_error("Failed to activate MSGQ namespace: " + std::string(std::strerror(errno)));
+  active_ = true;
+}
+void ScopedMsgqPrefix::restore() noexcept {
+  if (!active_) return;
+  if (previous_prefix_) setenv("OPENPILOT_PREFIX", previous_prefix_->c_str(), 1);
+  else unsetenv("OPENPILOT_PREFIX");
+  active_ = false;
+}
+StreamBridgeProcess::StreamBridgeProcess(std::chrono::milliseconds terminate_grace) : terminate_grace_(terminate_grace) {}
+StreamBridgeProcess::~StreamBridgeProcess() { stop(); }
+void StreamBridgeProcess::start(const std::filesystem::path &executable, const std::vector<std::string> &arguments,
+                                const std::filesystem::path &cleanup_path) {
+  if (owned()) throw std::runtime_error("A stream bridge process is already running");
+  if (executable.empty()) throw std::runtime_error("Stream bridge executable path is empty");
+  executable_ = executable.string();
+  std::vector<std::string> storage{executable_};
+  storage.insert(storage.end(), arguments.begin(), arguments.end());
+  if (!cleanup_path.empty()) { storage.push_back(std::to_string(getpid())); storage.push_back(cleanup_path.string()); }
+  std::vector<char *> argv;
+  for (std::string &argument : storage) argv.push_back(argument.data());
+  argv.push_back(nullptr);
+  const int result = posix_spawn(&pid_, executable_.c_str(), nullptr, nullptr, argv.data(), environ);
+  if (result != 0) {
+    pid_ = -1; throw std::runtime_error("Failed to start stream bridge " + executable_ + ": " + std::strerror(result));
+  }
+  const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(100);
+  do {
+    check_running();
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+  } while (std::chrono::steady_clock::now() < deadline);
+  check_running();
+}
+void StreamBridgeProcess::check_running() {
+  if (!owned()) throw std::runtime_error("Stream bridge process is not running");
+  int status = 0;
+  const pid_t result = wait_nointr(pid_, &status, WNOHANG);
+  if (result == 0) return;
+  if (result == pid_) {
+    pid_ = -1; throw std::runtime_error(exit_error(executable_, status));
+  }
+  if (result < 0 && errno == ECHILD) {
+    pid_ = -1; throw std::runtime_error(executable_ + " is no longer waitable");
+  }
+  if (result < 0) throw std::runtime_error("Failed to inspect stream bridge " + executable_ + ": " + std::strerror(errno));
+}
+void StreamBridgeProcess::stop() noexcept {
+  if (!owned()) return;
+  const pid_t child = pid_;
+  int status = 0;
+  const pid_t initial = wait_nointr(child, &status, WNOHANG);
+  if (initial == child || (initial < 0 && errno == ECHILD)) { pid_ = -1; return; }
+  kill(child, SIGTERM);
+  const auto deadline = std::chrono::steady_clock::now() + terminate_grace_;
+  while (std::chrono::steady_clock::now() < deadline) {
+    const pid_t result = wait_nointr(child, &status, WNOHANG);
+    if (result == child || (result < 0 && errno == ECHILD)) {
+      pid_ = -1; return;
+    }
+    if (result < 0) break;
+    std::this_thread::sleep_for(std::chrono::milliseconds(5));
+  }
+  kill(child, SIGKILL);
+  wait_nointr(child, &status, 0);
+  pid_ = -1;
+}

openpilot/tools/jotpluggler/stream_bridge.h

@@ -0,0 +1,45 @@
+#pragma once
+
+#include <chrono>
+#include <filesystem>
+#include <optional>
+#include <string>
+#include <vector>
+
+#include <sys/types.h>
+inline constexpr char kStreamMsgqAddress[] = "127.0.0.1";
+std::string stream_bridge_whitelist(const std::vector<std::string> &services);
+class ScopedMsgqPrefix {
+public:
+  ScopedMsgqPrefix();
+  ~ScopedMsgqPrefix();
+  ScopedMsgqPrefix(const ScopedMsgqPrefix &) = delete;
+  ScopedMsgqPrefix &operator=(const ScopedMsgqPrefix &) = delete;
+  void activate();
+  void restore() noexcept;
+  const std::string &prefix() const { return prefix_; }
+  const std::filesystem::path &path() const { return path_; }
+private:
+  std::string prefix_;
+  std::filesystem::path path_;
+  std::optional<std::string> previous_prefix_;
+  bool active_ = false;
+};
+
+class StreamBridgeProcess {
+public:
+  explicit StreamBridgeProcess(std::chrono::milliseconds terminate_grace = std::chrono::seconds(3));
+  ~StreamBridgeProcess();
+  StreamBridgeProcess(const StreamBridgeProcess &) = delete;
+  StreamBridgeProcess &operator=(const StreamBridgeProcess &) = delete;
+  void start(const std::filesystem::path &executable, const std::vector<std::string> &arguments = {},
+             const std::filesystem::path &cleanup_path = {});
+  void check_running();
+  void stop() noexcept;
+  bool owned() const { return pid_ > 0; }
+  pid_t pid() const { return pid_; }
+private:
+  std::chrono::milliseconds terminate_grace_;
+  std::string executable_;
+  pid_t pid_ = -1;
+};