sunnypilot v2026.002.000

version: sunnypilot v2026.002.000 (staging)
date: 2026-07-21T07:44:02
master commit: 2c334ede443d7391d27575af8c854a095ba702a8

msgq_repo/msgq/.gitignore

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

msgq_repo/msgq/__init__.py

@@ -0,0 +1,74 @@
+# must be built with scons
+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
+from msgq.ipc_pyx import MultiplePublishersError, IpcError
+
+from typing import Optional, List, Union
+
+__all__ = [
+  "Context",
+  "Poller",
+  "SubSocket",
+  "PubSocket",
+  "SocketEventHandle",
+  "MultiplePublishersError",
+  "IpcError",
+  "toggle_fake_events",
+  "set_fake_prefix",
+  "get_fake_prefix",
+  "delete_fake_prefix",
+  "wait_for_one_event",
+  "NO_TRAVERSAL_LIMIT",
+  "context",
+  "fake_event_handle",
+  "pub_sock",
+  "sub_sock",
+  "drain_sock_raw",
+]
+
+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

msgq_repo/msgq/event.cc

@@ -0,0 +1,244 @@
+#include <cerrno>
+#include <chrono>
+#include <cstdint>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <string>
+#include <vector>
+
+#include <fcntl.h>
+#include <poll.h>
+#include <sys/mman.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+#include "msgq/event.h"
+
+namespace {
+
+size_t event_fifo_counter = 0;
+
+[[noreturn]] void throw_errno(const std::string& msg) {
+  throw std::runtime_error(msg + ", errno: " + std::to_string(errno) + " pid: " + std::to_string(getpid()));
+}
+
+int open_event_fifo(const char* path) {
+  if (path[0] == '\0') return -1;
+  int fd = open(path, O_RDWR | O_NONBLOCK);
+  if (fd < 0 && errno != ENOENT) throw_errno("Could not open event fifo");
+  return fd;
+}
+
+// poll() that retries on EINTR with a monotonic deadline so signal storms
+// don't extend the effective timeout.
+int poll_events(pollfd *fds, nfds_t nfds, int timeout_sec) {
+  auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(timeout_sec);
+  while (true) {
+    int timeout_ms = -1;
+    if (timeout_sec >= 0) {
+      auto remaining = std::chrono::duration_cast<std::chrono::milliseconds>(
+          deadline - std::chrono::steady_clock::now()).count();
+      timeout_ms = remaining > 0 ? static_cast<int>(remaining) : 0;
+    }
+    int ret = poll(fds, nfds, timeout_ms);
+    if (ret >= 0) return ret;
+    if (errno != EINTR) throw_errno("Event poll failed");
+  }
+}
+
+// macOS limits shm_open names to ~31 chars, so hash the (prefix, identifier, endpoint)
+// tuple into a fixed-length name.
+std::string event_shm_name(const std::string& endpoint, const std::string& identifier) {
+  const char* op_prefix = std::getenv("OPENPILOT_PREFIX");
+  std::string key = std::string(op_prefix ? op_prefix : "") + '\x1f' + identifier + '\x1f' + endpoint;
+
+  uint64_t h = 14695981039346656037ULL;
+  for (char c : key) {
+    h ^= static_cast<uint8_t>(c);
+    h *= 1099511628211ULL;
+  }
+
+  char buf[32];
+  std::snprintf(buf, sizeof(buf), "/msgq_%016llx", static_cast<unsigned long long>(h));
+  return buf;
+}
+
+}  // namespace
+
+void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_name_out) {
+  std::string name = event_shm_name(endpoint, identifier);
+
+  int shm_fd = shm_open(name.c_str(), O_RDWR | O_CREAT, 0664);
+  if (shm_fd < 0) throw_errno("Could not open shared memory");
+
+  // macOS rejects ftruncate on an already-sized shm object, so only size it once.
+  struct stat st;
+  if (fstat(shm_fd, &st) < 0) {
+    close(shm_fd);
+    throw_errno("Could not stat shared memory");
+  }
+  if (st.st_size < static_cast<off_t>(sizeof(EventState))) {
+    if (ftruncate(shm_fd, sizeof(EventState)) < 0) {
+      close(shm_fd);
+      throw_errno("Could not truncate shared memory");
+    }
+  }
+
+  char *mem = reinterpret_cast<char*>(mmap(NULL, sizeof(EventState), PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0));
+  close(shm_fd);
+  if (mem == MAP_FAILED) throw_errno("Could not map shared memory");
+
+  if (shm_mem != nullptr) *shm_mem = mem;
+  if (shm_name_out != nullptr) *shm_name_out = name;
+}
+
+SocketEventHandle::SocketEventHandle(std::string endpoint, std::string identifier, bool override) {
+  char *mem;
+  event_state_shm_mmap(endpoint, identifier, &mem, &this->shm_name);
+
+  this->state = (EventState*)mem;
+  this->owns_fifos = override;
+
+  if (override) {
+    std::string base = "/tmp/msgq_event_" + std::to_string(getpid()) + "_" + std::to_string(event_fifo_counter++);
+    for (size_t i = 0; i < 2; i++) {
+      std::string p = base + "." + std::to_string(i);
+      if (p.size() >= EVENT_PATH_MAX) {
+        throw std::runtime_error("Event path too long: " + p);
+      }
+      unlink(p.c_str());
+      if (mkfifo(p.c_str(), 0664) < 0) throw_errno("Could not create event fifo");
+      std::memcpy(this->state->paths[i], p.c_str(), p.size() + 1);
+    }
+    this->state->enabled = false;
+  }
+
+  for (size_t i = 0; i < 2; i++) {
+    this->fds[i] = open_event_fifo(this->state->paths[i]);
+  }
+}
+
+SocketEventHandle::~SocketEventHandle() {
+  if (this->state == nullptr) return;
+  for (int fd : this->fds) {
+    if (fd >= 0) close(fd);
+  }
+  if (this->owns_fifos) {
+    unlink(this->state->paths[RECV_CALLED]);
+    unlink(this->state->paths[RECV_READY]);
+    shm_unlink(this->shm_name.c_str());
+  }
+  munmap(this->state, sizeof(EventState));
+}
+
+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->fds[RECV_CALLED]);
+}
+
+Event SocketEventHandle::recv_ready() {
+  return Event(this->fds[RECV_READY]);
+}
+
+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");
+  return prefix == nullptr ? "" : std::string(prefix);
+}
+
+Event::Event(int fd): event_fd(fd) {}
+
+void Event::set() const {
+  throw_if_invalid();
+
+  char val = 1;
+  while (true) {
+    ssize_t count = write(this->event_fd, &val, sizeof(val));
+    if (count == sizeof(val)) return;
+    if (errno == EINTR) continue;
+    if (errno == EAGAIN || errno == EWOULDBLOCK) return;
+    throw_errno("Event write failed");
+  }
+}
+
+int Event::clear() const {
+  throw_if_invalid();
+
+  int total = 0;
+  char buf[64];
+  while (true) {
+    ssize_t count = read(this->event_fd, buf, sizeof(buf));
+    if (count > 0) {
+      total += static_cast<int>(count);
+      continue;
+    }
+    if (count == 0 || errno == EAGAIN || errno == EWOULDBLOCK) return total;
+    if (errno == EINTR) continue;
+    throw_errno("Event read failed");
+  }
+}
+
+void Event::wait(int timeout_sec) const {
+  throw_if_invalid();
+
+  pollfd fds = {this->event_fd, POLLIN, 0};
+  if (poll_events(&fds, 1, timeout_sec) == 0) {
+    throw std::runtime_error("Event timed out pid: " + std::to_string(getpid()));
+  }
+}
+
+bool Event::peek() const {
+  throw_if_invalid();
+
+  pollfd fds = {this->event_fd, POLLIN, 0};
+  return poll_events(&fds, 1, 0) > 0;
+}
+
+bool Event::is_valid() const {
+  return event_fd >= 0;
+}
+
+int Event::fd() const {
+  return event_fd;
+}
+
+int Event::wait_for_one(const std::vector<Event>& events, int timeout_sec) {
+  pollfd fds[events.size()];
+  for (size_t i = 0; i < events.size(); i++) {
+    fds[i] = {events[i].fd(), POLLIN, 0};
+  }
+
+  if (poll_events(fds, events.size(), timeout_sec) == 0) {
+    throw std::runtime_error("Event timed out pid: " + std::to_string(getpid()));
+  }
+
+  for (size_t i = 0; i < events.size(); i++) {
+    if (fds[i].revents & POLLIN) {
+      return static_cast<int>(i);
+    }
+  }
+
+  throw std::runtime_error("Event poll failed, no events ready");
+}

msgq_repo/msgq/event.h

@@ -0,0 +1,64 @@
+#pragma once
+
+#include <cstddef>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+#define CEREAL_EVENTS_PREFIX std::string("cereal_events")
+
+constexpr size_t EVENT_PATH_MAX = 128;
+
+void event_state_shm_mmap(std::string endpoint, std::string identifier, char **shm_mem, std::string *shm_name);
+
+enum EventPurpose {
+  RECV_CALLED,
+  RECV_READY
+};
+
+struct EventState {
+  char paths[2][EVENT_PATH_MAX];
+  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_name;
+  EventState* state = nullptr;
+  bool owns_fifos = false;
+  int fds[2] = {-1, -1};
+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();
+};

msgq_repo/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;
+}

msgq_repo/msgq/impl_fake.h

@@ -0,0 +1,75 @@
+#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:
+  EventState *state = nullptr;
+  int fds[2] = {-1, -1};
+
+  void ensure_fifos_open() {
+    for (size_t i = 0; i < 2; i++) {
+      if (fds[i] < 0 && state->paths[i][0] != '\0') {
+        fds[i] = open(state->paths[i], O_RDWR | O_NONBLOCK);
+      }
+    }
+  }
+
+public:
+  FakeSubSocket(): TSubSocket() {}
+  ~FakeSubSocket() {
+    for (int fd : fds) {
+      if (fd >= 0) close(fd);
+    }
+    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;
+    ensure_fifos_open();
+
+    return TSubSocket::connect(context, endpoint, address, conflate, check_endpoint, segment_size);
+  }
+
+  Message *receive(bool non_blocking=false) override {
+    if (this->state->enabled) {
+      ensure_fifos_open();
+      Event(fds[EventPurpose::RECV_CALLED]).set();
+      Event(fds[EventPurpose::RECV_READY]).wait();
+      Event(fds[EventPurpose::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() {}
+};

msgq_repo/msgq/impl_msgq.cc

@@ -0,0 +1,169 @@
+#include <cassert>
+#include <cstring>
+#include <iostream>
+#include <cstdlib>
+#include <cerrno>
+#include <string>
+#include <vector>
+
+#include "msgq/impl_msgq.h"
+
+void Message::init(size_t sz) {
+  size = sz;
+  data = new char[size];
+}
+
+void Message::init(char * d, size_t sz) {
+  size = sz;
+  data = new char[size];
+  memcpy(data, d, size);
+}
+
+void Message::takeOwnership(char * d, size_t sz) {
+  size = sz;
+  data = d;
+}
+
+void Message::close() {
+  if (size > 0){
+    delete[] data;
+  }
+  size = 0;
+}
+
+Message::~Message() {
+  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;
+  }
+
+  msgq_init_subscriber(q);
+
+  if (conflate){
+    q->read_conflate = true;
+  }
+
+  timeout = -1;
+
+  return 0;
+}
+
+
+Message * MSGQSubSocket::receive(bool non_blocking){
+  msgq_msg_t msg;
+
+  Message *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 Message;
+    r->takeOwnership(msg.data, msg.size);
+  }
+
+  return r;
+}
+
+void MSGQSubSocket::setTimeout(int t){
+  timeout = t;
+}
+
+MSGQSubSocket::~MSGQSubSocket(){
+  if (q != NULL){
+    msgq_close_queue(q);
+    delete q;
+  }
+}
+
+int PubSocket::connect(Context *context, std::string endpoint, bool check_endpoint, size_t segment_size){
+  assert(context);
+
+  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 PubSocket::sendMessage(Message *message){
+  msgq_msg_t msg;
+  msg.data = message->getData();
+  msg.size = message->getSize();
+
+  return msgq_msg_send(&msg, q);
+}
+
+int PubSocket::send(char *data, size_t size){
+  msgq_msg_t msg;
+  msg.data = data;
+  msg.size = size;
+
+  return msgq_msg_send(&msg, q);
+}
+
+bool PubSocket::all_readers_updated() {
+  return msgq_all_readers_updated(q);
+}
+
+PubSocket::~PubSocket(){
+  if (q != NULL){
+    msgq_close_queue(q);
+    delete q;
+  }
+}
+
+
+void MSGQPoller::registerSocket(SubSocket * socket){
+  assert(num_polls + 1 < MAX_POLLERS);
+  polls[num_polls].q = static_cast<MSGQSubSocket*>(socket)->getQueue();
+
+  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;
+}

msgq_repo/msgq/impl_msgq.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include <string>
+#include <vector>
+
+#include "msgq/ipc.h"
+#include "msgq/msgq.h"
+
+#define MAX_POLLERS 128
+
+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);
+  msgq_queue_t * getQueue() {return q;}
+  Message *receive(bool non_blocking=false);
+  ~MSGQSubSocket();
+};
+
+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(){}
+};

msgq_repo/msgq/ipc.cc

@@ -0,0 +1,71 @@
+#include <cassert>
+#include <iostream>
+#include <string>
+
+#include "msgq/ipc.h"
+#include "msgq/impl_msgq.h"
+#include "msgq/impl_fake.h"
+
+bool messaging_use_fake(){
+  char* fake_enabled = std::getenv("CEREAL_FAKE");
+  return fake_enabled != NULL;
+}
+
+Context * Context::create(){
+  return new Context();
+}
+
+SubSocket * SubSocket::create(){
+  if (messaging_use_fake()) {
+    return new FakeSubSocket<MSGQSubSocket>();
+  }
+  return new MSGQSubSocket();
+}
+
+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(){
+  return new PubSocket();
+}
+
+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(){
+  if (messaging_use_fake()) {
+    return new FakePoller();
+  }
+  return new MSGQPoller();
+}
+
+Poller * Poller::create(std::vector<SubSocket*> sockets){
+  Poller * p = Poller::create();
+
+  for (auto s : sockets){
+    p->registerSocket(s);
+  }
+  return p;
+}

msgq_repo/msgq/ipc.h

@@ -0,0 +1,69 @@
+#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
+
+class Context {
+public:
+  static Context * create();
+  ~Context(){}
+};
+
+class Message {
+private:
+  char * data = nullptr;
+  size_t size = 0;
+public:
+  void init(size_t size);
+  void init(char * data, size_t size);
+  void takeOwnership(char * data, size_t size);
+  void close();
+  size_t getSize(){return size;}
+  char * getData(){return data;}
+  ~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;
+  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 {
+private:
+  struct msgq_queue_t * q = nullptr;
+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();
+  static PubSocket * create();
+  static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true, size_t segment_size=0);
+  ~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(){}
+};

msgq_repo/msgq/ipc.pxd

@@ -0,0 +1,68 @@
+# 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":
+  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

msgq_repo/msgq/ipc_pyx.pyx

@@ -0,0 +1,256 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+import time
+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 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
+
+
+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;
+
+  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()
+
+  def wait_for_readers(self, double timeout=1.0, double interval=0.001):
+    deadline = time.monotonic() + timeout
+    while time.monotonic() < deadline:
+      if self.all_readers_updated():
+        return
+      time.sleep(interval)
+    raise TimeoutError("subscriber did not connect")

msgq_repo/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

msgq_repo/msgq/msgq.cc

@@ -0,0 +1,504 @@
+#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
+  }
+}
+
+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){
+    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->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) {
+  #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
+}
+
+void 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;
+
+    // No more slots available. Reset all subscribers to kick out inactive ones
+    if (new_num_readers > NUM_READERS){
+      //std::cout << "Warning, evicting all subscribers!" << std::endl;
+      *q->num_readers = 0;
+
+      for (size_t i = 0; i < NUM_READERS; i++){
+        *q->read_valids[i] = false;
+
+        uint64_t old_uid = *q->read_uids[i];
+        *q->read_uids[i] = 0;
+
+        // Wake up reader in case they are in a poll
+        thread_signal(old_uid & 0xFFFFFFFF);
+      }
+
+      continue;
+    }
+
+    // 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);
+}
+
+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);
+
+  // 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]){
+    //std::cout << q->endpoint << ": Reader was evicted, reconnecting" << std::endl;
+    msgq_init_subscriber(q);
+    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]){
+    //std::cout << q->endpoint << ": Reader was evicted, reconnecting" << std::endl;
+    msgq_init_subscriber(q);
+    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;
+}

msgq_repo/msgq/msgq.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <cstdint>
+#include <cstring>
+#include <string>
+#include <atomic>
+
+#define DEFAULT_SEGMENT_SIZE (1 * 1024 * 1024)
+#define NUM_READERS 25
+#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;
+
+  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);
+void msgq_init_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);

msgq_repo/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);
+  }
+}

msgq_repo/msgq/test_runner.cc

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

msgq_repo/msgq/tests/test_fake.py

@@ -0,0 +1,204 @@
+import multiprocessing
+import unittest
+import msgq
+from parameterized import parameterized_class
+from typing import Optional
+
+WAIT_TIMEOUT = 5
+
+
+def set_event_run(endpoint, identifier):
+  handle = msgq.fake_event_handle(endpoint, identifier=identifier, override=False)
+  handle.recv_called_event.set()
+
+
+def daemon_repub_process_run(prefix: Optional[str] = None):
+  # Re-apply fake config in the child. Python 3.14+ defaults to forkserver on
+  # Linux, which does not inherit setenv() calls from the parent.
+  msgq.toggle_fake_events(True)
+  if prefix is not None:
+    msgq.set_fake_prefix(prefix)
+  else:
+    msgq.delete_fake_prefix()
+
+  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)
+
+
+class TestEvents(unittest.TestCase):
+
+  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:
+      self.fail("event.wait() timed out")
+
+  def test_wait_multiprocess(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    p = multiprocessing.Process(target=set_event_run, args=("carState", ""))
+    p.start()
+    try:
+      event.wait(WAIT_TIMEOUT)
+      assert event.peek()
+    except RuntimeError:
+      self.fail("event.wait() timed out")
+    finally:
+      p.kill()
+      p.join()
+
+  def test_wait_zero_timeout(self):
+    handle = msgq.fake_event_handle("carState")
+    event = handle.recv_called_event
+
+    try:
+      event.wait(0)
+      self.fail("event.wait() did not time out")
+    except RuntimeError:
+      assert not event.peek()
+
+  def test_wait_for_one(self):
+    handles = [msgq.fake_event_handle(s) for s in ("carState", "controlsState")]
+    handles[1].recv_called_event.set()
+    assert msgq.wait_for_one_event([h.recv_called_event for h in handles], WAIT_TIMEOUT) == 1
+
+
+@parameterized_class([{"prefix": None}, {"prefix": "test"}])
+class TestFakeSockets(unittest.TestCase):
+  prefix: Optional[str] = None
+
+  def setUp(self):
+    super().setUp()
+    msgq.toggle_fake_events(True)
+    if self.prefix is not None:
+      msgq.set_fake_prefix(self.prefix)
+    else:
+      msgq.delete_fake_prefix()
+
+  def tearDown(self):
+    msgq.toggle_fake_events(False)
+    msgq.delete_fake_prefix()
+    super().tearDown()
+
+  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 no event state
+    _ = msgq.pub_sock("ubloxGnss")
+
+    handle = msgq.fake_event_handle("ubloxGnss", override=False)
+
+    assert not handle.enabled
+    assert handle.recv_called_event.fd == -1
+    assert handle.recv_ready_event.fd == -1
+
+  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:
+      self.fail("event.wait() timed out")
+
+  def test_synced_pub_sub(self):
+    carState_handle = msgq.fake_event_handle("carState", enable=True)
+    recv_called = carState_handle.recv_called_event
+    recv_ready = carState_handle.recv_ready_event
+
+    pub_sock = msgq.pub_sock("carState")
+
+    p = multiprocessing.Process(target=daemon_repub_process_run, args=(self.prefix,))
+    p.start()
+
+    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:
+      self.fail("event.wait() timed out")
+    finally:
+      p.kill()
+      p.join()

msgq_repo/msgq/tests/test_messaging.py

@@ -0,0 +1,57 @@
+import random
+import time
+import string
+import unittest
+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)])
+
+class TestPubSubSockets(unittest.TestCase):
+
+  def test_pub_sub(self):
+    sock = random_sock()
+    pub_sock = msgq.pub_sock(sock)
+    sub_sock = msgq.sub_sock(sock, conflate=False, timeout=None)
+    for _ in range(100):
+      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)
+
+      sent_msgs = []
+      for __ in range(num_msgs):
+        msg = random_bytes()
+        pub_sock.send(msg)
+        sent_msgs.append(msg)
+      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_ms = 5
+    sub_sock = msgq.sub_sock(sock, timeout=timeout_ms)
+
+    start_time = time.monotonic()
+    recvd = sub_sock.receive()
+    elapsed = time.monotonic() - start_time
+    assert recvd is None
+    assert elapsed >= timeout_ms / 1000
+    assert elapsed < 5  # this can be noisy due to other load on the system

msgq_repo/msgq/tests/test_poller.py

@@ -0,0 +1,135 @@
+import unittest
+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(unittest.TestCase):
+  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)
+
+      pub.wait_for_readers()
+
+      # 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)
+
+      pub.wait_for_readers()
+      c = msgq.Context()
+      for _ in range(10):
+        msgq.SubSocket().connect(c, SERVICE_NAME)
+
+      # 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 self.assertRaises(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)
+
+    pub.wait_for_readers()
+
+    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:
+        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)
+
+    pub.wait_for_readers()
+    pub.send(b'a')
+    pub.send(b'b')
+
+    assert b'b' == sub.receive()
+
+    del pub
+    del sub
+    context.term()

msgq_repo/msgq/visionipc/.gitignore

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

msgq_repo/msgq/visionipc/__init__.py

@@ -0,0 +1,9 @@
+from msgq.visionipc.visionipc_pyx import VisionBuf, VisionIpcClient, VisionIpcServer, VisionStreamType, get_endpoint_name
+
+__all__ = [
+  "VisionBuf",
+  "VisionIpcClient",
+  "VisionIpcServer",
+  "VisionStreamType",
+  "get_endpoint_name",
+]

msgq_repo/msgq/visionipc/tests/test_visionipc.py

@@ -0,0 +1,103 @@
+import struct
+import unittest
+from typing import Optional
+from msgq.visionipc import VisionIpcServer, VisionIpcClient, VisionStreamType
+
+
+class TestVisionIpc(unittest.TestCase):
+  server: Optional[VisionIpcServer]
+  client: Optional[VisionIpcClient]
+
+  def setUp(self):
+    self.server = None
+    self.client = None
+
+  def tearDown(self):
+    self.client = None
+    self.server = None
+
+  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
+
+    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()
+
+  def test_available_streams(self):
+    stream_types = (VisionStreamType.VISION_STREAM_ROAD, VisionStreamType.VISION_STREAM_WIDE_ROAD)
+    self.setup_vipc("camerad", *stream_types)
+    available_streams = VisionIpcClient.available_streams("camerad", True)
+    assert available_streams == {stream.value for stream in stream_types}
+
+  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
+
+  def test_send_single_buffer(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD)
+    assert self.server is not None
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = bytearray(self.client.buffer_len)
+    struct.pack_into("<Q", buf, 0, 1234)
+    self.server.send(VisionStreamType.VISION_STREAM_ROAD, buf, frame_id=1337)
+
+    recv_buf = self.client.recv()
+    assert recv_buf is not None
+    data = recv_buf.data
+    assert isinstance(data, memoryview)
+    assert struct.unpack_from("<Q", data, 0)[0] == 1234
+    assert len(data) == self.client.buffer_len
+    assert data[8:].nbytes == self.client.buffer_len - 8
+    assert self.client.frame_id == 1337
+    assert recv_buf.frame_id == 1337
+
+  def test_no_conflate(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD)
+    assert self.server is not None
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = bytearray(self.client.buffer_len)
+    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
+
+  def test_conflate(self):
+    self.setup_vipc("camerad", VisionStreamType.VISION_STREAM_ROAD, conflate=True)
+    assert self.server is not None
+    assert self.client is not None
+    assert self.client.buffer_len is not None
+    buf = bytearray(self.client.buffer_len)
+    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(timeout_ms=5)
+    assert recv_buf is None

msgq_repo/msgq/visionipc/visionbuf.cc

@@ -0,0 +1,79 @@
+#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);
+
+  int ret = ftruncate(*fd, len);
+  assert(ret == 0);
+  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::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);
+}
+
+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;
+}
+
+int VisionBuf::sync(int dir) {
+  return 0;
+}
+
+int VisionBuf::free() {
+  int err = munmap(this->addr, this->mmap_len);
+  if (err != 0) return err;
+
+  err = close(this->fd);
+  return err;
+}
+
+uint64_t VisionBuf::get_frame_id() {
+  return *frame_id;
+}
+
+void VisionBuf::set_frame_id(uint64_t id) {
+  *frame_id = id;
+}

msgq_repo/msgq/visionipc/visionbuf.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include "msgq/visionipc/visionipc.h"
+
+#define VISIONBUF_SYNC_FROM_DEVICE 0
+#define VISIONBUF_SYNC_TO_DEVICE 1
+
+enum VisionStreamType {
+  VISION_STREAM_ROAD,
+  VISION_STREAM_DRIVER,
+  VISION_STREAM_WIDE_ROAD,
+
+  VISION_STREAM_MAP,
+  VISION_STREAM_MAX,
+};
+
+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;
+
+  // ion
+  int handle = 0;
+
+  void allocate(size_t len);
+  void import();
+  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();
+};

msgq_repo/msgq/visionipc/visionbuf_ion.cc

@@ -0,0 +1,140 @@
+#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 <linux/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;                                                      \
+  })
+
+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 + 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);
+}
+
+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);
+}
+
+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;
+}
+
+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 = 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));
+}
+
+uint64_t VisionBuf::get_frame_id() {
+  return *frame_id;
+}
+
+void VisionBuf::set_frame_id(uint64_t id) {
+  *frame_id = id;
+}

msgq_repo/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;
+  }
+}

msgq_repo/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;
+};

msgq_repo/msgq/visionipc/visionipc.pxd

@@ -0,0 +1,53 @@
+# 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":
+  cdef enum VisionStreamType:
+    pass
+
+  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
+    void set_frame_id(uint64_t id)
+    uint64_t get_frame_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 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)
+    VisionBuf * recv(VisionIpcBufExtra *, int) nogil
+    bool connect(bool)
+    bool is_connected()
+    @staticmethod
+    set[VisionStreamType] getAvailableStreams(string, bool)

msgq_repo/msgq/visionipc/visionipc_client.cc

@@ -0,0 +1,162 @@
+#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());
+  bool logged_retry = false;
+  while (socket_fd < 0 && blocking) {
+    if (!logged_retry) {
+      std::cout << "VisionIpcClient connecting" << std::endl;
+      logged_retry = true;
+    }
+    std::this_thread::sleep_for(std::chrono::milliseconds(20));
+    socket_fd = ipc_connect(ipc_path.c_str());
+  }
+  return socket_fd;
+}
+
+VisionIpcClient::VisionIpcClient(std::string name_, VisionStreamType type_, bool conflate) : name(name_), type(type_) {
+  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);
+  }
+
+  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;
+}

msgq_repo/msgq/visionipc/visionipc_client.h

@@ -0,0 +1,28 @@
+#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;
+
+public:
+  bool connected = false;
+  VisionStreamType type;
+  int num_buffers = 0;
+  VisionBuf buffers[VISIONIPC_MAX_FDS];
+  VisionIpcClient(std::string name, VisionStreamType type, bool conflate);
+  ~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);
+};

msgq_repo/msgq/visionipc/visionipc_pyx.pxd

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

msgq_repo/msgq/visionipc/visionipc_pyx.pyx

@@ -0,0 +1,170 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+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 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')
+
+
+cpdef enum VisionStreamType:
+  VISION_STREAM_ROAD
+  VISION_STREAM_DRIVER
+  VISION_STREAM_WIDE_ROAD
+  VISION_STREAM_MAP
+
+
+cdef class VisionBuf:
+  @staticmethod
+  cdef create(cppVisionBuf * cbuf):
+    buf = VisionBuf()
+    buf.buf = cbuf
+    return buf
+
+  @property
+  def data(self):
+    cdef unsigned char[:] data = <unsigned char[:self.buf.len]> self.buf.addr
+    return memoryview(data)
+
+  @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
+
+  @property
+  def frame_id(self):
+    return self.buf.get_frame_id()
+
+
+cdef class VisionIpcServer:
+  cdef cppVisionIpcServer * server
+
+  def __init__(self, string name):
+    self.server = new cppVisionIpcServer(name)
+
+  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
+    extra.valid = False
+
+    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):
+    self.client = new cppVisionIpcClient(name, stream, conflate)
+
+  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):
+    cdef cppVisionBuf * buf
+    # release the GIL, this can block for timeout_ms
+    with nogil:
+      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)

msgq_repo/msgq/visionipc/visionipc_server.cc

@@ -0,0 +1,210 @@
+#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/visionipc/visionipc.h"
+#include "msgq/visionipc/visionipc_server.h"
+#include "msgq/logger/logger.h"
+
+std::string get_endpoint_name(std::string name, VisionStreamType type){
+  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_) : name(name_) {
+  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 = width * height * 3 / 2;
+  size_t stride = width;
+  size_t 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;
+
+    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
+  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 = VisionStreamType::VISION_STREAM_MAX;
+    int r = ipc_sendrecv_with_fds(false, fd, &type, sizeof(type), nullptr, 0, nullptr);
+    if (r != sizeof(type)) {
+      close(fd);
+      if (should_exit) break;
+      continue;
+    }
+
+    // send available stream types
+    if (type == VisionStreamType::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 ion metadata
+      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;
+  if (listener_thread.joinable()) {
+    int sock = ipc_connect(get_ipc_path(name).c_str());
+    if (sock >= 0) {
+      close(sock);
+    }
+    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;
+}

msgq_repo/msgq/visionipc/visionipc_server.h

@@ -0,0 +1,40 @@
+#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:
+  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);
+  ~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();
+};