selfdrive/common

selfdrive/common/SConscript

@@ -0,0 +1,36 @@
+Import('env', 'arch', 'SHARED')
+
+if SHARED:
+  fxn = env.SharedLibrary
+else:
+  fxn = env.Library
+
+_common = fxn('common', ['params.cc', 'swaglog.c', 'util.c', 'cqueue.c'], LIBS="json")
+_visionipc = fxn('visionipc', ['visionipc.c', 'ipc.c'])
+
+files = [
+  'buffering.c',
+  'clutil.c',
+  'efd.c',
+  'glutil.c',
+  'visionimg.cc',
+]
+
+if arch == "aarch64":
+  defines = {}
+  files += [
+    'framebuffer.cc',
+    'touch.c',
+    'visionbuf_ion.c',
+  ]
+  _gpu_libs = ['gui', 'adreno_utils']
+else:
+  defines = {"CLU_NO_CACHE": None}
+  files += [
+    'visionbuf_cl.c',
+  ]
+  _gpu_libs = ["GL"]
+
+_gpucommon = fxn('gpucommon', files, CPPDEFINES=defines, LIBS=_gpu_libs)
+Export('_common', '_visionipc', '_gpucommon', '_gpu_libs')
+

selfdrive/common/buffering.c

@@ -0,0 +1,438 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "common/efd.h"
+
+#include "buffering.h"
+
+void tbuffer_init(TBuffer *tb, int num_bufs, const char* name) {
+  assert(num_bufs >= 3);
+
+  memset(tb, 0, sizeof(TBuffer));
+  tb->reading = (bool*)calloc(num_bufs, sizeof(bool));
+  assert(tb->reading);
+  tb->pending_idx = -1;
+  tb->num_bufs = num_bufs;
+  tb->name = name;
+
+  pthread_mutex_init(&tb->lock, NULL);
+  pthread_cond_init(&tb->cv, NULL);
+  tb->efd = efd_init();
+  assert(tb->efd >= 0);
+}
+
+void tbuffer_init2(TBuffer *tb, int num_bufs, const char* name,
+                   void (*release_cb)(void* c, int idx),
+                   void* cb_cookie) {
+
+  tbuffer_init(tb, num_bufs, name);
+
+  tb->release_cb = release_cb;
+  tb->cb_cookie = cb_cookie;
+}
+
+int tbuffer_efd(TBuffer *tb) {
+  return tb->efd;
+}
+
+int tbuffer_select(TBuffer *tb) {
+  pthread_mutex_lock(&tb->lock);
+
+  int i;
+  for (i=0; i<tb->num_bufs; i++) {
+    if (!tb->reading[i] && i != tb->pending_idx) {
+      break;
+    }
+  }
+  assert(i < tb->num_bufs);
+
+  pthread_mutex_unlock(&tb->lock);
+  return i;
+}
+
+void tbuffer_dispatch(TBuffer *tb, int idx) {
+  pthread_mutex_lock(&tb->lock);
+
+  if (tb->pending_idx != -1) {
+    //printf("tbuffer (%s) dropped!\n", tb->name ? tb->name : "?");
+    if (tb->release_cb) {
+      tb->release_cb(tb->cb_cookie, tb->pending_idx);
+    }
+    tb->pending_idx = -1;
+  }
+
+  tb->pending_idx = idx;
+
+  efd_write(tb->efd);
+  pthread_cond_signal(&tb->cv);
+
+  pthread_mutex_unlock(&tb->lock);  
+}
+
+int tbuffer_acquire(TBuffer *tb) {
+  pthread_mutex_lock(&tb->lock);
+
+  if (tb->stopped) {
+    pthread_mutex_unlock(&tb->lock);
+    return -1;
+  }
+
+  while (tb->pending_idx == -1) {
+    pthread_cond_wait(&tb->cv, &tb->lock);
+
+    if (tb->stopped) {
+      pthread_mutex_unlock(&tb->lock);
+      return -1;
+    }
+  }
+
+  efd_clear(tb->efd);
+
+  int ret = tb->pending_idx;
+  assert(ret < tb->num_bufs);
+
+  tb->reading[ret] = true;
+  tb->pending_idx = -1;
+
+  pthread_mutex_unlock(&tb->lock);
+
+  return ret;
+}
+
+static void tbuffer_release_locked(TBuffer *tb, int idx) {
+  assert(idx < tb->num_bufs);
+  if (!tb->reading[idx]) {
+    printf("!! releasing tbuffer we aren't reading %d\n", idx);
+  }
+
+  if (tb->release_cb) {
+    tb->release_cb(tb->cb_cookie, idx);
+  }
+
+  tb->reading[idx] = false;
+}
+
+void tbuffer_release(TBuffer *tb, int idx) {
+  pthread_mutex_lock(&tb->lock);
+  tbuffer_release_locked(tb, idx);
+  pthread_mutex_unlock(&tb->lock);
+}
+
+void tbuffer_release_all(TBuffer *tb) {
+  pthread_mutex_lock(&tb->lock);
+  for (int i=0; i<tb->num_bufs; i++) {
+    if (tb->reading[i]) {
+      tbuffer_release_locked(tb, i);
+    }
+  }
+  pthread_mutex_unlock(&tb->lock);
+}
+
+void tbuffer_stop(TBuffer *tb) {
+  pthread_mutex_lock(&tb->lock);
+  tb->stopped = true;
+  efd_write(tb->efd);
+  pthread_cond_signal(&tb->cv);
+  pthread_mutex_unlock(&tb->lock);
+}
+
+
+
+
+
+
+
+
+
+
+
+void pool_init(Pool *s, int num_bufs) {
+  assert(num_bufs > 3);
+
+  memset(s, 0, sizeof(*s));
+  s->num_bufs = num_bufs;
+
+  s->refcnt = (int*)calloc(num_bufs, sizeof(int));
+  s->ts = (int*)calloc(num_bufs, sizeof(int));
+
+  s->counter = 1;
+
+  pthread_mutex_init(&s->lock, NULL);
+}
+
+void pool_init2(Pool *s, int num_bufs,
+  void (*release_cb)(void* c, int idx), void* cb_cookie) {
+
+  pool_init(s, num_bufs);
+  s->cb_cookie = cb_cookie;
+  s->release_cb = release_cb;
+
+}
+
+
+void pool_acquire(Pool *s, int idx) {
+  pthread_mutex_lock(&s->lock);
+
+  assert(idx >= 0 && idx < s->num_bufs);
+
+  s->refcnt[idx]++;
+
+  pthread_mutex_unlock(&s->lock);
+}
+
+static void pool_release_locked(Pool *s, int idx) {
+  // printf("release %d refcnt %d\n", idx, s->refcnt[idx]);
+
+  assert(idx >= 0 && idx < s->num_bufs);
+
+  assert(s->refcnt[idx] > 0);
+  s->refcnt[idx]--;
+
+  // printf("release %d -> %d, %p\n", idx, s->refcnt[idx], s->release_cb);
+  if (s->refcnt[idx] == 0 && s->release_cb) {
+    // printf("call %p\b", s->release_cb);
+    s->release_cb(s->cb_cookie, idx);
+  }
+}
+
+void pool_release(Pool *s, int idx) {
+  pthread_mutex_lock(&s->lock);
+  pool_release_locked(s, idx);
+  pthread_mutex_unlock(&s->lock);
+}
+
+TBuffer* pool_get_tbuffer(Pool *s) {
+  pthread_mutex_lock(&s->lock);
+
+  assert(s->num_tbufs < POOL_MAX_TBUFS);
+  TBuffer* tbuf = &s->tbufs[s->num_tbufs];
+  s->num_tbufs++;
+  tbuffer_init2(tbuf, s->num_bufs,
+                "pool", (void (*)(void *, int))pool_release, s);
+
+  bool stopped = s->stopped;
+  pthread_mutex_unlock(&s->lock);
+
+  // Stop the tbuffer so we can return a valid object.
+  // We must stop here because the pool_stop may have already been called,
+  // in which case tbuffer_stop may never be called again.
+  if (stopped) {
+    tbuffer_stop(tbuf);
+  }
+  return tbuf;
+}
+
+PoolQueue* pool_get_queue(Pool *s) {
+  pthread_mutex_lock(&s->lock);
+
+  int i;
+  for (i = 0; i < POOL_MAX_QUEUES; i++) {
+    if (!s->queues[i].inited) {
+      break;
+    }
+  }
+  assert(i < POOL_MAX_QUEUES);
+
+  PoolQueue *c = &s->queues[i];
+  memset(c, 0, sizeof(*c));
+
+  c->pool = s;
+  c->inited = true;
+
+  c->efd = efd_init();
+  assert(c->efd >= 0);
+
+  c->num_bufs = s->num_bufs;
+  c->num = c->num_bufs+1;
+  c->idx = (int*)malloc(sizeof(int)*c->num);
+  memset(c->idx, -1, sizeof(int)*c->num);
+
+  pthread_mutex_init(&c->lock, NULL);
+  pthread_cond_init(&c->cv, NULL);
+
+  pthread_mutex_unlock(&s->lock);
+  return c;
+}
+
+void pool_release_queue(PoolQueue *c) {
+  Pool *s = c->pool;
+
+  pthread_mutex_lock(&s->lock);
+  pthread_mutex_lock(&c->lock);
+
+  for (int i=0; i<c->num; i++) {
+    if (c->idx[i] != -1) {
+      pool_release_locked(s, c->idx[i]);
+    }
+  }
+
+  close(c->efd);
+  free(c->idx);
+
+  c->inited = false;
+
+  pthread_mutex_unlock(&c->lock);
+
+  pthread_mutex_destroy(&c->lock);
+  pthread_cond_destroy(&c->cv);
+
+  pthread_mutex_unlock(&s->lock);
+}
+
+int pool_select(Pool *s) {
+  pthread_mutex_lock(&s->lock);
+
+  int i;
+  for (i=0; i<s->num_bufs; i++) {
+    if (s->refcnt[i] == 0) {
+      break;
+    }
+  }
+
+  if (i >= s->num_bufs) {
+    // overwrite the oldest
+    // still being using in a queue or tbuffer :/
+
+    int min_k = 0;
+    int min_ts = s->ts[0];
+    for (int k=1; k<s->num_bufs; k++) {
+      if (s->ts[k] < min_ts) {
+        min_ts = s->ts[k];
+        min_k = k;
+      }
+    }
+    i = min_k;
+    printf("pool is full! evicted %d\n", min_k);
+
+    // might be really bad if the user is doing pointery stuff
+    if (s->release_cb) {
+      s->release_cb(s->cb_cookie, min_k);
+    }
+  }
+
+  s->refcnt[i]++;
+
+  s->ts[i] = s->counter;
+  s->counter++;
+
+  pthread_mutex_unlock(&s->lock);
+
+  return i;
+}
+
+void pool_push(Pool *s, int idx) {
+  pthread_mutex_lock(&s->lock);
+
+  // printf("push %d head %d tail %d\n", idx, s->head, s->tail);
+
+  assert(idx >= 0 && idx < s->num_bufs);
+
+  s->ts[idx] = s->counter;
+  s->counter++;
+
+  assert(s->refcnt[idx] > 0);
+  s->refcnt[idx]--; //push is a implcit release
+
+  int num_tbufs = s->num_tbufs;
+  s->refcnt[idx] += num_tbufs;
+
+  // dispatch pool queues
+  for (int i=0; i<POOL_MAX_QUEUES; i++) {
+    PoolQueue *c = &s->queues[i];
+    if (!c->inited) continue;
+    
+    pthread_mutex_lock(&c->lock);
+    if (((c->head+1) % c->num) == c->tail) {
+      // queue is full. skip for now
+      pthread_mutex_unlock(&c->lock);
+      continue;
+    }
+
+    s->refcnt[idx]++;
+
+    c->idx[c->head] = idx;
+    c->head = (c->head+1) % c->num;
+    assert(c->head != c->tail);
+    pthread_mutex_unlock(&c->lock);
+
+    efd_write(c->efd);
+    pthread_cond_signal(&c->cv);
+  }
+
+  pthread_mutex_unlock(&s->lock);
+
+  for (int i=0; i<num_tbufs; i++) {
+    tbuffer_dispatch(&s->tbufs[i], idx);
+  }
+}
+
+int poolq_pop(PoolQueue *c) {
+  pthread_mutex_lock(&c->lock);
+
+  if (c->stopped) {
+    pthread_mutex_unlock(&c->lock);
+    return -1;
+  }
+
+  while (c->head == c->tail) {
+    pthread_cond_wait(&c->cv, &c->lock);
+
+    if (c->stopped) {
+      pthread_mutex_unlock(&c->lock);
+      return -1;
+    }
+  }
+
+  // printf("pop head %d tail %d\n", s->head, s->tail);
+
+  assert(c->head != c->tail);
+
+  int r = c->idx[c->tail];
+  c->idx[c->tail] = -1;
+  c->tail = (c->tail+1) % c->num;
+
+  // queue event is level triggered
+  if (c->head == c->tail) {
+    efd_clear(c->efd);
+  }
+
+  // printf("pop %d head %d tail %d\n", r, s->head, s->tail);
+
+  assert(r >= 0 && r < c->num_bufs);
+
+  pthread_mutex_unlock(&c->lock);
+
+  return r;
+}
+
+int poolq_efd(PoolQueue *c) {
+  return c->efd;
+}
+
+void poolq_release(PoolQueue *c, int idx) {
+  pool_release(c->pool, idx);
+}
+
+void pool_stop(Pool *s) {
+  for (int i=0; i<s->num_tbufs; i++) {
+    tbuffer_stop(&s->tbufs[i]);
+  }
+
+  pthread_mutex_lock(&s->lock);
+  s->stopped = true;
+  for (int i=0; i<POOL_MAX_QUEUES; i++) {
+    PoolQueue *c = &s->queues[i];
+    if (!c->inited) continue;
+
+    pthread_mutex_lock(&c->lock);
+    c->stopped = true;
+    pthread_mutex_unlock(&c->lock);
+    efd_write(c->efd);
+    pthread_cond_signal(&c->cv);
+  }
+  pthread_mutex_unlock(&s->lock);
+}

selfdrive/common/buffering.h

@@ -0,0 +1,123 @@
+#ifndef BUFFERING_H
+#define BUFFERING_H
+
+#include <stdbool.h>
+#include <pthread.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Tripple buffering helper
+
+typedef struct TBuffer {
+    pthread_mutex_t lock;
+    pthread_cond_t cv;
+    int efd;
+
+    bool* reading;
+    int pending_idx;
+
+    int num_bufs;
+    const char* name;
+
+    void (*release_cb)(void* c, int idx);
+    void *cb_cookie;
+
+    bool stopped;
+} TBuffer;
+
+// num_bufs must be at least the number of buffers that can be acquired simultaniously plus two
+void tbuffer_init(TBuffer *tb, int num_bufs, const char* name);
+
+void tbuffer_init2(TBuffer *tb, int num_bufs, const char* name,
+  void (*release_cb)(void* c, int idx),
+  void* cb_cookie);
+
+// returns an eventfd that signals if a buffer is ready and tbuffer_acquire shouldn't to block.
+// useful to polling on multiple tbuffers.
+int tbuffer_efd(TBuffer *tb);
+
+// Chooses a buffer that's not reading or pending
+int tbuffer_select(TBuffer *tb);
+
+// Called when the writer is done with their buffer
+//  - Wakes up the reader if it's waiting
+//  - releases the pending buffer if the reader's too slow
+void tbuffer_dispatch(TBuffer *tb, int idx);
+
+// Called when the reader wants a new buffer, will return -1 when stopped
+int tbuffer_acquire(TBuffer *tb);
+
+// Called when the reader is done with their buffer
+void tbuffer_release(TBuffer *tb, int idx);
+
+void tbuffer_release_all(TBuffer *tb);
+
+void tbuffer_stop(TBuffer *tb);
+
+
+
+
+// pool: buffer pool + queue thing...
+
+#define POOL_MAX_TBUFS 8
+#define POOL_MAX_QUEUES 8
+
+typedef struct Pool Pool;
+
+typedef struct PoolQueue {
+  pthread_mutex_t lock;
+  pthread_cond_t cv;
+  Pool* pool;
+  bool inited;
+  bool stopped;
+  int efd;
+  int num_bufs;
+  int num;
+  int head, tail;
+  int* idx;
+} PoolQueue;
+
+int poolq_pop(PoolQueue *s);
+int poolq_efd(PoolQueue *s);
+void poolq_release(PoolQueue *c, int idx);
+
+typedef struct Pool {
+  pthread_mutex_t lock;
+  bool stopped;
+  int num_bufs;
+  int counter;
+
+  int* ts;
+  int* refcnt;
+
+  void (*release_cb)(void* c, int idx);
+  void *cb_cookie;
+
+  int num_tbufs;
+  TBuffer tbufs[POOL_MAX_TBUFS];
+  PoolQueue queues[POOL_MAX_QUEUES];
+} Pool;
+
+void pool_init(Pool *s, int num_bufs);
+void pool_init2(Pool *s, int num_bufs,
+  void (*release_cb)(void* c, int idx), void* cb_cookie);
+
+TBuffer* pool_get_tbuffer(Pool *s);
+
+PoolQueue* pool_get_queue(Pool *s);
+void pool_release_queue(PoolQueue *q);
+
+int pool_select(Pool *s);
+void pool_push(Pool *s, int idx);
+void pool_acquire(Pool *s, int idx);
+void pool_release(Pool *s, int idx);
+void pool_stop(Pool *s);
+
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#endif

selfdrive/common/clutil.c

@@ -0,0 +1,418 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <string.h>
+#include <unistd.h>
+#include <inttypes.h>
+#include <sys/stat.h>
+
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#include "common/util.h"
+
+#include "clutil.h"
+
+typedef struct CLUProgramIndex {
+  uint64_t index_hash;
+  const uint8_t* bin_data;
+  const uint8_t* bin_end;
+} CLUProgramIndex;
+
+#ifdef CLU_NO_SRC
+#include "clcache_bins.h"
+#else
+static const CLUProgramIndex clu_index[] = {};
+#endif
+
+void clu_init(void) {
+#ifndef CLU_NO_SRC
+  mkdir("/tmp/clcache", 0777);
+  unlink("/tmp/clcache/index.cli");
+#endif
+}
+
+cl_program cl_create_program_from_file(cl_context ctx, const char* path) {
+  char* src_buf = read_file(path, NULL);
+  assert(src_buf);
+
+  int err = 0;
+  cl_program ret = clCreateProgramWithSource(ctx, 1, (const char**)&src_buf, NULL, &err);
+  assert(err == 0);
+
+  free(src_buf);
+
+  return ret;
+}
+
+static char* get_version_string(cl_platform_id platform) {
+  size_t size = 0;
+  int err;
+  err = clGetPlatformInfo(platform, CL_PLATFORM_VERSION, 0, NULL, &size);
+  assert(err == 0);
+  char *str = malloc(size);
+  assert(str);
+  err = clGetPlatformInfo(platform, CL_PLATFORM_VERSION, size, str, NULL);
+  assert(err == 0);
+  return str;
+}
+
+void cl_print_info(cl_platform_id platform, cl_device_id device) {
+  char str[4096];
+
+  clGetPlatformInfo(platform, CL_PLATFORM_VENDOR, sizeof(str), str, NULL);
+  printf("vendor: '%s'\n", str);
+
+  char* version = get_version_string(platform);
+  printf("platform version: '%s'\n", version);
+  free(version);
+
+  clGetPlatformInfo(platform, CL_PLATFORM_PROFILE, sizeof(str), str, NULL);
+  printf("profile: '%s'\n", str);
+
+  clGetPlatformInfo(platform, CL_PLATFORM_EXTENSIONS, sizeof(str), str, NULL);
+  printf("extensions: '%s'\n", str);
+
+  clGetDeviceInfo(device, CL_DEVICE_NAME, sizeof(str), str, NULL);
+  printf("name: '%s'\n", str);
+
+  clGetDeviceInfo(device, CL_DEVICE_VERSION, sizeof(str), str, NULL);
+  printf("device version: '%s'\n", str);
+
+  size_t sz;
+  clGetDeviceInfo(device, CL_DEVICE_MAX_WORK_GROUP_SIZE, sizeof(sz), &sz, NULL);
+  printf("max work group size: %u\n", sz);
+
+  cl_device_type type;
+  clGetDeviceInfo(device, CL_DEVICE_TYPE, sizeof(type), &type, NULL);
+  printf("type = 0x%04x = ", (unsigned int)type);
+  switch(type) {
+  case CL_DEVICE_TYPE_CPU:
+    printf("CL_DEVICE_TYPE_CPU\n");
+    break;
+  case CL_DEVICE_TYPE_GPU:
+    printf("CL_DEVICE_TYPE_GPU\n");
+    break;
+  case CL_DEVICE_TYPE_ACCELERATOR:
+    printf("CL_DEVICE_TYPE_ACCELERATOR\n");
+    break;
+  default:
+    printf("Other...\n" );
+    break;
+  }
+}
+
+void cl_print_build_errors(cl_program program, cl_device_id device) {
+  cl_build_status status;
+  clGetProgramBuildInfo(program, device, CL_PROGRAM_BUILD_STATUS,
+          sizeof(cl_build_status), &status, NULL);
+
+  size_t log_size;
+  clGetProgramBuildInfo(program, device,
+          CL_PROGRAM_BUILD_LOG, 0, NULL, &log_size);
+
+  char* log = calloc(log_size+1, 1);
+  assert(log);
+
+  clGetProgramBuildInfo(program, device,
+          CL_PROGRAM_BUILD_LOG, log_size+1, log, NULL);
+
+  printf("build failed; status=%d, log:\n%s\n",
+          status, log);
+
+  free(log);
+}
+
+uint64_t clu_index_hash(const char* s) {
+  size_t sl = strlen(s);
+  assert(sl < 128);
+  uint64_t x = 0;
+  for (int i=127; i>=0; i--) {
+    x *= 65599ULL;
+    x += (uint8_t)s[i<sl ? sl-1-i : sl];
+  }
+  return x ^ (x >> 32);
+}
+
+uint64_t clu_fnv_hash(const uint8_t *data, size_t len) {
+  /* 64 bit Fowler/Noll/Vo FNV-1a hash code */
+  uint64_t hval = 0xcbf29ce484222325ULL;
+  const uint8_t *dp = data;
+  const uint8_t *de = data + len;
+  while (dp < de) {
+    hval ^= (uint64_t) *dp++;
+    hval += (hval << 1) + (hval << 4) + (hval << 5) +
+        (hval << 7) + (hval << 8) + (hval << 40);
+  }
+
+  return hval;
+}
+
+cl_program cl_cached_program_from_hash(cl_context ctx, cl_device_id device_id, uint64_t hash) {
+  int err;
+
+  char cache_path[1024];
+  snprintf(cache_path, sizeof(cache_path), "/tmp/clcache/%016" PRIx64 ".clb", hash);
+
+  size_t bin_size;
+  uint8_t *bin = read_file(cache_path, &bin_size);
+  if (!bin) {
+    return NULL;
+  }
+
+  cl_program prg = clCreateProgramWithBinary(ctx, 1, &device_id, &bin_size, (const uint8_t**)&bin, NULL, &err);
+  assert(err == 0);
+
+  free(bin);
+
+  err = clBuildProgram(prg, 1, &device_id, NULL, NULL, NULL);
+  assert(err == 0);
+
+  return prg;
+}
+
+static uint8_t* get_program_binary(cl_program prg, size_t *out_size) {
+  int err;
+
+  cl_uint num_devices;
+  err = clGetProgramInfo(prg, CL_PROGRAM_NUM_DEVICES, sizeof(num_devices), &num_devices, NULL);
+  assert(err == 0);
+  assert(num_devices == 1);
+
+  size_t binary_size = 0;
+  err = clGetProgramInfo(prg, CL_PROGRAM_BINARY_SIZES, sizeof(binary_size), &binary_size, NULL);
+  assert(err == 0);
+  assert(binary_size > 0);
+
+  uint8_t *binary_buf = malloc(binary_size);
+  assert(binary_buf);
+
+  uint8_t* bufs[1] = { binary_buf, };
+
+  err = clGetProgramInfo(prg, CL_PROGRAM_BINARIES, sizeof(bufs), &bufs, NULL);
+  assert(err == 0);
+
+  *out_size = binary_size;
+  return binary_buf;
+}
+
+cl_program cl_cached_program_from_string(cl_context ctx, cl_device_id device_id,
+                                         const char* src, const char* args,
+                                         uint64_t *out_hash) {
+  int err;
+
+  cl_platform_id platform;
+  err = clGetDeviceInfo(device_id, CL_DEVICE_PLATFORM, sizeof(platform), &platform, NULL);
+  assert(err == 0);
+
+  const char* platform_version = get_version_string(platform);
+
+  const size_t hash_len = strlen(platform_version)+1+strlen(src)+1+strlen(args)+1;
+  char* hash_buf = malloc(hash_len);
+  assert(hash_buf);
+  memset(hash_buf, 0, hash_len);
+  snprintf(hash_buf, hash_len, "%s%c%s%c%s", platform_version, 1, src, 1, args);
+  free((void*)platform_version);
+
+  uint64_t hash = clu_fnv_hash((uint8_t*)hash_buf, hash_len);
+  free(hash_buf);
+
+  cl_program prg = NULL;
+#ifndef CLU_NO_CACHE
+  prg = cl_cached_program_from_hash(ctx, device_id, hash);
+#endif
+  if (prg == NULL) {
+    prg = clCreateProgramWithSource(ctx, 1, (const char**)&src, NULL, &err);
+    assert(err == 0);
+
+    err = clBuildProgram(prg, 1, &device_id, args, NULL, NULL);
+    if (err != 0) {
+      cl_print_build_errors(prg, device_id);
+    }
+    assert(err == 0);
+
+#ifndef CLU_NO_CACHE
+    // write program binary to cache
+
+    size_t binary_size;
+    uint8_t *binary_buf = get_program_binary(prg, &binary_size);
+
+    char cache_path[1024];
+    snprintf(cache_path, sizeof(cache_path), "/tmp/clcache/%016" PRIx64 ".clb", hash);
+    FILE* of = fopen(cache_path, "wb");
+    assert(of);
+    fwrite(binary_buf, 1, binary_size, of);
+    fclose(of);
+
+    free(binary_buf);
+#endif
+  }
+
+  if (out_hash) *out_hash = hash;
+  return prg;
+}
+
+cl_program cl_cached_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args,
+                                       uint64_t *out_hash) {
+  char* src_buf = read_file(path, NULL);
+  assert(src_buf);
+  cl_program ret = cl_cached_program_from_string(ctx, device_id, src_buf, args, out_hash);
+  free(src_buf);
+  return ret;
+}
+
+static void add_index(uint64_t index_hash, uint64_t src_hash) {
+  FILE *f = fopen("/tmp/clcache/index.cli", "a");
+  assert(f);
+  fprintf(f, "%016" PRIx64 " %016" PRIx64 "\n", index_hash, src_hash);
+  fclose(f);
+}
+
+
+cl_program cl_program_from_index(cl_context ctx, cl_device_id device_id, uint64_t index_hash) {
+  int err;
+
+  int i;
+  for (i=0; i<ARRAYSIZE(clu_index); i++) {
+    if (clu_index[i].index_hash == index_hash) {
+      break;
+    }
+  }
+  if (i >= ARRAYSIZE(clu_index)) {
+    assert(false);
+  }
+
+  size_t bin_size = clu_index[i].bin_end - clu_index[i].bin_data;
+  const uint8_t *bin_data = clu_index[i].bin_data;
+
+  cl_program prg = clCreateProgramWithBinary(ctx, 1, &device_id, &bin_size, (const uint8_t**)&bin_data, NULL, &err);
+  assert(err == 0);
+
+  err = clBuildProgram(prg, 1, &device_id, NULL, NULL, NULL);
+  assert(err == 0);
+
+  return prg;
+}
+
+cl_program cl_index_program_from_string(cl_context ctx, cl_device_id device_id,
+                                        const char* src, const char* args,
+                                        uint64_t index_hash) {
+  uint64_t src_hash = 0;
+  cl_program ret = cl_cached_program_from_string(ctx, device_id, src, args, &src_hash);
+#ifndef CLU_NO_CACHE
+  add_index(index_hash, src_hash);
+#endif
+  return ret;
+}
+
+cl_program cl_index_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args,
+                                      uint64_t index_hash) {
+  uint64_t src_hash = 0;
+  cl_program ret = cl_cached_program_from_file(ctx, device_id, path, args, &src_hash);
+#ifndef CLU_NO_CACHE
+  add_index(index_hash, src_hash);
+#endif
+  return ret;
+}
+
+/*
+ * Given a cl code and return a string represenation
+ */
+const char* cl_get_error_string(int err) {
+    switch (err) {
+        case 0: return "CL_SUCCESS";
+        case -1: return "CL_DEVICE_NOT_FOUND";
+        case -2: return "CL_DEVICE_NOT_AVAILABLE";
+        case -3: return "CL_COMPILER_NOT_AVAILABLE";
+        case -4: return "CL_MEM_OBJECT_ALLOCATION_FAILURE";
+        case -5: return "CL_OUT_OF_RESOURCES";
+        case -6: return "CL_OUT_OF_HOST_MEMORY";
+        case -7: return "CL_PROFILING_INFO_NOT_AVAILABLE";
+        case -8: return "CL_MEM_COPY_OVERLAP";
+        case -9: return "CL_IMAGE_FORMAT_MISMATCH";
+        case -10: return "CL_IMAGE_FORMAT_NOT_SUPPORTED";
+        case -12: return "CL_MAP_FAILURE";
+        case -13: return "CL_MISALIGNED_SUB_BUFFER_OFFSET";
+        case -14: return "CL_EXEC_STATUS_ERROR_FOR_EVENTS_IN_WAIT_LIST";
+        case -15: return "CL_COMPILE_PROGRAM_FAILURE";
+        case -16: return "CL_LINKER_NOT_AVAILABLE";
+        case -17: return "CL_LINK_PROGRAM_FAILURE";
+        case -18: return "CL_DEVICE_PARTITION_FAILED";
+        case -19: return "CL_KERNEL_ARG_INFO_NOT_AVAILABLE";
+        case -30: return "CL_INVALID_VALUE";
+        case -31: return "CL_INVALID_DEVICE_TYPE";
+        case -32: return "CL_INVALID_PLATFORM";
+        case -33: return "CL_INVALID_DEVICE";
+        case -34: return "CL_INVALID_CONTEXT";
+        case -35: return "CL_INVALID_QUEUE_PROPERTIES";
+        case -36: return "CL_INVALID_COMMAND_QUEUE";
+        case -37: return "CL_INVALID_HOST_PTR";
+        case -38: return "CL_INVALID_MEM_OBJECT";
+        case -39: return "CL_INVALID_IMAGE_FORMAT_DESCRIPTOR";
+        case -40: return "CL_INVALID_IMAGE_SIZE";
+        case -41: return "CL_INVALID_SAMPLER";
+        case -42: return "CL_INVALID_BINARY";
+        case -43: return "CL_INVALID_BUILD_OPTIONS";
+        case -44: return "CL_INVALID_PROGRAM";
+        case -45: return "CL_INVALID_PROGRAM_EXECUTABLE";
+        case -46: return "CL_INVALID_KERNEL_NAME";
+        case -47: return "CL_INVALID_KERNEL_DEFINITION";
+        case -48: return "CL_INVALID_KERNEL";
+        case -49: return "CL_INVALID_ARG_INDEX";
+        case -50: return "CL_INVALID_ARG_VALUE";
+        case -51: return "CL_INVALID_ARG_SIZE";
+        case -52: return "CL_INVALID_KERNEL_ARGS";
+        case -53: return "CL_INVALID_WORK_DIMENSION";
+        case -54: return "CL_INVALID_WORK_GROUP_SIZE";
+        case -55: return "CL_INVALID_WORK_ITEM_SIZE";
+        case -56: return "CL_INVALID_GLOBAL_OFFSET";
+        case -57: return "CL_INVALID_EVENT_WAIT_LIST";
+        case -58: return "CL_INVALID_EVENT";
+        case -59: return "CL_INVALID_OPERATION";
+        case -60: return "CL_INVALID_GL_OBJECT";
+        case -61: return "CL_INVALID_BUFFER_SIZE";
+        case -62: return "CL_INVALID_MIP_LEVEL";
+        case -63: return "CL_INVALID_GLOBAL_WORK_SIZE";
+        case -64: return "CL_INVALID_PROPERTY";
+        case -65: return "CL_INVALID_IMAGE_DESCRIPTOR";
+        case -66: return "CL_INVALID_COMPILER_OPTIONS";
+        case -67: return "CL_INVALID_LINKER_OPTIONS";
+        case -68: return "CL_INVALID_DEVICE_PARTITION_COUNT";
+        case -69: return "CL_INVALID_PIPE_SIZE";
+        case -70: return "CL_INVALID_DEVICE_QUEUE";
+        case -71: return "CL_INVALID_SPEC_ID";
+        case -72: return "CL_MAX_SIZE_RESTRICTION_EXCEEDED";
+        case -1002: return "CL_INVALID_D3D10_DEVICE_KHR";
+        case -1003: return "CL_INVALID_D3D10_RESOURCE_KHR";
+        case -1004: return "CL_D3D10_RESOURCE_ALREADY_ACQUIRED_KHR";
+        case -1005: return "CL_D3D10_RESOURCE_NOT_ACQUIRED_KHR";
+        case -1006: return "CL_INVALID_D3D11_DEVICE_KHR";
+        case -1007: return "CL_INVALID_D3D11_RESOURCE_KHR";
+        case -1008: return "CL_D3D11_RESOURCE_ALREADY_ACQUIRED_KHR";
+        case -1009: return "CL_D3D11_RESOURCE_NOT_ACQUIRED_KHR";
+        case -1010: return "CL_INVALID_DX9_MEDIA_ADAPTER_KHR";
+        case -1011: return "CL_INVALID_DX9_MEDIA_SURFACE_KHR";
+        case -1012: return "CL_DX9_MEDIA_SURFACE_ALREADY_ACQUIRED_KHR";
+        case -1013: return "CL_DX9_MEDIA_SURFACE_NOT_ACQUIRED_KHR";
+        case -1093: return "CL_INVALID_EGL_OBJECT_KHR";
+        case -1092: return "CL_EGL_RESOURCE_NOT_ACQUIRED_KHR";
+        case -1001: return "CL_PLATFORM_NOT_FOUND_KHR";
+        case -1057: return "CL_DEVICE_PARTITION_FAILED_EXT";
+        case -1058: return "CL_INVALID_PARTITION_COUNT_EXT";
+        case -1059: return "CL_INVALID_PARTITION_NAME_EXT";
+        case -1094: return "CL_INVALID_ACCELERATOR_INTEL";
+        case -1095: return "CL_INVALID_ACCELERATOR_TYPE_INTEL";
+        case -1096: return "CL_INVALID_ACCELERATOR_DESCRIPTOR_INTEL";
+        case -1097: return "CL_ACCELERATOR_TYPE_NOT_SUPPORTED_INTEL";
+        case -1000: return "CL_INVALID_GL_SHAREGROUP_REFERENCE_KHR";
+        case -1098: return "CL_INVALID_VA_API_MEDIA_ADAPTER_INTEL";
+        case -1099: return "CL_INVALID_VA_API_MEDIA_SURFACE_INTEL";
+        case -1100: return "CL_VA_API_MEDIA_SURFACE_ALREADY_ACQUIRED_INTEL";
+        case -1101: return "CL_VA_API_MEDIA_SURFACE_NOT_ACQUIRED_INTEL";
+        default: return "CL_UNKNOWN_ERROR";
+    }
+}

selfdrive/common/clutil.h

@@ -0,0 +1,87 @@
+#ifndef CLUTIL_H
+#define CLUTIL_H
+
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void clu_init(void);
+
+cl_program cl_create_program_from_file(cl_context ctx, const char* path);
+void cl_print_info(cl_platform_id platform, cl_device_id device);
+void cl_print_build_errors(cl_program program, cl_device_id device);
+void cl_print_build_errors(cl_program program, cl_device_id device);
+
+cl_program cl_cached_program_from_hash(cl_context ctx, cl_device_id device_id, uint64_t hash);
+cl_program cl_cached_program_from_string(cl_context ctx, cl_device_id device_id,
+                                         const char* src, const char* args,
+                                         uint64_t *out_hash);
+cl_program cl_cached_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args,
+                                       uint64_t *out_hash);
+
+cl_program cl_program_from_index(cl_context ctx, cl_device_id device_id, uint64_t index_hash);
+
+cl_program cl_index_program_from_string(cl_context ctx, cl_device_id device_id,
+                                        const char* src, const char* args,
+                                        uint64_t index_hash);
+cl_program cl_index_program_from_file(cl_context ctx, cl_device_id device_id, const char* path, const char* args,
+                                      uint64_t index_hash);
+
+uint64_t clu_index_hash(const char *s);
+uint64_t clu_fnv_hash(const uint8_t *data, size_t len);
+
+const char* cl_get_error_string(int err);
+
+static inline int cl_check_error(int err) {
+  if (err != 0) {
+    fprintf(stderr, "%s\n", cl_get_error_string(err));
+    exit(1);
+  }
+  return err;
+}
+
+
+// // string hash macro. compiler, I'm so sorry.
+#define CLU_H1(s,i,x)   (x*65599ULL+(uint8_t)s[(i)<strlen(s)?strlen(s)-1-(i):strlen(s)])
+#define CLU_H4(s,i,x)   CLU_H1(s,i,CLU_H1(s,i+1,CLU_H1(s,i+2,CLU_H1(s,i+3,x))))
+#define CLU_H16(s,i,x)  CLU_H4(s,i,CLU_H4(s,i+4,CLU_H4(s,i+8,CLU_H4(s,i+12,x))))
+#define CLU_H64(s,i,x)  CLU_H16(s,i,CLU_H16(s,i+16,CLU_H16(s,i+32,CLU_H16(s,i+48,x))))
+// #define CLU_H256(s,i,x) CLU_H64(s,i,CLU_H64(s,i+64,CLU_H64(s,i+128,CLU_H64(s,i+192,x))))
+#define CLU_H128(s,i,x) CLU_H64(s,i,CLU_H64(s,i+64,x))
+#define CLU_HASH(s)    ((uint64_t)(CLU_H128(s,0,0)^(CLU_H128(s,0,0)>>32)))
+
+#define CLU_STRINGIFY(x) #x
+#define CLU_STRINGIFY2(x) CLU_STRINGIFY(x)
+#define CLU_LINESTR CLU_STRINGIFY2(__LINE__)
+
+#ifdef CLU_NO_SRC
+
+ #define CLU_LOAD_FROM_STRING(ctx, device_id, src, args) \
+  cl_program_from_index(ctx, device_id, CLU_HASH("\1" __FILE__ "\1" CLU_LINESTR) ^ clu_fnv_hash((const uint8_t*)__func__, strlen(__func__)) ^ clu_fnv_hash((const uint8_t*)args, strlen(args)))
+ #define CLU_LOAD_FROM_FILE(ctx, device_id, path, args) \
+  cl_program_from_index(ctx, device_id, CLU_HASH("\2" path) ^ clu_fnv_hash((const uint8_t*)args, strlen(args)))
+
+#else
+
+ #define CLU_LOAD_FROM_STRING(ctx, device_id, src, args) \
+  cl_index_program_from_string(ctx, device_id, src, args, clu_index_hash("\1" __FILE__ "\1" CLU_LINESTR) ^ clu_fnv_hash((const uint8_t*)__func__, strlen(__func__)) ^ clu_fnv_hash((const uint8_t*)args, strlen(args)))
+ #define CLU_LOAD_FROM_FILE(ctx, device_id, path, args) \
+  cl_index_program_from_file(ctx, device_id, path, args, clu_index_hash("\2" path) ^ clu_fnv_hash((const uint8_t*)args, strlen(args)))
+
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/cqueue.c

@@ -0,0 +1,52 @@
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#include "cqueue.h"
+
+void queue_init(Queue *q) {
+  memset(q, 0, sizeof(*q));
+  TAILQ_INIT(&q->q);
+  pthread_mutex_init(&q->lock, NULL);
+  pthread_cond_init(&q->cv, NULL);
+}
+
+void* queue_pop(Queue *q) {
+  pthread_mutex_lock(&q->lock);
+  while (TAILQ_EMPTY(&q->q)) {
+    pthread_cond_wait(&q->cv, &q->lock);
+  }
+  QueueEntry *entry = TAILQ_FIRST(&q->q);
+  TAILQ_REMOVE(&q->q, entry, entries);
+  pthread_mutex_unlock(&q->lock);
+
+  void* r = entry->data;
+  free(entry);
+  return r;
+}
+
+void* queue_try_pop(Queue *q) {
+  pthread_mutex_lock(&q->lock);
+
+  void* r = NULL;
+  if (!TAILQ_EMPTY(&q->q)) {
+    QueueEntry *entry = TAILQ_FIRST(&q->q);
+    TAILQ_REMOVE(&q->q, entry, entries);
+    r = entry->data;
+    free(entry);
+  }
+
+  pthread_mutex_unlock(&q->lock);
+  return r;
+}
+
+void queue_push(Queue *q, void *data) {
+  QueueEntry *entry = calloc(1, sizeof(QueueEntry));
+  assert(entry);
+  entry->data = data;
+
+  pthread_mutex_lock(&q->lock);
+  TAILQ_INSERT_TAIL(&q->q, entry, entries);
+  pthread_cond_signal(&q->cv);
+  pthread_mutex_unlock(&q->lock);
+}

selfdrive/common/cqueue.h

@@ -0,0 +1,33 @@
+#ifndef COMMON_CQUEUE_H
+#define COMMON_CQUEUE_H
+
+#include <sys/queue.h>
+#include <pthread.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// a blocking queue
+
+typedef struct QueueEntry {
+  TAILQ_ENTRY(QueueEntry) entries;
+  void *data;
+} QueueEntry;
+
+typedef struct Queue {
+  TAILQ_HEAD(queue, QueueEntry) q;
+  pthread_mutex_t lock;
+  pthread_cond_t cv;
+} Queue;
+
+void queue_init(Queue *q);
+void* queue_pop(Queue *q);
+void* queue_try_pop(Queue *q);
+void queue_push(Queue *q, void *data);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#endif

selfdrive/common/efd.c

@@ -0,0 +1,56 @@
+#include <stdlib.h>
+#include <assert.h>
+
+#ifdef __linux__
+#include <sys/eventfd.h>
+#else
+#include <sys/time.h>
+#include <sys/event.h>
+#define EVENT_IDENT 42
+#endif
+
+#include "efd.h"
+
+
+int efd_init() {
+#ifdef __linux__
+  return eventfd(0, EFD_CLOEXEC);
+#else
+  int fd = kqueue();
+  assert(fd >= 0);
+
+  struct kevent kev;
+  EV_SET(&kev, EVENT_IDENT, EVFILT_USER, EV_ADD | EV_CLEAR, 0, 0, NULL);
+
+  struct timespec timeout = {0, 0};
+  int err = kevent(fd, &kev, 1, NULL, 0, &timeout);
+  assert(err != -1);
+
+  return fd;
+#endif
+}
+
+void efd_write(int fd) {
+#ifdef __linux__
+  eventfd_write(fd, 1);
+#else
+  struct kevent kev;
+  EV_SET(&kev, EVENT_IDENT, EVFILT_USER, 0, NOTE_TRIGGER, 0, NULL);
+
+  struct timespec timeout = {0, 0};
+  int err = kevent(fd, &kev, 1, NULL, 0, &timeout);
+  assert(err != -1);
+#endif
+}
+
+void efd_clear(int fd) {
+#ifdef __linux__
+  eventfd_t efd_cnt;
+  eventfd_read(fd, &efd_cnt);
+#else
+  struct kevent kev;
+  struct timespec timeout = {0, 0};
+  int nfds = kevent(fd, NULL, 0, &kev, 1, &timeout);
+  assert(nfds != -1);
+#endif
+}

selfdrive/common/efd.h

@@ -0,0 +1,17 @@
+#ifndef EFD_H
+#define EFD_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// event fd: a semaphore that can be poll()'d
+int efd_init();
+void efd_write(int fd);
+void efd_clear(int fd);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/framebuffer.cc

@@ -0,0 +1,142 @@
+#include <cstdio>
+#include <cstdlib>
+#include <cassert>
+
+#include <ui/DisplayInfo.h>
+
+#include <gui/ISurfaceComposer.h>
+#include <gui/Surface.h>
+#include <gui/SurfaceComposerClient.h>
+
+
+#include <GLES2/gl2.h>
+#include <EGL/eglext.h>
+
+#define BACKLIGHT_CONTROL "/sys/class/leds/lcd-backlight/brightness"
+#define BACKLIGHT_LEVEL "205"
+
+using namespace android;
+
+struct FramebufferState {
+    sp<SurfaceComposerClient> session;
+    sp<IBinder> dtoken;
+    DisplayInfo dinfo;
+    sp<SurfaceControl> control;
+
+    sp<Surface> s;
+    EGLDisplay display;
+
+    EGLint egl_major, egl_minor;
+    EGLConfig config;
+    EGLSurface surface;
+    EGLContext context;
+};
+
+extern "C" void framebuffer_set_power(FramebufferState *s, int mode) {
+  SurfaceComposerClient::setDisplayPowerMode(s->dtoken, mode);
+}
+
+extern "C" FramebufferState* framebuffer_init(
+    const char* name, int32_t layer, int alpha,
+    int *out_w, int *out_h) {
+  status_t status;
+  int success;
+
+  FramebufferState *s = new FramebufferState;
+
+  s->session = new SurfaceComposerClient();
+  assert(s->session != NULL);
+
+  s->dtoken = SurfaceComposerClient::getBuiltInDisplay(
+                ISurfaceComposer::eDisplayIdMain);
+  assert(s->dtoken != NULL);
+
+  status = SurfaceComposerClient::getDisplayInfo(s->dtoken, &s->dinfo);
+  assert(status == 0);
+
+  //int orientation = 3; // rotate framebuffer 270 degrees
+  int orientation = 1; // rotate framebuffer 90 degrees
+  if(orientation == 1 || orientation == 3) {
+      int temp = s->dinfo.h;
+      s->dinfo.h = s->dinfo.w;
+      s->dinfo.w = temp;
+  }
+
+  printf("dinfo %dx%d\n", s->dinfo.w, s->dinfo.h);
+
+  Rect destRect(s->dinfo.w, s->dinfo.h);
+  s->session->setDisplayProjection(s->dtoken, orientation, destRect, destRect);
+
+  s->control = s->session->createSurface(String8(name),
+                  s->dinfo.w, s->dinfo.h, PIXEL_FORMAT_RGBX_8888);
+  assert(s->control != NULL);
+
+  SurfaceComposerClient::openGlobalTransaction();
+  status = s->control->setLayer(layer);
+  SurfaceComposerClient::closeGlobalTransaction();
+  assert(status == 0);
+
+  s->s = s->control->getSurface();
+  assert(s->s != NULL);
+
+  // init opengl and egl
+  const EGLint attribs[] = {
+    EGL_RED_SIZE,     8,
+    EGL_GREEN_SIZE,   8,
+    EGL_BLUE_SIZE,    8,
+    EGL_ALPHA_SIZE,   alpha ? 8 : 0,
+    EGL_DEPTH_SIZE,   0,
+    EGL_STENCIL_SIZE, 8,
+    EGL_RENDERABLE_TYPE, EGL_OPENGL_ES3_BIT_KHR,
+    EGL_NONE,
+  };
+
+  s->display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+  assert(s->display != EGL_NO_DISPLAY);
+
+  success = eglInitialize(s->display, &s->egl_major, &s->egl_minor);
+  assert(success);
+
+  printf("egl version %d.%d\n", s->egl_major, s->egl_minor);
+  
+  EGLint num_configs;
+  success = eglChooseConfig(s->display, attribs, &s->config, 1, &num_configs);
+  assert(success);
+
+  s->surface = eglCreateWindowSurface(s->display, s->config, s->s.get(), NULL);
+  assert(s->surface != EGL_NO_SURFACE);
+
+  const EGLint context_attribs[] = {
+    EGL_CONTEXT_CLIENT_VERSION, 3,
+    EGL_NONE,
+  };
+  s->context = eglCreateContext(s->display, s->config, NULL, context_attribs);
+  assert(s->context != EGL_NO_CONTEXT);
+
+  EGLint w, h;
+  eglQuerySurface(s->display, s->surface, EGL_WIDTH, &w);
+  eglQuerySurface(s->display, s->surface, EGL_HEIGHT, &h);
+  printf("egl w %d h %d\n", w, h);
+
+  success = eglMakeCurrent(s->display, s->surface, s->surface, s->context);
+  assert(success);
+
+  printf("gl version %s\n", glGetString(GL_VERSION));
+
+
+  // set brightness
+  int brightness_fd = open(BACKLIGHT_CONTROL, O_RDWR);
+  const char brightness_level[] = BACKLIGHT_LEVEL;
+  write(brightness_fd, brightness_level, strlen(brightness_level));
+
+  if (out_w) *out_w = w;
+  if (out_h) *out_h = h;
+
+  return s;
+}
+
+extern "C" void framebuffer_swap(FramebufferState *s) {
+  eglSwapBuffers(s->display, s->surface);
+  assert(glGetError() == GL_NO_ERROR);
+}
+

selfdrive/common/framebuffer.h

@@ -0,0 +1,50 @@
+#ifndef FRAMEBUFFER_H
+#define FRAMEBUFFER_H
+
+#include <EGL/eglext.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct FramebufferState FramebufferState;
+
+FramebufferState* framebuffer_init(
+    const char* name, int32_t layer, int alpha,
+    int *out_w, int *out_h);
+
+void framebuffer_set_power(FramebufferState *s, int mode);
+void framebuffer_swap(FramebufferState *s);
+
+/* Display power modes */
+enum {
+    /* The display is turned off (blanked). */
+    HWC_POWER_MODE_OFF      = 0,
+    /* The display is turned on and configured in a low power state
+     * that is suitable for presenting ambient information to the user,
+     * possibly with lower fidelity than normal but greater efficiency. */
+    HWC_POWER_MODE_DOZE     = 1,
+    /* The display is turned on normally. */
+    HWC_POWER_MODE_NORMAL   = 2,
+    /* The display is configured as in HWC_POWER_MODE_DOZE but may
+     * stop applying frame buffer updates from the graphics subsystem.
+     * This power mode is effectively a hint from the doze dream to
+     * tell the hardware that it is done drawing to the display for the
+     * time being and that the display should remain on in a low power
+     * state and continue showing its current contents indefinitely
+     * until the mode changes.
+     *
+     * This mode may also be used as a signal to enable hardware-based doze
+     * functionality.  In this case, the doze dream is effectively
+     * indicating that the hardware is free to take over the display
+     * and manage it autonomously to implement low power always-on display
+     * functionality. */
+    HWC_POWER_MODE_DOZE_SUSPEND  = 3,
+};
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/glutil.c

@@ -0,0 +1,71 @@
+#include <stdlib.h>
+#include <stdio.h>
+
+#include <GLES3/gl3.h>
+
+#include "glutil.h"
+
+GLuint load_shader(GLenum shaderType, const char *src) {
+  GLint status = 0, len = 0;
+  GLuint shader;
+
+  if (!(shader = glCreateShader(shaderType)))
+    return 0;
+
+  glShaderSource(shader, 1, &src, NULL);
+  glCompileShader(shader);
+  glGetShaderiv(shader, GL_COMPILE_STATUS, &status);
+
+  if (status)
+    return shader;
+
+  glGetShaderiv(shader, GL_INFO_LOG_LENGTH, &len);
+  if (len) {
+    char *msg = (char*)malloc(len);
+    if (msg) {
+      glGetShaderInfoLog(shader, len, NULL, msg);
+      msg[len-1] = 0;
+      fprintf(stderr, "error compiling shader:\n%s\n", msg);
+      free(msg);
+    }
+  }
+  glDeleteShader(shader);
+  return 0;
+}
+
+GLuint load_program(const char *vert_src, const char *frag_src) {
+  GLuint vert, frag, prog;
+  GLint status = 0, len = 0;
+
+  if (!(vert = load_shader(GL_VERTEX_SHADER, vert_src)))
+    return 0;
+  if (!(frag = load_shader(GL_FRAGMENT_SHADER, frag_src)))
+    goto fail_frag;
+  if (!(prog = glCreateProgram()))
+    goto fail_prog;
+
+  glAttachShader(prog, vert);
+  glAttachShader(prog, frag);
+  glLinkProgram(prog);
+
+  glGetProgramiv(prog, GL_LINK_STATUS, &status);
+  if (status)
+    return prog;
+
+  glGetProgramiv(prog, GL_INFO_LOG_LENGTH, &len);
+  if (len) {
+    char *buf = (char*) malloc(len);
+    if (buf) {
+      glGetProgramInfoLog(prog, len, NULL, buf);
+      buf[len-1] = 0;
+      fprintf(stderr, "error linking program:\n%s\n", buf);
+      free(buf);
+    }
+  }
+  glDeleteProgram(prog);
+fail_prog:
+  glDeleteShader(frag);
+fail_frag:
+  glDeleteShader(vert);
+  return 0;
+}

selfdrive/common/glutil.h

@@ -0,0 +1,8 @@
+#ifndef COMMON_GLUTIL_H
+#define COMMON_GLUTIL_H
+
+#include <GLES3/gl3.h>
+GLuint load_shader(GLenum shaderType, const char *src);
+GLuint load_program(const char *vert_src, const char *frag_src);
+
+#endif

selfdrive/common/ipc.c

@@ -0,0 +1,119 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.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>
+
+#include "ipc.h"
+
+int ipc_connect(const char* socket_path) {
+  int err;
+
+  int sock = socket(AF_UNIX, SOCK_SEQPACKET, 0);
+  assert(sock >= 0);
+  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 = socket(AF_UNIX, SOCK_SEQPACKET, 0);
+  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) {
+  int err;
+
+  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);
+      // printf("send clen %d -> %d\n", num_fds, cmsg->cmsg_len);
+    }
+    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);
+      // printf("recv clen %d -> %d\n", cmsg->cmsg_len, recv_fds);
+      // assert(cmsg->cmsg_len == CMSG_LEN(sizeof(int) * num_fds));
+
+      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;
+  }
+}

selfdrive/common/ipc.h

@@ -0,0 +1,19 @@
+#ifndef IPC_H
+#define IPC_H
+
+#include <stdbool.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+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);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#endif

selfdrive/common/mat.h

@@ -0,0 +1,88 @@
+#ifndef COMMON_MAT_H
+#define COMMON_MAT_H
+
+typedef struct vec3 {
+	float v[3];
+} vec3;
+
+typedef struct vec4 {
+  float v[4];
+} vec4;
+
+typedef struct mat3 {
+	float v[3*3];
+} mat3;
+
+typedef struct mat4 {
+  float v[4*4];
+} mat4;
+
+static inline mat3 matmul3(const mat3 a, const mat3 b) {
+  mat3 ret = {{0.0}};
+  for (int r=0; r<3; r++) {
+    for (int c=0; c<3; c++) {
+      float v = 0.0;
+      for (int k=0; k<3; k++) {
+        v += a.v[r*3+k] * b.v[k*3+c];
+      }
+      ret.v[r*3+c] = v;
+    }
+  }
+  return ret;
+}
+
+static inline vec3 matvecmul3(const mat3 a, const vec3 b) {
+  vec3 ret = {{0.0}};
+  for (int r=0; r<3; r++) {
+    for (int c=0; c<3; c++) {
+      ret.v[r] += a.v[r*3+c] * b.v[c];
+    }
+  }
+  return ret;
+}
+
+static inline mat4 matmul(const mat4 a, const mat4 b) {
+  mat4 ret = {{0.0}};
+  for (int r=0; r<4; r++) {
+    for (int c=0; c<4; c++) {
+      float v = 0.0;
+      for (int k=0; k<4; k++) {
+        v += a.v[r*4+k] * b.v[k*4+c];
+      }
+      ret.v[r*4+c] = v;
+    }
+  }
+  return ret;
+}
+
+static inline vec4 matvecmul(const mat4 a, const vec4 b) {
+  vec4 ret = {{0.0}};
+  for (int r=0; r<4; r++) {
+    for (int c=0; c<4; c++) {
+      ret.v[r] += a.v[r*4+c] * b.v[c];
+    }
+  }
+  return ret;
+}
+
+// scales the input and output space of a transformation matrix
+// that assumes pixel-center origin.
+static inline mat3 transform_scale_buffer(const mat3 in, float s) {
+  // in_pt = ( transform(out_pt/s + 0.5) - 0.5) * s
+
+  mat3 transform_out = {{
+    1.0f/s, 0.0f, 0.5f,
+    0.0f, 1.0f/s, 0.5f,
+    0.0f, 0.0f, 1.0f,
+  }};
+
+  mat3 transform_in = {{
+    s,  0.0f, -0.5f*s,
+    0.0f, s, -0.5f*s,
+    0.0f, 0.0f, 1.0f,
+  }};
+
+  return matmul3(transform_in, matmul3(in, transform_out));
+}
+
+#endif

selfdrive/common/messaging.h

@@ -0,0 +1,15 @@
+// the c version of cereal/messaging.py
+
+#include <zmq.h>
+
+// TODO: refactor to take in service instead of endpoint?
+void *sub_sock(void *ctx, const char *endpoint) {
+  void* sock = zmq_socket(ctx, ZMQ_SUB);
+  assert(sock);
+  zmq_setsockopt(sock, ZMQ_SUBSCRIBE, "", 0);
+  int reconnect_ivl = 500;
+  zmq_setsockopt(sock, ZMQ_RECONNECT_IVL_MAX, &reconnect_ivl, sizeof(reconnect_ivl));
+  zmq_connect(sock, endpoint);
+  return sock;
+}
+

selfdrive/common/modeldata.h

@@ -0,0 +1,40 @@
+#ifndef MODELDATA_H
+#define MODELDATA_H
+
+#define MODEL_PATH_DISTANCE 192
+#define POLYFIT_DEGREE 4
+#define SPEED_PERCENTILES 10
+#define DESIRE_PRED_SIZE 32
+#define OTHER_META_SIZE 4
+
+typedef struct PathData {
+  float points[MODEL_PATH_DISTANCE];
+  float prob;
+  float std;
+  float stds[MODEL_PATH_DISTANCE];
+  float poly[POLYFIT_DEGREE];
+} PathData;
+
+typedef struct LeadData {
+  float dist;
+  float prob;
+  float std;
+  float rel_y;
+  float rel_y_std;
+  float rel_v;
+  float rel_v_std;
+  float rel_a;
+  float rel_a_std;
+} LeadData;
+
+typedef struct ModelData {
+  PathData path;
+  PathData left_lane;
+  PathData right_lane;
+  LeadData lead;
+  LeadData lead_future;
+  float meta[OTHER_META_SIZE + DESIRE_PRED_SIZE];
+  float speed[SPEED_PERCENTILES];
+} ModelData;
+
+#endif

selfdrive/common/mutex.h

@@ -0,0 +1,13 @@
+#ifndef COMMON_MUTEX_H
+#define COMMON_MUTEX_H
+
+#include <pthread.h>
+
+static inline void mutex_init_reentrant(pthread_mutex_t *mutex) {
+  pthread_mutexattr_t attr;
+  pthread_mutexattr_init(&attr);
+  pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_RECURSIVE);
+  pthread_mutex_init(mutex, &attr);
+}
+
+#endif

selfdrive/common/params.cc

@@ -0,0 +1,307 @@
+#include "common/params.h"
+
+#ifndef _GNU_SOURCE
+#define _GNU_SOURCE
+#endif  // _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <dirent.h>
+#include <sys/file.h>
+#include <sys/stat.h>
+
+#include <map>
+#include <string>
+
+#include "common/util.h"
+#include "common/utilpp.h"
+
+
+namespace {
+
+template <typename T>
+T* null_coalesce(T* a, T* b) {
+  return a != NULL ? a : b;
+}
+
+static const char* default_params_path = null_coalesce(
+    const_cast<const char*>(getenv("PARAMS_PATH")), "/data/params");
+
+}  // namespace
+
+static int fsync_dir(const char* path){
+  int result = 0;
+  int fd = open(path, O_RDONLY);
+
+  if (fd < 0){
+    result = -1;
+    goto cleanup;
+  }
+
+  result = fsync(fd);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+ cleanup:
+  int result_close = 0;
+  if (fd >= 0){
+    result_close = close(fd);
+  }
+
+  if (result_close < 0) {
+    return result_close;
+  } else {
+    return result;
+  }
+}
+
+int write_db_value(const char* params_path, const char* key, const char* value,
+                   size_t value_size) {
+  // Information about safely and atomically writing a file: https://lwn.net/Articles/457667/
+  // 1) Create temp file
+  // 2) Write data to temp file
+  // 3) fsync() the temp file
+  // 4) rename the temp file to the real name
+  // 5) fsync() the containing directory
+
+  int lock_fd = -1;
+  int tmp_fd = -1;
+  int result;
+  char tmp_path[1024];
+  char path[1024];
+  ssize_t bytes_written;
+
+  if (params_path == NULL) {
+    params_path = default_params_path;
+  }
+
+  // Write value to temp.
+  result =
+      snprintf(tmp_path, sizeof(tmp_path), "%s/.tmp_value_XXXXXX", params_path);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  tmp_fd = mkstemp(tmp_path);
+  bytes_written = write(tmp_fd, value, value_size);
+  if (bytes_written != value_size) {
+    result = -20;
+    goto cleanup;
+  }
+
+  // Build lock path
+  result = snprintf(path, sizeof(path), "%s/.lock", params_path);
+  if (result < 0) {
+    goto cleanup;
+  }
+  lock_fd = open(path, 0);
+
+  // Build key path
+  result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // Take lock.
+  result = flock(lock_fd, LOCK_EX);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // change permissions to 0666 for apks
+  result = fchmod(tmp_fd, 0666);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // fsync to force persist the changes.
+  result = fsync(tmp_fd);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // Move temp into place.
+  result = rename(tmp_path, path);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // fsync parent directory
+  result = snprintf(path, sizeof(path), "%s/d", params_path);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  result = fsync_dir(path);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+cleanup:
+  // Release lock.
+  if (lock_fd >= 0) {
+    close(lock_fd);
+  }
+  if (tmp_fd >= 0) {
+    if (result < 0) {
+      remove(tmp_path);
+    }
+    close(tmp_fd);
+  }
+  return result;
+}
+
+int delete_db_value(const char* params_path, const char* key) {
+  int lock_fd = -1;
+  int result;
+  char path[1024];
+
+  if (params_path == NULL) {
+    params_path = default_params_path;
+  }
+
+  // Build lock path, and open lockfile
+  result = snprintf(path, sizeof(path), "%s/.lock", params_path);
+  if (result < 0) {
+    goto cleanup;
+  }
+  lock_fd = open(path, 0);
+
+  // Take lock.
+  result = flock(lock_fd, LOCK_EX);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // Build key path
+  result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // Delete value.
+  result = remove(path);
+  if (result != 0) {
+    result = ERR_NO_VALUE;
+    goto cleanup;
+  }
+
+  // fsync parent directory
+  result = snprintf(path, sizeof(path), "%s/d", params_path);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  result = fsync_dir(path);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+cleanup:
+  // Release lock.
+  if (lock_fd >= 0) {
+    close(lock_fd);
+  }
+  return result;
+}
+
+int read_db_value(const char* params_path, const char* key, char** value,
+                  size_t* value_sz) {
+  int lock_fd = -1;
+  int result;
+  char path[1024];
+
+  if (params_path == NULL) {
+    params_path = default_params_path;
+  }
+
+  result = snprintf(path, sizeof(path), "%s/.lock", params_path);
+  if (result < 0) {
+    goto cleanup;
+  }
+  lock_fd = open(path, 0);
+
+  result = snprintf(path, sizeof(path), "%s/d/%s", params_path, key);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // Take lock.
+  result = flock(lock_fd, LOCK_EX);
+  if (result < 0) {
+    goto cleanup;
+  }
+
+  // Read value.
+  // TODO(mgraczyk): If there is a lot of contention, we can release the lock
+  //                 after opening the file, before reading.
+  *value = static_cast<char*>(read_file(path, value_sz));
+  if (*value == NULL) {
+    result = -22;
+    goto cleanup;
+  }
+
+  result = 0;
+
+cleanup:
+  // Release lock.
+  if (lock_fd >= 0) {
+    close(lock_fd);
+  }
+  return result;
+}
+
+void read_db_value_blocking(const char* params_path, const char* key,
+                            char** value, size_t* value_sz) {
+  while (1) {
+    const int result = read_db_value(params_path, key, value, value_sz);
+    if (result == 0) {
+      return;
+    } else {
+      // Sleep for 0.1 seconds.
+      usleep(100000);
+    }
+  }
+}
+
+int read_db_all(const char* params_path, std::map<std::string, std::string> *params) {
+  int err = 0;
+
+  if (params_path == NULL) {
+    params_path = default_params_path;
+  }
+
+  std::string lock_path = util::string_format("%s/.lock", params_path);
+
+  int lock_fd = open(lock_path.c_str(), 0);
+  if (lock_fd < 0) return -1;
+
+  err = flock(lock_fd, LOCK_EX);
+  if (err < 0) return err;
+
+  std::string key_path = util::string_format("%s/d", params_path);
+  DIR *d = opendir(key_path.c_str());
+  if (!d) {
+    close(lock_fd);
+    return -1;
+  }
+
+  struct dirent *de = NULL;
+  while ((de = readdir(d))) {
+    if (!isalnum(de->d_name[0])) continue;
+    std::string key = std::string(de->d_name);
+
+    if (key == "AccessToken") continue;
+
+    std::string value = util::read_file(util::string_format("%s/%s", key_path.c_str(), key.c_str()));
+
+    (*params)[key] = value;
+  }
+
+  closedir(d);
+
+  close(lock_fd);
+  return 0;
+}

selfdrive/common/params.h

@@ -0,0 +1,47 @@
+#ifndef _SELFDRIVE_COMMON_PARAMS_H_
+#define _SELFDRIVE_COMMON_PARAMS_H_
+
+#include <stddef.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define ERR_NO_VALUE -33
+
+int write_db_value(const char* params_path, const char* key, const char* value,
+                   size_t value_size);
+
+// Reads a value from the params database.
+// Inputs:
+//  params_path: The path of the database, or NULL to use the default.
+//  key: The key to read.
+//  value: A pointer where a newly allocated string containing the db value will
+//         be written.
+//  value_sz: A pointer where the size of value will be written. Does not
+//            include the NULL terminator.
+//
+// Returns: Negative on failure, otherwise 0.
+int read_db_value(const char* params_path, const char* key, char** value,
+                  size_t* value_sz);
+
+// Delete a value from the params database.
+// Inputs are the same as read_db_value, without value and value_sz.
+int delete_db_value(const char* params_path, const char* key);
+
+// Reads a value from the params database, blocking until successful.
+// Inputs are the same as read_db_value.
+void read_db_value_blocking(const char* params_path, const char* key,
+                            char** value, size_t* value_sz);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#ifdef __cplusplus
+#include <map>
+#include <string>
+int read_db_all(const char* params_path, std::map<std::string, std::string> *params);
+#endif
+
+#endif  // _SELFDRIVE_COMMON_PARAMS_H_

selfdrive/common/spinner.c

@@ -0,0 +1,182 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <math.h>
+#include <unistd.h>
+#include <assert.h>
+#include <ctype.h>
+
+#include <GLES3/gl3.h>
+#include <EGL/egl.h>
+#include <EGL/eglext.h>
+
+#include "nanovg.h"
+#define NANOVG_GLES3_IMPLEMENTATION
+#include "nanovg_gl.h"
+#include "nanovg_gl_utils.h"
+
+#include "framebuffer.h"
+#include "spinner.h"
+
+#define SPINTEXT_LENGTH 128
+
+// external resources linked in
+extern const unsigned char _binary_opensans_semibold_ttf_start[];
+extern const unsigned char _binary_opensans_semibold_ttf_end[];
+
+extern const unsigned char _binary_img_spinner_track_png_start[];
+extern const unsigned char _binary_img_spinner_track_png_end[];
+
+extern const unsigned char _binary_img_spinner_comma_png_start[];
+extern const unsigned char _binary_img_spinner_comma_png_end[];
+
+bool stdin_input_available() {
+  struct timeval timeout;
+  timeout.tv_sec = 0;
+  timeout.tv_usec = 0;
+
+  fd_set fds;
+  FD_ZERO(&fds);
+  FD_SET(STDIN_FILENO, &fds);
+  select(STDIN_FILENO+1, &fds, NULL, NULL, &timeout);
+  return (FD_ISSET(0, &fds));
+}
+
+int spin(int argc, char** argv) {
+  int err;
+
+  bool draw_progress = false;
+  float progress_val = 0.0;
+
+  char spintext[SPINTEXT_LENGTH];
+  spintext[0] = 0;
+
+  const char* spintext_arg = NULL;
+  if (argc >= 2) {
+    strncpy(spintext, argv[1], SPINTEXT_LENGTH);
+  }
+
+  // spinner
+  int fb_w, fb_h;
+  FramebufferState *fb = framebuffer_init("spinner", 0x00001000, false,
+                                          &fb_w, &fb_h);
+  assert(fb);
+
+  NVGcontext *vg = nvgCreateGLES3(NVG_ANTIALIAS | NVG_STENCIL_STROKES);
+  assert(vg);
+
+  int font = nvgCreateFontMem(vg, "Bold", (unsigned char*)_binary_opensans_semibold_ttf_start, _binary_opensans_semibold_ttf_end-_binary_opensans_semibold_ttf_start, 0);
+  assert(font >= 0);
+
+  int spinner_img = nvgCreateImageMem(vg, 0, (unsigned char*)_binary_img_spinner_track_png_start, _binary_img_spinner_track_png_end - _binary_img_spinner_track_png_start);
+  assert(spinner_img >= 0);
+  int spinner_img_s = 360;
+  int spinner_img_x = ((fb_w/2)-(spinner_img_s/2));
+  int spinner_img_y = 260;
+  int spinner_img_xc = (fb_w/2);
+  int spinner_img_yc = (fb_h/2)-100;
+  int spinner_comma_img = nvgCreateImageMem(vg, 0, (unsigned char*)_binary_img_spinner_comma_png_start, _binary_img_spinner_comma_png_end - _binary_img_spinner_comma_png_start);
+  assert(spinner_comma_img >= 0);
+
+  for (int cnt = 0; ; cnt++) {
+    // Check stdin for new text
+    if (stdin_input_available()){
+      fgets(spintext, SPINTEXT_LENGTH, stdin);
+      spintext[strcspn(spintext, "\n")] = 0;
+
+      // Check if number (update progress bar)
+      size_t len = strlen(spintext);
+      bool is_number = len > 0;
+      for (int i = 0; i < len; i++){
+        if (!isdigit(spintext[i])){
+          is_number = false;
+          break;
+        }
+      }
+
+      if (is_number) {
+        progress_val = (float)(atoi(spintext)) / 100.0;
+        progress_val = fmin(1.0, progress_val);
+        progress_val = fmax(0.0, progress_val);
+      }
+
+      draw_progress = is_number;
+    }
+
+    glClearColor(0.1, 0.1, 0.1, 1.0);
+    glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
+    glEnable(GL_BLEND);
+    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
+    nvgBeginFrame(vg, fb_w, fb_h, 1.0f);
+
+    // background
+    nvgBeginPath(vg);
+    NVGpaint bg = nvgLinearGradient(vg, fb_w, 0, fb_w, fb_h,
+    nvgRGBA(0, 0, 0, 175), nvgRGBA(0, 0, 0, 255));
+    nvgFillPaint(vg, bg);
+    nvgRect(vg, 0, 0, fb_w, fb_h);
+    nvgFill(vg);
+
+    // spin track
+    nvgSave(vg);
+    nvgTranslate(vg, spinner_img_xc, spinner_img_yc);
+    nvgRotate(vg, (3.75*M_PI * cnt/120.0));
+    nvgTranslate(vg, -spinner_img_xc, -spinner_img_yc);
+    NVGpaint spinner_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
+      spinner_img_s, spinner_img_s, 0, spinner_img, 0.6f);
+    nvgBeginPath(vg);
+    nvgFillPaint(vg, spinner_imgPaint);
+    nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
+    nvgFill(vg);
+    nvgRestore(vg);
+
+    // comma
+    NVGpaint comma_imgPaint = nvgImagePattern(vg, spinner_img_x, spinner_img_y,
+      spinner_img_s, spinner_img_s, 0, spinner_comma_img, 1.0f);
+    nvgBeginPath(vg);
+    nvgFillPaint(vg, comma_imgPaint);
+    nvgRect(vg, spinner_img_x, spinner_img_y, spinner_img_s, spinner_img_s);
+    nvgFill(vg);
+
+    if (draw_progress){
+      // draw progress bar
+      int progress_width = 1000;
+      int progress_x = fb_w/2-progress_width/2;
+      int progress_y = 775;
+      int progress_height = 25;
+
+      NVGpaint paint = nvgBoxGradient(
+          vg, progress_x + 1, progress_y + 1,
+          progress_width - 2, progress_height, 3, 4, nvgRGB(27, 27, 27), nvgRGB(27, 27, 27));
+      nvgBeginPath(vg);
+      nvgRoundedRect(vg, progress_x, progress_y, progress_width, progress_height, 12);
+      nvgFillPaint(vg, paint);
+      nvgFill(vg);
+
+      int bar_pos = ((progress_width - 2) * progress_val);
+
+      paint = nvgBoxGradient(
+          vg, progress_x, progress_y,
+          bar_pos+1.5f, progress_height-1, 3, 4,
+          nvgRGB(245, 245, 245), nvgRGB(105, 105, 105));
+
+      nvgBeginPath(vg);
+      nvgRoundedRect(
+          vg, progress_x+1, progress_y+1,
+          bar_pos, progress_height-2, 12);
+      nvgFillPaint(vg, paint);
+      nvgFill(vg);
+    } else {
+      // message
+      nvgTextAlign(vg, NVG_ALIGN_CENTER | NVG_ALIGN_TOP);
+      nvgFontSize(vg, 96.0f);
+      nvgText(vg, fb_w/2, (fb_h*2/3)+24, spintext, NULL);
+    }
+
+    nvgEndFrame(vg);
+    framebuffer_swap(fb);
+    assert(glGetError() == GL_NO_ERROR);
+  }
+
+  return 0;
+}

selfdrive/common/spinner.h

@@ -0,0 +1,14 @@
+#ifndef COMMON_SPINNER_H
+#define COMMON_SPINNER_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+int spin(int argc, char** argv);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/swaglog.c

@@ -0,0 +1,119 @@
+#define _GNU_SOURCE
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+
+#include <pthread.h>
+#include <zmq.h>
+#include <json.h>
+
+#include "common/timing.h"
+#include "common/version.h"
+
+#include "swaglog.h"
+
+typedef struct LogState {
+  pthread_mutex_t lock;
+  bool inited;
+  JsonNode *ctx_j;
+  void *zctx;
+  void *sock;
+  int print_level;
+} LogState;
+
+static LogState s = {
+  .lock = PTHREAD_MUTEX_INITIALIZER,
+};
+
+static void cloudlog_bind_locked(const char* k, const char* v) {
+  json_append_member(s.ctx_j, k, json_mkstring(v));
+}
+
+static void cloudlog_init() {
+  if (s.inited) return;
+  s.ctx_j = json_mkobject();
+  s.zctx = zmq_ctx_new();
+  s.sock = zmq_socket(s.zctx, ZMQ_PUSH);
+  zmq_connect(s.sock, "ipc:///tmp/logmessage");
+
+  s.print_level = CLOUDLOG_WARNING;
+  const char* print_level = getenv("LOGPRINT");
+  if (print_level) {
+    if (strcmp(print_level, "debug") == 0) {
+      s.print_level = CLOUDLOG_DEBUG;
+    } else if (strcmp(print_level, "info") == 0) {
+      s.print_level = CLOUDLOG_INFO;
+    } else if (strcmp(print_level, "warning") == 0) {
+      s.print_level = CLOUDLOG_WARNING;
+    }
+  }
+
+  // openpilot bindings
+  char* dongle_id = getenv("DONGLE_ID");
+  if (dongle_id) {
+    cloudlog_bind_locked("dongle_id", dongle_id);
+  }
+  cloudlog_bind_locked("version", COMMA_VERSION);
+  bool dirty = !getenv("CLEAN");
+  json_append_member(s.ctx_j, "dirty", json_mkbool(dirty));
+
+  s.inited = true;
+}
+
+void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
+                const char* fmt, ...) {
+  pthread_mutex_lock(&s.lock);
+  cloudlog_init();
+
+  char* msg_buf = NULL;
+  va_list args;
+  va_start(args, fmt);
+  vasprintf(&msg_buf, fmt, args);
+  va_end(args);
+
+  if (!msg_buf) {
+    pthread_mutex_unlock(&s.lock);
+    return;
+  }
+
+  if (levelnum >= s.print_level) {
+    printf("%s: %s\n", filename, msg_buf);
+  }
+
+  JsonNode *log_j = json_mkobject();
+  assert(log_j);
+
+  json_append_member(log_j, "msg", json_mkstring(msg_buf));
+  json_append_member(log_j, "ctx", s.ctx_j);
+  json_append_member(log_j, "levelnum", json_mknumber(levelnum));
+  json_append_member(log_j, "filename", json_mkstring(filename));
+  json_append_member(log_j, "lineno", json_mknumber(lineno));
+  json_append_member(log_j, "funcname", json_mkstring(func));
+  json_append_member(log_j, "created", json_mknumber(seconds_since_epoch()));
+
+  char* log_s = json_encode(log_j);
+  assert(log_s);
+
+  json_remove_from_parent(s.ctx_j);  
+
+  json_delete(log_j);
+  free(msg_buf);
+
+  char levelnum_c = levelnum;
+  zmq_send(s.sock, &levelnum_c, 1, ZMQ_NOBLOCK | ZMQ_SNDMORE);
+  zmq_send(s.sock, log_s, strlen(log_s), ZMQ_NOBLOCK);
+  free(log_s);
+
+  pthread_mutex_unlock(&s.lock);
+}
+
+void cloudlog_bind(const char* k, const char* v) {
+  pthread_mutex_lock(&s.lock);
+  cloudlog_init();
+  cloudlog_bind_locked(k, v);
+  pthread_mutex_unlock(&s.lock);
+}

selfdrive/common/swaglog.h

@@ -0,0 +1,68 @@
+#ifndef SWAGLOG_H
+#define SWAGLOG_H
+
+#include "selfdrive/common/timing.h"
+
+#define CLOUDLOG_DEBUG 10
+#define CLOUDLOG_INFO 20
+#define CLOUDLOG_WARNING 30
+#define CLOUDLOG_ERROR 40
+#define CLOUDLOG_CRITICAL 50
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+void cloudlog_e(int levelnum, const char* filename, int lineno, const char* func,
+                const char* fmt, ...) /*__attribute__ ((format (printf, 6, 7)))*/;
+
+void cloudlog_bind(const char* k, const char* v);
+
+#ifdef __cplusplus
+}
+#endif
+
+#define cloudlog(lvl, fmt, ...) cloudlog_e(lvl, __FILE__, __LINE__, \
+                                           __func__, \
+                                           fmt, ## __VA_ARGS__)
+
+#define cloudlog_rl(burst, millis, lvl, fmt, ...)   \
+{                                                   \
+  static uint64_t __begin = 0;                      \
+  static int __printed = 0;                         \
+  static int __missed = 0;                          \
+                                                    \
+  int __burst = (burst);                            \
+  int __millis = (millis);                          \
+  uint64_t __ts = nanos_since_boot();               \
+                                                    \
+  if (!__begin) __begin = __ts;                     \
+                                                    \
+  if (__begin + __millis*1000000ULL < __ts) {       \
+    if (__missed) {                                 \
+      cloudlog(CLOUDLOG_WARNING, "cloudlog: %d messages supressed", __missed); \
+    }                                               \
+    __begin = 0;                                    \
+    __printed = 0;                                  \
+    __missed = 0;                                   \
+  }                                                 \
+                                                    \
+  if (__printed < __burst) {                        \
+    cloudlog(lvl, fmt, ## __VA_ARGS__);             \
+    __printed++;                                    \
+  } else {                                          \
+    __missed++;                                     \
+  }                                                 \
+}
+
+#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__)
+
+#define LOGD_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_DEBUG, fmt, ## __VA_ARGS__)
+#define LOG_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_INFO, fmt, ## __VA_ARGS__)
+#define LOGW_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_WARNING, fmt, ## __VA_ARGS__)
+#define LOGE_100(fmt, ...) cloudlog_rl(2, 100, CLOUDLOG_ERROR, fmt, ## __VA_ARGS__)
+
+#endif

selfdrive/common/test_params.c

@@ -0,0 +1,64 @@
+#include "selfdrive/common/params.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+static const char* const kUsage = "%s: read|write|read_block params_path key [value]\n";
+
+int main(int argc, const char* argv[]) {
+  if (argc < 4) {
+    printf(kUsage, argv[0]);
+    return 0;
+  }
+
+  const char* params_path = argv[2];
+  const char* key = argv[3];
+  if (strcmp(argv[1], "read") == 0) {
+    char* value;
+    size_t value_size;
+    int result = read_db_value(params_path, key, &value, &value_size);
+    if (result >= 0) {
+      fprintf(stdout, "Read %zu bytes: ", value_size);
+      fwrite(value, 1, value_size, stdout);
+      fprintf(stdout, "\n");
+      free(value);
+    } else {
+      fprintf(stderr, "Error reading: %d\n", result);
+      return -1;
+    }
+  } else if (strcmp(argv[1], "write") == 0) {
+    if (argc < 5) {
+      fprintf(stderr, "Error: write value required\n");
+      return 1;
+    }
+
+    const char* value = argv[4];
+    const size_t value_size = strlen(value);
+    int result = write_db_value(params_path, key, value, value_size);
+    if (result >= 0) {
+      fprintf(stdout, "Wrote %s to %s\n", value, key);
+    } else {
+      fprintf(stderr, "Error writing: %d\n", result);
+      return -1;
+    }
+  } else if (strcmp(argv[1], "read_block") == 0) {
+    char* value;
+    size_t value_size;
+    read_db_value_blocking(params_path, key, &value, &value_size);
+    fprintf(stdout, "Read %zu bytes: ", value_size);
+    fwrite(value, 1, value_size, stdout);
+    fprintf(stdout, "\n");
+    free(value);
+  } else {
+    printf(kUsage, argv[0]);
+    return 1;
+  }
+
+  return 0;
+}
+
+// BUILD:
+// $ gcc -I$HOME/one selfdrive/common/test_params.c selfdrive/common/params.c selfdrive/common/util.c -o ./test_params
+// $ seq 0 100000 | xargs -P20 -I{} ./test_params write /data/params DongleId {} && sleep 0.1 &
+// $ while ./test_params read /data/params DongleId; do sleep 0.05; done

selfdrive/common/timing.h

@@ -0,0 +1,54 @@
+#ifndef COMMON_TIMING_H
+#define COMMON_TIMING_H
+
+#include <stdint.h>
+#include <time.h>
+
+#ifdef __APPLE__
+#define CLOCK_BOOTTIME CLOCK_MONOTONIC
+#endif
+
+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 double millis_since_boot() {
+  struct timespec t;
+  clock_gettime(CLOCK_BOOTTIME, &t);
+  return t.tv_sec * 1000.0 + t.tv_nsec * 1e-6;
+}
+
+static inline double seconds_since_boot() {
+  struct timespec t;
+  clock_gettime(CLOCK_BOOTTIME, &t);
+  return (double)t.tv_sec + t.tv_nsec * 1e-9;;
+}
+
+static inline uint64_t nanos_since_epoch() {
+  struct timespec t;
+  clock_gettime(CLOCK_REALTIME, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+static inline double seconds_since_epoch() {
+  struct timespec t;
+  clock_gettime(CLOCK_REALTIME, &t);
+  return (double)t.tv_sec + t.tv_nsec * 1e-9;
+}
+
+// you probably should use nanos_since_boot instead
+static inline uint64_t nanos_monotonic() {
+  struct timespec t;
+  clock_gettime(CLOCK_MONOTONIC, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+static inline uint64_t nanos_monotonic_raw() {
+  struct timespec t;
+  clock_gettime(CLOCK_MONOTONIC_RAW, &t);
+  return t.tv_sec * 1000000000ULL + t.tv_nsec;
+}
+
+#endif

selfdrive/common/touch.c

@@ -0,0 +1,120 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <assert.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <dirent.h>
+#include <sys/poll.h>
+#include <linux/input.h>
+
+#include "touch.h"
+
+static int find_dev() {
+  int err;
+
+  int ret = -1;
+
+  DIR *dir = opendir("/dev/input");
+  assert(dir);
+  struct dirent* de = NULL;
+  while ((de = readdir(dir))) {
+    if (strncmp(de->d_name, "event", 5)) continue;
+
+    int fd = openat(dirfd(dir), de->d_name, O_RDONLY);
+    assert(fd >= 0);
+
+    unsigned char ev_bits[KEY_MAX / 8 + 1];
+    err = ioctl(fd, EVIOCGBIT(EV_ABS, sizeof(ev_bits)), ev_bits);
+    assert(err >= 0);
+
+    const int x_key = ABS_MT_POSITION_X / 8;
+    const int y_key = ABS_MT_POSITION_Y / 8;
+    if ((ev_bits[x_key] & (ABS_MT_POSITION_X - x_key)) &&
+        (ev_bits[y_key] & (ABS_MT_POSITION_Y - y_key))) {
+      ret = fd;
+      break;
+    }
+    close(fd);
+  }
+  closedir(dir);
+
+  return ret;
+}
+
+void touch_init(TouchState *s) {
+  s->fd = find_dev();
+  assert(s->fd >= 0);
+}
+
+int touch_poll(TouchState *s, int* out_x, int* out_y, int timeout) {
+  assert(out_x && out_y);
+  bool up = false;
+  while (true) {
+    struct pollfd polls[] = {{
+      .fd = s->fd,
+      .events = POLLIN,
+    }};
+    int err = poll(polls, 1, timeout);
+    if (err < 0) {
+      return -1;
+    }
+    if (!(polls[0].revents & POLLIN)) {
+      break;
+    }
+
+    struct input_event event;
+    err = read(polls[0].fd, &event, sizeof(event));
+    if (err < sizeof(event)) {
+      return -1;
+    }
+
+    switch (event.type) {
+    case EV_ABS:
+      if (event.code == ABS_MT_POSITION_X) {
+        s->last_x = event.value;
+      } else if (event.code == ABS_MT_POSITION_Y) {
+        s->last_y = event.value;
+      } else if (event.code == ABS_MT_TRACKING_ID && event.value != -1) {
+        up = true;
+      }
+      break;
+    default:
+      break;
+    }
+  }
+  if (up) {
+    // adjust for flippening
+    *out_x = s->last_y;
+    *out_y = 1080 - s->last_x;
+  }
+  return up;
+}
+
+int touch_read(TouchState *s, int* out_x, int* out_y) {
+  assert(out_x && out_y);
+  struct input_event event;
+  int err = read(s->fd, &event, sizeof(event));
+  if (err < sizeof(event)) {
+    return -1;
+  }
+  bool up = false;
+  switch (event.type) {
+  case EV_ABS:
+    if (event.code == ABS_MT_POSITION_X) {
+      s->last_x = event.value;
+    } else if (event.code == ABS_MT_POSITION_Y) {
+      s->last_y = event.value;
+    }
+    up = true;
+    break;
+  default:
+    break;
+  }
+  if (up) {
+    // adjust for flippening
+    *out_x = s->last_y;
+    *out_y = 1080 - s->last_x;
+  }
+  return up;
+}

selfdrive/common/touch.h

@@ -0,0 +1,21 @@
+#ifndef TOUCH_H
+#define TOUCH_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct TouchState {
+  int fd;
+  int last_x, last_y;
+} TouchState;
+
+void touch_init(TouchState *s);
+int touch_poll(TouchState *s, int *out_x, int *out_y, int timeout);
+int touch_read(TouchState *s, int* out_x, int* out_y);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/util.c

@@ -0,0 +1,61 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include <unistd.h>
+
+#ifdef __linux__
+#include <sys/prctl.h>
+#include <sys/syscall.h>
+#include <sched.h>
+#endif
+
+void* read_file(const char* path, size_t* out_len) {
+  FILE* f = fopen(path, "r");
+  if (!f) {
+    return NULL;
+  }
+  fseek(f, 0, SEEK_END);
+  long f_len = ftell(f);
+  rewind(f);
+
+  char* buf = (char*)calloc(f_len, 1);
+  assert(buf);
+
+  size_t num_read = fread(buf, f_len, 1, f);
+  fclose(f);
+
+  if (num_read != 1) {
+    free(buf);
+    return NULL;
+  }
+
+  if (out_len) {
+    *out_len = f_len;
+  }
+
+  return buf;
+}
+
+void set_thread_name(const char* name) {
+#ifdef __linux__
+  // pthread_setname_np is dumb (fails instead of truncates)
+  prctl(PR_SET_NAME, (unsigned long)name, 0, 0, 0);
+#endif
+}
+
+int set_realtime_priority(int level) {
+#ifdef __linux__
+
+  long tid = syscall(SYS_gettid);
+
+  // should match python using chrt
+  struct sched_param sa;
+  memset(&sa, 0, sizeof(sa));
+  sa.sched_priority = level;
+  return sched_setscheduler(tid, SCHED_FIFO, &sa);
+#else
+  return -1;
+#endif
+}
+

selfdrive/common/util.h

@@ -0,0 +1,48 @@
+#ifndef COMMON_UTIL_H
+#define COMMON_UTIL_H
+
+#include <stdio.h>
+
+#ifndef __cplusplus
+
+#define min(a,b) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+   _a < _b ? _a : _b; })
+
+#define max(a,b) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+   _a > _b ? _a : _b; })
+
+#endif
+
+#define clamp(a,b,c) \
+ ({ __typeof__ (a) _a = (a); \
+     __typeof__ (b) _b = (b); \
+     __typeof__ (c) _c = (c); \
+   _a < _b ? _b : (_a > _c ? _c : _a); })
+
+#define ARRAYSIZE(x) (sizeof(x)/sizeof(x[0]))
+
+#define ALIGN(x, align) (((x) + (align)-1) & ~((align)-1))
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// Reads a file into a newly allocated buffer.
+//
+// Returns NULL on failure, otherwise the NULL-terminated file contents.
+// The result must be freed by the caller.
+void* read_file(const char* path, size_t* out_len);
+
+void set_thread_name(const char* name);
+
+int set_realtime_priority(int level);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/utilpp.h

@@ -0,0 +1,66 @@
+#ifndef UTILPP_H
+#define UTILPP_H
+
+#include <cstdio>
+#include <unistd.h>
+
+#include <string>
+#include <memory>
+#include <sstream>
+#include <fstream>
+
+namespace util {
+
+inline bool starts_with(std::string s, std::string prefix) {
+  return s.compare(0, prefix.size(), prefix) == 0;
+}
+
+template<typename ... Args>
+inline std::string string_format( const std::string& format, Args ... args ) {
+    size_t size = snprintf( nullptr, 0, format.c_str(), args ... ) + 1;
+    std::unique_ptr<char[]> buf( new char[ size ] );
+    snprintf( buf.get(), size, format.c_str(), args ... );
+    return std::string( buf.get(), buf.get() + size - 1 );
+}
+
+inline std::string read_file(std::string fn) {
+  std::ifstream t(fn);
+  std::stringstream buffer;
+  buffer << t.rdbuf();
+  return buffer.str();
+}
+
+inline std::string tohex(const uint8_t* buf, size_t buf_size) {
+  std::unique_ptr<char[]> hexbuf(new char[buf_size*2+1]);
+  for (size_t i=0; i < buf_size; i++) {
+    sprintf(&hexbuf[i*2], "%02x", buf[i]);
+  }
+  hexbuf[buf_size*2] = 0;
+  return std::string(hexbuf.get(), hexbuf.get() + buf_size*2);
+}
+
+inline std::string base_name(std::string const & path) {
+  size_t pos = path.find_last_of("/");
+  if (pos == std::string::npos) return path;
+  return path.substr(pos + 1);
+}
+
+inline std::string dir_name(std::string const & path) {
+  size_t pos = path.find_last_of("/");
+  if (pos == std::string::npos) return "";
+  return path.substr(0, pos);
+}
+
+inline std::string readlink(std::string path) {
+  char buff[4096];
+  ssize_t len = ::readlink(path.c_str(), buff, sizeof(buff)-1);
+  if (len != -1) {
+    buff[len] = '\0';
+    return std::string(buff);
+  }
+  return "";
+}
+
+}
+
+#endif

selfdrive/common/version.h

@@ -0,0 +1 @@
+#define COMMA_VERSION "0.7.2"

selfdrive/common/visionbuf.h

@@ -0,0 +1,39 @@
+#ifndef IONBUF_H
+#define IONBUF_H
+
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct VisionBuf {
+  size_t len;
+  void* addr;
+  int handle;
+  int fd;
+
+  cl_context ctx;
+  cl_device_id device_id;
+  cl_mem buf_cl;
+  cl_command_queue copy_q;
+} VisionBuf;
+
+#define VISIONBUF_SYNC_FROM_DEVICE 0
+#define VISIONBUF_SYNC_TO_DEVICE 1
+
+VisionBuf visionbuf_allocate(size_t len);
+VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem);
+cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx);
+void visionbuf_sync(const VisionBuf* buf, int dir);
+void visionbuf_free(const VisionBuf* buf);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/visionbuf_cl.c

@@ -0,0 +1,110 @@
+#include "visionbuf.h"
+
+#include <stdio.h>
+#include <fcntl.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <sys/mman.h>
+#include <sys/types.h>
+
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+int offset = 0;
+void *malloc_with_fd(size_t len, int *fd) {
+  char full_path[0x100];
+  snprintf(full_path, sizeof(full_path)-1, "/dev/shm/visionbuf_%d_%d", getpid(), offset++);
+  *fd = open(full_path, O_RDWR | O_CREAT, 0777);
+  unlink(full_path);
+  ftruncate(*fd, len);
+  void *addr = mmap(NULL, len, PROT_READ | PROT_WRITE, MAP_SHARED, *fd, 0);
+  return addr;
+}
+
+VisionBuf visionbuf_allocate(size_t len) {
+  // const size_t alignment = 4096;
+  // void* addr = aligned_alloc(alignment, alignment * ((len - 1) / alignment + 1));
+  //void* addr = calloc(1, len);
+
+  int fd;
+  void *addr = malloc_with_fd(len, &fd);
+
+  return (VisionBuf){
+      .len = len, .addr = addr, .handle = 1, .fd = fd,
+  };
+}
+
+cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx) {
+  // HACK because this platform is just for convenience
+  VisionBuf *w_buf = (VisionBuf*)buf;
+  cl_mem ret;
+  *w_buf = visionbuf_allocate_cl(buf->len, device_id, ctx, &ret);
+  return ret;
+}
+
+VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem) {
+  int err;
+  assert(out_mem);
+
+#if __OPENCL_VERSION__ >= 200
+  void* host_ptr =
+      clSVMAlloc(ctx, CL_MEM_READ_WRITE | CL_MEM_SVM_FINE_GRAIN_BUFFER, len, 0);
+  assert(host_ptr);
+#else
+  int fd;
+  void* host_ptr = malloc_with_fd(len, &fd);
+
+  cl_command_queue q = clCreateCommandQueue(ctx, device_id, 0, &err);
+  assert(err == 0);
+#endif
+
+  cl_mem mem = clCreateBuffer(ctx, CL_MEM_READ_WRITE | CL_MEM_USE_HOST_PTR, len, host_ptr, &err);
+  assert(err == 0);
+
+  *out_mem = mem;
+
+  return (VisionBuf){
+      .len = len, .addr = host_ptr, .handle = 0, .fd = fd,
+      .device_id = device_id, .ctx = ctx, .buf_cl = mem,
+
+#if __OPENCL_VERSION__ < 200
+      .copy_q = q,
+#endif
+
+  };
+}
+
+void visionbuf_sync(const VisionBuf* buf, int dir) {
+  int err = 0;
+  if (!buf->buf_cl) return;
+
+#if __OPENCL_VERSION__ < 200
+  if (dir == VISIONBUF_SYNC_FROM_DEVICE) {
+    err = clEnqueueReadBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL);
+  } else {
+    err = clEnqueueWriteBuffer(buf->copy_q, buf->buf_cl, CL_FALSE, 0, buf->len, buf->addr, 0, NULL, NULL);
+  }
+  assert(err == 0);
+  clFinish(buf->copy_q);
+#endif
+}
+
+void visionbuf_free(const VisionBuf* buf) {
+  if (buf->handle) {
+    munmap(buf->addr, buf->len);
+    close(buf->fd);
+  } else {
+    int err = clReleaseMemObject(buf->buf_cl);
+    assert(err == 0);
+#if __OPENCL_VERSION__ >= 200
+    clSVMFree(buf->ctx, buf->addr);
+#else
+    munmap(buf->addr, buf->len);
+    close(buf->fd);
+#endif
+  }
+}

selfdrive/common/visionbuf_ion.c

@@ -0,0 +1,141 @@
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+#include <sys/mman.h>
+#include <sys/ioctl.h>
+
+#include <linux/ion.h>
+#include <CL/cl_ext.h>
+
+#include <msm_ion.h>
+
+#include "visionbuf.h"
+
+
+// just hard-code these for convenience
+// size_t device_page_size = 0;
+// clGetDeviceInfo(device_id, CL_DEVICE_PAGE_SIZE_QCOM,
+//                 sizeof(device_page_size), &device_page_size,
+//                 NULL);
+
+// size_t padding_cl = 0;
+// clGetDeviceInfo(device_id, CL_DEVICE_EXT_MEM_PADDING_IN_BYTES_QCOM,
+//                 sizeof(padding_cl), &padding_cl,
+//                 NULL);
+#define DEVICE_PAGE_SIZE_CL 4096
+#define PADDING_CL 0
+
+static int ion_fd = -1;
+static void ion_init() {
+  if (ion_fd == -1) {
+    ion_fd = open("/dev/ion", O_RDWR | O_NONBLOCK);
+  }
+}
+
+VisionBuf visionbuf_allocate(size_t len) {
+  int err;
+
+  ion_init();
+
+  struct ion_allocation_data ion_alloc = {0};
+  ion_alloc.len = len + PADDING_CL;
+  ion_alloc.align = 4096;
+  ion_alloc.heap_id_mask = 1 << ION_IOMMU_HEAP_ID;
+  ion_alloc.flags = ION_FLAG_CACHED;
+
+  err = 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 = ioctl(ion_fd, ION_IOC_SHARE, &ion_fd_data);
+  assert(err == 0);
+
+  void *addr = mmap(NULL, ion_alloc.len,
+                    PROT_READ | PROT_WRITE,
+                    MAP_SHARED, ion_fd_data.fd, 0);
+  assert(addr != MAP_FAILED);
+
+  memset(addr, 0, ion_alloc.len);
+
+  return (VisionBuf){
+    .len = len,
+    .addr = addr,
+    .handle = ion_alloc.handle,
+    .fd = ion_fd_data.fd,
+  };
+}
+
+VisionBuf visionbuf_allocate_cl(size_t len, cl_device_id device_id, cl_context ctx, cl_mem *out_mem) {
+  VisionBuf r = visionbuf_allocate(len);
+  *out_mem = visionbuf_to_cl(&r, device_id, ctx);
+  return r;
+}
+
+cl_mem visionbuf_to_cl(const VisionBuf* buf, cl_device_id device_id, cl_context ctx) {
+  int err = 0;
+
+  assert(((uintptr_t)buf->addr % DEVICE_PAGE_SIZE_CL) == 0);
+
+  cl_mem_ion_host_ptr ion_cl = {0};
+  ion_cl.ext_host_ptr.allocation_type = CL_MEM_ION_HOST_PTR_QCOM;
+  ion_cl.ext_host_ptr.host_cache_policy = CL_MEM_HOST_UNCACHED_QCOM;
+  ion_cl.ion_filedesc = buf->fd;
+  ion_cl.ion_hostptr = buf->addr;
+
+  cl_mem mem = clCreateBuffer(ctx,
+                              CL_MEM_USE_HOST_PTR | CL_MEM_EXT_HOST_PTR_QCOM,
+                              buf->len, &ion_cl, &err);
+  assert(err == 0);
+
+  return mem;
+}
+
+void visionbuf_sync(const VisionBuf* buf, int dir) {
+  int err;
+
+  struct ion_fd_data fd_data = {0};
+  fd_data.fd = buf->fd;
+  err = ioctl(ion_fd, ION_IOC_IMPORT, &fd_data);
+  assert(err == 0);
+
+  struct ion_flush_data flush_data = {0};
+  flush_data.handle = fd_data.handle;
+  flush_data.vaddr = buf->addr;
+  flush_data.offset = 0;
+  flush_data.length = buf->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};
+
+  switch (dir) {
+  case VISIONBUF_SYNC_FROM_DEVICE:
+    custom_data.cmd = ION_IOC_INV_CACHES;
+    break;
+  case VISIONBUF_SYNC_TO_DEVICE:
+    custom_data.cmd = ION_IOC_CLEAN_CACHES;
+    break;
+  default:
+    assert(0);
+  }
+
+  custom_data.arg = (unsigned long)&flush_data;
+  err = ioctl(ion_fd, ION_IOC_CUSTOM, &custom_data);
+  assert(err == 0);
+
+  struct ion_handle_data handle_data = {0};
+  handle_data.handle = fd_data.handle;
+  err = ioctl(ion_fd, ION_IOC_FREE, &handle_data);
+  assert(err == 0);
+}
+
+void visionbuf_free(const VisionBuf* buf) {
+  struct ion_handle_data handle_data = {
+    .handle = buf->handle,
+  };
+  int ret = ioctl(ion_fd, ION_IOC_FREE, &handle_data);
+  assert(ret == 0);
+}

selfdrive/common/visionimg.cc

@@ -0,0 +1,122 @@
+#include <cassert>
+
+#ifdef QCOM
+#include <system/graphics.h>
+#include <ui/GraphicBuffer.h>
+#include <ui/PixelFormat.h>
+#include <gralloc_priv.h>
+
+#include <GLES3/gl3.h>
+#define GL_GLEXT_PROTOTYPES
+#include <GLES2/gl2ext.h>
+
+#include <EGL/egl.h>
+#define EGL_EGLEXT_PROTOTYPES
+#include <EGL/eglext.h>
+
+#endif
+
+#include "common/util.h"
+#include "common/visionbuf.h"
+
+#include "common/visionimg.h"
+
+#ifdef QCOM
+
+using namespace android;
+
+// from libadreno_utils.so
+extern "C" void compute_aligned_width_and_height(int width,
+                                      int height,
+                                      int bpp,
+                                      int tile_mode,
+                                      int raster_mode,
+                                      int padding_threshold,
+                                      int *aligned_w,
+                                      int *aligned_h);
+#endif
+
+void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h) {
+#ifdef QCOM
+  compute_aligned_width_and_height(ALIGN(width, 32), ALIGN(height, 32), 3, 0, 0, 512, aligned_w, aligned_h);
+#else
+  *aligned_w = width; *aligned_h = height;
+#endif
+}
+
+VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf) {
+  int aligned_w = 0, aligned_h = 0;
+  visionimg_compute_aligned_width_and_height(width, height, &aligned_w, &aligned_h);
+
+  int stride = aligned_w * 3;
+  size_t size = aligned_w * aligned_h * 3;
+
+  VisionBuf buf = visionbuf_allocate(size);
+
+  *out_buf = buf;
+
+  return (VisionImg){
+    .fd = buf.fd,
+    .format = VISIONIMG_FORMAT_RGB24,
+    .width = width,
+    .height = height,
+    .stride = stride,
+    .size = size,
+    .bpp = 3,
+  };
+}
+
+#ifdef QCOM
+
+EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph) {
+  assert((img->size % img->stride) == 0);
+  assert((img->stride % img->bpp) == 0);
+
+  int format = 0;
+  if (img->format == VISIONIMG_FORMAT_RGB24) {
+    format = HAL_PIXEL_FORMAT_RGB_888;
+  } else {
+    assert(false);
+  }
+
+  private_handle_t* hnd = new private_handle_t(img->fd, img->size,
+                             private_handle_t::PRIV_FLAGS_USES_ION|private_handle_t::PRIV_FLAGS_FRAMEBUFFER,
+                             0, format,
+                             img->stride/img->bpp, img->size/img->stride,
+                             img->width, img->height);
+
+  GraphicBuffer* gb = new GraphicBuffer(img->width, img->height, (PixelFormat)format,
+                                        GraphicBuffer::USAGE_HW_TEXTURE, img->stride/img->bpp, hnd, false);
+  // GraphicBuffer is ref counted by EGLClientBuffer(ANativeWindowBuffer), no need and not possible to release.
+  *pph = hnd;
+  return (EGLClientBuffer) gb->getNativeBuffer();
+}
+
+GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph) {
+
+  EGLClientBuffer buf = visionimg_to_egl(img, pph);
+
+  EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+  assert(display != EGL_NO_DISPLAY);
+
+  EGLint img_attrs[] = { EGL_IMAGE_PRESERVED_KHR, EGL_TRUE, EGL_NONE };
+  EGLImageKHR image = eglCreateImageKHR(display, EGL_NO_CONTEXT,
+                                        EGL_NATIVE_BUFFER_ANDROID, buf, img_attrs);
+  assert(image != EGL_NO_IMAGE_KHR);
+
+  GLuint tex = 0;
+  glGenTextures(1, &tex);
+  glBindTexture(GL_TEXTURE_2D, tex);
+  glEGLImageTargetTexture2DOES(GL_TEXTURE_2D, image);
+  *pkhr = image;
+  return tex;
+}
+
+void visionimg_destroy_gl(EGLImageKHR khr, void *ph) {
+  EGLDisplay display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+  assert(display != EGL_NO_DISPLAY);
+  eglDestroyImageKHR(display, khr);
+  delete (private_handle_t*)ph;
+}
+
+#endif

selfdrive/common/visionimg.h

@@ -0,0 +1,36 @@
+#ifndef VISIONIMG_H
+#define VISIONIMG_H
+
+#include "common/visionbuf.h"
+
+#include <GLES3/gl3.h>
+#include <EGL/egl.h>
+#include <EGL/eglext.h>
+#undef Status
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define VISIONIMG_FORMAT_RGB24 1
+
+typedef struct VisionImg {
+  int fd;
+  int format;
+  int width, height, stride;
+  int bpp;
+  size_t size;
+} VisionImg;
+
+void visionimg_compute_aligned_width_and_height(int width, int height, int *aligned_w, int *aligned_h);
+VisionImg visionimg_alloc_rgb24(int width, int height, VisionBuf *out_buf);
+
+EGLClientBuffer visionimg_to_egl(const VisionImg *img, void **pph);
+GLuint visionimg_to_gl(const VisionImg *img, EGLImageKHR *pkhr, void **pph);
+void visionimg_destroy_gl(EGLImageKHR khr, void *ph);
+
+#ifdef __cplusplus
+}  // extern "C"
+#endif
+
+#endif

selfdrive/common/visionipc.c

@@ -0,0 +1,194 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.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>
+
+#include "ipc.h"
+
+#include "visionipc.h"
+
+typedef struct VisionPacketWire {
+  int type;
+  VisionPacketData d;
+} VisionPacketWire;
+
+int vipc_connect() {
+  return ipc_connect(VIPC_SOCKET_PATH);
+}
+
+
+int vipc_recv(int fd, VisionPacket *out_p) {
+  VisionPacketWire p = {0};
+  VisionPacket p2 = {0};
+  int ret = ipc_sendrecv_with_fds(false, fd, &p, sizeof(p), (int*)p2.fds, VIPC_MAX_FDS, &p2.num_fds);
+  if (ret < 0) {
+    printf("vipc_recv err: %s\n", strerror(errno));
+  } else {
+    p2.type = p.type;
+    p2.d = p.d;
+    *out_p = p2;
+  }
+  //printf("%d = vipc_recv(%d, %d): %d %d %d %u\n", ret, fd, p2.num_fds, out_p->d.stream_bufs.type, out_p->d.stream_bufs.width, out_p->d.stream_bufs.height, out_p->d.stream_bufs.buf_len);
+  return ret;
+}
+
+int vipc_send(int fd, const VisionPacket *p2) {
+  assert(p2->num_fds <= VIPC_MAX_FDS);
+
+  VisionPacketWire p = {
+    .type = p2->type,
+    .d = p2->d,
+  };
+  int ret = ipc_sendrecv_with_fds(true, fd, (void*)&p, sizeof(p), (int*)p2->fds, p2->num_fds, NULL);
+  //printf("%d = vipc_send(%d, %d): %d %d %d %u\n", ret, fd, p2->num_fds, p2->d.stream_bufs.type, p2->d.stream_bufs.width, p2->d.stream_bufs.height, p2->d.stream_bufs.buf_len);
+  return ret;
+}
+
+void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
+                     int num_fds, const int* fds) {
+  for (int i=0; i<num_fds; i++) {
+    if (bufs[i].addr) {
+      munmap(bufs[i].addr, bufs[i].len);
+      bufs[i].addr = NULL;
+      close(bufs[i].fd);
+    }
+    bufs[i].fd = fds[i];
+    bufs[i].len = stream_bufs->buf_len;
+    bufs[i].addr = mmap(NULL, bufs[i].len,
+                        PROT_READ | PROT_WRITE,
+                        MAP_SHARED, bufs[i].fd, 0);
+    // printf("b %d %zu -> %p\n", bufs[i].fd, bufs[i].len, bufs[i].addr);
+    assert(bufs[i].addr != MAP_FAILED);
+  }
+}
+
+
+int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info) {
+  int err;
+
+  memset(s, 0, sizeof(*s));
+
+  s->last_idx = -1;
+
+  s->ipc_fd = vipc_connect();
+  if (s->ipc_fd < 0) return -1;
+
+  VisionPacket p = {
+    .type = VIPC_STREAM_SUBSCRIBE,
+    .d = { .stream_sub = {
+      .type = type,
+      .tbuffer = tbuffer,
+    }, },
+  };
+  err = vipc_send(s->ipc_fd, &p);
+  if (err < 0) {
+    close(s->ipc_fd);
+    return -1;
+  }
+
+  VisionPacket rp;
+  err = vipc_recv(s->ipc_fd, &rp);
+  if (err <= 0) {
+    close(s->ipc_fd);
+    return -1;
+  }
+  assert(rp.type = VIPC_STREAM_BUFS);
+  assert(rp.d.stream_bufs.type == type);
+
+  s->bufs_info = rp.d.stream_bufs;
+
+  s->num_bufs = rp.num_fds;
+  s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf));
+  assert(s->bufs);
+
+  vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
+
+  if (out_bufs_info) {
+    *out_bufs_info = s->bufs_info;
+  }
+
+  return 0;
+}
+
+void visionstream_release(VisionStream *s) {
+  int err;
+  if (s->last_idx >= 0) {
+    VisionPacket rep = {
+      .type = VIPC_STREAM_RELEASE,
+      .d = { .stream_rel = {
+        .type = s->last_type,
+        .idx = s->last_idx,
+      }}
+    };
+    err = vipc_send(s->ipc_fd, &rep);
+    s->last_idx = -1;
+  }
+}
+
+VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) {
+  int err;
+
+  VisionPacket rp;
+  err = vipc_recv(s->ipc_fd, &rp);
+  if (err <= 0) {
+    return NULL;
+  }
+  assert(rp.type == VIPC_STREAM_ACQUIRE);
+
+  if (s->last_idx >= 0) {
+    VisionPacket rep = {
+      .type = VIPC_STREAM_RELEASE,
+      .d = { .stream_rel = {
+        .type = s->last_type,
+        .idx = s->last_idx,
+      }}
+    };
+    err = vipc_send(s->ipc_fd, &rep);
+    if (err <= 0) {
+      return NULL;
+    }
+  }
+
+  s->last_type = rp.d.stream_acq.type;
+  s->last_idx = rp.d.stream_acq.idx;
+  assert(s->last_idx < s->num_bufs);
+
+  if (out_extra) {
+    *out_extra = rp.d.stream_acq.extra;
+  }
+
+  return &s->bufs[s->last_idx];
+}
+
+void visionstream_destroy(VisionStream *s) {
+  int err;
+
+  if (s->last_idx >= 0) {
+    VisionPacket rep = {
+      .type = VIPC_STREAM_RELEASE,
+      .d = { .stream_rel = {
+        .type = s->last_type,
+        .idx = s->last_idx,
+      }}
+    };
+    err = vipc_send(s->ipc_fd, &rep);
+    s->last_idx = -1;
+  }
+
+  for (int i=0; i<s->num_bufs; i++) {
+    if (s->bufs[i].addr) {
+      munmap(s->bufs[i].addr, s->bufs[i].len);
+      s->bufs[i].addr = NULL;
+      close(s->bufs[i].fd);
+    }
+  }
+  if (s->bufs) free(s->bufs);
+  close(s->ipc_fd);
+}

selfdrive/common/visionipc.h

@@ -0,0 +1,113 @@
+#ifndef VISIONIPC_H
+#define VISIONIPC_H
+
+#include <stddef.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#define VIPC_SOCKET_PATH "/tmp/vision_socket"
+#define VIPC_MAX_FDS 64
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef enum VisionIPCPacketType {
+  VIPC_INVALID = 0,
+  VIPC_STREAM_SUBSCRIBE,
+  VIPC_STREAM_BUFS,
+  VIPC_STREAM_ACQUIRE,
+  VIPC_STREAM_RELEASE,
+} VisionIPCPacketType;
+
+typedef enum VisionStreamType {
+  VISION_STREAM_RGB_BACK,
+  VISION_STREAM_RGB_FRONT,
+  VISION_STREAM_YUV,
+  VISION_STREAM_YUV_FRONT,
+  VISION_STREAM_MAX,
+} VisionStreamType;
+
+typedef struct VisionUIInfo {
+  int big_box_x, big_box_y;
+  int big_box_width, big_box_height;
+  int transformed_width, transformed_height;
+
+  int front_box_x, front_box_y;
+  int front_box_width, front_box_height;
+} VisionUIInfo;
+
+typedef struct VisionStreamBufs {
+  VisionStreamType type;
+
+  int width, height, stride;
+  size_t buf_len;
+
+  union {
+    VisionUIInfo ui_info;
+  } buf_info;
+} VisionStreamBufs;
+
+typedef struct VIPCBufExtra {
+  // only for yuv
+  uint32_t frame_id;
+  uint64_t timestamp_eof;
+} VIPCBufExtra;
+
+typedef union VisionPacketData {
+  struct {
+    VisionStreamType type;
+    bool tbuffer;
+  } stream_sub;
+  VisionStreamBufs stream_bufs;
+  struct {
+    VisionStreamType type;
+    int idx;
+    VIPCBufExtra extra;
+  } stream_acq;
+  struct {
+    VisionStreamType type;
+    int idx;
+  } stream_rel;
+} VisionPacketData;
+
+typedef struct VisionPacket {
+  int type;
+  VisionPacketData d;
+  int num_fds;
+  int fds[VIPC_MAX_FDS];
+} VisionPacket;
+
+int vipc_connect(void);
+int vipc_recv(int fd, VisionPacket *out_p);
+int vipc_send(int fd, const VisionPacket *p);
+
+typedef struct VIPCBuf {
+  int fd;
+  size_t len;
+  void* addr;
+} VIPCBuf;
+void vipc_bufs_load(VIPCBuf *bufs, const VisionStreamBufs *stream_bufs,
+                     int num_fds, const int* fds);
+
+
+
+typedef struct VisionStream {
+  int ipc_fd;
+  int last_idx;
+  int last_type;
+  int num_bufs;
+  VisionStreamBufs bufs_info;
+  VIPCBuf *bufs;
+} VisionStream;
+
+int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info);
+void visionstream_release(VisionStream *s);
+VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra);
+void visionstream_destroy(VisionStream *s);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/common/visionstream.c

@@ -0,0 +1,124 @@
+
+
+int visionstream_init(VisionStream *s, VisionStreamType type, bool tbuffer, VisionStreamBufs *out_bufs_info) {
+  int err;
+
+  memset(s, 0, sizeof(*s));
+
+  s->last_idx = -1;
+
+  s->ipc_fd = vipc_connect();
+  if (s->ipc_fd < 0) return -1;
+
+  VisionPacket p = {
+    .type = VIPC_STREAM_SUBSCRIBE,
+    .d = { .stream_sub = {
+      .type = type,
+      .tbuffer = tbuffer,
+    }, },
+  };
+  err = vipc_send(s->ipc_fd, &p);
+  if (err < 0) {
+    close(s->ipc_fd);
+    return -1;
+  }
+
+  VisionPacket rp;
+  err = vipc_recv(s->ipc_fd, &rp);
+  if (err <= 0) {
+    close(s->ipc_fd);
+    return -1;
+  }
+  assert(rp.type = VIPC_STREAM_BUFS);
+  assert(rp.d.stream_bufs.type == type);
+
+  s->bufs_info = rp.d.stream_bufs;
+
+  s->num_bufs = rp.num_fds;
+  s->bufs = calloc(s->num_bufs, sizeof(VIPCBuf));
+  assert(s->bufs);
+
+  vipc_bufs_load(s->bufs, &rp.d.stream_bufs, s->num_bufs, rp.fds);
+
+  if (out_bufs_info) {
+    *out_bufs_info = s->bufs_info;
+  }
+
+  return 0;
+}
+
+void visionstream_release(VisionStream *s) {
+  int err;
+  if (s->last_idx >= 0) {
+    VisionPacket rep = {
+      .type = VIPC_STREAM_RELEASE,
+      .d = { .stream_rel = {
+        .type = s->last_type,
+        .idx = s->last_idx,
+      }}
+    };
+    err = vipc_send(s->ipc_fd, &rep);
+    s->last_idx = -1;
+  }
+}
+
+VIPCBuf* visionstream_get(VisionStream *s, VIPCBufExtra *out_extra) {
+  int err;
+
+  VisionPacket rp;
+  err = vipc_recv(s->ipc_fd, &rp);
+  if (err <= 0) {
+    return NULL;
+  }
+  assert(rp.type == VIPC_STREAM_ACQUIRE);
+
+  if (s->last_idx >= 0) {
+    VisionPacket rep = {
+      .type = VIPC_STREAM_RELEASE,
+      .d = { .stream_rel = {
+        .type = s->last_type,
+        .idx = s->last_idx,
+      }}
+    };
+    err = vipc_send(s->ipc_fd, &rep);
+    if (err <= 0) {
+      return NULL;
+    }
+  }
+
+  s->last_type = rp.d.stream_acq.type;
+  s->last_idx = rp.d.stream_acq.idx;
+  assert(s->last_idx < s->num_bufs);
+
+  if (out_extra) {
+    *out_extra = rp.d.stream_acq.extra;
+  }
+
+  return &s->bufs[s->last_idx];
+}
+
+void visionstream_destroy(VisionStream *s) {
+  int err;
+
+  if (s->last_idx >= 0) {
+    VisionPacket rep = {
+      .type = VIPC_STREAM_RELEASE,
+      .d = { .stream_rel = {
+        .type = s->last_type,
+        .idx = s->last_idx,
+      }}
+    };
+    err = vipc_send(s->ipc_fd, &rep);
+    s->last_idx = -1;
+  }
+
+  for (int i=0; i<s->num_bufs; i++) {
+    if (s->bufs[i].addr) {
+      munmap(s->bufs[i].addr, s->bufs[i].len);
+      s->bufs[i].addr = NULL;
+      close(s->bufs[i].fd);
+    }
+  }
+  if (s->bufs) free(s->bufs);
+  close(s->ipc_fd);
+}