openpilot v0.9.6 release

date: 2024-01-12T10:13:37
master commit: ba792d576a49a0899b88a753fa1c52956bedf9e6

cereal/messaging/.gitignore

@@ -0,0 +1,10 @@
+demo
+bridge
+test_runner
+*.o
+*.os
+*.d
+*.a
+*.so
+messaging_pyx.cpp
+build/

cereal/messaging/__init__.py

@@ -0,0 +1,303 @@
+# must be built with scons
+from .messaging_pyx import Context, Poller, SubSocket, PubSocket, SocketEventHandle, toggle_fake_events, \
+                                set_fake_prefix, get_fake_prefix, delete_fake_prefix, wait_for_one_event
+from .messaging_pyx import MultiplePublishersError, MessagingError
+
+import os
+import capnp
+import time
+
+from typing import Optional, List, Union, Dict, Deque
+from collections import deque
+
+from cereal import log
+from cereal.services import SERVICE_LIST
+
+assert MultiplePublishersError
+assert MessagingError
+assert toggle_fake_events
+assert set_fake_prefix
+assert get_fake_prefix
+assert delete_fake_prefix
+assert wait_for_one_event
+
+NO_TRAVERSAL_LIMIT = 2**64-1
+AVG_FREQ_HISTORY = 100
+
+context = Context()
+
+
+def fake_event_handle(endpoint: str, identifier: Optional[str] = None, override: bool = True, enable: bool = False) -> SocketEventHandle:
+  identifier = identifier or get_fake_prefix()
+  handle = SocketEventHandle(endpoint, identifier, override)
+  if override:
+    handle.enabled = enable
+
+  return handle
+
+
+def log_from_bytes(dat: bytes) -> capnp.lib.capnp._DynamicStructReader:
+  with log.Event.from_bytes(dat, traversal_limit_in_words=NO_TRAVERSAL_LIMIT) as msg:
+    return msg
+
+
+def new_message(service: Optional[str], size: Optional[int] = None, **kwargs) -> capnp.lib.capnp._DynamicStructBuilder:
+  args = {
+    'valid': False,
+    'logMonoTime': int(time.monotonic() * 1e9),
+    **kwargs
+  }
+  dat = log.Event.new_message(**args)
+  if service is not None:
+    if size is None:
+      dat.init(service)
+    else:
+      dat.init(service, size)
+  return dat
+
+
+def pub_sock(endpoint: str) -> PubSocket:
+  sock = PubSocket()
+  sock.connect(context, endpoint)
+  return sock
+
+
+def sub_sock(endpoint: str, poller: Optional[Poller] = None, addr: str = "127.0.0.1",
+             conflate: bool = False, timeout: Optional[int] = None) -> SubSocket:
+  sock = SubSocket()
+  sock.connect(context, endpoint, addr.encode('utf8'), conflate)
+
+  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
+
+
+def drain_sock(sock: SubSocket, wait_for_one: bool = False) -> List[capnp.lib.capnp._DynamicStructReader]:
+  """Receive all message currently available on the queue"""
+  ret: List[capnp.lib.capnp._DynamicStructReader] = []
+  while 1:
+    if wait_for_one and len(ret) == 0:
+      dat = sock.receive()
+    else:
+      dat = sock.receive(non_blocking=True)
+
+    if dat is None:  # Timeout hit
+      break
+
+    dat = log_from_bytes(dat)
+    ret.append(dat)
+
+  return ret
+
+
+# TODO: print when we drop packets?
+def recv_sock(sock: SubSocket, wait: bool = False) -> Optional[capnp.lib.capnp._DynamicStructReader]:
+  """Same as drain sock, but only returns latest message. Consider using conflate instead."""
+  dat = None
+
+  while 1:
+    if wait and dat is None:
+      rcv = sock.receive()
+    else:
+      rcv = sock.receive(non_blocking=True)
+
+    if rcv is None:  # Timeout hit
+      break
+
+    dat = rcv
+
+  if dat is not None:
+    dat = log_from_bytes(dat)
+
+  return dat
+
+
+def recv_one(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]:
+  dat = sock.receive()
+  if dat is not None:
+    dat = log_from_bytes(dat)
+  return dat
+
+
+def recv_one_or_none(sock: SubSocket) -> Optional[capnp.lib.capnp._DynamicStructReader]:
+  dat = sock.receive(non_blocking=True)
+  if dat is not None:
+    dat = log_from_bytes(dat)
+  return dat
+
+
+def recv_one_retry(sock: SubSocket) -> capnp.lib.capnp._DynamicStructReader:
+  """Keep receiving until we get a message"""
+  while True:
+    dat = sock.receive()
+    if dat is not None:
+      return log_from_bytes(dat)
+
+
+class SubMaster:
+  def __init__(self, services: List[str], poll: Optional[List[str]] = None,
+               ignore_alive: Optional[List[str]] = None, ignore_avg_freq: Optional[List[str]] = None,
+               ignore_valid: Optional[List[str]] = None, addr: str = "127.0.0.1"):
+    self.frame = -1
+    self.updated = {s: False for s in services}
+    self.rcv_time = {s: 0. for s in services}
+    self.rcv_frame = {s: 0 for s in services}
+    self.alive = {s: False for s in services}
+    self.freq_ok = {s: False for s in services}
+    self.recv_dts: Dict[str, Deque[float]] = {s: deque(maxlen=AVG_FREQ_HISTORY) for s in services}
+    self.sock = {}
+    self.freq = {}
+    self.data = {}
+    self.valid = {}
+    self.logMonoTime = {}
+
+    self.poller = Poller()
+    self.non_polled_services = [s for s in services if poll is not None and
+                                len(poll) and s not in poll]
+
+    self.ignore_average_freq = [] if ignore_avg_freq is None else ignore_avg_freq
+    self.ignore_alive = [] if ignore_alive is None else ignore_alive
+    self.ignore_valid = [] if ignore_valid is None else ignore_valid
+    self.simulation = bool(int(os.getenv("SIMULATION", "0")))
+
+    for s in services:
+      if addr is not None:
+        p = self.poller if s not in self.non_polled_services else None
+        self.sock[s] = sub_sock(s, poller=p, addr=addr, conflate=True)
+      self.freq[s] = SERVICE_LIST[s].frequency
+
+      try:
+        data = new_message(s)
+      except capnp.lib.capnp.KjException:  # pylint: disable=c-extension-no-member
+        data = new_message(s, 0) # lists
+
+      self.data[s] = getattr(data.as_reader(), s)
+      self.logMonoTime[s] = 0
+      # TODO: this should default to False
+      self.valid[s] = True
+
+  def __getitem__(self, s: str) -> capnp.lib.capnp._DynamicStructReader:
+    return self.data[s]
+
+  def _check_avg_freq(self, s):
+    return self.rcv_time[s] > 1e-5 and self.freq[s] > 1e-5 and (s not in self.non_polled_services) \
+            and (s not in self.ignore_average_freq)
+
+  def update(self, timeout: int = 1000) -> None:
+    msgs = []
+    for sock in self.poller.poll(timeout):
+      msgs.append(recv_one_or_none(sock))
+
+    # non-blocking receive for non-polled sockets
+    for s in self.non_polled_services:
+      msgs.append(recv_one_or_none(self.sock[s]))
+    self.update_msgs(time.monotonic(), msgs)
+
+  def update_msgs(self, cur_time: float, msgs: List[capnp.lib.capnp._DynamicStructReader]) -> None:
+    self.frame += 1
+    self.updated = dict.fromkeys(self.updated, False)
+    for msg in msgs:
+      if msg is None:
+        continue
+
+      s = msg.which()
+      self.updated[s] = True
+
+      if self._check_avg_freq(s):
+        self.recv_dts[s].append(cur_time - self.rcv_time[s])
+
+      self.rcv_time[s] = cur_time
+      self.rcv_frame[s] = self.frame
+      self.data[s] = getattr(msg, s)
+      self.logMonoTime[s] = msg.logMonoTime
+      self.valid[s] = msg.valid
+
+      if self.simulation:
+        self.freq_ok[s] = True
+        self.alive[s] = True
+
+    if not self.simulation:
+      for s in self.data:
+        # arbitrary small number to avoid float comparison. If freq is 0, we can skip the check
+        if self.freq[s] > 1e-5:
+          # alive if delay is within 10x the expected frequency
+          self.alive[s] = (cur_time - self.rcv_time[s]) < (10. / self.freq[s])
+
+          # TODO: check if update frequency is high enough to not drop messages
+          # freq_ok if average frequency is higher than 90% of expected frequency
+          if self._check_avg_freq(s):
+            if len(self.recv_dts[s]) > 0:
+              avg_dt = sum(self.recv_dts[s]) / len(self.recv_dts[s])
+              expected_dt = 1 / (self.freq[s] * 0.90)
+              self.freq_ok[s] = (avg_dt < expected_dt)
+            else:
+              self.freq_ok[s] = False
+          else:
+            self.freq_ok[s] = True
+        else:
+          self.freq_ok[s] = True
+          self.alive[s] = True
+
+  def all_alive(self, service_list=None) -> bool:
+    if service_list is None:  # check all
+      service_list = self.alive.keys()
+    return all(self.alive[s] for s in service_list if s not in self.ignore_alive)
+
+  def all_freq_ok(self, service_list=None) -> bool:
+    if service_list is None:  # check all
+      service_list = self.alive.keys()
+    return all(self.freq_ok[s] for s in service_list if s not in self.ignore_alive)
+
+  def all_valid(self, service_list=None) -> bool:
+    if service_list is None:  # check all
+      service_list = self.valid.keys()
+    return all(self.valid[s] for s in service_list if s not in self.ignore_valid)
+
+  def all_checks(self, service_list=None) -> bool:
+    if service_list is None:  # check all
+      service_list = self.alive.keys()
+    return self.all_alive(service_list=service_list) \
+           and self.all_freq_ok(service_list=service_list) \
+           and self.all_valid(service_list=service_list)
+
+
+class PubMaster:
+  def __init__(self, services: List[str]):
+    self.sock = {}
+    for s in services:
+      self.sock[s] = pub_sock(s)
+
+  def send(self, s: str, dat: Union[bytes, capnp.lib.capnp._DynamicStructBuilder]) -> None:
+    if not isinstance(dat, bytes):
+      dat = dat.to_bytes()
+    self.sock[s].send(dat)
+
+  def wait_for_readers_to_update(self, s: str, timeout: int, dt: float = 0.05) -> bool:
+    for _ in range(int(timeout*(1./dt))):
+      if self.sock[s].all_readers_updated():
+        return True
+      time.sleep(dt)
+    return False
+
+  def all_readers_updated(self, s: str) -> bool:
+    return self.sock[s].all_readers_updated()  # type: ignore

cereal/messaging/bridge.cc

@@ -0,0 +1,92 @@
+#include <algorithm>
+#include <cassert>
+#include <csignal>
+#include <iostream>
+#include <map>
+#include <string>
+
+typedef void (*sighandler_t)(int sig);
+
+#include "cereal/services.h"
+#include "cereal/messaging/impl_msgq.h"
+#include "cereal/messaging/impl_zmq.h"
+
+std::atomic<bool> do_exit = false;
+static void set_do_exit(int sig) {
+  do_exit = true;
+}
+
+void sigpipe_handler(int sig) {
+  assert(sig == SIGPIPE);
+  std::cout << "SIGPIPE received" << std::endl;
+}
+
+static std::vector<std::string> get_services(std::string whitelist_str, bool zmq_to_msgq) {
+  std::vector<std::string> service_list;
+  for (const auto& it : services) {
+    std::string name = it.second.name;
+    bool in_whitelist = whitelist_str.find(name) != std::string::npos;
+    if (name == "plusFrame" || name == "uiLayoutState" || (zmq_to_msgq && !in_whitelist)) {
+      continue;
+    }
+    service_list.push_back(name);
+  }
+  return service_list;
+}
+
+int main(int argc, char** argv) {
+  signal(SIGPIPE, (sighandler_t)sigpipe_handler);
+  signal(SIGINT, (sighandler_t)set_do_exit);
+  signal(SIGTERM, (sighandler_t)set_do_exit);
+
+  bool zmq_to_msgq = argc > 2;
+  std::string ip = zmq_to_msgq ? argv[1] : "127.0.0.1";
+  std::string whitelist_str = zmq_to_msgq ? std::string(argv[2]) : "";
+
+  Poller *poller;
+  Context *pub_context;
+  Context *sub_context;
+  if (zmq_to_msgq) {  // republishes zmq debugging messages as msgq
+    poller = new ZMQPoller();
+    pub_context = new MSGQContext();
+    sub_context = new ZMQContext();
+  } else {
+    poller = new MSGQPoller();
+    pub_context = new ZMQContext();
+    sub_context = new MSGQContext();
+  }
+
+  std::map<SubSocket*, PubSocket*> sub2pub;
+  for (auto endpoint : get_services(whitelist_str, zmq_to_msgq)) {
+    PubSocket * pub_sock;
+    SubSocket * sub_sock;
+    if (zmq_to_msgq) {
+      pub_sock = new MSGQPubSocket();
+      sub_sock = new ZMQSubSocket();
+    } else {
+      pub_sock = new ZMQPubSocket();
+      sub_sock = new MSGQSubSocket();
+    }
+    pub_sock->connect(pub_context, endpoint);
+    sub_sock->connect(sub_context, endpoint, ip, false);
+
+    poller->registerSocket(sub_sock);
+    sub2pub[sub_sock] = pub_sock;
+  }
+
+  while (!do_exit) {
+    for (auto sub_sock : poller->poll(100)) {
+      Message * msg = sub_sock->receive();
+      if (msg == NULL) continue;
+      int ret;
+      do {
+        ret = sub2pub[sub_sock]->sendMessage(msg);
+      } while (ret == -1 && errno == EINTR && !do_exit);
+      assert(ret >= 0 || do_exit);
+      delete msg;
+
+      if (do_exit) break;
+    }
+  }
+  return 0;
+}

cereal/messaging/event.cc

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

cereal/messaging/event.h

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

cereal/messaging/impl_fake.cc

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

cereal/messaging/impl_fake.h

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

cereal/messaging/impl_msgq.cc

@@ -0,0 +1,215 @@
+#include <cassert>
+#include <cstring>
+#include <iostream>
+#include <cstdlib>
+#include <csignal>
+#include <cerrno>
+
+#include "cereal/services.h"
+#include "cereal/messaging/impl_msgq.h"
+
+
+volatile sig_atomic_t msgq_do_exit = 0;
+
+void sig_handler(int signal) {
+  assert(signal == SIGINT || signal == SIGTERM);
+  msgq_do_exit = 1;
+}
+
+static bool service_exists(std::string path){
+  return services.count(path) > 0;
+}
+
+
+MSGQContext::MSGQContext() {
+}
+
+MSGQContext::~MSGQContext() {
+}
+
+void MSGQMessage::init(size_t sz) {
+  size = sz;
+  data = new char[size];
+}
+
+void MSGQMessage::init(char * d, size_t sz) {
+  size = sz;
+  data = new char[size];
+  memcpy(data, d, size);
+}
+
+void MSGQMessage::takeOwnership(char * d, size_t sz) {
+  size = sz;
+  data = d;
+}
+
+void MSGQMessage::close() {
+  if (size > 0){
+    delete[] data;
+  }
+  size = 0;
+}
+
+MSGQMessage::~MSGQMessage() {
+  this->close();
+}
+
+int MSGQSubSocket::connect(Context *context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){
+  assert(context);
+  assert(address == "127.0.0.1");
+
+  if (check_endpoint && !service_exists(std::string(endpoint))){
+    std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl;
+  }
+
+  q = new msgq_queue_t;
+  int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_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_do_exit = 0;
+
+  void (*prev_handler_sigint)(int);
+  void (*prev_handler_sigterm)(int);
+  if (!non_blocking){
+    prev_handler_sigint = std::signal(SIGINT, sig_handler);
+    prev_handler_sigterm = std::signal(SIGTERM, sig_handler);
+  }
+
+  msgq_msg_t msg;
+
+  MSGQMessage *r = NULL;
+
+  int rc = msgq_msg_recv(&msg, q);
+
+  // Hack to implement blocking read with a poller. Don't use this
+  while (!non_blocking && rc == 0 && msgq_do_exit == 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 (!non_blocking){
+    std::signal(SIGINT, prev_handler_sigint);
+    std::signal(SIGTERM, prev_handler_sigterm);
+  }
+
+  errno = msgq_do_exit ? EINTR : 0;
+
+  if (rc > 0){
+    if (msgq_do_exit){
+      msgq_msg_close(&msg); // Free unused message on exit
+    } else {
+      r = new MSGQMessage;
+      r->takeOwnership(msg.data, msg.size);
+    }
+  }
+
+  return (Message*)r;
+}
+
+void MSGQSubSocket::setTimeout(int t){
+  timeout = t;
+}
+
+MSGQSubSocket::~MSGQSubSocket(){
+  if (q != NULL){
+    msgq_close_queue(q);
+    delete q;
+  }
+}
+
+int MSGQPubSocket::connect(Context *context, std::string endpoint, bool check_endpoint){
+  assert(context);
+
+  if (check_endpoint && !service_exists(std::string(endpoint))){
+    std::cout << "Warning, " << std::string(endpoint) << " is not in service list." << std::endl;
+  }
+
+  q = new msgq_queue_t;
+  int r = msgq_new_queue(q, endpoint.c_str(), DEFAULT_SEGMENT_SIZE);
+  if (r != 0){
+    return r;
+  }
+
+  msgq_init_publisher(q);
+
+  return 0;
+}
+
+int MSGQPubSocket::sendMessage(Message *message){
+  msgq_msg_t msg;
+  msg.data = message->getData();
+  msg.size = message->getSize();
+
+  return msgq_msg_send(&msg, q);
+}
+
+int MSGQPubSocket::send(char *data, size_t size){
+  msgq_msg_t msg;
+  msg.data = data;
+  msg.size = size;
+
+  return msgq_msg_send(&msg, q);
+}
+
+bool MSGQPubSocket::all_readers_updated() {
+  return msgq_all_readers_updated(q);
+}
+
+MSGQPubSocket::~MSGQPubSocket(){
+  if (q != NULL){
+    msgq_close_queue(q);
+    delete q;
+  }
+}
+
+
+void MSGQPoller::registerSocket(SubSocket * socket){
+  assert(num_polls + 1 < MAX_POLLERS);
+  polls[num_polls].q = (msgq_queue_t*)socket->getRawSocket();
+
+  sockets.push_back(socket);
+  num_polls++;
+}
+
+std::vector<SubSocket*> MSGQPoller::poll(int timeout){
+  std::vector<SubSocket*> r;
+
+  msgq_poll(polls, num_polls, timeout);
+  for (size_t i = 0; i < num_polls; i++){
+    if (polls[i].revents){
+      r.push_back(sockets[i]);
+    }
+  }
+
+  return r;
+}

cereal/messaging/impl_msgq.h

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

cereal/messaging/impl_zmq.cc

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

cereal/messaging/impl_zmq.h

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

cereal/messaging/messaging.cc

@@ -0,0 +1,120 @@
+#include <cassert>
+#include <iostream>
+
+#include "cereal/messaging/messaging.h"
+#include "cereal/messaging/impl_zmq.h"
+#include "cereal/messaging/impl_msgq.h"
+#include "cereal/messaging/impl_fake.h"
+
+#ifdef __APPLE__
+const bool MUST_USE_ZMQ = true;
+#else
+const bool MUST_USE_ZMQ = false;
+#endif
+
+bool messaging_use_zmq(){
+  if (std::getenv("ZMQ") || MUST_USE_ZMQ) {
+    if (std::getenv("OPENPILOT_PREFIX")) {
+      std::cerr << "OPENPILOT_PREFIX not supported with ZMQ backend\n";
+      assert(false);
+    }
+    return true;
+  }
+  return false;
+}
+
+bool messaging_use_fake(){
+  char* fake_enabled = std::getenv("CEREAL_FAKE");
+  return fake_enabled != NULL;
+}
+
+Context * Context::create(){
+  Context * c;
+  if (messaging_use_zmq()){
+    c = new ZMQContext();
+  } else {
+    c = new MSGQContext();
+  }
+  return c;
+}
+
+SubSocket * SubSocket::create(){
+  SubSocket * s;
+  if (messaging_use_fake()) {
+    if (messaging_use_zmq()) {
+      s = new FakeSubSocket<ZMQSubSocket>();
+    } else {
+      s = new FakeSubSocket<MSGQSubSocket>();
+    }
+  } else {
+    if (messaging_use_zmq()){
+      s = new ZMQSubSocket();
+    } else {
+      s = new MSGQSubSocket();
+    }
+  }
+
+  return s;
+}
+
+SubSocket * SubSocket::create(Context * context, std::string endpoint, std::string address, bool conflate, bool check_endpoint){
+  SubSocket *s = SubSocket::create();
+  int r = s->connect(context, endpoint, address, conflate, check_endpoint);
+
+  if (r == 0) {
+    return s;
+  } else {
+    std::cerr << "Error, failed to connect SubSocket to " << endpoint << ": " << strerror(errno) << std::endl;
+
+    delete s;
+    return nullptr;
+  }
+}
+
+PubSocket * PubSocket::create(){
+  PubSocket * s;
+  if (messaging_use_zmq()){
+    s = new ZMQPubSocket();
+  } else {
+    s = new MSGQPubSocket();
+  }
+
+  return s;
+}
+
+PubSocket * PubSocket::create(Context * context, std::string endpoint, bool check_endpoint){
+  PubSocket *s = PubSocket::create();
+  int r = s->connect(context, endpoint, check_endpoint);
+
+  if (r == 0) {
+    return s;
+  } else {
+    std::cerr << "Error, failed to bind PubSocket to " << endpoint << ": " << strerror(errno) << std::endl;
+
+    delete s;
+    return nullptr;
+  }
+}
+
+Poller * Poller::create(){
+  Poller * p;
+  if (messaging_use_fake()) {
+    p = new FakePoller();
+  } else {
+    if (messaging_use_zmq()){
+      p = new ZMQPoller();
+    } else {
+      p = new MSGQPoller();
+    }
+  }
+  return p;
+}
+
+Poller * Poller::create(std::vector<SubSocket*> sockets){
+  Poller * p = Poller::create();
+
+  for (auto s : sockets){
+    p->registerSocket(s);
+  }
+  return p;
+}

cereal/messaging/messaging.h

@@ -0,0 +1,162 @@
+#pragma once
+
+#include <cstddef>
+#include <map>
+#include <string>
+#include <vector>
+#include <utility>
+#include <time.h>
+
+#include <capnp/serialize.h>
+
+#include "cereal/gen/cpp/log.capnp.h"
+
+#ifdef __APPLE__
+#define CLOCK_BOOTTIME CLOCK_MONOTONIC
+#endif
+
+#define MSG_MULTIPLE_PUBLISHERS 100
+
+bool messaging_use_zmq();
+
+class Context {
+public:
+  virtual void * getRawContext() = 0;
+  static Context * create();
+  virtual ~Context(){}
+};
+
+class Message {
+public:
+  virtual void init(size_t size) = 0;
+  virtual void init(char * data, size_t size) = 0;
+  virtual void close() = 0;
+  virtual size_t getSize() = 0;
+  virtual char * getData() = 0;
+  virtual ~Message(){}
+};
+
+
+class SubSocket {
+public:
+  virtual int connect(Context *context, std::string endpoint, std::string address, bool conflate=false, bool check_endpoint=true) = 0;
+  virtual void setTimeout(int timeout) = 0;
+  virtual Message *receive(bool non_blocking=false) = 0;
+  virtual void * getRawSocket() = 0;
+  static SubSocket * create();
+  static SubSocket * create(Context * context, std::string endpoint, std::string address="127.0.0.1", bool conflate=false, bool check_endpoint=true);
+  virtual ~SubSocket(){}
+};
+
+class PubSocket {
+public:
+  virtual int connect(Context *context, std::string endpoint, bool check_endpoint=true) = 0;
+  virtual int sendMessage(Message *message) = 0;
+  virtual int send(char *data, size_t size) = 0;
+  virtual bool all_readers_updated() = 0;
+  static PubSocket * create();
+  static PubSocket * create(Context * context, std::string endpoint, bool check_endpoint=true);
+  static PubSocket * create(Context * context, std::string endpoint, int port, bool check_endpoint=true);
+  virtual ~PubSocket(){}
+};
+
+class Poller {
+public:
+  virtual void registerSocket(SubSocket *socket) = 0;
+  virtual std::vector<SubSocket*> poll(int timeout) = 0;
+  static Poller * create();
+  static Poller * create(std::vector<SubSocket*> sockets);
+  virtual ~Poller(){}
+};
+
+class SubMaster {
+public:
+  SubMaster(const std::vector<const char *> &service_list, const std::vector<const char *> &poll = {},
+            const char *address = nullptr, const std::vector<const char *> &ignore_alive = {});
+  void update(int timeout = 1000);
+  void update_msgs(uint64_t current_time, const std::vector<std::pair<std::string, cereal::Event::Reader>> &messages);
+  inline bool allAlive(const std::vector<const char *> &service_list = {}) { return all_(service_list, false, true); }
+  inline bool allValid(const std::vector<const char *> &service_list = {}) { return all_(service_list, true, false); }
+  inline bool allAliveAndValid(const std::vector<const char *> &service_list = {}) { return all_(service_list, true, true); }
+  void drain();
+  ~SubMaster();
+
+  uint64_t frame = 0;
+  bool updated(const char *name) const;
+  bool alive(const char *name) const;
+  bool valid(const char *name) const;
+  uint64_t rcv_frame(const char *name) const;
+  uint64_t rcv_time(const char *name) const;
+  cereal::Event::Reader &operator[](const char *name) const;
+
+private:
+  bool all_(const std::vector<const char *> &service_list, bool valid, bool alive);
+  Poller *poller_ = nullptr;
+  struct SubMessage;
+  std::map<SubSocket *, SubMessage *> messages_;
+  std::map<std::string, SubMessage *> services_;
+};
+
+class MessageBuilder : public capnp::MallocMessageBuilder {
+public:
+  MessageBuilder() = default;
+
+  cereal::Event::Builder initEvent(bool valid = true) {
+    cereal::Event::Builder event = initRoot<cereal::Event>();
+    struct timespec t;
+    clock_gettime(CLOCK_BOOTTIME, &t);
+    uint64_t current_time = t.tv_sec * 1000000000ULL + t.tv_nsec;
+    event.setLogMonoTime(current_time);
+    event.setValid(valid);
+    return event;
+  }
+
+  kj::ArrayPtr<capnp::byte> toBytes() {
+    heapArray_ = capnp::messageToFlatArray(*this);
+    return heapArray_.asBytes();
+  }
+
+  size_t getSerializedSize() {
+    return capnp::computeSerializedSizeInWords(*this) * sizeof(capnp::word);
+  }
+
+  int serializeToBuffer(unsigned char *buffer, size_t buffer_size) {
+    size_t serialized_size = getSerializedSize();
+    if (serialized_size > buffer_size) { return -1; }
+    kj::ArrayOutputStream out(kj::ArrayPtr<capnp::byte>(buffer, buffer_size));
+    capnp::writeMessage(out, *this);
+    return serialized_size;
+  }
+
+private:
+  kj::Array<capnp::word> heapArray_;
+};
+
+class PubMaster {
+public:
+  PubMaster(const std::vector<const char *> &service_list);
+  inline int send(const char *name, capnp::byte *data, size_t size) { return sockets_.at(name)->send((char *)data, size); }
+  int send(const char *name, MessageBuilder &msg);
+  ~PubMaster();
+
+private:
+  std::map<std::string, PubSocket *> sockets_;
+};
+
+class AlignedBuffer {
+public:
+  kj::ArrayPtr<const capnp::word> align(const char *data, const size_t size) {
+    words_size = size / sizeof(capnp::word) + 1;
+    if (aligned_buf.size() < words_size) {
+      aligned_buf = kj::heapArray<capnp::word>(words_size < 512 ? 512 : words_size);
+    }
+    memcpy(aligned_buf.begin(), data, size);
+    return aligned_buf.slice(0, words_size);
+  }
+  inline kj::ArrayPtr<const capnp::word> align(Message *m) {
+    return align(m->getData(), m->getSize());
+  }
+private:
+  kj::Array<capnp::word> aligned_buf;
+  size_t words_size;
+};

cereal/messaging/messaging.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 "cereal/messaging/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 "cereal/messaging/messaging.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()
+    int connect(Context *, string, string, bool)
+    Message * receive(bool)
+    void setTimeout(int)
+
+  cdef cppclass PubSocket:
+    @staticmethod
+    PubSocket * create()
+    int connect(Context *, string)
+    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

cereal/messaging/messaging_pyx.pyx

@@ -0,0 +1,247 @@
+# distutils: language = c++
+# cython: c_string_encoding=ascii, language_level=3
+
+import sys
+from libcpp.string cimport string
+from libcpp.vector cimport vector
+from libcpp cimport bool
+from libc cimport errno
+from libc.string cimport strerror
+from cython.operator import dereference
+
+
+from .messaging cimport Context as cppContext
+from .messaging cimport SubSocket as cppSubSocket
+from .messaging cimport PubSocket as cppPubSocket
+from .messaging cimport Poller as cppPoller
+from .messaging cimport Message as cppMessage
+from .messaging cimport Event as cppEvent, SocketEventHandle as cppSocketEventHandle
+
+
+class MessagingError(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(MessagingError):
+  pass
+
+
+def toggle_fake_events(bool enabled):
+  cppSocketEventHandle.toggle_fake_events(enabled)
+
+
+def set_fake_prefix(string prefix):
+  cppSocketEventHandle.set_fake_prefix(prefix)
+
+
+def get_fake_prefix():
+  return cppSocketEventHandle.fake_prefix()
+
+
+def delete_fake_prefix():
+  cppSocketEventHandle.set_fake_prefix(b"")
+
+
+def wait_for_one_event(list events, int timeout=-1):
+  cdef vector[cppEvent] items
+  for event in events:
+    items.push_back(dereference(<cppEvent*><size_t>event.ptr))
+  return cppEvent.wait_for_one(items, timeout)
+
+
+cdef class Event:
+  cdef cppEvent event;
+
+  def __cinit__(self):
+    pass
+
+  cdef setEvent(self, cppEvent event):
+    self.event = event
+
+  def set(self):
+    self.event.set()
+
+  def clear(self):
+    return self.event.clear()
+
+  def wait(self, int timeout=-1):
+    self.event.wait(timeout)
+
+  def peek(self):
+    return self.event.peek()
+
+  @property
+  def fd(self):
+    return self.event.fd()
+
+  @property
+  def ptr(self):
+    return <size_t><void*>&self.event
+
+
+cdef class SocketEventHandle:
+  cdef cppSocketEventHandle * handle;
+
+  def __cinit__(self, string endpoint, string identifier, bool override):
+    self.handle = new cppSocketEventHandle(endpoint, identifier, override)
+
+  def __dealloc__(self):
+    del self.handle
+
+  @property
+  def enabled(self):
+    return self.handle.is_enabled()
+
+  @enabled.setter
+  def enabled(self, bool value):
+    self.handle.set_enabled(value)
+
+  @property
+  def recv_called_event(self):
+    e = Event()
+    e.setEvent(self.handle.recv_called())
+
+    return e
+
+  @property
+  def recv_ready_event(self):
+    e = Event()
+    e.setEvent(self.handle.recv_ready())
+
+    return e
+
+
+cdef class Context:
+  cdef cppContext * context
+
+  def __cinit__(self):
+    self.context = cppContext.create()
+
+  def term(self):
+    del self.context
+    self.context = NULL
+
+  def __dealloc__(self):
+    pass
+    # Deleting the context will hang if sockets are still active
+    # TODO: Figure out a way to make sure the context is closed last
+    # del self.context
+
+
+cdef class Poller:
+  cdef cppPoller * poller
+  cdef list sub_sockets
+
+  def __cinit__(self):
+    self.sub_sockets = []
+    self.poller = cppPoller.create()
+
+  def __dealloc__(self):
+    del self.poller
+
+  def registerSocket(self, SubSocket socket):
+    self.sub_sockets.append(socket)
+    self.poller.registerSocket(socket.socket)
+
+  def poll(self, timeout):
+    sockets = []
+    cdef int t = timeout
+
+    with nogil:
+      result = self.poller.poll(t)
+
+    for s in result:
+      socket = SubSocket()
+      socket.setPtr(s)
+      sockets.append(socket)
+
+    return sockets
+
+
+cdef class SubSocket:
+  cdef cppSubSocket * socket
+  cdef bool is_owner
+
+  def __cinit__(self):
+    self.socket = cppSubSocket.create()
+    self.is_owner = True
+
+    if self.socket == NULL:
+      raise MessagingError
+
+  def __dealloc__(self):
+    if self.is_owner:
+      del self.socket
+
+  cdef setPtr(self, cppSubSocket * ptr):
+    if self.is_owner:
+      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):
+    r = self.socket.connect(context.context, endpoint, address, conflate)
+
+    if r != 0:
+      if errno.errno == errno.EADDRINUSE:
+        raise MultiplePublishersError(endpoint)
+      else:
+        raise MessagingError(endpoint)
+
+  def setTimeout(self, int timeout):
+    self.socket.setTimeout(timeout)
+
+  def receive(self, bool non_blocking=False):
+    msg = self.socket.receive(non_blocking)
+
+    if msg == NULL:
+      # If a blocking read returns no message check errno if SIGINT was caught in the C++ code
+      if errno.errno == errno.EINTR:
+        print("SIGINT received, exiting")
+        sys.exit(1)
+
+      return None
+    else:
+      sz = msg.getSize()
+      m = msg.getData()[:sz]
+      del msg
+
+      return m
+
+
+cdef class PubSocket:
+  cdef cppPubSocket * socket
+
+  def __cinit__(self):
+    self.socket = cppPubSocket.create()
+    if self.socket == NULL:
+      raise MessagingError
+
+  def __dealloc__(self):
+    del self.socket
+
+  def connect(self, Context context, string endpoint):
+    r = self.socket.connect(context.context, endpoint)
+
+    if r != 0:
+      if errno.errno == errno.EADDRINUSE:
+        raise MultiplePublishersError(endpoint)
+      else:
+        raise MessagingError(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 MessagingError
+
+  def all_readers_updated(self):
+    return self.socket.all_readers_updated()

cereal/messaging/msgq.cc

@@ -0,0 +1,468 @@
+#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 "cereal/messaging/msgq.h"
+
+void sigusr2_handler(int signal) {
+  assert(signal == SIGUSR2);
+}
+
+uint64_t msgq_get_uid(void){
+  std::random_device rd("/dev/urandom");
+  std::uniform_int_distribution<uint64_t> distribution(0, std::numeric_limits<uint32_t>::max());
+
+  #ifdef __APPLE__
+    // TODO: this doesn't work
+    uint64_t uid = distribution(rd) << 32 | getpid();
+  #else
+    uint64_t uid = distribution(rd) << 32 | syscall(SYS_gettid);
+  #endif
+
+  return uid;
+}
+
+int msgq_msg_init_size(msgq_msg_t * msg, size_t size){
+  msg->size = size;
+  msg->data = new(std::nothrow) char[size];
+
+  return (msg->data == NULL) ? -1 : 0;
+}
+
+
+int msgq_msg_init_data(msgq_msg_t * msg, char * data, size_t size) {
+  int r = msgq_msg_init_size(msg, size);
+
+  if (r == 0)
+    memcpy(msg->data, data, size);
+
+  return r;
+}
+
+int msgq_msg_close(msgq_msg_t * msg){
+  if (msg->size > 0)
+    delete[] msg->data;
+
+  msg->size = 0;
+
+  return 0;
+}
+
+void msgq_reset_reader(msgq_queue_t * q){
+  int id = q->reader_id;
+  q->read_valids[id]->store(true);
+  q->read_pointers[id]->store(*q->write_pointer);
+}
+
+void msgq_wait_for_subscriber(msgq_queue_t *q){
+  while (*q->num_readers == 0){
+    // wait for subscriber
+  }
+
+  return;
+}
+
+int msgq_new_queue(msgq_queue_t * q, const char * path, size_t size){
+  assert(size < 0xFFFFFFFF); // Buffer must be smaller than 2^32 bytes
+  std::signal(SIGUSR2, sigusr2_handler);
+
+  std::string full_path = "/dev/shm/";
+  const char* prefix = std::getenv("OPENPILOT_PREFIX");
+  if (prefix) {
+    full_path += std::string(prefix) + "/";
+  }
+  full_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;
+  }
+  char * mem = (char*)mmap(NULL, size + sizeof(msgq_header_t), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0);
+  close(fd);
+
+  if (mem == NULL){
+    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) {
+  #ifndef SYS_tkill
+    // TODO: this won't work for multithreaded programs
+    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;
+  struct timespec ts;
+  ts.tv_sec = ms / 1000;
+  ts.tv_nsec = (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;
+      }
+    }
+
+    // exit if we had a timeout and the sleep finished
+    if (timeout != -1 && ret == 0){
+      break;
+    }
+  }
+
+  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;
+}

cereal/messaging/msgq.h

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

cereal/messaging/socketmaster.cc

@@ -0,0 +1,210 @@
+#include <time.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string>
+#include <mutex>
+
+#include "cereal/services.h"
+#include "cereal/messaging/messaging.h"
+
+const bool SIMULATION = (getenv("SIMULATION") != nullptr) && (std::string(getenv("SIMULATION")) == "1");
+
+static inline uint64_t nanos_since_boot() {
+  struct timespec t;
+  clock_gettime(CLOCK_BOOTTIME, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+static inline bool inList(const std::vector<const char *> &list, const char *value) {
+  for (auto &v : list) {
+    if (strcmp(value, v) == 0) return true;
+  }
+  return false;
+}
+
+class MessageContext {
+public:
+  MessageContext() : ctx_(nullptr) {}
+  ~MessageContext() { delete ctx_; }
+  inline Context *context() {
+    std::call_once(init_flag, [=]() { ctx_ = Context::create(); });
+    return ctx_;
+  }
+private:
+  Context *ctx_;
+  std::once_flag init_flag;
+};
+
+MessageContext message_context;
+
+struct SubMaster::SubMessage {
+  std::string name;
+  SubSocket *socket = nullptr;
+  int freq = 0;
+  bool updated = false, alive = false, valid = true, ignore_alive;
+  uint64_t rcv_time = 0, rcv_frame = 0;
+  void *allocated_msg_reader = nullptr;
+  bool is_polled = false;
+  capnp::FlatArrayMessageReader *msg_reader = nullptr;
+  AlignedBuffer aligned_buf;
+  cereal::Event::Reader event;
+};
+
+SubMaster::SubMaster(const std::vector<const char *> &service_list, const std::vector<const char *> &poll,
+                     const char *address, const std::vector<const char *> &ignore_alive) {
+  poller_ = Poller::create();
+  for (auto name : service_list) {
+    assert(services.count(std::string(name)) > 0);
+
+    service serv = services.at(std::string(name));
+    SubSocket *socket = SubSocket::create(message_context.context(), name, address ? address : "127.0.0.1", true);
+    assert(socket != 0);
+    bool is_polled = inList(poll, name) || poll.empty();
+    if (is_polled) poller_->registerSocket(socket);
+    SubMessage *m = new SubMessage{
+      .name = name,
+      .socket = socket,
+      .freq = serv.frequency,
+      .ignore_alive = inList(ignore_alive, name),
+      .allocated_msg_reader = malloc(sizeof(capnp::FlatArrayMessageReader)),
+      .is_polled = is_polled};
+    m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader({});
+    messages_[socket] = m;
+    services_[name] = m;
+  }
+}
+
+void SubMaster::update(int timeout) {
+  for (auto &kv : messages_) kv.second->updated = false;
+
+  auto sockets = poller_->poll(timeout);
+
+  // add non-polled sockets for non-blocking receive
+  for (auto &kv : messages_) {
+    SubMessage *m = kv.second;
+    SubSocket *s = kv.first;
+    if (!m->is_polled) sockets.push_back(s);
+  }
+
+  uint64_t current_time = nanos_since_boot();
+
+  std::vector<std::pair<std::string, cereal::Event::Reader>> messages;
+
+  for (auto s : sockets) {
+    Message *msg = s->receive(true);
+    if (msg == nullptr) continue;
+
+    SubMessage *m = messages_.at(s);
+
+    m->msg_reader->~FlatArrayMessageReader();
+    capnp::ReaderOptions options;
+    options.traversalLimitInWords = kj::maxValue; // Don't limit
+    m->msg_reader = new (m->allocated_msg_reader) capnp::FlatArrayMessageReader(m->aligned_buf.align(msg), options);
+    delete msg;
+    messages.push_back({m->name, m->msg_reader->getRoot<cereal::Event>()});
+  }
+
+  update_msgs(current_time, messages);
+}
+
+void SubMaster::update_msgs(uint64_t current_time, const std::vector<std::pair<std::string, cereal::Event::Reader>> &messages){
+  if (++frame == UINT64_MAX) frame = 1;
+
+  for (auto &kv : messages) {
+    auto m_find = services_.find(kv.first);
+    if (m_find == services_.end()){
+      continue;
+    }
+    SubMessage *m = m_find->second;
+    m->event = kv.second;
+    m->updated = true;
+    m->rcv_time = current_time;
+    m->rcv_frame = frame;
+    m->valid = m->event.getValid();
+    if (SIMULATION) m->alive = true;
+  }
+
+  if (!SIMULATION) {
+    for (auto &kv : messages_) {
+      SubMessage *m = kv.second;
+      m->alive = (m->freq <= (1e-5) || ((current_time - m->rcv_time) * (1e-9)) < (10.0 / m->freq));
+    }
+  }
+}
+
+bool SubMaster::all_(const std::vector<const char *> &service_list, bool valid, bool alive) {
+  int found = 0;
+  for (auto &kv : messages_) {
+    SubMessage *m = kv.second;
+    if (service_list.size() == 0 || inList(service_list, m->name.c_str())) {
+      found += (!valid || m->valid) && (!alive || (m->alive || m->ignore_alive));
+    }
+  }
+  return service_list.size() == 0 ? found == messages_.size() : found == service_list.size();
+}
+
+void SubMaster::drain() {
+  while (true) {
+    auto polls = poller_->poll(0);
+    if (polls.size() == 0)
+      break;
+
+    for (auto sock : polls) {
+      Message *msg = sock->receive(true);
+      delete msg;
+    }
+  }
+}
+
+bool SubMaster::updated(const char *name) const {
+  return services_.at(name)->updated;
+}
+
+bool SubMaster::alive(const char *name) const {
+  return services_.at(name)->alive;
+}
+
+bool SubMaster::valid(const char *name) const {
+  return services_.at(name)->valid;
+}
+
+uint64_t SubMaster::rcv_frame(const char *name) const {
+  return services_.at(name)->rcv_frame;
+}
+
+uint64_t SubMaster::rcv_time(const char *name) const {
+  return services_.at(name)->rcv_time;
+}
+
+cereal::Event::Reader &SubMaster::operator[](const char *name) const {
+  return services_.at(name)->event;
+}
+
+SubMaster::~SubMaster() {
+  delete poller_;
+  for (auto &kv : messages_) {
+    SubMessage *m = kv.second;
+    m->msg_reader->~FlatArrayMessageReader();
+    free(m->allocated_msg_reader);
+    delete m->socket;
+    delete m;
+  }
+}
+
+PubMaster::PubMaster(const std::vector<const char *> &service_list) {
+  for (auto name : service_list) {
+    assert(services.count(name) > 0);
+    PubSocket *socket = PubSocket::create(message_context.context(), name);
+    assert(socket);
+    sockets_[name] = socket;
+  }
+}
+
+int PubMaster::send(const char *name, MessageBuilder &msg) {
+  auto bytes = msg.toBytes();
+  return send(name, bytes.begin(), bytes.size());
+}
+
+PubMaster::~PubMaster() {
+  for (auto s : sockets_) delete s.second;
+}