IQ.Pilot Release Commit @ b6534c0

artifacts/package_sources/msgq/.gitignore

@@ -0,0 +1,20 @@
+/gen/
+*.egg-info/
+*.tmp
+*.pyc
+__pycache__
+.*.swp
+.*.swo
+*.os
+*.so
+*.o
+*.a
+uv.lock
+catch2/
+
+test_runner
+
+libmessaging.*
+libmessaging_shared.*
+.sconsign.dblite
+.mypy_cache/

artifacts/package_sources/msgq/MANIFEST.in

@@ -0,0 +1,5 @@
+include SConstruct SConscript
+graft msgq
+graft site_scons
+graft third_party
+global-exclude __pycache__ *.pyc *.o *.os *.d

artifacts/package_sources/msgq/README.md

@@ -0,0 +1,54 @@
+# MSGQ: A lock free single producer multi consumer message queue
+
+## What is this library?
+MSGQ is a generic high performance IPC pub sub system with a single publisher and multiple subscribers. MSGQ is designed to be a high performance replacement for ZMQ-like SUB/PUB patterns. It uses a ring buffer in shared memory to efficiently read and write data. Each read requires a copy. Writing can be done without a copy, as long as the size of the data is known in advance. While MSGQ is the core of this library, this library also allows replacing the MSGQ backend with ZMQ or a spoofed implementation that can be used for deterministic testing. This library also contains visionipc, an IPC system specifically for large contiguous buffers (like images/video).
+
+## Storage
+The storage for the queue consists of an area of metadata, and the actual buffer. The metadata contains:
+
+1. A counter to the number of readers that are active
+2. A pointer to the head of the queue for writing. From now on referred to as *write pointer*
+3. A cycle counter for the writer. This counter is incremented when the writer wraps around
+4. N pointers, pointing to the current read position for all the readers. From now on referred to as *read pointer*
+5. N counters,  counting the number of cycles for all the readers
+6. N booleans, indicating validity for all the readers. From now on referred to as *validity flag*
+
+The counter and the pointer are both 32 bit values, packed into 64 bit so they can be read and written atomically.
+
+The data buffer is a ring buffer. All messages are prefixed by an 8 byte size field, followed by the data. A size of -1 indicates a wrap-around, and means the next message is stored at the beginning of the buffer.
+
+
+## Writing
+Writing involves the following steps:
+
+1. Check if the area that is to be written overlaps with any of the read pointers, mark those readers as invalid by clearing the validity flag.
+2. Write the message
+3. Increase the write pointer by the size of the message
+
+In case there is not enough space at the end of the buffer, a special empty message with a prefix of -1 is written. The cycle counter is incremented by one. In this case step 1 will check there are no read pointers pointing to the remainder of the buffer. Then another write cycle will start with the actual message.
+
+There always needs to be 8 bytes of empty space at the end of the buffer. By doing this there is always space to write the -1.
+
+## Reset reader
+When the reader is lagging too much behind the read pointer becomes invalid and no longer points to the beginning of a valid message. To reset a reader to the current write pointer, the following steps are performed:
+
+1. Set valid flag
+2. Set read cycle counter to that of the writer
+3. Set read pointer to write pointer
+
+## Reading
+Reading involves the following steps:
+
+1. Read the size field at the current read pointer
+2. Read the validity flag
+3. Copy the data out of the buffer
+4. Increase the read pointer by the size of the message
+5. Check the validity flag again
+
+Before starting the copy, the valid flag is checked. This is to prevent a race condition where the size prefix was invalid, and the read could read outside of the buffer. Make sure that step 1 and 2 are not reordered by your compiler or CPU.
+
+If a writer overwrites the data while it's being copied out, the data will be invalid. Therefore the validity flag is also checked after reading it. The order of step 4 and 5 does not matter.
+
+If at steps 2 or 5 the validity flag is not set, the reader is reset. Any data that was already read is discarded. After the reader is reset, the reading starts from the beginning.
+
+If a message with size -1 is encountered, step 3 and 4 are replaced by increasing the cycle counter and setting the read pointer to the beginning of the buffer. After that another read is performed.

artifacts/package_sources/msgq/SConscript

@@ -0,0 +1,48 @@
+Import('env', 'envCython', 'arch', 'common')
+
+
+visionipc_dir = Dir('msgq/visionipc')
+gen_dir = Dir('gen')
+
+
+# Build msgq
+msgq_objects = env.SharedObject([
+  'msgq/ipc.cc',
+  'msgq/event.cc',
+  'msgq/impl_zmq.cc',
+  'msgq/impl_msgq.cc',
+  'msgq/impl_fake.cc',
+  'msgq/msgq.cc',
+])
+msgq = env.Library('msgq/libmsgq', msgq_objects)
+msgq_python = envCython.Program('msgq/ipc_pyx.so', 'msgq/ipc_pyx.pyx', LIBS=envCython["LIBS"]+[msgq, "zmq", common])
+
+# Build Vision IPC
+vipc_files = ['visionipc.cc', 'visionipc_server.cc', 'visionipc_client.cc', 'visionbuf.cc']
+vipc_sources = [f'{visionipc_dir.abspath}/{f}' for f in vipc_files]
+
+if arch == "larch64":
+  vipc_sources += [f'{visionipc_dir.abspath}/visionbuf_ion.cc']
+else:
+  vipc_sources += [f'{visionipc_dir.abspath}/visionbuf_cl.cc']
+
+vipc_objects = env.SharedObject(vipc_sources)
+visionipc = env.Library('msgq/visionipc/libvisionipc', vipc_objects)
+
+
+vipc_frameworks = []
+vipc_libs = envCython["LIBS"] + [visionipc, msgq, common, "zmq"]
+if arch == "Darwin":
+  vipc_frameworks.append('OpenCL')
+else:
+  vipc_libs.append('OpenCL')
+envCython.Program(f'{visionipc_dir.abspath}/visionipc_pyx.so', f'{visionipc_dir.abspath}/visionipc_pyx.pyx',
+                  LIBS=vipc_libs, FRAMEWORKS=vipc_frameworks)
+
+if GetOption('extras'):
+  env.Program('msgq/test_runner', ['msgq/test_runner.cc', 'msgq/msgq_tests.cc'], LIBS=[msgq, common])
+  env.Program(f'{visionipc_dir.abspath}/test_runner',
+             [f'{visionipc_dir.abspath}/test_runner.cc', f'{visionipc_dir.abspath}/visionipc_tests.cc'],
+              LIBS=['pthread'] + vipc_libs, FRAMEWORKS=vipc_frameworks)
+
+Export('visionipc', 'msgq', 'msgq_python')

artifacts/package_sources/msgq/SConstruct

@@ -0,0 +1,94 @@
+import os
+import platform
+import subprocess
+import sysconfig
+import numpy as np
+
+arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rstrip()
+if platform.system() == "Darwin":
+  arch = "Darwin"
+elif arch == "aarch64" and os.path.isfile('/TICI'):
+  arch = "larch64"
+
+common = ''
+
+cpppath = [
+  f"#/",
+  '#msgq/',
+  '#third_party/linux/include',
+  '/usr/lib/include',
+  '/opt/homebrew/include',
+  sysconfig.get_paths()['include'],
+]
+
+libpath = [
+  '/opt/homebrew/lib',
+]
+
+AddOption('--minimal',
+          action='store_false',
+          dest='extras',
+          default=True,
+          help='the minimum build. no tests, tools, etc.')
+
+AddOption('--asan',
+          action='store_true',
+          help='turn on ASAN')
+
+AddOption('--ubsan',
+          action='store_true',
+          help='turn on UBSan')
+
+ccflags = []
+ldflags = []
+if GetOption('ubsan'):
+  flags = [
+    "-fsanitize=undefined",
+    "-fno-sanitize-recover=undefined",
+  ]
+  ccflags += flags
+  ldflags += flags
+elif GetOption('asan'):
+  ccflags += ["-fsanitize=address", "-fno-omit-frame-pointer"]
+  ldflags += ["-fsanitize=address"]
+
+env = Environment(
+  ENV=os.environ,
+  CC='clang',
+  CXX='clang++',
+  CCFLAGS=[
+    "-g",
+    "-fPIC",
+    "-O2",
+    "-Wunused",
+    "-Werror",
+    "-Wshadow",
+    "-Wno-vla-cxx-extension",
+    "-Wno-unknown-warning-option",
+  ] + ccflags,
+  LDFLAGS=ldflags,
+  LINKFLAGS=ldflags,
+
+  CFLAGS="-std=gnu11",
+  CXXFLAGS="-std=c++1z",
+  CPPPATH=cpppath,
+  LIBPATH=libpath,
+  CYTHONCFILESUFFIX=".cpp",
+  tools=["default", "cython"]
+)
+
+Export('env', 'arch', 'common')
+
+envCython = env.Clone(LIBS=[])
+envCython["CPPPATH"] += [np.get_include()]
+envCython["CCFLAGS"] += ["-Wno-#warnings", "-Wno-shadow", "-Wno-deprecated-declarations"]
+envCython["CCFLAGS"].remove('-Werror')
+if arch == "Darwin":
+  envCython["LINKFLAGS"] = ["-bundle", "-undefined", "dynamic_lookup"]
+else:
+  envCython["LINKFLAGS"] = ["-pthread", "-shared"]
+
+Export('envCython')
+
+
+SConscript(['SConscript'])

artifacts/package_sources/msgq/codecov.yml

@@ -0,0 +1,8 @@
+comment: false
+coverage:
+  status:
+    project:
+      default:
+        informational: true
+    patch: off
+

artifacts/package_sources/msgq/lefthook.yml

@@ -0,0 +1,32 @@
+output:
+  - meta           # Print lefthook version
+  - summary        # Print summary block (successful and failed steps)
+  - empty_summary  # Print summary heading when there are no steps to run
+  - success        # Print successful steps
+  - failure        # Print failed steps printing
+  - execution      # Print any execution logs
+  #- execution_out  # Print execution output
+  #- execution_info # Print `EXECUTE > ...` logging
+  - skips          # Print "skip" (i.e. no files matched)
+
+test:
+  parallel: true
+  commands:
+    # *** static analysis ***
+    ruff:
+      run: ruff check .
+    ty:
+      run: ty check .
+    codespell:
+      run: codespell {files} -L ned,stdio,master --builtin clear,rare,informal,usage,code,names,en-GB_to_en-US -S uv.lock,*_pyx.cpp,catch2*
+      files: git ls-tree -r HEAD --name-only
+    cppcheck:
+      run: cppcheck --error-exitcode=1 --inline-suppr --language=c++ --force --quiet -j4 --check-level=exhaustive $(git ls-files '*.cc' | grep -v -E '(msgq_tests|test_runner)\.cc')
+    cpplint:
+      run: cpplint --exclude=msgq/catch2/ --exclude=msgq/ipc_pyx.cpp --exclude=msgq/visionipc/visionipc_pyx.cpp --recursive --quiet --counting=detailed --linelength=240 --filter=-build,-legal,-readability,-runtime,-whitespace,+build/include_subdir,+build/forward_decl,+build/include_what_you_use,+build/deprecated,+whitespace/comma,+whitespace/line_length,+whitespace/empty_if_body,+whitespace/empty_loop_body,+whitespace/empty_conditional_body,+whitespace/forcolon,+whitespace/parens,+whitespace/semicolon,+whitespace/tab,+readability/braces msgq/
+
+    # *** tests ***
+    test_runner:
+      run: msgq/test_runner
+    pytest:
+      run: pytest

artifacts/package_sources/msgq/msgq/.gitignore

@@ -0,0 +1 @@
+ipc_pyx.cpp

artifacts/package_sources/msgq/msgq/__init__.py

@@ -0,0 +1,69 @@
+import os
+
+INCLUDE_PATH = os.path.abspath(os.path.join(os.path.dirname(__file__), ".."))
+LIB_PATH = os.path.join(os.path.dirname(__file__), "libmsgq.a")
+VISIONIPC_LIB_PATH = os.path.join(os.path.dirname(__file__), "visionipc", "libvisionipc.a")
+PYTHON_LIB_PATH = os.path.join(os.path.dirname(__file__), "ipc_pyx.so")
+
+from msgq.ipc_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
+                                set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event, \
+                                context_is_zmq
+from msgq.ipc_pyx import MultiplePublishersError, IpcError
+
+from typing import Optional, List, Union
+
+assert MultiplePublishersError
+assert IpcError
+assert toggle_fake_events
+assert set_fake_prefix
+assert get_fake_prefix
+assert delete_fake_prefix
+assert wait_for_one_event
+assert context_is_zmq
+
+NO_TRAVERSAL_LIMIT = 2**64-1
+
+context = Context()
+
+
+def fake_event_handle(endpoint: str, identifier: Optional[Union[str, bytes]] = None, override: bool = True, enable: bool = False) -> SocketEventHandle:
+  ident = identifier if identifier is not None else get_fake_prefix()
+  handle = SocketEventHandle(endpoint, ident, override)
+  if override:
+    handle.enabled = enable
+
+  return handle
+
+def pub_sock(endpoint: str, segment_size: int = 0) -> PubSocket:
+  sock = PubSocket()
+  sock.connect(context, endpoint, segment_size)
+  return sock
+
+
+def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1",
+             conflate: bool = False, timeout: Optional[int] = None, segment_size: int = 0) -> SubSocket:
+  sock = SubSocket()
+  sock.connect(context, endpoint, addr.encode('utf8'), conflate, segment_size)
+
+  if timeout is not None:
+    sock.setTimeout(timeout)
+
+  if poller is not None:
+    poller.registerSocket(sock)
+  return sock
+
+def drain_sock_raw(sock: SubSocket, wait_for_one: bool = False) -> List[bytes]:
+  """Receive all message currently available on the queue"""
+  ret: List[bytes] = []
+  while 1:
+    if wait_for_one and len(ret) == 0:
+      dat = sock.receive()
+    else:
+      dat = sock.receive(non_blocking=True)
+
+    if dat is None:
+      break
+
+    ret.append(dat)
+
+  return ret

artifacts/package_sources/msgq/msgq/conftest.py

@@ -0,0 +1,14 @@
+import os
+import pytest
+import msgq
+
+@pytest.fixture(params=[False, True], ids=["msgq", "zmq"], autouse=True)
+def zmq_mode(request):
+  if request.param:
+    os.environ["ZMQ"] = "1"
+  else:
+    os.environ.pop("ZMQ", None)
+  msgq.context = msgq.Context()
+  assert msgq.context_is_zmq() == request.param
+  yield request.param
+  os.environ.pop("ZMQ", None)

artifacts/package_sources/msgq/msgq/event.cc

@@ -0,0 +1,237 @@
+#include <cassert>
+#include <cstring>
+#include <cstdlib>
+#include <iostream>
+#include <string>
+#include <exception>
+#include <filesystem>
+#include <vector>
+
+#include <unistd.h>
+#include <poll.h>
+#include <signal.h>
+#include <fcntl.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+
+#include "msgq/event.h"
+
+#ifndef __APPLE__
+#include <sys/eventfd.h>
+
+void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path) {
+  const char* op_prefix = std::getenv("OPENPILOT_PREFIX");
+
+  std::string full_path = "/dev/shm/msgq_";
+  if (op_prefix) {
+    full_path += std::string(op_prefix) + "/";
+  }
+  full_path += CEREAL_EVENTS_PREFIX + "/";
+  if (identifier.size() > 0) {
+    full_path += identifier + "/";
+  }
+  std::filesystem::create_directories(full_path);
+  full_path += endpoint;
+
+  int shm_fd = open(full_path.c_str(), O_RDWR | O_CREAT, 0664);
+  if (shm_fd < 0) {
+    throw std::runtime_error("Could not open shared memory file.");
+  }
+
+  int rc = ftruncate(shm_fd, sizeof(EventState));
+  if (rc < 0){
+    close(shm_fd);
+    throw std::runtime_error("Could not truncate shared memory file.");
+  }
+
+  char * mem = (char*)mmap(NULL, sizeof(EventState), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0);
+  close(shm_fd);
+  if (mem == nullptr) {
+    throw std::runtime_error("Could not map shared memory file.");
+  }
+
+  if (shm_mem != nullptr)
+    *shm_mem = mem;
+  if (shm_path != nullptr)
+    *shm_path = full_path;
+}
+
+SocketEventHandle::SocketEventHandle(std::string endpoint, std::string identifier, bool override) {
+  char *mem;
+  event_state_shm_mmap(endpoint, identifier, &mem, &this->shm_path);
+
+  this->state = (EventState*)mem;
+  if (override) {
+    this->state->fds[0] = eventfd(0, EFD_NONBLOCK);
+    this->state->fds[1] = eventfd(0, EFD_NONBLOCK);
+  }
+}
+
+SocketEventHandle::~SocketEventHandle() {
+  close(this->state->fds[0]);
+  close(this->state->fds[1]);
+  munmap(this->state, sizeof(EventState));
+  unlink(this->shm_path.c_str());
+}
+
+bool SocketEventHandle::is_enabled() {
+  return this->state->enabled;
+}
+
+void SocketEventHandle::set_enabled(bool enabled) {
+  this->state->enabled = enabled;
+}
+
+Event SocketEventHandle::recv_called() {
+  return Event(this->state->fds[0]);
+}
+
+Event SocketEventHandle::recv_ready() {
+  return Event(this->state->fds[1]);
+}
+
+void SocketEventHandle::toggle_fake_events(bool enabled) {
+  if (enabled)
+    setenv("CEREAL_FAKE", "1", true);
+  else
+    unsetenv("CEREAL_FAKE");
+}
+
+void SocketEventHandle::set_fake_prefix(std::string prefix) {
+  if (prefix.size() == 0) {
+    unsetenv("CEREAL_FAKE_PREFIX");
+  } else {
+    setenv("CEREAL_FAKE_PREFIX", prefix.c_str(), true);
+  }
+}
+
+std::string SocketEventHandle::fake_prefix() {
+  const char* prefix = std::getenv("CEREAL_FAKE_PREFIX");
+  if (prefix == nullptr) {
+    return "";
+  } else {
+    return std::string(prefix);
+  }
+}
+
+Event::Event(int fd): event_fd(fd) {}
+
+void Event::set() const {
+  throw_if_invalid();
+
+  uint64_t val = 1;
+  size_t count = write(this->event_fd, &val, sizeof(uint64_t));
+  assert(count == sizeof(uint64_t));
+}
+
+int Event::clear() const {
+  throw_if_invalid();
+
+  uint64_t val = 0;
+  // read the eventfd to clear it
+  read(this->event_fd, &val, sizeof(uint64_t));
+
+  return val;
+}
+
+void Event::wait(int timeout_sec) const {
+  throw_if_invalid();
+
+  int event_count;
+  struct pollfd fds = { this->event_fd, POLLIN, 0 };
+  struct timespec timeout = { timeout_sec, 0 };;
+
+  sigset_t signals;
+  sigfillset(&signals);
+  sigdelset(&signals, SIGALRM);
+  sigdelset(&signals, SIGINT);
+  sigdelset(&signals, SIGTERM);
+  sigdelset(&signals, SIGQUIT);
+
+  event_count = ppoll(&fds, 1, timeout_sec < 0 ? nullptr : &timeout, &signals);
+
+  if (event_count == 0) {
+    throw std::runtime_error("Event timed out pid: " + std::to_string(getpid()));
+  } else if (event_count < 0) {
+    throw std::runtime_error("Event poll failed, errno: " + std::to_string(errno) + " pid: " + std::to_string(getpid()));
+  }
+}
+
+bool Event::peek() const {
+  throw_if_invalid();
+
+  int event_count;
+
+  struct pollfd fds = { this->event_fd, POLLIN, 0 };
+
+  // poll with timeout zero to return status immediately
+  event_count = poll(&fds, 1, 0);
+
+  return event_count != 0;
+}
+
+bool Event::is_valid() const {
+  return event_fd != -1;
+}
+
+int Event::fd() const {
+  return event_fd;
+}
+
+int Event::wait_for_one(const std::vector<Event>& events, int timeout_sec) {
+  struct pollfd fds[events.size()];
+  for (size_t i = 0; i < events.size(); i++) {
+    fds[i] = { events[i].fd(), POLLIN, 0 };
+  }
+
+  struct timespec timeout = { timeout_sec, 0 };
+
+  sigset_t signals;
+  sigfillset(&signals);
+  sigdelset(&signals, SIGALRM);
+  sigdelset(&signals, SIGINT);
+  sigdelset(&signals, SIGTERM);
+  sigdelset(&signals, SIGQUIT);
+
+  int event_count = ppoll(fds, events.size(), timeout_sec < 0 ? nullptr : &timeout, &signals);
+
+  if (event_count == 0) {
+    throw std::runtime_error("Event timed out pid: " + std::to_string(getpid()));
+  } else if (event_count < 0) {
+    throw std::runtime_error("Event poll failed, errno: " + std::to_string(errno) + " pid: " + std::to_string(getpid()));
+  }
+
+  for (size_t i = 0; i < events.size(); i++) {
+    if (fds[i].revents & POLLIN) {
+      return i;
+    }
+  }
+
+  throw std::runtime_error("Event poll failed, no events ready");
+}
+#else
+// Stub implementation for Darwin, which does not support eventfd
+void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path) {}
+
+SocketEventHandle::SocketEventHandle(std::string endpoint, std::string identifier, bool override) {
+  std::cerr << "SocketEventHandle not supported on macOS" << std::endl;
+  assert(false);
+}
+SocketEventHandle::~SocketEventHandle() {}
+bool SocketEventHandle::is_enabled() { return this->state->enabled; }
+void SocketEventHandle::set_enabled(bool enabled) {}
+Event SocketEventHandle::recv_called() { return Event(); }
+Event SocketEventHandle::recv_ready() { return Event(); }
+void SocketEventHandle::toggle_fake_events(bool enabled) {}
+void SocketEventHandle::set_fake_prefix(std::string prefix) {}
+std::string SocketEventHandle::fake_prefix() { return ""; }
+
+Event::Event(int fd): event_fd(fd) {}
+void Event::set() const {}
+int Event::clear() const { return 0; }
+void Event::wait(int timeout_sec) const {}
+bool Event::peek() const { return false; }
+bool Event::is_valid() const { return false; }
+int Event::fd() const { return this->event_fd; }
+int Event::wait_for_one(const std::vector<Event>& events, int timeout_sec) { return -1; }
+#endif

artifacts/package_sources/msgq/msgq/event.h

@@ -0,0 +1,58 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#define CEREAL_EVENTS_PREFIX std::string("cereal_events")
+
+void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_path);
+
+enum EventPurpose {
+  RECV_CALLED,
+  RECV_READY
+};
+
+struct EventState {
+  int fds[2];
+  bool enabled;
+};
+
+class Event {
+private:
+  int event_fd = -1;
+
+  inline void throw_if_invalid() const {
+    if (!this->is_valid()) {
+      throw std::runtime_error("Event does not have valid file descriptor.");
+    }
+  }
+public:
+  Event(int fd = -1);
+
+  void set() const;
+  int clear() const;
+  void wait(int timeout_sec = -1) const;
+  bool peek() const;
+  bool is_valid() const;
+  int fd() const;
+
+  static int wait_for_one(const std::vector<Event>& events, int timeout_sec = -1);
+};
+
+class SocketEventHandle {
+private:
+  std::string shm_path;
+  EventState* state;
+public:
+  SocketEventHandle(std::string endpoint, std::string identifier = "", bool override = true);
+  ~SocketEventHandle();
+
+  bool is_enabled();
+  void set_enabled(bool enabled);
+  Event recv_called();
+  Event recv_ready();
+
+  static void toggle_fake_events(bool enabled);
+  static void set_fake_prefix(std::string prefix);
+  static std::string fake_prefix();
+};

artifacts/package_sources/msgq/msgq/impl_fake.cc

@@ -0,0 +1,10 @@
+#include <vector>
+#include "msgq/impl_fake.h"
+
+void FakePoller::registerSocket(SubSocket *socket) {
+  this->sockets.push_back(socket);
+}
+
+std::vector<SubSocket*> FakePoller::poll(int timeout) {
+  return this->sockets;
+}

artifacts/package_sources/msgq/msgq/impl_fake.h

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <cassert>
+#include <iostream>
+#include <string>
+#include <vector>
+#include <filesystem>
+
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include "msgq/ipc.h"
+#include "msgq/event.h"
+
+template<typename TSubSocket>
+class FakeSubSocket: public TSubSocket {
+private:
+  Event *recv_called = nullptr;
+  Event *recv_ready = nullptr;
+  EventState *state = nullptr;
+
+public:
+  FakeSubSocket(): TSubSocket() {}
+  ~FakeSubSocket() {
+    delete recv_called;
+    delete recv_ready;
+    if (state != nullptr) {
+      munmap(state, sizeof(EventState));
+    }
+  }
+
+  int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true, size_t segment_size=0) override {
+    const char* cereal_prefix = std::getenv("CEREAL_FAKE_PREFIX");
+
+    char* mem;
+    std::string identifier = cereal_prefix != nullptr ?  std::string(cereal_prefix) : "";
+    event_state_shm_mmap(endpoint, identifier, &mem, nullptr);
+
+    this->state = (EventState*)mem;
+    this->recv_called = new Event(state->fds[EventPurpose::RECV_CALLED]);
+    this->recv_ready = new Event(state->fds[EventPurpose::RECV_READY]);
+
+    return TSubSocket::connect(context, endpoint, address, conflate, check_endpoint, segment_size);
+  }
+
+  Message *receive(bool non_blocking=false) override {
+    if (this->state->enabled) {
+      this->recv_called->set();
+      this->recv_ready->wait();
+      this->recv_ready->clear();
+    }
+
+    return TSubSocket::receive(non_blocking);
+  }
+};
+
+class FakePoller: public Poller {
+private:
+  std::vector<SubSocket*> sockets;
+
+public:
+  void registerSocket(SubSocket *socket) override;
+  std::vector<SubSocket*> poll(int timeout) override;
+  ~FakePoller() {}
+};

artifacts/package_sources/msgq/msgq/impl_msgq.cc

@@ -0,0 +1,186 @@
+#include <cassert>
+#include <cstring>
+#include <iostream>
+#include <cstdlib>
+#include <cerrno>
+#include <string>
+#include <vector>
+
+#include "msgq/impl_msgq.h"
+
+MSGQContext::MSGQContext() {
+}
+
+MSGQContext::~MSGQContext() {
+}
+
+void MSGQMessage::init(size_t sz) {
+  size = sz;
+  data = new char[size];
+}
+
+void MSGQMessage::init(char * d, size_t sz) {
+  size = sz;
+  data = new char[size];
+  memcpy(data, d, size);
+}
+
+void MSGQMessage::takeOwnership(char * d, size_t sz) {
+  size = sz;
+  data = d;
+}
+
+void MSGQMessage::close() {
+  if (size > 0){
+    delete[] data;
+  }
+  size = 0;
+}
+
+MSGQMessage::~MSGQMessage() {
+  this->close();
+}
+
+int MSGQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate, bool check_endpoint, size_t segment_size){
+  assert(context);
+  assert(address == "127.0.0.1");
+
+  q = new msgq_queue_t;
+  size_t size = segment_size > 0 ? segment_size : DEFAULT_SEGMENT_SIZE;
+  int r = msgq_new_queue(q, endpoint.c_str(), size);
+  if (r != 0){
+    return r;
+  }
+
+  r = msgq_init_subscriber(q);
+  if (r != 0){
+    msgq_close_queue(q);
+    delete q;
+    q = NULL;
+    return r;
+  }
+
+  if (conflate){
+    q->read_conflate = true;
+  }
+
+  timeout = -1;
+
+  return 0;
+}
+
+
+Message * MSGQSubSocket::receive(bool non_blocking){
+  msgq_msg_t msg;
+
+  MSGQMessage *r = NULL;
+
+  int rc = msgq_msg_recv(&msg, q);
+
+  // Hack to implement blocking read with a poller. Don't use this
+  while (!non_blocking && rc == 0){
+    msgq_pollitem_t items[1];
+    items[0].q = q;
+
+    int t = (timeout != -1) ? timeout : 100;
+
+    int n = msgq_poll(items, 1, t);
+    rc = msgq_msg_recv(&msg, q);
+
+    // The poll indicated a message was ready, but the receive failed. Try again
+    if (n == 1 && rc == 0){
+      continue;
+    }
+
+    if (timeout != -1){
+      break;
+    }
+  }
+
+  if (rc > 0){
+    r = new MSGQMessage;
+    r->takeOwnership(msg.data, msg.size);
+  }
+
+  return (Message*)r;
+}
+
+void MSGQSubSocket::setTimeout(int t){
+  timeout = t;
+}
+
+MSGQSubSocket::~MSGQSubSocket(){
+  if (q != NULL){
+    msgq_close_queue(q);
+    delete q;
+  }
+}
+
+int MSGQPubSocket::connect(Context *context, std::string endpoint, bool check_endpoint, size_t segment_size){
+  assert(context);
+
+  // TODO
+  //if (check_endpoint && !service_exists(std::string(endpoint))){
+  //  std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl;
+  //}
+
+  q = new msgq_queue_t;
+  size_t size = segment_size > 0 ? segment_size : DEFAULT_SEGMENT_SIZE;
+  int r = msgq_new_queue(q, endpoint.c_str(), size);
+  if (r != 0){
+    return r;
+  }
+
+  msgq_init_publisher(q);
+
+  return 0;
+}
+
+int MSGQPubSocket::sendMessage(Message *message){
+  msgq_msg_t msg;
+  msg.data = message->getData();
+  msg.size = message->getSize();
+
+  return msgq_msg_send(&msg, q);
+}
+
+int MSGQPubSocket::send(char *data, size_t size){
+  msgq_msg_t msg;
+  msg.data = data;
+  msg.size = size;
+
+  return msgq_msg_send(&msg, q);
+}
+
+bool MSGQPubSocket::all_readers_updated() {
+  return msgq_all_readers_updated(q);
+}
+
+MSGQPubSocket::~MSGQPubSocket(){
+  if (q != NULL){
+    msgq_close_queue(q);
+    delete q;
+  }
+}
+
+
+void MSGQPoller::registerSocket(SubSocket * socket){
+  assert(num_polls + 1 < MAX_POLLERS);
+  polls[num_polls].q = (msgq_queue_t*)socket->getRawSocket();
+
+  sockets.push_back(socket);
+  num_polls++;
+}
+
+std::vector<SubSocket*> MSGQPoller::poll(int timeout){
+  std::vector<SubSocket*> r;
+
+  msgq_poll(polls, num_polls, timeout);
+  for (size_t i = 0; i < num_polls; i++){
+    if (polls[i].revents){
+      r.push_back(sockets[i]);
+    }
+  }
+
+  return r;
+}

artifacts/package_sources/msgq/msgq/impl_msgq.h

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "msgq/ipc.h"
+#include "msgq/msgq.h"
+
+#define MAX_POLLERS 128
+
+class MSGQContext : public Context {
+private:
+  void * context = NULL;
+public:
+  MSGQContext();
+  void * getRawContext() {return context;}
+  ~MSGQContext();
+};
+
+class MSGQMessage : public Message {
+private:
+  char * data;
+  size_t size;
+public:
+  void init(size_t size);
+  void init(char *data, size_t size);
+  void takeOwnership(char *data, size_t size);
+  size_t getSize(){return size;}
+  char * getData(){return data;}
+  void close();
+  ~MSGQMessage();
+};
+
+class MSGQSubSocket : public SubSocket {
+private:
+  msgq_queue_t * q = NULL;
+  int timeout;
+public:
+  int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true, size_t segment_size=0);
+  void setTimeout(int timeout);
+  void * getRawSocket() {return (void*)q;}
+  Message *receive(bool non_blocking=false);
+  ~MSGQSubSocket();
+};
+
+class MSGQPubSocket : public PubSocket {
+private:
+  msgq_queue_t * q = NULL;
+public:
+  int connect(Context *context, std::string endpoint, bool check_endpoint=true, size_t segment_size=0);
+  int sendMessage(Message *message);
+  int send(char *data, size_t size);
+  bool all_readers_updated();
+  ~MSGQPubSocket();
+};
+
+class MSGQPoller : public Poller {
+private:
+  std::vector<SubSocket*> sockets;
+  msgq_pollitem_t polls[MAX_POLLERS];
+  size_t num_polls = 0;
+
+public:
+  void registerSocket(SubSocket *socket);
+  std::vector<SubSocket*> poll(int timeout);
+  ~MSGQPoller(){}
+};

artifacts/package_sources/msgq/msgq/impl_zmq.cc

@@ -0,0 +1,179 @@
+#include <cassert>
+#include <cstring>
+#include <iostream>
+#include <cstdlib>
+#include <cerrno>
+#include <unistd.h>
+#include <string>
+#include <vector>
+
+#include "msgq/impl_zmq.h"
+
+static size_t fnv1a_hash(const std::string &str) {
+    const size_t fnv_prime = 0x100000001b3;
+    size_t hash_value = 0xcbf29ce484222325;
+    for (char c : str) {
+        hash_value ^= (unsigned char)c;
+        hash_value *= fnv_prime;
+    }
+    return hash_value;
+}
+
+//FIXME: This is a hack to get the port number from the socket name, might have collisions
+static int get_port(std::string endpoint) {
+    size_t hash_value = fnv1a_hash(endpoint);
+    int start_port = 8023;
+    int max_port = 65535;
+    int port = start_port + (hash_value % (max_port - start_port));
+    return port;
+}
+
+ZMQContext::ZMQContext() {
+  context = zmq_ctx_new();
+}
+
+ZMQContext::~ZMQContext() {
+  zmq_ctx_term(context);
+}
+
+void ZMQMessage::init(size_t sz) {
+  size = sz;
+  data = new char[size];
+}
+
+void ZMQMessage::init(char * d, size_t sz) {
+  size = sz;
+  data = new char[size];
+  memcpy(data, d, size);
+}
+
+void ZMQMessage::close() {
+  if (size > 0){
+    delete[] data;
+  }
+  size = 0;
+}
+
+ZMQMessage::~ZMQMessage() {
+  this->close();
+}
+
+
+int ZMQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate, bool check_endpoint, size_t segment_size){
+  sock = zmq_socket(context->getRawContext(), ZMQ_SUB);
+  if (sock == NULL){
+    return -1;
+  }
+
+  zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
+
+  if (conflate){
+    int arg = 1;
+    zmq_setsockopt(sock, ZMQ_CONFLATE, &arg, sizeof(int));
+  }
+
+  int reconnect_ivl = 500;
+  zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl));
+
+
+  full_endpoint = "tcp://" + address + ":";
+  if (check_endpoint){
+    full_endpoint += std::to_string(get_port(endpoint));
+  } else {
+    full_endpoint += endpoint;
+  }
+
+  return zmq_connect(sock, full_endpoint.c_str());
+}
+
+
+Message * ZMQSubSocket::receive(bool non_blocking){
+  zmq_msg_t msg;
+  assert(zmq_msg_init(&msg) == 0);
+
+  int flags = non_blocking ? ZMQ_DONTWAIT : 0;
+  int rc = zmq_msg_recv(&msg, sock, flags);
+  Message *r = NULL;
+
+  if (rc >= 0){
+    // Make a copy to ensure the data is aligned
+    r = new ZMQMessage;
+    r->init((char*)zmq_msg_data(&msg), zmq_msg_size(&msg));
+  }
+
+  zmq_msg_close(&msg);
+  return r;
+}
+
+void ZMQSubSocket::setTimeout(int timeout){
+  zmq_setsockopt(sock, ZMQ_RCVTIMEO, &timeout, sizeof(int));
+}
+
+ZMQSubSocket::~ZMQSubSocket(){
+  zmq_close(sock);
+}
+
+int ZMQPubSocket::connect(Context *context, std::string endpoint, bool check_endpoint, size_t segment_size){
+  sock = zmq_socket(context->getRawContext(), ZMQ_PUB);
+  if (sock == NULL){
+    return -1;
+  }
+
+  full_endpoint = "tcp://*:";
+  if (check_endpoint){
+    full_endpoint += std::to_string(get_port(endpoint));
+  } else {
+    full_endpoint += endpoint;
+  }
+
+  // ZMQ pub sockets cannot be shared between processes, so we need to ensure pid stays the same
+  pid = getpid();
+
+  return zmq_bind(sock, full_endpoint.c_str());
+}
+
+int ZMQPubSocket::sendMessage(Message *message) {
+  assert(pid == getpid());
+  return zmq_send(sock, message->getData(), message->getSize(), ZMQ_DONTWAIT);
+}
+
+int ZMQPubSocket::send(char *data, size_t size) {
+  assert(pid == getpid());
+  return zmq_send(sock, data, size, ZMQ_DONTWAIT);
+}
+
+bool ZMQPubSocket::all_readers_updated() {
+  assert(false); // TODO not implemented
+  return false;
+}
+
+ZMQPubSocket::~ZMQPubSocket(){
+  zmq_close(sock);
+}
+
+
+void ZMQPoller::registerSocket(SubSocket * socket){
+  assert(num_polls + 1 < MAX_POLLERS);
+  polls[num_polls].socket = socket->getRawSocket();
+  polls[num_polls].events = ZMQ_POLLIN;
+
+  sockets.push_back(socket);
+  num_polls++;
+}
+
+std::vector<SubSocket*> ZMQPoller::poll(int timeout){
+  std::vector<SubSocket*> r;
+
+  int rc = zmq_poll(polls, num_polls, timeout);
+  if (rc < 0){
+    return r;
+  }
+
+  for (size_t i = 0; i < num_polls; i++){
+    if (polls[i].revents){
+      r.push_back(sockets[i]);
+    }
+  }
+
+  return r;
+}

artifacts/package_sources/msgq/msgq/impl_zmq.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include <zmq.h>
+#include <string>
+#include <vector>
+
+#include "msgq/ipc.h"
+
+#define MAX_POLLERS 128
+
+class ZMQContext : public Context {
+private:
+  void * context = NULL;
+public:
+  ZMQContext();
+  void * getRawContext() {return context;}
+  ~ZMQContext();
+};
+
+class ZMQMessage : public Message {
+private:
+  char * data;
+  size_t size;
+public:
+  void init(size_t size);
+  void init(char *data, size_t size);
+  size_t getSize(){return size;}
+  char * getData(){return data;}
+  void close();
+  ~ZMQMessage();
+};
+
+class ZMQSubSocket : public SubSocket {
+private:
+  void * sock;
+  std::string full_endpoint;
+public:
+  int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true, size_t segment_size=0);
+  void setTimeout(int timeout);
+  void * getRawSocket() {return sock;}
+  Message *receive(bool non_blocking=false);
+  ~ZMQSubSocket();
+};
+
+class ZMQPubSocket : public PubSocket {
+private:
+  void * sock;
+  std::string full_endpoint;
+  int pid = -1;
+public:
+  int connect(Context *context, std::string endpoint, bool check_endpoint=true, size_t segment_size=0);
+  int sendMessage(Message *message);
+  int send(char *data, size_t size);
+  bool all_readers_updated();
+  ~ZMQPubSocket();
+};
+
+class ZMQPoller : public Poller {
+private:
+  std::vector<SubSocket*> sockets;
+  zmq_pollitem_t polls[MAX_POLLERS];
+  size_t num_polls = 0;
+
+public:
+  void registerSocket(SubSocket *socket);
+  std::vector<SubSocket*> poll(int timeout);
+  ~ZMQPoller(){}
+};

artifacts/package_sources/msgq/msgq/ipc.cc

@@ -0,0 +1,108 @@
+#include <cassert>
+#include <iostream>
+#include <string>
+
+#include "msgq/ipc.h"
+#include "msgq/impl_zmq.h"
+#include "msgq/impl_msgq.h"
+#include "msgq/impl_fake.h"
+
+bool messaging_use_zmq(){
+  return std::getenv("ZMQ") != nullptr;
+}
+
+bool messaging_use_fake(){
+  char* fake_enabled = std::getenv("CEREAL_FAKE");
+  return fake_enabled != NULL;
+}
+
+Context * Context::create(){
+  Context * c;
+  if (messaging_use_zmq()){
+    c = new ZMQContext();
+  } else {
+    c = new MSGQContext();
+  }
+  return c;
+}
+
+SubSocket * SubSocket::create(){
+  SubSocket * s;
+  if (messaging_use_fake()) {
+    if (messaging_use_zmq()) {
+      s = new FakeSubSocket<ZMQSubSocket>();
+    } else {
+      s = new FakeSubSocket<MSGQSubSocket>();
+    }
+  } else {
+    if (messaging_use_zmq()){
+      s = new ZMQSubSocket();
+    } else {
+      s = new MSGQSubSocket();
+    }
+  }
+
+  return s;
+}
+
+SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address, bool conflate, bool check_endpoint, size_t segment_size){
+  SubSocket *s = SubSocket::create();
+  int r = s->connect(context, endpoint, address, conflate, check_endpoint, segment_size);
+
+  if (r == 0) {
+    return s;
+  } else {
+    std::cerr << "Error, failed to connect SubSocket to " << endpoint << ": " << strerror(errno) << std::endl;
+
+    delete s;
+    return nullptr;
+  }
+}
+
+PubSocket * PubSocket::create(){
+  PubSocket * s;
+  if (messaging_use_zmq()){
+    s = new ZMQPubSocket();
+  } else {
+    s = new MSGQPubSocket();
+  }
+
+  return s;
+}
+
+PubSocket * PubSocket::create(Context * context, std::string endpoint, bool check_endpoint, size_t segment_size){
+  PubSocket *s = PubSocket::create();
+  int r = s->connect(context, endpoint, check_endpoint, segment_size);
+
+  if (r == 0) {
+    return s;
+  } else {
+    std::cerr << "Error, failed to bind PubSocket to " << endpoint << ": " << strerror(errno) << std::endl;
+
+    delete s;
+    return nullptr;
+  }
+}
+
+Poller * Poller::create(){
+  Poller * p;
+  if (messaging_use_fake()) {
+    p = new FakePoller();
+  } else {
+    if (messaging_use_zmq()){
+      p = new ZMQPoller();
+    } else {
+      p = new MSGQPoller();
+    }
+  }
+  return p;
+}
+
+Poller * Poller::create(std::vector<SubSocket*> sockets){
+  Poller * p = Poller::create();
+
+  for (auto s : sockets){
+    p->registerSocket(s);
+  }
+  return p;
+}

artifacts/package_sources/msgq/msgq/ipc.h

@@ -0,0 +1,67 @@
+#pragma once
+
+#include <cstddef>
+#include <map>
+#include <string>
+#include <vector>
+#include <utility>
+#include <time.h>
+
+
+
+#ifdef __APPLE__
+#define CLOCK_BOOTTIME CLOCK_MONOTONIC
+#endif
+
+#define MSG_MULTIPLE_PUBLISHERS 100
+
+bool messaging_use_zmq();
+
+class Context {
+public:
+  virtual void * getRawContext() = 0;
+  static Context * create();
+  virtual ~Context(){}
+};
+
+class Message {
+public:
+  virtual void init(size_t size) = 0;
+  virtual void init(char * data, size_t size) = 0;
+  virtual void close() = 0;
+  virtual size_t getSize() = 0;
+  virtual char * getData() = 0;
+  virtual ~Message(){}
+};
+
+
+class SubSocket {
+public:
+  virtual int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true, size_t segment_size=0) = 0;
+  virtual void setTimeout(int timeout) = 0;
+  virtual Message *receive(bool non_blocking=false) = 0;
+  virtual void * getRawSocket() = 0;
+  static SubSocket * create();
+  static SubSocket * create(Context * context, std::string endpoint, std::string address="127.0.0.1", bool conflate=false, bool check_endpoint=true, size_t segment_size=0);
+  virtual ~SubSocket(){}
+};
+
+class PubSocket {
+public:
+  virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true, size_t segment_size=0) = 0;
+  virtual int sendMessage(Message *message) = 0;
+  virtual int send(char *data, size_t size) = 0;
+  virtual bool all_readers_updated() = 0;
+  static PubSocket * create();
+  static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true, size_t segment_size=0);
+  virtual ~PubSocket(){}
+};
+
+class Poller {
+public:
+  virtual void registerSocket(SubSocket *socket) = 0;
+  virtual std::vector<SubSocket*> poll(int timeout) = 0;
+  static Poller * create();
+  static Poller * create(std::vector<SubSocket*> sockets);
+  virtual ~Poller(){}
+};

artifacts/package_sources/msgq/msgq/ipc.pxd

@@ -0,0 +1,70 @@
+# distutils: language = c++
+#cython: language_level=3
+
+from libcpp.string cimport string
+from libcpp.vector cimport vector
+from libcpp cimport bool
+
+
+cdef extern from "msgq/impl_fake.h":
+  cdef cppclass Event:
+    @staticmethod
+    int wait_for_one(vector[Event], int) except +
+
+    Event()
+    Event(int)
+    void set()
+    int clear()
+    void wait(int) except +
+    bool peek()
+    int fd()
+
+  cdef cppclass SocketEventHandle:
+    @staticmethod
+    void toggle_fake_events(bool)
+    @staticmethod
+    void set_fake_prefix(string)
+    @staticmethod
+    string fake_prefix()
+
+    SocketEventHandle(string, string, bool)
+    bool is_enabled()
+    void set_enabled(bool)
+    Event recv_called()
+    Event recv_ready()
+
+
+cdef extern from "msgq/ipc.h":
+  bool messaging_use_zmq()
+
+  cdef cppclass Context:
+    @staticmethod
+    Context * create()
+
+  cdef cppclass Message:
+    void init(size_t)
+    void init(char *, size_t)
+    void close()
+    size_t getSize()
+    char *getData()
+
+  cdef cppclass SubSocket:
+    @staticmethod
+    SubSocket * create() nogil
+    int connect(Context *, string, string, bool, bool, size_t) nogil
+    Message * receive(bool) nogil
+    void setTimeout(int) nogil
+
+  cdef cppclass PubSocket:
+    @staticmethod
+    PubSocket * create()
+    int connect(Context *, string, bool, size_t)
+    int sendMessage(Message *)
+    int send(char *, size_t)
+    bool all_readers_updated()
+
+  cdef cppclass Poller:
+    @staticmethod
+    Poller * create()
+    void registerSocket(SubSocket *)
+    vector[SubSocket*] poll(int) nogil

artifacts/package_sources/msgq/msgq/ipc_pyx.pyx

@@ -0,0 +1,256 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+import sys
+from libcpp.string cimport string
+from libcpp.vector cimport vector
+from libcpp cimport bool
+from libc cimport errno
+from libc.string cimport strerror
+from cython.operator import dereference
+
+
+from .ipc cimport messaging_use_zmq
+from .ipc cimport Context as cppContext
+from .ipc cimport SubSocket as cppSubSocket
+from .ipc cimport PubSocket as cppPubSocket
+from .ipc cimport Poller as cppPoller
+from .ipc cimport Message as cppMessage
+from .ipc cimport Event as cppEvent, SocketEventHandle as cppSocketEventHandle
+
+
+def context_is_zmq():
+  return messaging_use_zmq()
+
+
+class IpcError(Exception):
+  def __init__(self, endpoint=None):
+    suffix = f"with {endpoint.decode('utf-8')}" if endpoint else ""
+    message = f"Messaging failure {suffix}: {strerror(errno.errno).decode('utf-8')}"
+    super().__init__(message)
+
+
+class MultiplePublishersError(IpcError):
+  pass
+
+
+def toggle_fake_events(bool enabled):
+  cppSocketEventHandle.toggle_fake_events(enabled)
+
+
+def set_fake_prefix(string prefix):
+  cppSocketEventHandle.set_fake_prefix(prefix)
+
+
+def get_fake_prefix():
+  return cppSocketEventHandle.fake_prefix()
+
+
+def delete_fake_prefix():
+  cppSocketEventHandle.set_fake_prefix(b"")
+
+
+def wait_for_one_event(list events, int timeout=-1):
+  cdef vector[cppEvent] items
+  for event in events:
+    items.push_back(dereference(<cppEvent*><size_t>event.ptr))
+  return cppEvent.wait_for_one(items, timeout)
+
+
+cdef class Event:
+  cdef cppEvent event;
+
+  def __cinit__(self):
+    pass
+
+  cdef setEvent(self, cppEvent event):
+    self.event = event
+
+  def set(self):
+    self.event.set()
+
+  def clear(self):
+    return self.event.clear()
+
+  def wait(self, int timeout=-1):
+    self.event.wait(timeout)
+
+  def peek(self):
+    return self.event.peek()
+
+  @property
+  def fd(self):
+    return self.event.fd()
+
+  @property
+  def ptr(self):
+    return <size_t><void*>&self.event
+
+
+cdef class SocketEventHandle:
+  cdef cppSocketEventHandle * handle;
+
+  def __cinit__(self, string endpoint, string identifier, bool override):
+    self.handle = new cppSocketEventHandle(endpoint, identifier, override)
+
+  def __dealloc__(self):
+    del self.handle
+
+  @property
+  def enabled(self):
+    return self.handle.is_enabled()
+
+  @enabled.setter
+  def enabled(self, bool value):
+    self.handle.set_enabled(value)
+
+  @property
+  def recv_called_event(self):
+    e = Event()
+    e.setEvent(self.handle.recv_called())
+
+    return e
+
+  @property
+  def recv_ready_event(self):
+    e = Event()
+    e.setEvent(self.handle.recv_ready())
+
+    return e
+
+
+cdef class Context:
+  cdef cppContext * context
+
+  def __cinit__(self):
+    self.context = cppContext.create()
+
+  def term(self):
+    del self.context
+    self.context = NULL
+
+  def __dealloc__(self):
+    pass
+    # Deleting the context will hang if sockets are still active
+    # TODO: Figure out a way to make sure the context is closed last
+    # del self.context
+
+
+cdef class Poller:
+  cdef cppPoller * poller
+  cdef list sub_sockets
+
+  def __cinit__(self):
+    self.sub_sockets = []
+    self.poller = cppPoller.create()
+
+  def __dealloc__(self):
+    del self.poller
+
+  def registerSocket(self, SubSocket socket):
+    self.sub_sockets.append(socket)
+    self.poller.registerSocket(socket.socket)
+
+  def poll(self, timeout):
+    sockets = []
+    cdef int t = timeout
+
+    with nogil:
+      result = self.poller.poll(t)
+
+    for s in result:
+      socket = SubSocket()
+      socket.setPtr(s)
+      sockets.append(socket)
+
+    return sockets
+
+
+cdef class SubSocket:
+  cdef cppSubSocket * socket
+  cdef bool is_owner
+
+  def __cinit__(self):
+    with nogil:
+      self.socket = cppSubSocket.create()
+
+    self.is_owner = True
+    if self.socket == NULL:
+      raise IpcError
+
+  def __dealloc__(self):
+    if self.is_owner:
+      with nogil:
+        del self.socket
+
+  cdef setPtr(self, cppSubSocket * ptr):
+    if self.is_owner:
+      with nogil:
+        del self.socket
+
+    self.is_owner = False
+    self.socket = ptr
+
+  def connect(self, Context context, string endpoint, string address=b"127.0.0.1", bool conflate=False, size_t segment_size=0):
+    cdef int r
+    with nogil:
+      r = self.socket.connect(context.context, endpoint, address, conflate, True, segment_size)
+
+    if r != 0:
+      if errno.errno == errno.EADDRINUSE:
+        raise MultiplePublishersError(endpoint)
+      else:
+        raise IpcError(endpoint)
+
+  def setTimeout(self, int timeout):
+    with nogil:
+      self.socket.setTimeout(timeout)
+
+  def receive(self, bool non_blocking=False):
+    cdef cppMessage *msg
+    with nogil:
+      msg = self.socket.receive(non_blocking)
+
+    if msg == NULL:
+      return None
+    else:
+      sz = msg.getSize()
+      m = msg.getData()[:sz]
+      with nogil:
+        del msg
+
+      return m
+
+
+cdef class PubSocket:
+  cdef cppPubSocket * socket
+
+  def __cinit__(self):
+    self.socket = cppPubSocket.create()
+    if self.socket == NULL:
+      raise IpcError
+
+  def __dealloc__(self):
+    del self.socket
+
+  def connect(self, Context context, string endpoint, size_t segment_size=0):
+    r = self.socket.connect(context.context, endpoint, True, segment_size)
+
+    if r != 0:
+      if errno.errno == errno.EADDRINUSE:
+        raise MultiplePublishersError(endpoint)
+      else:
+        raise IpcError(endpoint)
+
+  def send(self, bytes data):
+    length = len(data)
+    r = self.socket.send(<char*>data, length)
+
+    if r != length:
+      if errno.errno == errno.EADDRINUSE:
+        raise MultiplePublishersError
+      else:
+        raise IpcError
+
+  def all_readers_updated(self):
+    return self.socket.all_readers_updated()

artifacts/package_sources/msgq/msgq/logger/logger.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#ifdef SWAGLOG
+// cppcheck-suppress preprocessorErrorDirective
+#include SWAGLOG
+#else
+
+#define CLOUDLOG_DEBUG 10
+#define CLOUDLOG_INFO 20
+#define CLOUDLOG_WARNING 30
+#define CLOUDLOG_ERROR 40
+#define CLOUDLOG_CRITICAL 50
+
+#define cloudlog(lvl, fmt, ...) printf(fmt "\n", ## __VA_ARGS__)
+
+#define LOGD(fmt, ...) cloudlog(CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
+#define LOG(fmt, ...) cloudlog(CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
+#define LOGW(fmt, ...) cloudlog(CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
+#define LOGE(fmt, ...) cloudlog(CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
+
+#endif

artifacts/package_sources/msgq/msgq/msgq.cc

@@ -0,0 +1,622 @@
+#include <iostream>
+#include <cassert>
+#include <cerrno>
+#include <cmath>
+#include <cstring>
+#include <cstdint>
+#include <chrono>
+#include <algorithm>
+#include <cstdlib>
+#include <csignal>
+#include <random>
+#include <string>
+#include <limits>
+
+#include <poll.h>
+#include <sys/ioctl.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <sys/syscall.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <stdio.h>
+
+#include "msgq/msgq.h"
+
+void sigusr2_handler(int signal) {
+  assert(signal == SIGUSR2);
+}
+
+uint64_t msgq_get_uid(void){
+  std::random_device rd("/dev/urandom");
+  std::uniform_int_distribution<uint64_t> distribution(0, std::numeric_limits<uint32_t>::max());
+
+  #ifdef __APPLE__
+    // TODO: this doesn't work
+    uint64_t uid = distribution(rd) << 32 | getpid();
+  #else
+    uint64_t uid = distribution(rd) << 32 | syscall(SYS_gettid);
+  #endif
+
+  return uid;
+}
+
+int msgq_msg_init_size(msgq_msg_t * msg, size_t size){
+  msg->size = size;
+  msg->data = new(std::nothrow) char[size];
+
+  return (msg->data == NULL) ? -1 : 0;
+}
+
+
+int msgq_msg_init_data(msgq_msg_t * msg, char * data, size_t size) {
+  int r = msgq_msg_init_size(msg, size);
+
+  if (r == 0)
+    memcpy(msg->data, data, size);
+
+  return r;
+}
+
+int msgq_msg_close(msgq_msg_t * msg){
+  if (msg->size > 0)
+    delete[] msg->data;
+
+  msg->size = 0;
+
+  return 0;
+}
+
+void msgq_reset_reader(msgq_queue_t * q){
+  int id = q->reader_id;
+  q->read_valids[id]->store(true);
+  q->read_pointers[id]->store(*q->write_pointer);
+}
+
+void msgq_wait_for_subscriber(msgq_queue_t *q){
+  while (*q->num_readers == 0){
+    // wait for subscriber
+  }
+
+  return;
+}
+
+int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size){
+  assert(size < 0xFFFFFFFF); // Buffer must be smaller than 2^32 bytes
+  std::signal(SIGUSR2, sigusr2_handler);
+
+#ifdef __APPLE__
+  std::string base_path = "/tmp/msgq_";
+#else
+  std::string base_path = "/dev/shm/msgq_";
+#endif
+  const char* prefix = std::getenv("OPENPILOT_PREFIX");
+  if (prefix) {
+    base_path += std::string(prefix) + "/";
+  }
+  std::string full_path = base_path + path;
+
+  auto fd = open(full_path.c_str(), O_RDWR | O_CREAT, 0664);
+  if (fd < 0) {
+    std::cout << "Warning, could not open: " << full_path << std::endl;
+    return -1;
+  }
+
+  int rc = ftruncate(fd, size + sizeof(msgq_header_t));
+  if (rc < 0){
+    close(fd);
+    return -1;
+  }
+
+  int mmap_flags = MAP_SHARED;
+#ifdef MAP_POPULATE
+  if (std::getenv("MSGQ_PREALLOC")) {
+    mmap_flags |= MAP_POPULATE;
+  }
+#endif
+
+  char * mem = (char*)mmap(NULL, size + sizeof(msgq_header_t), PROT_READ | PROT_WRITE, mmap_flags, fd, 0);
+  close(fd);
+
+  if (mem == MAP_FAILED){
+    return -1;
+  }
+
+  q->mmap_p = mem;
+
+  msgq_header_t *header = (msgq_header_t *)mem;
+
+  // Setup pointers to header segment
+  q->num_readers = reinterpret_cast<std::atomic<uint64_t>*>(&header->num_readers);
+  q->write_pointer = reinterpret_cast<std::atomic<uint64_t>*>(&header->write_pointer);
+  q->write_uid = reinterpret_cast<std::atomic<uint64_t>*>(&header->write_uid);
+
+  for (size_t i = 0; i < NUM_READERS; i++){
+    q->read_pointers[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_pointers[i]);
+    q->read_valids[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_valids[i]);
+    q->read_uids[i] = reinterpret_cast<std::atomic<uint64_t>*>(&header->read_uids[i]);
+  }
+
+  q->data = mem + sizeof(msgq_header_t);
+  q->size = size;
+  q->reader_id = -1;
+
+  q->endpoint = path;
+  q->read_conflate = false;
+
+  return 0;
+}
+
+void msgq_close_queue(msgq_queue_t *q){
+  if (q->mmap_p != NULL){
+    msgq_remove_subscriber(q);
+    munmap(q->mmap_p, q->size + sizeof(msgq_header_t));
+  }
+}
+
+
+void msgq_init_publisher(msgq_queue_t * q) {
+  //std::cout << "Starting publisher" << std::endl;
+  uint64_t uid = msgq_get_uid();
+
+  q->last_reap_us = 0;
+  *q->write_uid = uid;
+  *q->num_readers = 0;
+
+  for (size_t i = 0; i < NUM_READERS; i++){
+    *q->read_valids[i] = false;
+    *q->read_uids[i] = 0;
+  }
+
+  q->write_uid_local = uid;
+}
+
+static void thread_signal(uint32_t tid) {
+  if (tid == 0) return;
+  #ifdef __APPLE__
+    // macOS doesn't have tkill, rely on polling instead
+    (void)tid;
+  #elif !defined(SYS_tkill)
+    // fallback for systems without tkill
+    kill(tid, SIGUSR2);
+  #else
+    syscall(SYS_tkill, tid, SIGUSR2);
+  #endif
+}
+
+#ifdef __linux__
+// Zero reader slots whose owning thread died without closing, so the notify loop
+// stops signaling their TIDs: the kernel recycles TIDs, and a stale slot means
+// every send fires SIGUSR2 at an unrelated process — fatal for one with no
+// handler installed (e.g. a subscriber's short-lived compiler child). Amortized
+// to ~1Hz per publisher; reclaim-at-overflow alone leaves stale slots signaled
+// indefinitely while the table has free space.
+static void msgq_reap_dead_readers(msgq_queue_t *q) {
+  int64_t now_us = std::chrono::duration_cast<std::chrono::microseconds>(
+      std::chrono::steady_clock::now().time_since_epoch()).count();
+  if (now_us - q->last_reap_us < 1000000)
+    return;
+  q->last_reap_us = now_us;
+
+  uint64_t num_readers = std::min<uint64_t>(NUM_READERS, *q->num_readers);
+  for (uint64_t i = 0; i < num_readers; i++){
+    uint64_t slot_uid = *q->read_uids[i];
+    uint32_t slot_tid = slot_uid & 0xFFFFFFFF;
+    if (slot_uid == 0 || slot_tid == 0)
+      continue;
+    char proc_path[32];
+    snprintf(proc_path, sizeof(proc_path), "/proc/%u", slot_tid);
+    struct stat st;
+    if (stat(proc_path, &st) == 0)
+      continue;
+    uint64_t expected = slot_uid;
+    if (std::atomic_compare_exchange_strong(q->read_uids[i], &expected, (uint64_t)0)){
+      *q->read_valids[i] = false;
+    }
+  }
+}
+#endif
+
+// claim slot i (its uid CAS already succeeded) for the calling thread
+static void msgq_claim_slot(msgq_queue_t * q, size_t i, uint64_t uid) {
+  q->reader_id = i;
+  q->read_uid_local = uid;
+  *q->read_valids[i] = false;
+  *q->read_pointers[i] = 0;
+}
+
+int msgq_init_subscriber(msgq_queue_t * q) {
+  assert(q != NULL);
+  assert(q->num_readers != NULL);
+
+  uint64_t uid = msgq_get_uid();
+
+  // Get reader id
+  while (true){
+    uint64_t cur_num_readers = *q->num_readers;
+    uint64_t new_num_readers = cur_num_readers + 1;
+
+    // Table full: reuse a released or dead slot. num_readers is an append-only
+    // high-water mark, so only slots below it are serviced by the publisher's
+    // notify/invalidate loops — never claim at or above it.
+    if (new_num_readers > NUM_READERS){
+      bool reclaimed = false;
+
+      // a slot zeroed by msgq_remove_subscriber (clean close)
+      for (size_t i = 0; i < cur_num_readers && !reclaimed; i++){
+        uint64_t expected = 0;
+        if (*q->read_uids[i] == 0 &&
+            std::atomic_compare_exchange_strong(q->read_uids[i], &expected, uid)){
+          msgq_claim_slot(q, i, uid);
+          reclaimed = true;
+        }
+      }
+
+#ifdef __linux__
+      // a slot whose owning thread died without closing; /proc/<tid> is stat-able
+      // for any live thread (incl. non-leaders), no procfs elsewhere
+      for (size_t i = 0; i < cur_num_readers && !reclaimed; i++){
+        uint64_t slot_uid = *q->read_uids[i];
+        uint32_t slot_tid = slot_uid & 0xFFFFFFFF;
+        if (slot_uid == 0 || slot_tid == 0)
+          continue;
+        char proc_path[32];
+        snprintf(proc_path, sizeof(proc_path), "/proc/%u", slot_tid);
+        struct stat st;
+        if (stat(proc_path, &st) == 0)
+          continue;
+        // claim atomically so two registrants can't take the same dead slot
+        std::atomic<uint64_t> *slot = q->read_uids[i];
+        uint64_t expected = slot_uid;
+        if (std::atomic_compare_exchange_strong(slot, &expected, uid)){
+          msgq_claim_slot(q, i, uid);
+          reclaimed = true;
+        }
+      }
+#endif
+      if (reclaimed)
+        break;
+
+      // NUM_READERS live readers: fail this registration. Evicting live readers
+      // (the old fallback) storms every healthy subscriber of the service and
+      // its SIGUSR2 flood interrupts their unrelated syscalls.
+      static std::atomic<int64_t> last_full_log_us(0);
+      int64_t now_us = std::chrono::duration_cast<std::chrono::microseconds>(
+          std::chrono::steady_clock::now().time_since_epoch()).count();
+      int64_t last_us = last_full_log_us.load(std::memory_order_relaxed);
+      if (now_us - last_us > 1000000 &&
+          last_full_log_us.compare_exchange_strong(last_us, now_us)) {
+        std::cout << "msgq warning: subscriber table full for " << q->endpoint
+                  << " (" << NUM_READERS << " live readers), registration refused" << std::endl;
+        // stdout only lives in tmux scrollback and dies with a reboot; also append
+        // to the persistent issue-debug file so overflow is diagnosable post-crash
+        FILE *f = fopen("/data/community/iqpilot_issue_debug.txt", "a");
+        if (f != NULL) {
+          fprintf(f, "[msgq] subscriber table full for %s (%d live readers), registration refused\n",
+                  q->endpoint.c_str(), NUM_READERS);
+          fclose(f);
+        }
+      }
+      errno = ENOBUFS;
+      return -1;
+    }
+
+    // Use atomic compare and swap to handle race condition
+    // where two subscribers start at the same time
+    if (std::atomic_compare_exchange_strong(q->num_readers,
+                                            &cur_num_readers,
+                                            new_num_readers)){
+      q->reader_id = cur_num_readers;
+      q->read_uid_local = uid;
+
+      // We start with read_valid = false,
+      // on the first read the read pointer will be synchronized with the write pointer
+      *q->read_valids[cur_num_readers] = false;
+      *q->read_pointers[cur_num_readers] = 0;
+      *q->read_uids[cur_num_readers] = uid;
+      break;
+    }
+  }
+
+  //std::cout << "New subscriber id: " << q->reader_id << " uid: " << q->read_uid_local << " " << q->endpoint << std::endl;
+  msgq_reset_reader(q);
+  return 0;
+}
+
+void msgq_remove_subscriber(msgq_queue_t * q) {
+  if (q->reader_id < 0 || q->num_readers == NULL)
+    return;
+  // release only if the slot is still ours — a publisher restart or a reclaim
+  // may have handed it to another reader
+  std::atomic<uint64_t> *slot = q->read_uids[q->reader_id];
+  uint64_t expected = q->read_uid_local;
+  if (std::atomic_compare_exchange_strong(slot, &expected, (uint64_t)0)){
+    *q->read_valids[q->reader_id] = false;
+  }
+  q->reader_id = -1;
+}
+
+int msgq_msg_send(msgq_msg_t * msg, msgq_queue_t *q){
+  // Die if we are no longer the active publisher
+  if (q->write_uid_local != *q->write_uid){
+    std::cout << "Killing old publisher: " << q->endpoint << std::endl;
+    errno = EADDRINUSE;
+    return -1;
+  }
+
+  uint64_t total_msg_size = ALIGN(msg->size + sizeof(int64_t));
+
+  // We need to fit at least three messages in the queue,
+  // then we can always safely access the last message
+  assert(3 * total_msg_size <= q->size);
+
+  uint64_t num_readers = *q->num_readers;
+
+  uint32_t write_cycles, write_pointer;
+  UNPACK64(write_cycles, write_pointer, *q->write_pointer);
+
+  char *p = q->data + write_pointer; // add base offset
+
+  // Check remaining space
+  // Always leave space for a wraparound tag for the next message, including alignment
+  int64_t remaining_space = q->size - write_pointer - total_msg_size - sizeof(int64_t);
+  if (remaining_space <= 0){
+    // Write -1 size tag indicating wraparound
+    *(int64_t*)p = -1;
+
+    // Invalidate all readers that are beyond the write pointer
+    // TODO: should we handle the case where a new reader shows up while this is running?
+    for (uint64_t i = 0; i < num_readers; i++){
+      uint64_t read_pointer = *q->read_pointers[i];
+      uint64_t read_cycles = read_pointer >> 32;
+      read_pointer &= 0xFFFFFFFF;
+
+      if ((read_pointer > write_pointer) && (read_cycles != write_cycles)) {
+        *q->read_valids[i] = false;
+      }
+    }
+
+    // Update global and local copies of write pointer and write_cycles
+    write_pointer = 0;
+    write_cycles = write_cycles + 1;
+    PACK64(*q->write_pointer, write_cycles, write_pointer);
+
+    // Set actual pointer to the beginning of the data segment
+    p = q->data;
+  }
+
+  // Invalidate readers that are in the area that will be written
+  uint64_t start = write_pointer;
+  uint64_t end = ALIGN(start + sizeof(int64_t) + msg->size);
+
+  for (uint64_t i = 0; i < num_readers; i++){
+    uint32_t read_cycles, read_pointer;
+    UNPACK64(read_cycles, read_pointer, *q->read_pointers[i]);
+
+    if ((read_pointer >= start) && (read_pointer < end) && (read_cycles != write_cycles)) {
+      *q->read_valids[i] = false;
+    }
+  }
+
+
+  // Write size tag
+  std::atomic<int64_t> *size_p = reinterpret_cast<std::atomic<int64_t>*>(p);
+  *size_p = msg->size;
+
+  // Copy data
+  memcpy(p + sizeof(int64_t), msg->data, msg->size);
+  __sync_synchronize();
+
+  // Update write pointer
+  uint32_t new_ptr = ALIGN(write_pointer + msg->size + sizeof(int64_t));
+  PACK64(*q->write_pointer, write_cycles, new_ptr);
+
+#ifdef __linux__
+  msgq_reap_dead_readers(q);
+#endif
+
+  // Notify readers
+  for (uint64_t i = 0; i < num_readers; i++){
+    uint64_t reader_uid = *q->read_uids[i];
+    thread_signal(reader_uid & 0xFFFFFFFF);
+  }
+
+  return msg->size;
+}
+
+
+int msgq_msg_ready(msgq_queue_t * q){
+ start:
+  int id = q->reader_id;
+  assert(id >= 0); // Make sure subscriber is initialized
+
+  if (q->read_uid_local != *q->read_uids[id]){
+    // publisher restart (or slot takeover) invalidated us; reconnect. If the
+    // table is momentarily full, report not-ready and retry on the next call.
+    if (msgq_init_subscriber(q) != 0){
+      return 0;
+    }
+    goto start;
+  }
+
+  // Check valid
+  if (!*q->read_valids[id]){
+    msgq_reset_reader(q);
+    goto start;
+  }
+
+  uint32_t read_cycles, read_pointer;
+  UNPACK64(read_cycles, read_pointer, *q->read_pointers[id]);
+  UNUSED(read_cycles);
+
+  uint32_t write_cycles, write_pointer;
+  UNPACK64(write_cycles, write_pointer, *q->write_pointer);
+  UNUSED(write_cycles);
+
+  // Check if new message is available
+  return (read_pointer != write_pointer);
+}
+
+int msgq_msg_recv(msgq_msg_t * msg, msgq_queue_t * q){
+ start:
+  int id = q->reader_id;
+  assert(id >= 0); // Make sure subscriber is initialized
+
+  if (q->read_uid_local != *q->read_uids[id]){
+    // publisher restart (or slot takeover) invalidated us; reconnect. If the
+    // table is momentarily full, report no-message and retry on the next call.
+    if (msgq_init_subscriber(q) != 0){
+      msg->size = 0;
+      return 0;
+    }
+    goto start;
+  }
+
+  // Check valid
+  if (!*q->read_valids[id]){
+    msgq_reset_reader(q);
+    goto start;
+  }
+
+  uint32_t read_cycles, read_pointer;
+  UNPACK64(read_cycles, read_pointer, *q->read_pointers[id]);
+
+  uint32_t write_cycles, write_pointer;
+  UNPACK64(write_cycles, write_pointer, *q->write_pointer);
+  UNUSED(write_cycles);
+
+  char * p = q->data + read_pointer;
+
+  // Check if new message is available
+  if (read_pointer == write_pointer) {
+    msg->size = 0;
+    return 0;
+  }
+
+  // Read potential message size
+  std::atomic<int64_t> *size_p = reinterpret_cast<std::atomic<int64_t>*>(p);
+  std::int64_t size = *size_p;
+
+  // Check if the size that was read is valid
+  if (!*q->read_valids[id]){
+    msgq_reset_reader(q);
+    goto start;
+  }
+
+  // If size is -1 the buffer was full, and we need to wrap around
+  if (size == -1){
+    read_cycles++;
+    PACK64(*q->read_pointers[id], read_cycles, 0);
+    goto start;
+  }
+
+  // crashing is better than passing garbage data to the consumer
+  // the size will have weird value if it was overwritten by data accidentally
+  assert((uint64_t)size < q->size);
+  assert(size > 0);
+
+  uint32_t new_read_pointer = ALIGN(read_pointer + sizeof(std::int64_t) + size);
+
+  // If conflate is true, check if this is the latest message, else start over
+  if (q->read_conflate){
+    if (new_read_pointer != write_pointer){
+      // Update read pointer
+      PACK64(*q->read_pointers[id], read_cycles, new_read_pointer);
+      goto start;
+    }
+  }
+
+  // Copy message
+  if (msgq_msg_init_size(msg, size) < 0)
+    return -1;
+
+  __sync_synchronize();
+  memcpy(msg->data, p + sizeof(int64_t), size);
+  __sync_synchronize();
+
+  // Update read pointer
+  PACK64(*q->read_pointers[id], read_cycles, new_read_pointer);
+
+  // Check if the actual data that was copied is valid
+  if (!*q->read_valids[id]){
+    msgq_msg_close(msg);
+    msgq_reset_reader(q);
+    goto start;
+  }
+
+
+  return msg->size;
+}
+
+
+
+int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout){
+  int num = 0;
+
+  // Check if messages ready
+  for (size_t i = 0; i < nitems; i++) {
+    items[i].revents = msgq_msg_ready(items[i].q);
+    if (items[i].revents) num++;
+  }
+
+  int ms = (timeout == -1) ? 100 : timeout;
+
+#ifdef __APPLE__
+  // On macOS, signals can't interrupt nanosleep, so poll more frequently
+  int poll_ms = std::min(ms, 10);
+  int remaining_ms = ms;
+#else
+  int poll_ms = ms;
+#endif
+
+  struct timespec ts;
+  ts.tv_sec = poll_ms / 1000;
+  ts.tv_nsec = (poll_ms % 1000) * 1000 * 1000;
+
+
+  while (num == 0) {
+    int ret;
+
+    ret = nanosleep(&ts, &ts);
+
+    // Check if messages ready
+    for (size_t i = 0; i < nitems; i++) {
+      if (items[i].revents == 0 && msgq_msg_ready(items[i].q)){
+        num += 1;
+        items[i].revents = 1;
+      }
+    }
+
+#ifdef __APPLE__
+    // exit if we had a timeout and we've exhausted it
+    if (timeout != -1 && ret == 0){
+      remaining_ms -= poll_ms;
+      if (remaining_ms <= 0){
+        break;
+      }
+      poll_ms = std::min(remaining_ms, 10);
+      ts.tv_sec = poll_ms / 1000;
+      ts.tv_nsec = (poll_ms % 1000) * 1000 * 1000;
+    }
+#else
+    // exit if we had a timeout and the sleep finished
+    if (timeout != -1 && ret == 0){
+      break;
+    }
+#endif
+  }
+
+  return num;
+}
+
+bool msgq_all_readers_updated(msgq_queue_t *q) {
+  uint64_t num_readers = *q->num_readers;
+  for (uint64_t i = 0; i < num_readers; i++) {
+    if (*q->read_valids[i] && *q->write_pointer != *q->read_pointers[i]) {
+      return false;
+    }
+  }
+  return num_readers > 0;
+}

artifacts/package_sources/msgq/msgq/msgq.h

@@ -0,0 +1,78 @@
+#pragma once
+
+#include <cstdint>
+#include <cstring>
+#include <string>
+#include <atomic>
+
+#define DEFAULT_SEGMENT_SIZE (1 * 1024 * 1024)
+// IQ.Pilot: raised from 15 — the fork's extra daemons put carState at ~17 subscribers.
+// Overflow no longer evicts live readers: closed slots are deregistered and reusable,
+// dead-process slots are reclaimed, and a registration that still finds the table full
+// fails with ENOBUFS instead of storming every healthy subscriber.
+// NOTE: changing this value changes the shared-memory header layout — every process on
+// the device must run the same build, and /dev/shm must be recreated (reboot) on upgrade.
+#define NUM_READERS 32
+#define ALIGN(n) ((n + (8 - 1)) & -8)
+
+#define UNUSED(x) (void)x
+#define UNPACK64(higher, lower, input) do {uint64_t tmp = input; higher = tmp >> 32; lower = tmp & 0xFFFFFFFF;} while (0)
+#define PACK64(output, higher, lower) output = ((uint64_t)higher << 32) | ((uint64_t)lower & 0xFFFFFFFF)
+
+struct  msgq_header_t {
+  uint64_t num_readers;
+  uint64_t write_pointer;
+  uint64_t write_uid;
+  uint64_t read_pointers[NUM_READERS];
+  uint64_t read_valids[NUM_READERS];
+  uint64_t read_uids[NUM_READERS];
+};
+
+struct msgq_queue_t {
+  std::atomic<uint64_t> *num_readers;
+  std::atomic<uint64_t> *write_pointer;
+  std::atomic<uint64_t> *write_uid;
+  std::atomic<uint64_t> *read_pointers[NUM_READERS];
+  std::atomic<uint64_t> *read_valids[NUM_READERS];
+  std::atomic<uint64_t> *read_uids[NUM_READERS];
+  char * mmap_p;
+  char * data;
+  size_t size;
+  int reader_id;
+  uint64_t read_uid_local;
+  uint64_t write_uid_local;
+  int64_t last_reap_us;
+
+  bool read_conflate;
+  std::string endpoint;
+};
+
+struct msgq_msg_t {
+  size_t size;
+  char * data;
+};
+
+struct msgq_pollitem_t {
+  msgq_queue_t *q;
+  int revents;
+};
+
+void msgq_wait_for_subscriber(msgq_queue_t *q);
+void msgq_reset_reader(msgq_queue_t *q);
+
+int msgq_msg_init_size(msgq_msg_t *msg, size_t size);
+int msgq_msg_init_data(msgq_msg_t *msg, char * data, size_t size);
+int msgq_msg_close(msgq_msg_t *msg);
+
+int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size);
+void msgq_close_queue(msgq_queue_t *q);
+void msgq_init_publisher(msgq_queue_t * q);
+int msgq_init_subscriber(msgq_queue_t * q);
+void msgq_remove_subscriber(msgq_queue_t * q);
+
+int msgq_msg_send(msgq_msg_t *msg, msgq_queue_t *q);
+int msgq_msg_recv(msgq_msg_t *msg, msgq_queue_t *q);
+int msgq_msg_ready(msgq_queue_t * q);
+int msgq_poll(msgq_pollitem_t * items, size_t nitems, int timeout);
+
+bool msgq_all_readers_updated(msgq_queue_t *q);

artifacts/package_sources/msgq/msgq/msgq_tests.cc

@@ -0,0 +1,432 @@
+#include "catch2/catch.hpp"
+#include "msgq/msgq.h"
+
+static void cleanup_test_queue() {
+#ifdef __APPLE__
+  remove("/tmp/msgq_test_queue");
+#else
+  remove("/dev/shm/msgq_test_queue");
+#endif
+}
+
+TEST_CASE("ALIGN")
+{
+  REQUIRE(ALIGN(0) == 0);
+  REQUIRE(ALIGN(1) == 8);
+  REQUIRE(ALIGN(7) == 8);
+  REQUIRE(ALIGN(8) == 8);
+  REQUIRE(ALIGN(99999) == 100000);
+}
+
+TEST_CASE("msgq_msg_init_size")
+{
+  const size_t msg_size = 30;
+  msgq_msg_t msg;
+
+  msgq_msg_init_size(&msg, msg_size);
+  REQUIRE(msg.size == msg_size);
+
+  msgq_msg_close(&msg);
+}
+
+TEST_CASE("msgq_msg_init_data")
+{
+  const size_t msg_size = 30;
+  char *data = new char[msg_size];
+
+  for (size_t i = 0; i < msg_size; i++)
+  {
+    data[i] = i;
+  }
+
+  msgq_msg_t msg;
+  msgq_msg_init_data(&msg, data, msg_size);
+
+  REQUIRE(msg.size == msg_size);
+  REQUIRE(memcmp(msg.data, data, msg_size) == 0);
+
+  delete[] data;
+  msgq_msg_close(&msg);
+}
+
+TEST_CASE("msgq_init_subscriber")
+{
+  cleanup_test_queue();
+  msgq_queue_t q;
+  msgq_new_queue(&q, "test_queue", 1024);
+  REQUIRE(*q.num_readers == 0);
+
+  q.reader_id = 1;
+  *q.read_valids[0] = false;
+  *q.read_pointers[0] = ((uint64_t)1 << 32);
+
+  *q.write_pointer = 255;
+
+  msgq_init_subscriber(&q);
+  REQUIRE(q.read_conflate == false);
+  REQUIRE(*q.read_valids[0] == true);
+  REQUIRE((*q.read_pointers[0] >> 32) == 0);
+  REQUIRE((*q.read_pointers[0] & 0xFFFFFFFF) == 255);
+}
+
+TEST_CASE("msgq_msg_send first message")
+{
+  cleanup_test_queue();
+  msgq_queue_t q;
+  msgq_new_queue(&q, "test_queue", 1024);
+  msgq_init_publisher(&q);
+
+  REQUIRE(*q.write_pointer == 0);
+
+  size_t msg_size = 128;
+
+  SECTION("Aligned message size")
+  {
+  }
+  SECTION("Unaligned message size")
+  {
+    msg_size--;
+  }
+  char *data = new char[msg_size];
+
+  for (size_t i = 0; i < msg_size; i++)
+  {
+    data[i] = i;
+  }
+
+  msgq_msg_t msg;
+  msgq_msg_init_data(&msg, data, msg_size);
+
+  msgq_msg_send(&msg, &q);
+  REQUIRE(*(int64_t *)q.data == msg_size); // Check size tag
+  REQUIRE(*q.write_pointer == 128 + sizeof(int64_t));
+  REQUIRE(memcmp(q.data + sizeof(int64_t), data, msg_size) == 0);
+
+  delete[] data;
+  msgq_msg_close(&msg);
+}
+
+TEST_CASE("msgq_msg_send test wraparound")
+{
+  cleanup_test_queue();
+  msgq_queue_t q;
+  msgq_new_queue(&q, "test_queue", 1024);
+  msgq_init_publisher(&q);
+
+  REQUIRE((*q.write_pointer & 0xFFFFFFFF) == 0);
+  REQUIRE((*q.write_pointer >> 32) == 0);
+
+  const size_t msg_size = 120;
+  msgq_msg_t msg;
+  msgq_msg_init_size(&msg, msg_size);
+
+  for (int i = 0; i < 8; i++)
+  {
+    msgq_msg_send(&msg, &q);
+  }
+  // Check 8th message was written at the beginning
+  REQUIRE((*q.write_pointer & 0xFFFFFFFF) == msg_size + sizeof(int64_t));
+
+  // Check cycle count
+  REQUIRE((*q.write_pointer >> 32) == 1);
+
+  // Check wraparound tag
+  char *tag_location = q.data;
+  tag_location += 7 * (msg_size + sizeof(int64_t));
+  REQUIRE(*(int64_t *)tag_location == -1);
+
+  msgq_msg_close(&msg);
+}
+
+TEST_CASE("msgq_msg_recv test wraparound")
+{
+  cleanup_test_queue();
+  msgq_queue_t q_pub, q_sub;
+  msgq_new_queue(&q_pub, "test_queue", 1024);
+  msgq_new_queue(&q_sub, "test_queue", 1024);
+
+  msgq_init_publisher(&q_pub);
+  msgq_init_subscriber(&q_sub);
+
+  REQUIRE((*q_pub.write_pointer >> 32) == 0);
+  REQUIRE((*q_sub.read_pointers[0] >> 32) == 0);
+
+  const size_t msg_size = 120;
+  msgq_msg_t msg1;
+  msgq_msg_init_size(&msg1, msg_size);
+
+  SECTION("Check cycle counter after reset")
+  {
+    for (int i = 0; i < 8; i++)
+    {
+      msgq_msg_send(&msg1, &q_pub);
+    }
+
+    msgq_msg_t msg2;
+    msgq_msg_recv(&msg2, &q_sub);
+    REQUIRE(msg2.size == 0); // Reader had to reset
+    msgq_msg_close(&msg2);
+  }
+  SECTION("Check cycle counter while keeping up with writer")
+  {
+    for (int i = 0; i < 8; i++)
+    {
+      msgq_msg_send(&msg1, &q_pub);
+
+      msgq_msg_t msg2;
+      msgq_msg_recv(&msg2, &q_sub);
+      REQUIRE(msg2.size > 0);
+      msgq_msg_close(&msg2);
+    }
+  }
+
+  REQUIRE((*q_sub.read_pointers[0] >> 32) == 1);
+  msgq_msg_close(&msg1);
+}
+
+TEST_CASE("msgq_msg_send test invalidation")
+{
+  cleanup_test_queue();
+  msgq_queue_t q_pub, q_sub;
+  msgq_new_queue(&q_pub, "test_queue", 1024);
+  msgq_new_queue(&q_sub, "test_queue", 1024);
+
+  msgq_init_publisher(&q_pub);
+  msgq_init_subscriber(&q_sub);
+  *q_sub.write_pointer = (uint64_t)1 << 32;
+
+  REQUIRE(*q_sub.read_valids[0] == true);
+
+  SECTION("read pointer in tag")
+  {
+    *q_sub.read_pointers[0] = 0;
+  }
+  SECTION("read pointer in data section")
+  {
+    *q_sub.read_pointers[0] = 64;
+  }
+  SECTION("read pointer in wraparound section")
+  {
+    *q_pub.write_pointer = ((uint64_t)1 << 32) | 1000; // Writer is one cycle ahead
+    *q_sub.read_pointers[0] = 1020;
+  }
+
+  msgq_msg_t msg;
+  msgq_msg_init_size(&msg, 128);
+  msgq_msg_send(&msg, &q_pub);
+
+  REQUIRE(*q_sub.read_valids[0] == false);
+
+  msgq_msg_close(&msg);
+}
+
+TEST_CASE("msgq_init_subscriber init 2 subscribers")
+{
+  cleanup_test_queue();
+  msgq_queue_t q1, q2;
+  msgq_new_queue(&q1, "test_queue", 1024);
+  msgq_new_queue(&q2, "test_queue", 1024);
+
+  *q1.num_readers = 0;
+
+  REQUIRE(*q1.num_readers == 0);
+  REQUIRE(*q2.num_readers == 0);
+
+  msgq_init_subscriber(&q1);
+  REQUIRE(*q1.num_readers == 1);
+  REQUIRE(*q2.num_readers == 1);
+  REQUIRE(q1.reader_id == 0);
+
+  msgq_init_subscriber(&q2);
+  REQUIRE(*q1.num_readers == 2);
+  REQUIRE(*q2.num_readers == 2);
+  REQUIRE(q2.reader_id == 1);
+}
+
+TEST_CASE("Write 1 msg, read 1 msg", "[integration]")
+{
+  cleanup_test_queue();
+  const size_t msg_size = 128;
+  msgq_queue_t writer, reader;
+
+  msgq_new_queue(&writer, "test_queue", 1024);
+  msgq_new_queue(&reader, "test_queue", 1024);
+
+  msgq_init_publisher(&writer);
+  msgq_init_subscriber(&reader);
+
+  // Build 128 byte message
+  msgq_msg_t outgoing_msg;
+  msgq_msg_init_size(&outgoing_msg, msg_size);
+
+  for (size_t i = 0; i < msg_size; i++)
+  {
+    outgoing_msg.data[i] = i;
+  }
+
+  REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size);
+
+  msgq_msg_t incoming_msg1;
+  REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size);
+  REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0);
+
+  // Verify that there are no more messages
+  msgq_msg_t incoming_msg2;
+  REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == 0);
+
+  msgq_msg_close(&outgoing_msg);
+  msgq_msg_close(&incoming_msg1);
+  msgq_msg_close(&incoming_msg2);
+}
+
+TEST_CASE("Write 2 msg, read 2 msg - conflate = false", "[integration]")
+{
+  cleanup_test_queue();
+  const size_t msg_size = 128;
+  msgq_queue_t writer, reader;
+
+  msgq_new_queue(&writer, "test_queue", 1024);
+  msgq_new_queue(&reader, "test_queue", 1024);
+
+  msgq_init_publisher(&writer);
+  msgq_init_subscriber(&reader);
+
+  // Build 128 byte message
+  msgq_msg_t outgoing_msg;
+  msgq_msg_init_size(&outgoing_msg, msg_size);
+
+  for (size_t i = 0; i < msg_size; i++)
+  {
+    outgoing_msg.data[i] = i;
+  }
+
+  REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size);
+  REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size);
+
+  msgq_msg_t incoming_msg1;
+  REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size);
+  REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0);
+
+  msgq_msg_t incoming_msg2;
+  REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == msg_size);
+  REQUIRE(memcmp(incoming_msg2.data, outgoing_msg.data, msg_size) == 0);
+
+  msgq_msg_close(&outgoing_msg);
+  msgq_msg_close(&incoming_msg1);
+  msgq_msg_close(&incoming_msg2);
+}
+
+TEST_CASE("Write 2 msg, read 2 msg - conflate = true", "[integration]")
+{
+  cleanup_test_queue();
+  const size_t msg_size = 128;
+  msgq_queue_t writer, reader;
+
+  msgq_new_queue(&writer, "test_queue", 1024);
+  msgq_new_queue(&reader, "test_queue", 1024);
+
+  msgq_init_publisher(&writer);
+  msgq_init_subscriber(&reader);
+  reader.read_conflate = true;
+
+  // Build 128 byte message
+  msgq_msg_t outgoing_msg;
+  msgq_msg_init_size(&outgoing_msg, msg_size);
+
+  for (size_t i = 0; i < msg_size; i++)
+  {
+    outgoing_msg.data[i] = i;
+  }
+
+  REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size);
+  REQUIRE(msgq_msg_send(&outgoing_msg, &writer) == msg_size);
+
+  msgq_msg_t incoming_msg1;
+  REQUIRE(msgq_msg_recv(&incoming_msg1, &reader) == msg_size);
+  REQUIRE(memcmp(incoming_msg1.data, outgoing_msg.data, msg_size) == 0);
+
+  // Verify that there are no more messages
+  msgq_msg_t incoming_msg2;
+  REQUIRE(msgq_msg_recv(&incoming_msg2, &reader) == 0);
+
+  msgq_msg_close(&outgoing_msg);
+  msgq_msg_close(&incoming_msg1);
+  msgq_msg_close(&incoming_msg2);
+}
+
+TEST_CASE("1 publisher, 1 slow subscriber", "[integration]")
+{
+  cleanup_test_queue();
+  msgq_queue_t writer, reader;
+
+  msgq_new_queue(&writer, "test_queue", 1024);
+  msgq_new_queue(&reader, "test_queue", 1024);
+
+  msgq_init_publisher(&writer);
+  msgq_init_subscriber(&reader);
+
+  int n_received = 0;
+  int n_skipped = 0;
+
+  for (uint64_t i = 0; i < 1e5; i++)
+  {
+    msgq_msg_t outgoing_msg;
+    msgq_msg_init_data(&outgoing_msg, (char *)&i, sizeof(uint64_t));
+    msgq_msg_send(&outgoing_msg, &writer);
+    msgq_msg_close(&outgoing_msg);
+
+    if (i % 10 == 0)
+    {
+      msgq_msg_t msg1;
+      msgq_msg_recv(&msg1, &reader);
+
+      if (msg1.size == 0)
+      {
+        n_skipped++;
+      }
+      else
+      {
+        n_received++;
+      }
+      msgq_msg_close(&msg1);
+    }
+  }
+
+  // TODO: verify these numbers by hand
+  REQUIRE(n_received == 8572);
+  REQUIRE(n_skipped == 1428);
+}
+
+TEST_CASE("1 publisher, 2 subscribers", "[integration]")
+{
+  cleanup_test_queue();
+  msgq_queue_t writer, reader1, reader2;
+
+  msgq_new_queue(&writer, "test_queue", 1024);
+  msgq_new_queue(&reader1, "test_queue", 1024);
+  msgq_new_queue(&reader2, "test_queue", 1024);
+
+  msgq_init_publisher(&writer);
+  msgq_init_subscriber(&reader1);
+  msgq_init_subscriber(&reader2);
+
+  for (uint64_t i = 0; i < 1024 * 3; i++)
+  {
+    msgq_msg_t outgoing_msg;
+    msgq_msg_init_data(&outgoing_msg, (char *)&i, sizeof(uint64_t));
+    msgq_msg_send(&outgoing_msg, &writer);
+
+    msgq_msg_t msg1, msg2;
+    msgq_msg_recv(&msg1, &reader1);
+    msgq_msg_recv(&msg2, &reader2);
+
+    REQUIRE(msg1.size == sizeof(uint64_t));
+    REQUIRE(msg2.size == sizeof(uint64_t));
+    REQUIRE(*(uint64_t *)msg1.data == i);
+    REQUIRE(*(uint64_t *)msg2.data == i);
+
+    msgq_msg_close(&outgoing_msg);
+    msgq_msg_close(&msg1);
+    msgq_msg_close(&msg2);
+  }
+}

artifacts/package_sources/msgq/msgq/test_runner.cc

@@ -0,0 +1,2 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"

artifacts/package_sources/msgq/msgq/tests/test_fake.py

@@ -0,0 +1,193 @@
+import pytest
+import os
+import multiprocessing
+import platform
+import msgq
+from parameterized import parameterized_class
+from typing import Optional
+
+WAIT_TIMEOUT = 5
+
+
+@pytest.mark.skipif(condition=platform.system() == "Darwin", reason="Events not supported on macOS")
+class TestEvents:
+
+  def test_mutation(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    assert not event.peek()
+    event.set()
+    assert event.peek()
+    event.clear()
+    assert not event.peek()
+
+    del event
+
+  def test_wait(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    event.set()
+    try:
+      event.wait(WAIT_TIMEOUT)
+      assert event.peek()
+    except RuntimeError:
+      pytest.fail("event.wait() timed out")
+
+  def test_wait_multiprocess(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    def set_event_run():
+      event.set()
+
+    try:
+      p = multiprocessing.Process(target=set_event_run)
+      p.start()
+      event.wait(WAIT_TIMEOUT)
+      assert event.peek()
+    except RuntimeError:
+      pytest.fail("event.wait() timed out")
+
+    p.kill()
+
+  def test_wait_zero_timeout(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    try:
+      event.wait(0)
+      pytest.fail("event.wait() did not time out")
+    except RuntimeError:
+      assert not event.peek()
+
+
+@pytest.mark.skipif(condition=platform.system() == "Darwin", reason="FakeSockets not supported on macOS")
+@parameterized_class([{"prefix": None}, {"prefix": "test"}])
+class TestFakeSockets:
+  prefix: Optional[str] = None
+
+  @pytest.fixture(autouse=True)
+  def zmq_mode(self):
+    os.environ.pop("ZMQ", None)
+    msgq.context = msgq.Context()
+    yield
+
+  def setup_method(self):
+    msgq.toggle_fake_events(True)
+    if self.prefix is not None:
+      msgq.set_fake_prefix(self.prefix)
+    else:
+      msgq.delete_fake_prefix()
+
+  def teardown_method(self):
+    msgq.toggle_fake_events(False)
+    msgq.delete_fake_prefix()
+
+  def test_event_handle_init(self):
+    handle = msgq.fake_event_handle("controlsState", override=True)
+
+    assert not handle.enabled
+    assert handle.recv_called_event.fd >= 0
+    assert handle.recv_ready_event.fd >= 0
+
+  def test_non_managed_socket_state(self):
+    # non managed socket should have zero state
+    _ = msgq.pub_sock("ubloxGnss")
+
+    handle = msgq.fake_event_handle("ubloxGnss", override=False)
+
+    assert not handle.enabled
+    assert handle.recv_called_event.fd == 0
+    assert handle.recv_ready_event.fd == 0
+
+  def test_managed_socket_state(self):
+    # managed socket should not change anything about the state
+    handle = msgq.fake_event_handle("ubloxGnss")
+    handle.enabled = True
+
+    expected_enabled = handle.enabled
+    expected_recv_called_fd = handle.recv_called_event.fd
+    expected_recv_ready_fd = handle.recv_ready_event.fd
+
+    _ = msgq.pub_sock("ubloxGnss")
+
+    assert handle.enabled == expected_enabled
+    assert handle.recv_called_event.fd == expected_recv_called_fd
+    assert handle.recv_ready_event.fd == expected_recv_ready_fd
+
+  def test_sockets_enable_disable(self):
+    carState_handle = msgq.fake_event_handle("ubloxGnss", enable=True)
+    recv_called = carState_handle.recv_called_event
+    recv_ready = carState_handle.recv_ready_event
+
+    pub_sock = msgq.pub_sock("ubloxGnss")
+    sub_sock = msgq.sub_sock("ubloxGnss")
+
+    try:
+      carState_handle.enabled = True
+      recv_ready.set()
+      pub_sock.send(b"test")
+      _ = sub_sock.receive()
+      assert recv_called.peek()
+      recv_called.clear()
+
+      carState_handle.enabled = False
+      recv_ready.set()
+      pub_sock.send(b"test")
+      _ = sub_sock.receive()
+      assert not recv_called.peek()
+    except RuntimeError:
+      pytest.fail("event.wait() timed out")
+
+  def test_synced_pub_sub(self):
+    def daemon_repub_process_run():
+      pub_sock = msgq.pub_sock("ubloxGnss")
+      sub_sock = msgq.sub_sock("carState")
+
+      frame = -1
+      while True:
+        frame += 1
+        msg = sub_sock.receive(non_blocking=True)
+        if msg is None:
+          print("none received")
+          continue
+
+        bts = frame.to_bytes(8, 'little')
+        pub_sock.send(bts)
+
+    carState_handle = msgq.fake_event_handle("carState", enable=True)
+    recv_called = carState_handle.recv_called_event
+    recv_ready = carState_handle.recv_ready_event
+
+    p = multiprocessing.Process(target=daemon_repub_process_run)
+    p.start()
+
+    pub_sock = msgq.pub_sock("carState")
+    sub_sock = msgq.sub_sock("ubloxGnss")
+
+    try:
+      for i in range(10):
+        recv_called.wait(WAIT_TIMEOUT)
+        recv_called.clear()
+
+        if i == 0:
+          sub_sock.receive(non_blocking=True)
+
+        bts = i.to_bytes(8, 'little')
+        pub_sock.send(bts)
+
+        recv_ready.set()
+        recv_called.wait(WAIT_TIMEOUT)
+
+        msg = sub_sock.receive(non_blocking=True)
+        assert msg is not None
+        assert len(msg) == 8
+
+        frame = int.from_bytes(msg, 'little')
+        assert frame == i
+    except RuntimeError:
+      pytest.fail("event.wait() timed out")
+    finally:
+      p.kill()

artifacts/package_sources/msgq/msgq/tests/test_messaging.py

@@ -0,0 +1,68 @@
+import os
+import random
+import time
+import string
+import msgq
+
+def random_sock():
+  return ''.join(random.choices(string.ascii_uppercase + string.digits, k=10))
+
+def random_bytes(length=1000):
+  return bytes([random.randrange(0xFF) for _ in range(length)])
+
+def zmq_sleep(t=1):
+  if "ZMQ" in os.environ:
+    time.sleep(t)
+
+class TestPubSubSockets:
+
+  def setup_method(self):
+    # ZMQ pub socket takes too long to die
+    # sleep to prevent multiple publishers error between tests
+    zmq_sleep()
+
+  def test_pub_sub(self):
+    sock = random_sock()
+    pub_sock = msgq.pub_sock(sock)
+    sub_sock = msgq.sub_sock(sock, conflate=False, timeout=None)
+    zmq_sleep(3)
+
+    for _ in range(1000):
+      msg = random_bytes()
+      pub_sock.send(msg)
+      recvd = sub_sock.receive()
+      assert msg == recvd
+
+  def test_conflate(self):
+    sock = random_sock()
+    pub_sock = msgq.pub_sock(sock)
+    for conflate in [True, False]:
+      num_msgs = random.randint(3, 10)
+      sub_sock = msgq.sub_sock(sock, conflate=conflate, timeout=None)
+      zmq_sleep()
+
+      sent_msgs = []
+      for __ in range(num_msgs):
+        msg = random_bytes()
+        pub_sock.send(msg)
+        sent_msgs.append(msg)
+      time.sleep(0.1)
+      recvd_msgs = msgq.drain_sock_raw(sub_sock)
+      if conflate:
+        assert len(recvd_msgs) == 1
+        assert recvd_msgs[0] == sent_msgs[-1]
+      else:
+        assert len(recvd_msgs) == len(sent_msgs)
+        for rec_msg, sent_msg in zip(recvd_msgs, sent_msgs):
+          assert rec_msg == sent_msg
+
+  def test_receive_timeout(self):
+    sock = random_sock()
+    timeout = random.randrange(200)
+    sub_sock = msgq.sub_sock(sock, timeout=timeout)
+    zmq_sleep()
+
+    start_time = time.monotonic()
+    recvd = sub_sock.receive()
+    assert (time.monotonic() - start_time) < (timeout + 0.1)
+    assert recvd is None

artifacts/package_sources/msgq/msgq/tests/test_poller.py

@@ -0,0 +1,138 @@
+import pytest
+import time
+import msgq
+import concurrent.futures
+
+SERVICE_NAME = 'myService'
+
+def poller():
+  context = msgq.Context()
+
+  p = msgq.Poller()
+
+  sub = msgq.SubSocket()
+  sub.connect(context, SERVICE_NAME)
+  p.registerSocket(sub)
+
+  socks = p.poll(10000)
+  r = [s.receive(non_blocking=True) for s in socks]
+
+  return r
+
+
+class TestPoller:
+  def test_poll_once(self):
+    context = msgq.Context()
+
+    pub = msgq.PubSocket()
+    pub.connect(context, SERVICE_NAME)
+
+    with concurrent.futures.ThreadPoolExecutor() as e:
+      poll = e.submit(poller)
+
+      time.sleep(0.1)  # Slow joiner syndrome
+
+      # Send message
+      pub.send(b"a")
+
+      # Wait for poll result
+      result = poll.result()
+
+    del pub
+    context.term()
+
+    assert result == [b"a"]
+
+  def test_poll_and_create_many_subscribers(self):
+    context = msgq.Context()
+
+    pub = msgq.PubSocket()
+    pub.connect(context, SERVICE_NAME)
+
+    with concurrent.futures.ThreadPoolExecutor() as e:
+      poll = e.submit(poller)
+
+      time.sleep(0.1)  # Slow joiner syndrome
+      c = msgq.Context()
+      for _ in range(10):
+        msgq.SubSocket().connect(c, SERVICE_NAME)
+
+      time.sleep(0.1)
+
+      # Send message
+      pub.send(b"a")
+
+      # Wait for poll result
+      result = poll.result()
+
+    del pub
+    context.term()
+
+    assert result == [b"a"]
+
+  def test_multiple_publishers_exception(self):
+    context = msgq.Context()
+
+    with pytest.raises(msgq.MultiplePublishersError):
+      pub1 = msgq.PubSocket()
+      pub1.connect(context, SERVICE_NAME)
+
+      pub2 = msgq.PubSocket()
+      pub2.connect(context, SERVICE_NAME)
+
+      pub1.send(b"a")
+
+    del pub1
+    del pub2
+    context.term()
+
+  def test_multiple_messages(self):
+    context = msgq.Context()
+
+    pub = msgq.PubSocket()
+    pub.connect(context, SERVICE_NAME)
+
+    sub = msgq.SubSocket()
+    sub.connect(context, SERVICE_NAME)
+
+    time.sleep(0.1)  # Slow joiner
+
+    for i in range(1, 100):
+      pub.send(b'a'*i)
+
+    msg_seen = False
+    i = 1
+    while True:
+      r = sub.receive(non_blocking=True)
+
+      if r is not None:
+        assert b'a'*i == r
+
+        msg_seen = True
+        i += 1
+
+      if r is None and msg_seen:  # ZMQ sometimes receives nothing on the first receive
+        break
+
+    del pub
+    del sub
+    context.term()
+
+  def test_conflate(self):
+    context = msgq.Context()
+
+    pub = msgq.PubSocket()
+    pub.connect(context, SERVICE_NAME)
+
+    sub = msgq.SubSocket()
+    sub.connect(context, SERVICE_NAME, conflate=True)
+
+    time.sleep(0.1)  # Slow joiner
+    pub.send(b'a')
+    pub.send(b'b')
+
+    assert b'b' == sub.receive()
+
+    del pub
+    del sub
+    context.term()

artifacts/package_sources/msgq/msgq/visionipc/.gitignore

@@ -0,0 +1,2 @@
+visionipc_pyx.cpp
+*.so

artifacts/package_sources/msgq/msgq/visionipc/__init__.py

@@ -0,0 +1,5 @@
+from msgq.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, get_endpoint_name
+assert VisionBuf
+assert VisionIpcClient
+assert VisionIpcServer
+assert get_endpoint_name

artifacts/package_sources/msgq/msgq/visionipc/test_runner.cc

@@ -0,0 +1,2 @@
+#define CATCH_CONFIG_MAIN
+#include "catch2/catch.hpp"

artifacts/package_sources/msgq/msgq/visionipc/tests/test_visionipc.py

@@ -0,0 +1,115 @@
+import os
+import time
+import random
+from enum import IntEnum
+from typing import Optional
+import numpy as np
+from msgq.visionipc import VisionIpcServer, VisionIpcClient
+
+
+class VisionStreamType(IntEnum):
+  VISION_STREAM_ROAD = 0
+  VISION_STREAM_DRIVER = 1
+  VISION_STREAM_WIDE_ROAD = 2
+  VISION_STREAM_MAP = 3
+
+def zmq_sleep(t=1):
+  if "ZMQ" in os.environ:
+    time.sleep(t)
+
+
+class TestVisionIpc:
+  server: VisionIpcServer
+  client: Optional[VisionIpcClient]
+
+  def setup_vipc(self, name, *stream_types, num_buffers=1, width=100, height=100, conflate=False):
+    self.server = VisionIpcServer(name)
+    for stream_type in stream_types:
+      self.server.create_buffers(stream_type, num_buffers, width, height)
+    self.server.start_listener()
+
+    if len(stream_types):
+      self.client = VisionIpcClient(name, stream_types[0], conflate)
+      assert self.client.connect(True)
+    else:
+      self.client = None
+
+    zmq_sleep()
+    return self.server, self.client
+
+  def test_connect(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD)
+    assert self.client is not None
+    assert self.client.is_connected
+    del self.client
+    del self.server
+
+  def test_available_streams(self):
+    for k in range(4):
+      stream_types = set(random.choices([x.value for x in VisionStreamType], k=k))
+      self.setup_vipc("camerad", *stream_types)
+      available_streams = VisionIpcClient.available_streams("camerad", True)
+      assert available_streams == stream_types
+      del self.client
+      del self.server
+
+  def test_buffers(self):
+    width, height, num_buffers = 100, 200, 5
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD, num_buffers=num_buffers, width=width, height=height)
+    assert self.client is not None
+    assert self.client.width == width
+    assert self.client.height == height
+    assert self.client.buffer_len is not None and self.client.buffer_len > 0
+    assert self.client.num_buffers == num_buffers
+    del self.client
+    del self.server
+
+  def test_send_single_buffer(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD)
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = np.zeros(self.client.buffer_len, dtype=np.uint8)
+    buf.view('<i4')[0] = 1234
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=1337)
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    assert recv_buf.data.view('<i4')[0] == 1234
+    assert self.client.frame_id == 1337
+    del self.client
+    del self.server
+
+  def test_no_conflate(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD)
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = np.zeros(self.client.buffer_len, dtype=np.uint8)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=1)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=2)
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    assert self.client.frame_id == 1
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    assert self.client.frame_id == 2
+    del self.client
+    del self.server
+
+  def test_conflate(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD, conflate=True)
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = np.zeros(self.client.buffer_len, dtype=np.uint8)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=1)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=2)
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    assert self.client.frame_id == 2
+
+    recv_buf = self.client.recv()
+    assert recv_buf is None
+    del self.client
+    del self.server

artifacts/package_sources/msgq/msgq/visionipc/visionbuf.cc

@@ -0,0 +1,20 @@
+#include "msgq/visionipc/visionbuf.h"
+
+void VisionBuf::init_yuv(size_t init_width, size_t init_height, size_t init_stride, size_t init_uv_offset){
+  this->width = init_width;
+  this->height = init_height;
+  this->stride = init_stride;
+  this->uv_offset = init_uv_offset;
+
+  this->y = (uint8_t *)this->addr;
+  this->uv = this->y + this->uv_offset;
+}
+
+
+uint64_t VisionBuf::get_frame_id() {
+  return *frame_id;
+}
+
+void VisionBuf::set_frame_id(uint64_t id) {
+  *frame_id = id;
+}

artifacts/package_sources/msgq/msgq/visionipc/visionbuf.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include "msgq/visionipc/visionipc.h"
+
+#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#define VISIONBUF_SYNC_FROM_DEVICE 0
+#define VISIONBUF_SYNC_TO_DEVICE 1
+
+using VisionStreamType = int;
+constexpr VisionStreamType VISION_STREAM_MAX = 4;
+
+class VisionBuf {
+ public:
+  size_t len = 0;
+  size_t mmap_len = 0;
+  void * addr = nullptr;
+  uint64_t *frame_id;
+  int fd = 0;
+
+  size_t width = 0;
+  size_t height = 0;
+  size_t stride = 0;
+  size_t uv_offset = 0;
+
+  // YUV
+  uint8_t * y = nullptr;
+  uint8_t * uv = nullptr;
+
+  // Visionipc
+  uint64_t server_id = 0;
+  size_t idx = 0;
+  VisionStreamType type;
+
+  // OpenCL
+  cl_mem buf_cl = nullptr;
+  cl_command_queue copy_q = nullptr;
+
+  // ion
+  int handle = 0;
+
+  void allocate(size_t len);
+  void import();
+  void init_cl(cl_device_id device_id, cl_context ctx);
+  void init_yuv(size_t width, size_t height, size_t stride, size_t uv_offset);
+  int sync(int dir);
+  int free();
+
+  void set_frame_id(uint64_t id);
+  uint64_t get_frame_id();
+};

artifacts/package_sources/msgq/msgq/visionipc/visionbuf_cl.cc

@@ -0,0 +1,94 @@
+#include "msgq/visionipc/visionbuf.h"
+
+#include <atomic>
+#include <stdio.h>
+#include <fcntl.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/mman.h>
+#include <sys/types.h>
+
+std::atomic<int> offset = 0;
+
+static void *malloc_with_fd(size_t len, int *fd) {
+  char full_path[0x100];
+
+#ifdef __APPLE__
+  snprintf(full_path, sizeof(full_path)-1, "/tmp/visionbuf_%d_%d", getpid(), offset++);
+#else
+  snprintf(full_path, sizeof(full_path)-1, "/dev/shm/msgq_visionbuf_%d_%d", getpid(), offset++);
+#endif
+
+  *fd = open(full_path, O_RDWR | O_CREAT, 0664);
+  assert(*fd >= 0);
+
+  unlink(full_path);
+
+  ftruncate(*fd, len);
+  void *addr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
+  assert(addr != MAP_FAILED);
+
+  return addr;
+}
+
+void VisionBuf::allocate(size_t length) {
+  this->len = length;
+  this->mmap_len = this->len + sizeof(uint64_t);
+  this->addr = malloc_with_fd(this->mmap_len, &this->fd);
+  this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len);
+}
+
+void VisionBuf::init_cl(cl_device_id device_id, cl_context ctx){
+  int err;
+
+  this->copy_q = clCreateCommandQueue(ctx, device_id, 0, &err);
+  assert(err == 0);
+
+  this->buf_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, this->len, this->addr, &err);
+  assert(err == 0);
+}
+
+
+void VisionBuf::import(){
+  assert(this->fd >= 0);
+  this->addr = mmap(NULL, this->mmap_len, PROT_READ | PROT_WRITE, MAP_SHARED, this->fd, 0);
+  assert(this->addr != MAP_FAILED);
+
+  this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len);
+}
+
+
+int VisionBuf::sync(int dir) {
+  int err = 0;
+  if (!this->buf_cl) return 0;
+
+  if (dir == VISIONBUF_SYNC_FROM_DEVICE) {
+    err = clEnqueueReadBuffer(this->copy_q, this->buf_cl, CL_FALSE, 0, this->len, this->addr, 0, NULL, NULL);
+  } else {
+    err = clEnqueueWriteBuffer(this->copy_q, this->buf_cl, CL_FALSE, 0, this->len, this->addr, 0, NULL, NULL);
+  }
+
+  if (err == 0){
+    err = clFinish(this->copy_q);
+  }
+
+  return err;
+}
+
+int VisionBuf::free() {
+  int err = 0;
+  if (this->buf_cl){
+    err = clReleaseMemObject(this->buf_cl);
+    if (err != 0) return err;
+
+    err = clReleaseCommandQueue(this->copy_q);
+    if (err != 0) return err;
+  }
+
+  err = munmap(this->addr, this->mmap_len);
+  if (err != 0) return err;
+
+  err = close(this->fd);
+  return err;
+}

artifacts/package_sources/msgq/msgq/visionipc/visionbuf_ion.cc

@@ -0,0 +1,161 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <errno.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include <linux/ion.h>
+#include <CL/cl_ext.h>
+
+#include <msm_ion.h>
+
+#include "msgq/visionipc/visionbuf.h"
+
+// keep trying if x gets interrupted by a signal
+#define HANDLE_EINTR(x)                                       \
+  ({                                                          \
+    decltype(x) ret;                                          \
+    int try_cnt = 0;                                          \
+    do {                                                      \
+      ret = (x);                                              \
+    } while (ret == -1 && errno == EINTR && try_cnt++ < 100); \
+    ret;                                                      \
+  })
+
+// just hard-code these for convenience
+// size_t device_page_size = 0;
+// clGetDeviceInfo(device_id, CL_DEVICE_PAGE_SIZE_QCOM,
+//                 sizeof(device_page_size), &device_page_size,
+//                 NULL);
+
+// size_t padding_cl = 0;
+// clGetDeviceInfo(device_id, CL_DEVICE_EXT_MEM_PADDING_IN_BYTES_QCOM,
+//                 sizeof(padding_cl), &padding_cl,
+//                 NULL);
+#define DEVICE_PAGE_SIZE_CL 4096
+#define PADDING_CL 0
+
+struct IonFileHandle {
+  IonFileHandle() {
+    fd = open("/dev/ion", O_RDWR | O_NONBLOCK);
+    assert(fd >= 0);
+  }
+  ~IonFileHandle() {
+    close(fd);
+  }
+  int fd = -1;
+};
+
+int ion_fd() {
+  static IonFileHandle fh;
+  return fh.fd;
+}
+
+void VisionBuf::allocate(size_t length) {
+  struct ion_allocation_data ion_alloc = {0};
+  ion_alloc.len = length + PADDING_CL + sizeof(uint64_t);
+  ion_alloc.align = 4096;
+  ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID;
+  ion_alloc.flags = ION_FLAG_CACHED;
+
+  int err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_ALLOC, &ion_alloc));
+  assert(err == 0);
+
+  struct ion_fd_data ion_fd_data = {0};
+  ion_fd_data.handle = ion_alloc.handle;
+  err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_SHARE, &ion_fd_data));
+  assert(err == 0);
+
+  void *mmap_addr = mmap(NULL, ion_alloc.len,
+                         PROT_READ | PROT_WRITE,
+                         MAP_SHARED, ion_fd_data.fd, 0);
+  assert(mmap_addr != MAP_FAILED);
+
+  memset(mmap_addr, 0, ion_alloc.len);
+
+  this->len = length;
+  this->mmap_len = ion_alloc.len;
+  this->addr = mmap_addr;
+  this->handle = ion_alloc.handle;
+  this->fd = ion_fd_data.fd;
+  this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len + PADDING_CL);
+}
+
+void VisionBuf::import(){
+  int err;
+  assert(this->fd >= 0);
+
+  // Get handle
+  struct ion_fd_data fd_data = {0};
+  fd_data.fd = this->fd;
+  err = HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_IMPORT, &fd_data));
+  assert(err == 0);
+
+  this->handle = fd_data.handle;
+  this->addr = mmap(NULL, this->mmap_len, PROT_READ | PROT_WRITE, MAP_SHARED, this->fd, 0);
+  assert(this->addr != MAP_FAILED);
+
+  this->frame_id = (uint64_t*)((uint8_t*)this->addr + this->len + PADDING_CL);
+}
+
+void VisionBuf::init_cl(cl_device_id device_id, cl_context ctx) {
+  int err;
+
+  assert(((uintptr_t)this->addr % DEVICE_PAGE_SIZE_CL) == 0);
+
+  cl_mem_ion_host_ptr ion_cl = {0};
+  ion_cl.ext_host_ptr.allocation_type = CL_MEM_ION_HOST_PTR_QCOM;
+  ion_cl.ext_host_ptr.host_cache_policy = CL_MEM_HOST_UNCACHED_QCOM;
+  ion_cl.ion_filedesc = this->fd;
+  ion_cl.ion_hostptr = this->addr;
+
+  this->buf_cl = clCreateBuffer(ctx,
+                              CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM,
+                              this->len, &ion_cl, &err);
+  assert(err == 0);
+}
+
+
+int VisionBuf::sync(int dir) {
+  struct ion_flush_data flush_data = {0};
+  flush_data.handle = this->handle;
+  flush_data.vaddr = this->addr;
+  flush_data.offset = 0;
+  flush_data.length = this->len;
+
+  // ION_IOC_INV_CACHES ~= DMA_FROM_DEVICE
+  // ION_IOC_CLEAN_CACHES ~= DMA_TO_DEVICE
+  // ION_IOC_CLEAN_INV_CACHES ~= DMA_BIDIRECTIONAL
+
+  struct ion_custom_data custom_data = {0};
+
+   assert(dir == VISIONBUF_SYNC_FROM_DEVICE || dir == VISIONBUF_SYNC_TO_DEVICE);
+   custom_data.cmd = (dir == VISIONBUF_SYNC_FROM_DEVICE) ?
+     ION_IOC_INV_CACHES : ION_IOC_CLEAN_CACHES;
+
+  custom_data.arg = (unsigned long)&flush_data;
+  return HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_CUSTOM, &custom_data));
+}
+
+int VisionBuf::free() {
+  int err = 0;
+
+  if (this->buf_cl){
+    err = clReleaseMemObject(this->buf_cl);
+    if (err != 0) return err;
+  }
+
+  err = munmap(this->addr, this->mmap_len);
+  if (err != 0) return err;
+
+  err = close(this->fd);
+  if (err != 0) return err;
+
+  struct ion_handle_data handle_data = {.handle = this->handle};
+  return HANDLE_EINTR(ioctl(ion_fd(), ION_IOC_FREE, &handle_data));
+}

artifacts/package_sources/msgq/msgq/visionipc/visionipc.cc

@@ -0,0 +1,121 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <assert.h>
+#include <errno.h>
+
+#include <sys/mman.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+
+#ifdef __APPLE__
+#define getsocket() socket(AF_UNIX, SOCK_STREAM, 0)
+#else
+#define getsocket() socket(AF_UNIX, SOCK_SEQPACKET, 0)
+#endif
+
+#include "msgq/visionipc/visionipc.h"
+
+int ipc_connect(const char* socket_path) {
+  int err;
+
+  int sock = getsocket();
+
+  if (sock < 0) return -1;
+  struct sockaddr_un addr = {
+    .sun_family = AF_UNIX,
+  };
+  snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
+  err = connect(sock, (struct sockaddr*)&addr, sizeof(addr));
+  if (err != 0) {
+    close(sock);
+    return -1;
+  }
+
+  return sock;
+}
+
+int ipc_bind(const char* socket_path) {
+  int err;
+
+  unlink(socket_path);
+
+  int sock = getsocket();
+
+  struct sockaddr_un addr = {
+    .sun_family = AF_UNIX,
+  };
+  snprintf(addr.sun_path, sizeof(addr.sun_path), "%s", socket_path);
+  err = bind(sock, (struct sockaddr *)&addr, sizeof(addr));
+  assert(err == 0);
+
+  err = listen(sock, 3);
+  assert(err == 0);
+
+  return sock;
+}
+
+
+int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
+                          int *out_num_fds) {
+  char control_buf[CMSG_SPACE(sizeof(int) * num_fds)];
+  memset(control_buf, 0, CMSG_SPACE(sizeof(int) * num_fds));
+
+  struct iovec iov = {
+    .iov_base = buf,
+    .iov_len = buf_size,
+  };
+  struct msghdr msg = {
+    .msg_iov = &iov,
+    .msg_iovlen = 1,
+  };
+
+  if (num_fds > 0) {
+    assert(fds);
+
+    msg.msg_control = control_buf;
+    msg.msg_controllen = CMSG_SPACE(sizeof(int) * num_fds);
+  }
+
+  if (send) {
+    if (num_fds) {
+      struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
+      assert(cmsg);
+      cmsg->cmsg_level = SOL_SOCKET;
+      cmsg->cmsg_type = SCM_RIGHTS;
+      cmsg->cmsg_len = CMSG_LEN(sizeof(int) * num_fds);
+      memcpy(CMSG_DATA(cmsg), fds, sizeof(int) * num_fds);
+    }
+    return sendmsg(fd, &msg, 0);
+  } else {
+    int r = recvmsg(fd, &msg, 0);
+    if (r < 0) return r;
+
+    int recv_fds = 0;
+    if (msg.msg_controllen > 0) {
+      struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
+      assert(cmsg);
+      assert(cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_RIGHTS);
+      recv_fds = (cmsg->cmsg_len - CMSG_LEN(0));
+      assert(recv_fds > 0 && (recv_fds % sizeof(int)) == 0);
+      recv_fds /= sizeof(int);
+
+      assert(fds && recv_fds <= num_fds);
+      memcpy(fds, CMSG_DATA(cmsg), sizeof(int) * recv_fds);
+    }
+
+    if (msg.msg_flags) {
+      for (int i=0; i<recv_fds; i++) {
+        close(fds[i]);
+      }
+      return -1;
+    }
+
+    if (fds) {
+      assert(out_num_fds);
+      *out_num_fds = recv_fds;
+    }
+    return r;
+  }
+}

artifacts/package_sources/msgq/msgq/visionipc/visionipc.h

@@ -0,0 +1,25 @@
+#pragma once
+
+#include <cstdint>
+#include <cstddef>
+
+
+int ipc_connect(const char* socket_path);
+int ipc_bind(const char* socket_path);
+int ipc_sendrecv_with_fds(bool send, int fd, void *buf, size_t buf_size, int* fds, int num_fds,
+                          int *out_num_fds);
+
+constexpr int VISIONIPC_MAX_FDS = 128;
+
+struct VisionIpcBufExtra {
+  uint32_t frame_id;
+  uint64_t timestamp_sof;
+  uint64_t timestamp_eof;
+  bool valid;
+};
+
+struct VisionIpcPacket {
+  uint64_t server_id;
+  size_t idx;
+  struct VisionIpcBufExtra extra;
+};

artifacts/package_sources/msgq/msgq/visionipc/visionipc.pxd

@@ -0,0 +1,60 @@
+# distutils: language = c++
+#cython: language_level=3
+
+from libcpp.string cimport string
+from libcpp.vector cimport vector
+from libcpp.set cimport set
+from libc.stdint cimport uint32_t, uint64_t
+from libcpp cimport bool, int
+
+cdef extern from "msgq/visionipc/visionbuf.h":
+  struct _cl_device_id
+  struct _cl_context
+  struct _cl_mem
+
+  ctypedef _cl_device_id * cl_device_id
+  ctypedef _cl_context * cl_context
+  ctypedef _cl_mem * cl_mem
+
+  ctypedef int VisionStreamType
+
+  cdef cppclass VisionBuf:
+    void * addr
+    int fd
+    size_t len
+    size_t width
+    size_t height
+    size_t stride
+    size_t uv_offset
+    size_t idx
+    cl_mem buf_cl
+    void set_frame_id(uint64_t id)
+
+cdef extern from "msgq/visionipc/visionipc.h":
+  struct VisionIpcBufExtra:
+    uint32_t frame_id
+    uint64_t timestamp_sof
+    uint64_t timestamp_eof
+    bool valid
+
+cdef extern from "msgq/visionipc/visionipc_server.h":
+  string get_endpoint_name(string, VisionStreamType)
+
+  cdef cppclass VisionIpcServer:
+    VisionIpcServer(string, void*, void*)
+    void create_buffers(VisionStreamType, size_t, size_t, size_t)
+    void create_buffers_with_sizes(VisionStreamType, size_t, size_t, size_t, size_t, size_t, size_t)
+    VisionBuf * get_buffer(VisionStreamType)
+    void send(VisionBuf *, VisionIpcBufExtra *, bool)
+    void start_listener()
+
+cdef extern from "msgq/visionipc/visionipc_client.h":
+  cdef cppclass VisionIpcClient:
+    int num_buffers
+    VisionBuf buffers[1]
+    VisionIpcClient(string, VisionStreamType, bool, void*, void*)
+    VisionBuf * recv(VisionIpcBufExtra *, int)
+    bool connect(bool)
+    bool is_connected()
+    @staticmethod
+    set[VisionStreamType] getAvailableStreams(string, bool)

artifacts/package_sources/msgq/msgq/visionipc/visionipc_client.cc

@@ -0,0 +1,160 @@
+#include <chrono>
+#include <cassert>
+#include <iostream>
+#include <thread>
+#include <string>
+#include <set>
+
+#include <unistd.h>
+#include "msgq/visionipc/visionipc.h"
+#include "msgq/visionipc/visionipc_client.h"
+#include "msgq/visionipc/visionipc_server.h"
+#include "msgq/logger/logger.h"
+
+static int connect_to_vipc_server(const std::string &name, bool blocking) {
+  const std::string ipc_path = get_ipc_path(name);
+  int socket_fd = ipc_connect(ipc_path.c_str());
+  while (socket_fd < 0 && blocking) {
+    std::cout << "VisionIpcClient connecting" << std::endl;
+    std::this_thread::sleep_for(std::chrono::milliseconds(100));
+    socket_fd = ipc_connect(ipc_path.c_str());
+  }
+  return socket_fd;
+}
+
+VisionIpcClient::VisionIpcClient(std::string name, VisionStreamType type, bool conflate, cl_device_id device_id, cl_context ctx) : name(name), type(type), device_id(device_id), ctx(ctx) {
+  msg_ctx = Context::create();
+  sock = SubSocket::create(msg_ctx, get_endpoint_name(name, type), "127.0.0.1", conflate, false);
+
+  poller = Poller::create();
+  poller->registerSocket(sock);
+}
+
+// Connect is not thread safe. Do not use the buffers while calling connect
+bool VisionIpcClient::connect(bool blocking) {
+  connected = false;
+
+  // Cleanup old buffers on reconnect
+  for (size_t i = 0; i < num_buffers; i++) {
+    if (buffers[i].free() != 0) {
+      LOGE("Failed to free buffer %zu", i);
+    }
+  }
+
+  num_buffers = 0;
+
+  int socket_fd = connect_to_vipc_server(name, blocking);
+  if (socket_fd < 0) {
+    return false;
+  }
+  // Send stream type to server to request FDs
+  int r = ipc_sendrecv_with_fds(true, socket_fd, &type, sizeof(type), nullptr, 0, nullptr);
+  assert(r == sizeof(type));
+
+  // Get FDs
+  int fds[VISIONIPC_MAX_FDS] = {};
+  VisionBuf bufs[VISIONIPC_MAX_FDS] = {};
+  r = ipc_sendrecv_with_fds(false, socket_fd, &bufs, sizeof(bufs), fds, VISIONIPC_MAX_FDS, &num_buffers);
+  if (r < 0) {
+    // only expected error is server shutting down
+    assert(errno == ECONNRESET);
+    close(socket_fd);
+    return false;
+  }
+
+  assert(num_buffers >= 0);
+  assert(r == sizeof(VisionBuf) * num_buffers);
+
+  // Import buffers
+  for (size_t i = 0; i < num_buffers; i++) {
+    buffers[i] = bufs[i];
+    buffers[i].fd = fds[i];
+    buffers[i].import();
+    buffers[i].init_yuv(buffers[i].width, buffers[i].height, buffers[i].stride, buffers[i].uv_offset);
+
+    if (device_id) buffers[i].init_cl(device_id, ctx);
+  }
+
+  close(socket_fd);
+  connected = true;
+  return true;
+}
+
+VisionBuf * VisionIpcClient::recv(VisionIpcBufExtra * extra, const int timeout_ms) {
+  auto p = poller->poll(timeout_ms);
+
+  if (!p.size()) {
+    return nullptr;
+  }
+
+  Message * r = sock->receive(true);
+  if (r == nullptr) {
+    return nullptr;
+  }
+
+  // Get buffer
+  assert(r->getSize() == sizeof(VisionIpcPacket));
+  VisionIpcPacket *packet = (VisionIpcPacket*)r->getData();
+
+  // Check if packet index is out of bounds, indicating server has changed
+  if (packet->idx >= num_buffers) {
+    connected = false;
+    delete r;
+    return nullptr;
+  }
+
+  VisionBuf * buf = &buffers[packet->idx];
+
+  if (buf->server_id != packet->server_id) {
+    connected = false;
+    delete r;
+    return nullptr;
+  }
+
+  if (extra) {
+    *extra = packet->extra;
+  }
+
+  if (buf->sync(VISIONBUF_SYNC_TO_DEVICE) != 0) {
+    LOGE("Failed to sync buffer");
+  }
+
+  delete r;
+  return buf;
+}
+
+std::set<VisionStreamType> VisionIpcClient::getAvailableStreams(const std::string &name, bool blocking) {
+  int socket_fd = connect_to_vipc_server(name, blocking);
+  if (socket_fd < 0) {
+    return {};
+  }
+  // Send VISION_STREAM_MAX to server to request available streams
+  int request = VISION_STREAM_MAX;
+  int r = ipc_sendrecv_with_fds(true, socket_fd, &request, sizeof(request), nullptr, 0, nullptr);
+  assert(r == sizeof(request));
+
+  VisionStreamType available_streams[VISION_STREAM_MAX] = {};
+  r = ipc_sendrecv_with_fds(false, socket_fd, &available_streams, sizeof(available_streams), nullptr, 0, nullptr);
+  if (r < 0) {
+    // only expected error is server shutting down
+    assert(errno == ECONNRESET);
+    close(socket_fd);
+    return {};
+  }
+
+  assert(r % sizeof(VisionStreamType) == 0);
+  close(socket_fd);
+  return std::set<VisionStreamType>(available_streams, available_streams + r / sizeof(VisionStreamType));
+}
+
+VisionIpcClient::~VisionIpcClient() {
+  for (size_t i = 0; i < num_buffers; i++) {
+    if (buffers[i].free() != 0) {
+      LOGE("Failed to free buffer %zu", i);
+    }
+  }
+
+  delete sock;
+  delete poller;
+  delete msg_ctx;
+}

artifacts/package_sources/msgq/msgq/visionipc/visionipc_client.h

@@ -0,0 +1,31 @@
+#pragma once
+
+#include <set>
+#include <string>
+
+#include "msgq/ipc.h"
+#include "msgq/visionipc/visionbuf.h"
+
+
+class VisionIpcClient {
+private:
+  std::string name;
+  Context * msg_ctx;
+  SubSocket * sock;
+  Poller * poller;
+
+  cl_device_id device_id = nullptr;
+  cl_context ctx = nullptr;
+
+public:
+  bool connected = false;
+  VisionStreamType type;
+  int num_buffers = 0;
+  VisionBuf buffers[VISIONIPC_MAX_FDS];
+  VisionIpcClient(std::string name, VisionStreamType type, bool conflate, cl_device_id device_id=nullptr, cl_context ctx=nullptr);
+  ~VisionIpcClient();
+  VisionBuf * recv(VisionIpcBufExtra * extra=nullptr, const int timeout_ms=100);
+  bool connect(bool blocking=true);
+  bool is_connected() { return connected; }
+  static std::set<VisionStreamType> getAvailableStreams(const std::string &name, bool blocking = true);
+};

artifacts/package_sources/msgq/msgq/visionipc/visionipc_pyx.pxd

@@ -0,0 +1,15 @@
+# distutils: language = c++
+#cython: language_level=3
+
+from .visionipc cimport VisionBuf as cppVisionBuf
+from .visionipc cimport cl_device_id, cl_context
+
+cdef class CLContext:
+  cdef cl_device_id device_id
+  cdef cl_context context
+
+cdef class VisionBuf:
+  cdef cppVisionBuf * buf
+
+  @staticmethod
+  cdef create(cppVisionBuf*)

artifacts/package_sources/msgq/msgq/visionipc/visionipc_pyx.pyx

@@ -0,0 +1,162 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+import sys
+import numpy as np
+cimport numpy as cnp
+from cython.view cimport array
+from libc.string cimport memcpy
+from libc.stdint cimport uint32_t, uint64_t
+from libcpp cimport bool
+from libcpp.string cimport string
+
+from .visionipc cimport VisionIpcServer as cppVisionIpcServer
+from .visionipc cimport VisionIpcClient as cppVisionIpcClient
+from .visionipc cimport VisionBuf as cppVisionBuf
+from .visionipc cimport VisionStreamType
+from .visionipc cimport VisionIpcBufExtra
+from .visionipc cimport get_endpoint_name as cpp_get_endpoint_name
+
+
+def get_endpoint_name(string name, VisionStreamType stream):
+  return cpp_get_endpoint_name(name, stream).decode('utf-8')
+
+
+cdef class VisionBuf:
+  @staticmethod
+  cdef create(cppVisionBuf * cbuf):
+    buf = VisionBuf()
+    buf.buf = cbuf
+    return buf
+
+  @property
+  def data(self):
+    return np.asarray(<cnp.uint8_t[:self.buf.len]> self.buf.addr)
+
+  @property
+  def width(self):
+    return self.buf.width
+
+  @property
+  def height(self):
+    return self.buf.height
+
+  @property
+  def stride(self):
+    return self.buf.stride
+
+  @property
+  def uv_offset(self):
+    return self.buf.uv_offset
+
+  @property
+  def idx(self):
+    return self.buf.idx
+
+  @property
+  def fd(self):
+    return self.buf.fd
+
+
+cdef class VisionIpcServer:
+  cdef cppVisionIpcServer * server
+
+  def __init__(self, string name):
+    self.server = new cppVisionIpcServer(name, NULL, NULL)
+
+  def create_buffers(self, VisionStreamType tp, size_t num_buffers, size_t width, size_t height):
+    self.server.create_buffers(tp, num_buffers, width, height)
+
+  def create_buffers_with_sizes(self, VisionStreamType tp, size_t num_buffers, size_t width, size_t height, size_t size, size_t stride, size_t uv_offset):
+    self.server.create_buffers_with_sizes(tp, num_buffers, width, height, size, stride, uv_offset)
+
+  def send(self, VisionStreamType tp, const unsigned char[:] data, uint32_t frame_id=0, uint64_t timestamp_sof=0, uint64_t timestamp_eof=0):
+    cdef cppVisionBuf * buf = self.server.get_buffer(tp)
+
+    # Populate buffer
+    assert buf.len == len(data)
+    memcpy(buf.addr, &data[0], len(data))
+    buf.set_frame_id(frame_id)
+
+    cdef VisionIpcBufExtra extra
+    extra.frame_id = frame_id
+    extra.timestamp_sof = timestamp_sof
+    extra.timestamp_eof = timestamp_eof
+
+    self.server.send(buf, &extra, False)
+
+  def start_listener(self):
+    self.server.start_listener()
+
+  def __dealloc__(self):
+    del self.server
+
+
+cdef class VisionIpcClient:
+  cdef cppVisionIpcClient * client
+  cdef VisionIpcBufExtra extra
+
+  def __cinit__(self, string name, VisionStreamType stream, bool conflate, CLContext context = None):
+    if context:
+      self.client = new cppVisionIpcClient(name, stream, conflate, context.device_id, context.context)
+    else:
+      self.client = new cppVisionIpcClient(name, stream, conflate, NULL, NULL)
+
+  def __dealloc__(self):
+    del self.client
+
+  @property
+  def width(self):
+    return self.client.buffers[0].width if self.client.num_buffers else None
+
+  @property
+  def height(self):
+    return self.client.buffers[0].height if self.client.num_buffers else None
+
+  @property
+  def stride(self):
+    return self.client.buffers[0].stride if self.client.num_buffers else None
+
+  @property
+  def uv_offset(self):
+    return self.client.buffers[0].uv_offset if self.client.num_buffers else None
+
+  @property
+  def buffer_len(self):
+    return self.client.buffers[0].len if self.client.num_buffers else None
+
+  @property
+  def num_buffers(self):
+    return self.client.num_buffers
+
+  @property
+  def frame_id(self):
+    return self.extra.frame_id
+
+  @property
+  def timestamp_sof(self):
+    return self.extra.timestamp_sof
+
+  @property
+  def timestamp_eof(self):
+    return self.extra.timestamp_eof
+
+  @property
+  def valid(self):
+    return self.extra.valid
+
+  def recv(self, int timeout_ms=100):
+    buf = self.client.recv(&self.extra, timeout_ms)
+    if not buf:
+      return None
+    return VisionBuf.create(buf)
+
+  def connect(self, bool blocking):
+    return self.client.connect(blocking)
+
+  def is_connected(self):
+    return self.client.is_connected()
+
+  @staticmethod
+  def available_streams(string name, bool block):
+    return cppVisionIpcClient.getAvailableStreams(name, block)

artifacts/package_sources/msgq/msgq/visionipc/visionipc_server.cc

@@ -0,0 +1,215 @@
+#include <iostream>
+#include <chrono>
+#include <cassert>
+#include <random>
+#include <limits>
+#include <string>
+#include <vector>
+
+#include <poll.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include "msgq/ipc.h"
+#include "msgq/visionipc/visionipc.h"
+#include "msgq/visionipc/visionipc_server.h"
+#include "msgq/logger/logger.h"
+
+std::string get_endpoint_name(std::string name, VisionStreamType type){
+  if (messaging_use_zmq()){
+    assert(name == "camerad" || name == "navd");
+    return std::to_string(9000 + static_cast<int>(type));
+  } else {
+    return "visionipc_" + name + "_" + std::to_string(type);
+  }
+}
+
+std::string get_ipc_path(const std::string& name) {
+  std::string path = "/tmp/";
+  if (char* prefix = std::getenv("OPENPILOT_PREFIX")) {
+    path += std::string(prefix) + "_";
+  }
+  return path + "visionipc_" + name;
+}
+
+VisionIpcServer::VisionIpcServer(std::string name, cl_device_id device_id, cl_context ctx) : name(name), device_id(device_id), ctx(ctx) {
+  msg_ctx = Context::create();
+
+  std::random_device rd("/dev/urandom");
+  std::uniform_int_distribution<uint64_t> distribution(0, std::numeric_limits<uint64_t>::max());
+  server_id = distribution(rd);
+}
+
+void VisionIpcServer::create_buffers(VisionStreamType type, size_t num_buffers, size_t width, size_t height){
+  // TODO: assert that this type is not created yet
+  assert(num_buffers < VISIONIPC_MAX_FDS);
+
+  size_t size = 0;
+  size_t stride = 0;
+  size_t uv_offset = 0;
+
+  size = width * height * 3 / 2;
+  stride = width;
+  uv_offset = width * height;
+
+  create_buffers_with_sizes(type, num_buffers, width, height, size, stride, uv_offset);
+}
+
+void VisionIpcServer::create_buffers_with_sizes(VisionStreamType type, size_t num_buffers, size_t width, size_t height, size_t size, size_t stride, size_t uv_offset) {
+  // Create map + alloc requested buffers
+  for (size_t i = 0; i < num_buffers; i++){
+    VisionBuf* buf = new VisionBuf();
+    buf->allocate(size);
+    buf->idx = i;
+    buf->type = type;
+
+    if (device_id) buf->init_cl(device_id, ctx);
+
+    buf->init_yuv(width, height, stride, uv_offset);
+
+    buffers[type].push_back(buf);
+  }
+
+  cur_idx[type] = 0;
+
+  // Create msgq publisher for each of the `name` + type combos
+  // TODO: compute port number directly if using zmq
+  sockets[type] = PubSocket::create(msg_ctx, get_endpoint_name(name, type), false);
+}
+
+
+void VisionIpcServer::start_listener(){
+  listener_thread = std::thread(&VisionIpcServer::listener, this);
+}
+
+
+void VisionIpcServer::listener(){
+  LOGD("Starting listener for: %s", name.c_str());
+
+  const std::string ipc_path = get_ipc_path(name);
+  int sock = ipc_bind(ipc_path.c_str());
+  assert(sock >= 0);
+
+  while (!should_exit){
+    // Wait for incoming connection
+    struct pollfd polls[1] = {{0}};
+    polls[0].fd = sock;
+    polls[0].events = POLLIN;
+
+    int ret = poll(polls, 1, 100);
+    if (ret < 0) {
+      if (errno == EINTR || errno == EAGAIN) continue;
+      std::cout << "poll failed, stopping listener" << std::endl;
+      break;
+    }
+
+    if (should_exit) break;
+    if (!polls[0].revents) {
+      continue;
+    }
+
+    // Handle incoming request
+    int fd = accept(sock, NULL, NULL);
+    assert(fd >= 0);
+
+    VisionStreamType type = VISION_STREAM_MAX;
+    int r = ipc_sendrecv_with_fds(false, fd, &type, sizeof(type), nullptr, 0, nullptr);
+    assert(r == sizeof(type));
+
+    // send available stream types
+    if (type == VISION_STREAM_MAX) {
+      std::vector<VisionStreamType> available_stream_types;
+      for (auto& [stream_type, _] : buffers) {
+        available_stream_types.push_back(stream_type);
+      }
+      r = ipc_sendrecv_with_fds(true, fd, available_stream_types.data(), available_stream_types.size() * sizeof(VisionStreamType), nullptr, 0, nullptr);
+      assert(r == available_stream_types.size() * sizeof(VisionStreamType));
+      close(fd);
+      continue;
+    }
+
+    if (buffers.count(type) <= 0) {
+      std::cout << "got request for invalid buffer type: " << type << std::endl;
+      close(fd);
+      continue;
+    }
+
+    int fds[VISIONIPC_MAX_FDS] = {};
+    int num_fds = buffers[type].size();
+    VisionBuf bufs[VISIONIPC_MAX_FDS] = {};
+
+    for (int i = 0; i < num_fds; i++){
+      fds[i] = buffers[type][i]->fd;
+      bufs[i] = *buffers[type][i];
+
+      // Remove some private openCL/ion metadata
+      bufs[i].buf_cl = 0;
+      bufs[i].copy_q = 0;
+      bufs[i].handle = 0;
+
+      bufs[i].server_id = server_id;
+    }
+
+    r = ipc_sendrecv_with_fds(true, fd, &bufs, sizeof(VisionBuf) * num_fds, fds, num_fds, nullptr);
+
+    close(fd);
+  }
+
+  LOGD("Stopping listener for: %s", name.c_str());
+  close(sock);
+  unlink(ipc_path.c_str());
+}
+
+
+
+VisionBuf * VisionIpcServer::get_buffer(VisionStreamType type, int idx){
+  // Do we want to keep track if the buffer has been sent out yet and warn user?
+  assert(buffers.count(type));
+  auto b = buffers[type];
+  if (idx < 0) {
+    idx = cur_idx[type]++ % b.size();
+  } else {
+    assert(idx < b.size() && idx >= 0);
+    cur_idx[type] = idx;
+  }
+  return b[idx];
+}
+
+void VisionIpcServer::send(VisionBuf * buf, VisionIpcBufExtra * extra, bool sync){
+  if (sync) {
+    if (buf->sync(VISIONBUF_SYNC_FROM_DEVICE) != 0) {
+      LOGE("Failed to sync buffer");
+    }
+  }
+  assert(buffers.count(buf->type));
+  assert(buf->idx < buffers[buf->type].size());
+
+  // Send over correct msgq socket
+  VisionIpcPacket packet = {0};
+  packet.server_id = server_id;
+  packet.idx = buf->idx;
+  packet.extra = *extra;
+
+  sockets[buf->type]->send((char*)&packet, sizeof(packet));
+}
+
+VisionIpcServer::~VisionIpcServer(){
+  should_exit = true;
+  listener_thread.join();
+
+  // VisionBuf cleanup
+  for (auto const& [type, buf] : buffers) {
+    for (VisionBuf* b : buf){
+      if (b->free() != 0) {
+        LOGE("Failed to free buffer");
+      }
+      delete b;
+    }
+  }
+
+  // Messaging cleanup
+  for (auto const& [type, sock] : sockets) {
+    delete sock;
+  }
+  delete msg_ctx;
+}

artifacts/package_sources/msgq/msgq/visionipc/visionipc_server.h

@@ -0,0 +1,42 @@
+#pragma once
+#include <vector>
+#include <string>
+#include <thread>
+#include <atomic>
+#include <map>
+
+#include "msgq/ipc.h"
+#include "msgq/visionipc/visionbuf.h"
+
+std::string get_endpoint_name(std::string name, VisionStreamType type);
+std::string get_ipc_path(const std::string &name);
+
+class VisionIpcServer {
+ private:
+  cl_device_id device_id = nullptr;
+  cl_context ctx = nullptr;
+  uint64_t server_id;
+
+  std::atomic<bool> should_exit = false;
+  std::string name;
+  std::thread listener_thread;
+
+  std::map<VisionStreamType, std::atomic<size_t> > cur_idx;
+  std::map<VisionStreamType, std::vector<VisionBuf*> > buffers;
+
+  Context * msg_ctx;
+  std::map<VisionStreamType, PubSocket*> sockets;
+
+  void listener(void);
+
+ public:
+  VisionIpcServer(std::string name, cl_device_id device_id=nullptr, cl_context ctx=nullptr);
+  ~VisionIpcServer();
+
+  VisionBuf * get_buffer(VisionStreamType type, int idx = -1);
+
+  void create_buffers(VisionStreamType type, size_t num_buffers, size_t width, size_t height);
+  void create_buffers_with_sizes(VisionStreamType type, size_t num_buffers, size_t width, size_t height, size_t size, size_t stride, size_t uv_offset);
+  void send(VisionBuf * buf, VisionIpcBufExtra * extra, bool sync=true);
+  void start_listener();
+};

artifacts/package_sources/msgq/msgq/visionipc/visionipc_tests.cc

@@ -0,0 +1,142 @@
+#include <thread>
+#include <chrono>
+
+#include "catch2/catch.hpp"
+
+#include "msgq/visionipc/visionipc_server.h"
+#include "msgq/visionipc/visionipc_client.h"
+
+enum TestVisionStreamType : VisionStreamType {
+  VISION_STREAM_ROAD = 0,
+  VISION_STREAM_DRIVER = 1,
+  VISION_STREAM_WIDE_ROAD = 2,
+  VISION_STREAM_MAP = 3,
+};
+
+
+static void zmq_sleep(int milliseconds=1000){
+  if (messaging_use_zmq()){
+    std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds));
+  }
+}
+
+TEST_CASE("Connecting"){
+  VisionIpcServer server("camerad");
+  server.create_buffers(VISION_STREAM_ROAD, 1, 100, 100);
+  server.start_listener();
+
+  VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
+  REQUIRE(client.connect());
+
+  REQUIRE(client.connected);
+}
+
+TEST_CASE("getAvailableStreams"){
+  VisionIpcServer server("camerad");
+  server.create_buffers(VISION_STREAM_ROAD, 1, 100, 100);
+  server.create_buffers(VISION_STREAM_WIDE_ROAD, 1, 100, 100);
+  server.start_listener();
+  auto available_streams = VisionIpcClient::getAvailableStreams("camerad");
+  REQUIRE(available_streams.size() == 2);
+  REQUIRE(available_streams.count(VISION_STREAM_ROAD) == 1);
+  REQUIRE(available_streams.count(VISION_STREAM_WIDE_ROAD) == 1);
+}
+
+TEST_CASE("Check buffers"){
+  size_t width = 100, height = 200, num_buffers = 5;
+  VisionIpcServer server("camerad");
+  server.create_buffers(VISION_STREAM_ROAD, num_buffers, width, height);
+  server.start_listener();
+
+  VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
+  REQUIRE(client.connect());
+
+  REQUIRE(client.buffers[0].width == width);
+  REQUIRE(client.buffers[0].height == height);
+  REQUIRE(client.buffers[0].len);
+  REQUIRE(client.num_buffers == num_buffers);
+}
+
+TEST_CASE("Send single buffer"){
+  VisionIpcServer server("camerad");
+  server.create_buffers(VISION_STREAM_ROAD, 1, 100, 100);
+  server.start_listener();
+
+  VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
+  REQUIRE(client.connect());
+  zmq_sleep();
+
+  VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
+  REQUIRE(buf != nullptr);
+
+  *((uint64_t*)buf->addr) = 1234;
+
+  VisionIpcBufExtra extra = {0};
+  extra.frame_id = 1337;
+  buf->set_frame_id(extra.frame_id);
+
+  server.send(buf, &extra);
+
+  VisionIpcBufExtra extra_recv = {0};
+  VisionBuf * recv_buf = client.recv(&extra_recv);
+  REQUIRE(recv_buf != nullptr);
+  REQUIRE(*(uint64_t*)recv_buf->addr == 1234);
+  REQUIRE(extra_recv.frame_id == extra.frame_id);
+  REQUIRE(recv_buf->get_frame_id() == extra.frame_id);
+}
+
+
+TEST_CASE("Test no conflate"){
+  VisionIpcServer server("camerad");
+  server.create_buffers(VISION_STREAM_ROAD, 1, 100, 100);
+  server.start_listener();
+
+  VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, false);
+  REQUIRE(client.connect());
+  zmq_sleep();
+
+  VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
+  REQUIRE(buf != nullptr);
+
+  VisionIpcBufExtra extra = {0};
+  extra.frame_id = 1;
+  server.send(buf, &extra);
+  extra.frame_id = 2;
+  server.send(buf, &extra);
+
+  VisionIpcBufExtra extra_recv = {0};
+  VisionBuf * recv_buf = client.recv(&extra_recv);
+  REQUIRE(recv_buf != nullptr);
+  REQUIRE(extra_recv.frame_id == 1);
+
+  recv_buf = client.recv(&extra_recv);
+  REQUIRE(recv_buf != nullptr);
+  REQUIRE(extra_recv.frame_id == 2);
+}
+
+TEST_CASE("Test conflate"){
+  VisionIpcServer server("camerad");
+  server.create_buffers(VISION_STREAM_ROAD, 1, 100, 100);
+  server.start_listener();
+
+  VisionIpcClient client = VisionIpcClient("camerad", VISION_STREAM_ROAD, true);
+  REQUIRE(client.connect());
+  zmq_sleep();
+
+  VisionBuf * buf = server.get_buffer(VISION_STREAM_ROAD);
+  REQUIRE(buf != nullptr);
+
+  VisionIpcBufExtra extra = {0};
+  extra.frame_id = 1;
+  server.send(buf, &extra);
+  extra.frame_id = 2;
+  server.send(buf, &extra);
+
+  VisionIpcBufExtra extra_recv = {0};
+  VisionBuf * recv_buf = client.recv(&extra_recv);
+  REQUIRE(recv_buf != nullptr);
+  REQUIRE(extra_recv.frame_id == 2);
+
+  recv_buf = client.recv(&extra_recv);
+  REQUIRE(recv_buf == nullptr);
+}

artifacts/package_sources/msgq/pyproject.toml

@@ -0,0 +1,69 @@
+[project]
+name = "msgq"
+version = "0.0.1"
+description = "Code powering the comma.ai panda"
+readme = "README.md"
+requires-python = ">=3.11,<3.13"
+license = {text = "MIT"}
+authors = [{name = "comma.ai"}]
+classifiers = [
+  "Natural Language :: English",
+  "Programming Language :: Python :: 3",
+  "Topic :: System :: Hardware",
+]
+dependencies = []
+
+[build-system]
+requires = ["setuptools>=64", "Cython", "scons", "numpy"]
+build-backend = "setuptools.build_meta"
+
+[project.optional-dependencies]
+dev = [
+  "setuptools",  # for distutils
+  "Cython",
+  "scons",
+  "ruff",
+  "parameterized",
+  "coverage",
+  "numpy",
+  "pytest",
+  "pytest-retry",
+  "cppcheck",
+  "cpplint",
+  "codespell",
+  "ty",
+  "lefthook",
+]
+
+[tool.setuptools.package-data]
+msgq = ["*.h", "*.a", "*.so", "*.dylib", "visionipc/*.h", "visionipc/*.a", "visionipc/*.so", "visionipc/*.dylib"]
+
+[tool.setuptools.packages.find]
+include = ["msgq*"]
+
+# https://beta.ruff.rs/docs/configuration/#using-pyprojecttoml
+[tool.ruff]
+lint.select = ["E", "F", "W", "PIE", "C4", "ISC", "RUF100", "A"]
+lint.ignore = ["W292", "E741", "E402", "C408", "ISC003"]
+lint.flake8-implicit-str-concat.allow-multiline=false
+
+line-length = 160
+target-version="py311"
+
+[tool.ruff.lint.flake8-tidy-imports.banned-api]
+"pytest.main".msg = "pytest.main requires special handling that is easy to mess up!"
+"unittest".msg = "Use pytest"
+
+[tool.ty.src]
+exclude = ["site_scons/"]
+
+[tool.ty.rules]
+# Cython modules are compiled at build time, not available for static analysis
+unresolved-import = "ignore"
+
+[tool.pytest.ini_options]
+addopts = "--durations=10"
+testpaths = [
+  "msgq/tests",
+  "msgq/visionipc/tests",
+]

artifacts/package_sources/msgq/setup.py

@@ -0,0 +1,19 @@
+import os
+import subprocess
+
+from setuptools import Distribution, setup
+from setuptools.command.build_py import build_py
+
+
+class BinaryDistribution(Distribution):
+  def has_ext_modules(self):
+    return True
+
+
+class BuildPyWithScons(build_py):
+  def run(self):
+    subprocess.check_call(["scons", f"-j{os.cpu_count() or 1}", "--minimal"], cwd=os.path.dirname(os.path.abspath(__file__)))
+    super().run()
+
+
+setup(cmdclass={"build_py": BuildPyWithScons}, distclass=BinaryDistribution)

artifacts/package_sources/msgq/setup.sh

@@ -0,0 +1,51 @@
+#!/usr/bin/env bash
+set -e
+
+DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
+cd $DIR
+
+PLATFORM=$(uname -s)
+
+echo "installing dependencies"
+if [[ $PLATFORM == "Darwin" ]]; then
+  if ! command -v python3 &>/dev/null || ! pkg-config --exists libzmq 2>/dev/null; then
+    export HOMEBREW_NO_AUTO_UPDATE=1
+    brew install python3 zeromq
+  fi
+elif [[ $PLATFORM == "Linux" ]]; then
+  # for AGNOS since we clear the apt lists
+  if [[ ! -d /"var/lib/apt/" ]]; then
+    sudo apt update
+  fi
+
+  sudo apt-get install -y --no-install-recommends \
+    curl ca-certificates \
+    libzmq3-dev \
+    ocl-icd-opencl-dev opencl-headers \
+    python3-dev python3-pip python3-venv
+else
+  echo "WARNING: unsupported platform. skipping apt/brew install."
+fi
+
+# catch2
+if [ ! -d $DIR/msgq/catch2/ ]; then
+  rm -rf /tmp/catch2/ $DIR/msgq/catch2/
+  git clone -b v2.x --depth 1 https://github.com/catchorg/Catch2.git /tmp/catch2
+  pushd /tmp/catch2
+  mv single_include/* $DIR/msgq/
+  popd
+fi
+
+if ! command -v uv &>/dev/null; then
+  echo "'uv' is not installed. Installing 'uv'..."
+  curl -LsSf https://astral.sh/uv/install.sh | sh
+
+  # doesn't require sourcing on all platforms
+  set +e
+  source $HOME/.local/bin/env
+  set -e
+fi
+
+export UV_PROJECT_ENVIRONMENT="$DIR/.venv"
+uv sync --all-extras
+source "$DIR/.venv/bin/activate"

artifacts/package_sources/msgq/site_scons/site_tools/cython.py

@@ -0,0 +1,72 @@
+import re
+import SCons
+from SCons.Action import Action
+from SCons.Scanner import Scanner
+
+pyx_from_import_re = re.compile(r'^from\s+(\S+)\s+cimport', re.M)
+pyx_import_re = re.compile(r'^cimport\s+(\S+)', re.M)
+cdef_import_re = re.compile(r'^cdef extern from\s+.(\S+).:', re.M)
+
+
+def pyx_scan(node, env, path, arg=None):
+  contents = node.get_text_contents()
+
+  # from <module> cimport ...
+  matches = pyx_from_import_re.findall(contents)
+  # cimport <module>
+  matches += pyx_import_re.findall(contents)
+
+  # Modules can be either .pxd or .pyx files
+  files = [m.replace('.', '/') + '.pxd' for m in matches]
+  files += [m.replace('.', '/') + '.pyx' for m in matches]
+
+  # cdef extern from <file>
+  files += cdef_import_re.findall(contents)
+
+  # Handle relative imports
+  cur_dir = str(node.get_dir())
+  files = [cur_dir + f if f.startswith('/') else f for f in files]
+
+  # Filter out non-existing files (probably system imports)
+  files = [f for f in files if env.File(f).exists()]
+  return env.File(files)
+
+
+pyxscanner = Scanner(function=pyx_scan, skeys=['.pyx', '.pxd'], recursive=True)
+cythonAction = Action("$CYTHONCOM")
+
+
+def create_builder(env):
+  try:
+    cython = env['BUILDERS']['Cython']
+  except KeyError:
+    cython = SCons.Builder.Builder(
+      action=cythonAction,
+      emitter={},
+      suffix=cython_suffix_emitter,
+      single_source=1
+    )
+    env.Append(SCANNERS=pyxscanner)
+    env['BUILDERS']['Cython'] = cython
+  return cython
+
+def cython_suffix_emitter(env, source):
+  return "$CYTHONCFILESUFFIX"
+
+def generate(env):
+  env["CYTHON"] = "cythonize"
+  env["CYTHONCOM"] = "$CYTHON $CYTHONFLAGS $SOURCE"
+  env["CYTHONCFILESUFFIX"] = ".cpp"
+
+  c_file, _ = SCons.Tool.createCFileBuilders(env)
+
+  c_file.suffix['.pyx'] = cython_suffix_emitter
+  c_file.add_action('.pyx', cythonAction)
+
+  c_file.suffix['.py'] = cython_suffix_emitter
+  c_file.add_action('.py', cythonAction)
+
+  create_builder(env)
+
+def exists(env):
+  return True

artifacts/package_sources/msgq/test.sh

@@ -0,0 +1,18 @@
+#!/bin/bash
+set -e
+
+DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" >/dev/null && pwd)"
+cd $DIR
+
+source ./setup.sh
+
+# *** build ***
+scons -j8
+
+# *** lint + test ***
+lefthook run test
+
+# *** all done ***
+GREEN='\033[0;32m'
+NC='\033[0m'
+printf "\n${GREEN}All good!${NC} Finished build, lint, and test in ${SECONDS}s\n"

artifacts/package_sources/msgq/third_party/linux/include/linux/ion.h

@@ -0,0 +1,40 @@
+#ifndef _UAPI_LINUX_ION_H
+#define _UAPI_LINUX_ION_H
+
+#include <stddef.h>
+#include <linux/ioctl.h>
+
+typedef int ion_user_handle_t;
+
+#define ION_FLAG_CACHED 1
+
+struct ion_allocation_data {
+  size_t len;
+  size_t align;
+  unsigned int heap_id_mask;
+  unsigned int flags;
+  ion_user_handle_t handle;
+};
+
+struct ion_fd_data {
+  ion_user_handle_t handle;
+  int fd;
+};
+
+struct ion_handle_data {
+  ion_user_handle_t handle;
+};
+
+struct ion_custom_data {
+  unsigned int cmd;
+  unsigned long arg;
+};
+
+#define ION_IOC_MAGIC 'I'
+#define ION_IOC_ALLOC _IOWR(ION_IOC_MAGIC, 0, struct ion_allocation_data)
+#define ION_IOC_FREE _IOWR(ION_IOC_MAGIC, 1, struct ion_handle_data)
+#define ION_IOC_SHARE _IOWR(ION_IOC_MAGIC, 4, struct ion_fd_data)
+#define ION_IOC_IMPORT _IOWR(ION_IOC_MAGIC, 5, struct ion_fd_data)
+#define ION_IOC_CUSTOM _IOWR(ION_IOC_MAGIC, 6, struct ion_custom_data)
+
+#endif

artifacts/package_sources/msgq/third_party/linux/include/msm_ion.h

@@ -0,0 +1,22 @@
+#ifndef _UAPI_MSM_ION_H
+#define _UAPI_MSM_ION_H
+
+#include <linux/ion.h>
+
+#define ION_SYSTEM_HEAP_ID 25
+#define ION_IOMMU_HEAP_ID ION_SYSTEM_HEAP_ID
+
+struct ion_flush_data {
+  ion_user_handle_t handle;
+  int fd;
+  void *vaddr;
+  unsigned int offset;
+  unsigned int length;
+};
+
+#define ION_IOC_MSM_MAGIC 'M'
+#define ION_IOC_CLEAN_CACHES _IOWR(ION_IOC_MSM_MAGIC, 0, struct ion_flush_data)
+#define ION_IOC_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 1, struct ion_flush_data)
+#define ION_IOC_CLEAN_INV_CACHES _IOWR(ION_IOC_MSM_MAGIC, 2, struct ion_flush_data)
+
+#endif