logcatd, loggerd, mapd, modeld, proclogd

old-commit-hash: da079d47d7

selfdrive/logcatd/.gitignore

@@ -0,0 +1 @@
+logcatd

selfdrive/logcatd/SConscript

@@ -0,0 +1,2 @@
+Import('env', 'messaging')
+env.Program('logcatd.cc', LIBS=[messaging, 'cutils', 'zmq', 'czmq', 'capnp', 'kj'])

selfdrive/logcatd/logcatd.cc

@@ -0,0 +1,71 @@
+#include <cstdio>
+#include <cstdlib>
+#include <cassert>
+#include <android/log.h>
+
+//#include <log/log.h>
+#include <log/logger.h>
+#include <log/logprint.h>
+
+#include <capnp/serialize.h>
+#include "common/timing.h"
+#include "cereal/gen/cpp/log.capnp.h"
+#include "messaging.hpp"
+
+int main() {
+  int err;
+
+  struct logger_list *logger_list = android_logger_list_alloc(ANDROID_LOG_RDONLY, 0, 0);
+  assert(logger_list);
+  struct logger *main_logger = android_logger_open(logger_list, LOG_ID_MAIN);
+  assert(main_logger);
+  struct logger *radio_logger = android_logger_open(logger_list, LOG_ID_RADIO);
+  assert(radio_logger);
+  struct logger *system_logger = android_logger_open(logger_list, LOG_ID_SYSTEM);
+  assert(system_logger);
+  struct logger *crash_logger = android_logger_open(logger_list, LOG_ID_CRASH);
+  assert(crash_logger);
+//  struct logger *kernel_logger = android_logger_open(logger_list, LOG_ID_KERNEL);
+//  assert(kernel_logger);
+
+  Context * c = Context::create();
+  PubSocket * androidLog = PubSocket::create(c, "androidLog");
+  assert(androidLog != NULL);
+
+  while (1) {
+    log_msg log_msg;
+    err = android_logger_list_read(logger_list, &log_msg);
+    if (err <= 0) {
+      break;
+    }
+
+    AndroidLogEntry entry;
+    err = android_log_processLogBuffer(&log_msg.entry_v1, &entry);
+    if (err < 0) {
+      continue;
+    }
+
+    capnp::MallocMessageBuilder msg;
+    cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
+    auto androidEntry = event.initAndroidLogEntry();
+    androidEntry.setId(log_msg.id());
+    androidEntry.setTs(entry.tv_sec * 1000000000ULL + entry.tv_nsec);
+    androidEntry.setPriority(entry.priority);
+    androidEntry.setPid(entry.pid);
+    androidEntry.setTid(entry.tid);
+    androidEntry.setTag(entry.tag);
+    androidEntry.setMessage(entry.message);
+
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    androidLog->send((char*)bytes.begin(), bytes.size());
+  }
+
+  android_logger_list_close(logger_list);
+
+  delete c;
+  delete androidLog;
+
+  return 0;
+}

selfdrive/loggerd/.gitignore

@@ -0,0 +1 @@
+loggerd

selfdrive/loggerd/SConscript

@@ -0,0 +1,12 @@
+Import('env', 'arch', 'messaging', 'common', 'visionipc')
+
+src = ['loggerd.cc', 'logger.c']
+libs = ['zmq', 'czmq', 'capnp', 'kj', 'yaml-cpp', 'z',
+  'avformat', 'avcodec', 'swscale', 'avutil',
+  'yuv', 'bz2', common, 'json', messaging, visionipc]
+
+if arch == "aarch64":
+  src += ['encoder.c', 'raw_logger.cc']
+  libs += ['OmxVenc', 'OmxCore', 'cutils']
+
+env.Program(src, LIBS=libs)

selfdrive/loggerd/config.py

@@ -0,0 +1,29 @@
+import os
+
+if os.environ.get('LOGGERD_ROOT', False):
+  ROOT = os.environ['LOGGERD_ROOT']
+  print("Custom loggerd root: ", ROOT)
+else:
+  ROOT = '/data/media/0/realdata/'
+
+SEGMENT_LENGTH = 60
+
+
+def get_available_percent(default=None):
+    try:
+      statvfs = os.statvfs(ROOT)
+      available_percent = 100.0 * statvfs.f_bavail / statvfs.f_blocks
+    except OSError:
+      available_percent = default
+
+    return available_percent
+
+
+def get_available_bytes(default=None):
+    try:
+      statvfs = os.statvfs(ROOT)
+      available_bytes = statvfs.f_bavail * statvfs.f_frsize
+    except OSError:
+      available_bytes = default
+
+    return available_bytes

selfdrive/loggerd/deleter.py

@@ -0,0 +1,39 @@
+#!/usr/bin/env python3
+import os
+import shutil
+import threading
+from selfdrive.swaglog import cloudlog
+from selfdrive.loggerd.config import ROOT, get_available_bytes
+from selfdrive.loggerd.uploader import listdir_by_creation
+
+
+def deleter_thread(exit_event):
+  while not exit_event.is_set():
+    available_bytes = get_available_bytes()
+
+    if available_bytes is not None and available_bytes < (5 * 1024 * 1024 * 1024):
+      # remove the earliest directory we can
+      dirs = listdir_by_creation(ROOT)
+      for delete_dir in dirs:
+        delete_path = os.path.join(ROOT, delete_dir)
+
+        if any(name.endswith(".lock") for name in os.listdir(delete_path)):
+          continue
+
+        try:
+          cloudlog.info("deleting %s" % delete_path)
+          shutil.rmtree(delete_path)
+          break
+        except OSError:
+          cloudlog.exception("issue deleting %s" % delete_path)
+      exit_event.wait(.1)
+    else:
+      exit_event.wait(30)
+
+
+def main(gctx=None):
+  deleter_thread(threading.Event())
+
+
+if __name__ == "__main__":
+  main()

selfdrive/loggerd/encoder.c

@@ -0,0 +1,796 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <assert.h>
+
+#include <czmq.h>
+
+#include <pthread.h>
+
+#include <OMX_Component.h>
+#include <OMX_IndexExt.h>
+#include <OMX_VideoExt.h>
+#include <OMX_QCOMExtns.h>
+
+#include <libyuv.h>
+
+#include <android/log.h>
+
+#include <msm_media_info.h>
+
+#include "common/mutex.h"
+#include "common/swaglog.h"
+
+#include "encoder.h"
+
+#define ALOG(...) __android_log_print(ANDROID_LOG_VERBOSE, "omxapp", ##__VA_ARGS__)
+
+// encoder: lossey codec using hardware hevc
+static void wait_for_state(EncoderState *s, OMX_STATETYPE state) {
+  pthread_mutex_lock(&s->state_lock);
+  while (s->state != state) {
+    pthread_cond_wait(&s->state_cv, &s->state_lock);
+  }
+  pthread_mutex_unlock(&s->state_lock);
+}
+
+static OMX_ERRORTYPE event_handler(OMX_HANDLETYPE component, OMX_PTR app_data, OMX_EVENTTYPE event,
+                                   OMX_U32 data1, OMX_U32 data2, OMX_PTR event_data) {
+  EncoderState *s = app_data;
+
+  switch (event) {
+  case OMX_EventCmdComplete:
+    assert(data1 == OMX_CommandStateSet);
+    LOG("set state event 0x%x", data2);
+    pthread_mutex_lock(&s->state_lock);
+    s->state = data2;
+    pthread_cond_broadcast(&s->state_cv);
+    pthread_mutex_unlock(&s->state_lock);
+    break;
+  case OMX_EventError:
+    LOGE("OMX error 0x%08x", data1);
+    // assert(false);
+    break;
+  default:
+    LOGE("unhandled event %d", event);
+    assert(false);
+    break;
+  }
+
+  pthread_mutex_unlock(&s->state_lock);
+
+  return OMX_ErrorNone;
+}
+
+static OMX_ERRORTYPE empty_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data,
+                                       OMX_BUFFERHEADERTYPE *buffer) {
+  EncoderState *s = app_data;
+
+  // printf("empty_buffer_done\n");
+
+  queue_push(&s->free_in, (void*)buffer);
+
+  return OMX_ErrorNone;
+}
+
+
+static OMX_ERRORTYPE fill_buffer_done(OMX_HANDLETYPE component, OMX_PTR app_data,
+                                      OMX_BUFFERHEADERTYPE *buffer) {
+  EncoderState *s = app_data;
+
+  // printf("fill_buffer_done\n");
+
+  queue_push(&s->done_out, (void*)buffer);
+
+  return OMX_ErrorNone;
+}
+
+static OMX_CALLBACKTYPE omx_callbacks = {
+  .EventHandler = event_handler,
+  .EmptyBufferDone = empty_buffer_done,
+  .FillBufferDone = fill_buffer_done,
+};
+
+#define PORT_INDEX_IN 0
+#define PORT_INDEX_OUT 1
+
+static const char* omx_color_fomat_name(uint32_t format) {
+  switch (format) {
+  case OMX_COLOR_FormatUnused: return "OMX_COLOR_FormatUnused";
+  case OMX_COLOR_FormatMonochrome: return "OMX_COLOR_FormatMonochrome";
+  case OMX_COLOR_Format8bitRGB332: return "OMX_COLOR_Format8bitRGB332";
+  case OMX_COLOR_Format12bitRGB444: return "OMX_COLOR_Format12bitRGB444";
+  case OMX_COLOR_Format16bitARGB4444: return "OMX_COLOR_Format16bitARGB4444";
+  case OMX_COLOR_Format16bitARGB1555: return "OMX_COLOR_Format16bitARGB1555";
+  case OMX_COLOR_Format16bitRGB565: return "OMX_COLOR_Format16bitRGB565";
+  case OMX_COLOR_Format16bitBGR565: return "OMX_COLOR_Format16bitBGR565";
+  case OMX_COLOR_Format18bitRGB666: return "OMX_COLOR_Format18bitRGB666";
+  case OMX_COLOR_Format18bitARGB1665: return "OMX_COLOR_Format18bitARGB1665";
+  case OMX_COLOR_Format19bitARGB1666: return "OMX_COLOR_Format19bitARGB1666";
+  case OMX_COLOR_Format24bitRGB888: return "OMX_COLOR_Format24bitRGB888";
+  case OMX_COLOR_Format24bitBGR888: return "OMX_COLOR_Format24bitBGR888";
+  case OMX_COLOR_Format24bitARGB1887: return "OMX_COLOR_Format24bitARGB1887";
+  case OMX_COLOR_Format25bitARGB1888: return "OMX_COLOR_Format25bitARGB1888";
+  case OMX_COLOR_Format32bitBGRA8888: return "OMX_COLOR_Format32bitBGRA8888";
+  case OMX_COLOR_Format32bitARGB8888: return "OMX_COLOR_Format32bitARGB8888";
+  case OMX_COLOR_FormatYUV411Planar: return "OMX_COLOR_FormatYUV411Planar";
+  case OMX_COLOR_FormatYUV411PackedPlanar: return "OMX_COLOR_FormatYUV411PackedPlanar";
+  case OMX_COLOR_FormatYUV420Planar: return "OMX_COLOR_FormatYUV420Planar";
+  case OMX_COLOR_FormatYUV420PackedPlanar: return "OMX_COLOR_FormatYUV420PackedPlanar";
+  case OMX_COLOR_FormatYUV420SemiPlanar: return "OMX_COLOR_FormatYUV420SemiPlanar";
+  case OMX_COLOR_FormatYUV422Planar: return "OMX_COLOR_FormatYUV422Planar";
+  case OMX_COLOR_FormatYUV422PackedPlanar: return "OMX_COLOR_FormatYUV422PackedPlanar";
+  case OMX_COLOR_FormatYUV422SemiPlanar: return "OMX_COLOR_FormatYUV422SemiPlanar";
+  case OMX_COLOR_FormatYCbYCr: return "OMX_COLOR_FormatYCbYCr";
+  case OMX_COLOR_FormatYCrYCb: return "OMX_COLOR_FormatYCrYCb";
+  case OMX_COLOR_FormatCbYCrY: return "OMX_COLOR_FormatCbYCrY";
+  case OMX_COLOR_FormatCrYCbY: return "OMX_COLOR_FormatCrYCbY";
+  case OMX_COLOR_FormatYUV444Interleaved: return "OMX_COLOR_FormatYUV444Interleaved";
+  case OMX_COLOR_FormatRawBayer8bit: return "OMX_COLOR_FormatRawBayer8bit";
+  case OMX_COLOR_FormatRawBayer10bit: return "OMX_COLOR_FormatRawBayer10bit";
+  case OMX_COLOR_FormatRawBayer8bitcompressed: return "OMX_COLOR_FormatRawBayer8bitcompressed";
+  case OMX_COLOR_FormatL2: return "OMX_COLOR_FormatL2";
+  case OMX_COLOR_FormatL4: return "OMX_COLOR_FormatL4";
+  case OMX_COLOR_FormatL8: return "OMX_COLOR_FormatL8";
+  case OMX_COLOR_FormatL16: return "OMX_COLOR_FormatL16";
+  case OMX_COLOR_FormatL24: return "OMX_COLOR_FormatL24";
+  case OMX_COLOR_FormatL32: return "OMX_COLOR_FormatL32";
+  case OMX_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_COLOR_FormatYUV420PackedSemiPlanar";
+  case OMX_COLOR_FormatYUV422PackedSemiPlanar: return "OMX_COLOR_FormatYUV422PackedSemiPlanar";
+  case OMX_COLOR_Format18BitBGR666: return "OMX_COLOR_Format18BitBGR666";
+  case OMX_COLOR_Format24BitARGB6666: return "OMX_COLOR_Format24BitARGB6666";
+  case OMX_COLOR_Format24BitABGR6666: return "OMX_COLOR_Format24BitABGR6666";
+
+  case OMX_COLOR_FormatAndroidOpaque: return "OMX_COLOR_FormatAndroidOpaque";
+  case OMX_TI_COLOR_FormatYUV420PackedSemiPlanar: return "OMX_TI_COLOR_FormatYUV420PackedSemiPlanar";
+  case OMX_QCOM_COLOR_FormatYVU420SemiPlanar: return "OMX_QCOM_COLOR_FormatYVU420SemiPlanar";
+  case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka";
+  case OMX_SEC_COLOR_FormatNV12Tiled: return "OMX_SEC_COLOR_FormatNV12Tiled";
+  case OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m: return "OMX_QCOM_COLOR_FormatYUV420PackedSemiPlanar32m";
+
+  // case QOMX_COLOR_FormatYVU420SemiPlanar: return "QOMX_COLOR_FormatYVU420SemiPlanar";
+  case QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka: return "QOMX_COLOR_FormatYVU420PackedSemiPlanar32m4ka";
+  case QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar16m2ka";
+  // case QOMX_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka: return "QOMX_COLOR_FormatYUV420PackedSemiPlanar64x32Tile2m8ka";
+  // case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m";
+  case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mMultiView";
+  case QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed: return "QOMX_COLOR_FORMATYUV420PackedSemiPlanar32mCompressed";
+  case QOMX_COLOR_Format32bitRGBA8888: return "QOMX_COLOR_Format32bitRGBA8888";
+  case QOMX_COLOR_Format32bitRGBA8888Compressed: return "QOMX_COLOR_Format32bitRGBA8888Compressed";
+
+  default:
+    return "unkn";
+  }
+}
+
+void encoder_init(EncoderState *s, const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale) {
+  int err;
+
+  memset(s, 0, sizeof(*s));
+  s->filename = filename;
+  s->width = width;
+  s->height = height;
+  s->fps = fps;
+  mutex_init_reentrant(&s->lock);
+
+  if (!h265) {
+    s->remuxing = true;
+  }
+
+  if (downscale) {
+    s->downscale = true;
+    s->y_ptr2 = malloc(s->width*s->height);
+    s->u_ptr2 = malloc(s->width*s->height/4);
+    s->v_ptr2 = malloc(s->width*s->height/4);
+  }
+
+  s->segment = -1;
+
+  s->state = OMX_StateLoaded;
+
+  s->codec_config = NULL;
+
+  queue_init(&s->free_in);
+  queue_init(&s->done_out);
+
+  pthread_mutex_init(&s->state_lock, NULL);
+  pthread_cond_init(&s->state_cv, NULL);
+
+  if (h265) {
+    err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.hevc",
+                        s, &omx_callbacks);
+  } else {
+    err = OMX_GetHandle(&s->handle, (OMX_STRING)"OMX.qcom.video.encoder.avc",
+                        s, &omx_callbacks);
+  }
+  assert(err == OMX_ErrorNone);
+  // printf("handle: %p\n", s->handle);
+
+  // setup input port
+
+  OMX_PARAM_PORTDEFINITIONTYPE in_port = {0};
+  in_port.nSize = sizeof(in_port);
+  in_port.nPortIndex = (OMX_U32) PORT_INDEX_IN;
+  err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition,
+                         (OMX_PTR) &in_port);
+  assert(err == OMX_ErrorNone);
+
+  in_port.format.video.nFrameWidth = s->width;
+  in_port.format.video.nFrameHeight = s->height;
+  in_port.format.video.nStride = VENUS_Y_STRIDE(COLOR_FMT_NV12, s->width);
+  in_port.format.video.nSliceHeight = s->height;
+  // in_port.nBufferSize = (s->width * s->height * 3) / 2;
+  in_port.nBufferSize = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, s->width, s->height);
+  in_port.format.video.xFramerate = (s->fps * 65536);
+  in_port.format.video.eCompressionFormat = OMX_VIDEO_CodingUnused;
+  // in_port.format.video.eColorFormat = OMX_COLOR_FormatYUV420SemiPlanar;
+  in_port.format.video.eColorFormat = QOMX_COLOR_FORMATYUV420PackedSemiPlanar32m;
+
+  err = OMX_SetParameter(s->handle, OMX_IndexParamPortDefinition,
+                         (OMX_PTR) &in_port);
+  assert(err == OMX_ErrorNone);
+
+
+  err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition,
+                         (OMX_PTR) &in_port);
+  assert(err == OMX_ErrorNone);
+  s->num_in_bufs = in_port.nBufferCountActual;
+
+  // printf("in width: %d, stride: %d\n",
+  //   in_port.format.video.nFrameWidth, in_port.format.video.nStride);
+
+  // setup output port
+
+  OMX_PARAM_PORTDEFINITIONTYPE out_port = {0};
+  out_port.nSize = sizeof(out_port);
+  out_port.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
+  err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition,
+                         (OMX_PTR)&out_port);
+  assert(err == OMX_ErrorNone);
+  out_port.format.video.nFrameWidth = s->width;
+  out_port.format.video.nFrameHeight = s->height;
+  out_port.format.video.xFramerate = 0;
+  out_port.format.video.nBitrate = bitrate;
+  if (h265) {
+    out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingHEVC;
+  } else {
+    out_port.format.video.eCompressionFormat = OMX_VIDEO_CodingAVC;
+  }
+  out_port.format.video.eColorFormat = OMX_COLOR_FormatUnused;
+
+  err = OMX_SetParameter(s->handle, OMX_IndexParamPortDefinition,
+                         (OMX_PTR) &out_port);
+  assert(err == OMX_ErrorNone);
+
+  err = OMX_GetParameter(s->handle, OMX_IndexParamPortDefinition,
+                         (OMX_PTR) &out_port);
+  assert(err == OMX_ErrorNone);
+  s->num_out_bufs = out_port.nBufferCountActual;
+
+  OMX_VIDEO_PARAM_BITRATETYPE bitrate_type = {0};
+  bitrate_type.nSize = sizeof(bitrate_type);
+  bitrate_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
+  err = OMX_GetParameter(s->handle, OMX_IndexParamVideoBitrate,
+                         (OMX_PTR) &bitrate_type);
+  assert(err == OMX_ErrorNone);
+
+  bitrate_type.eControlRate = OMX_Video_ControlRateVariable;
+  bitrate_type.nTargetBitrate = bitrate;
+
+  err = OMX_SetParameter(s->handle, OMX_IndexParamVideoBitrate,
+                         (OMX_PTR) &bitrate_type);
+  assert(err == OMX_ErrorNone);
+
+  if (h265) {
+    // setup HEVC
+    OMX_VIDEO_PARAM_HEVCTYPE hecv_type = {0};
+    hecv_type.nSize = sizeof(hecv_type);
+    hecv_type.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
+    err = OMX_GetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc,
+                           (OMX_PTR) &hecv_type);
+    assert(err == OMX_ErrorNone);
+
+    hecv_type.eProfile = OMX_VIDEO_HEVCProfileMain;
+    hecv_type.eLevel = OMX_VIDEO_HEVCHighTierLevel5;
+
+    err = OMX_SetParameter(s->handle, (OMX_INDEXTYPE)OMX_IndexParamVideoHevc,
+                           (OMX_PTR) &hecv_type);
+    assert(err == OMX_ErrorNone);
+  } else {
+    // setup h264
+    OMX_VIDEO_PARAM_AVCTYPE avc = { 0 };
+    avc.nSize = sizeof(avc);
+    avc.nPortIndex = (OMX_U32) PORT_INDEX_OUT;
+    err = OMX_GetParameter(s->handle, OMX_IndexParamVideoAvc, &avc);
+    assert(err == OMX_ErrorNone);
+
+    avc.nBFrames = 0;
+    avc.nPFrames = 15;
+
+    avc.eProfile = OMX_VIDEO_AVCProfileBaseline;
+    avc.eLevel = OMX_VIDEO_AVCLevel31;
+
+    avc.nAllowedPictureTypes |= OMX_VIDEO_PictureTypeB;
+    avc.eLoopFilterMode = OMX_VIDEO_AVCLoopFilterEnable;
+
+    err = OMX_SetParameter(s->handle, OMX_IndexParamVideoAvc, &avc);
+    assert(err == OMX_ErrorNone);
+  }
+
+
+  // for (int i = 0; ; i++) {
+  //   OMX_VIDEO_PARAM_PORTFORMATTYPE video_port_format = {0};
+  //   video_port_format.nSize = sizeof(video_port_format);
+  //   video_port_format.nIndex = i;
+  //   video_port_format.nPortIndex = PORT_INDEX_IN;
+  //   if (OMX_GetParameter(s->handle, OMX_IndexParamVideoPortFormat, &video_port_format) != OMX_ErrorNone)
+  //       break;
+  //   printf("in %d: compression 0x%x format 0x%x %s\n", i,
+  //          video_port_format.eCompressionFormat, video_port_format.eColorFormat,
+  //          omx_color_fomat_name(video_port_format.eColorFormat));
+  // }
+
+  // for (int i=0; i<32; i++) {
+  //   OMX_VIDEO_PARAM_PROFILELEVELTYPE params = {0};
+  //   params.nSize = sizeof(params);
+  //   params.nPortIndex = PORT_INDEX_OUT;
+  //   params.nProfileIndex = i;
+  //   if (OMX_GetParameter(s->handle, OMX_IndexParamVideoProfileLevelQuerySupported, &params) != OMX_ErrorNone)
+  //       break;
+  //   printf("profile %d level 0x%x\n", params.eProfile, params.eLevel);
+  // }
+
+  err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateIdle, NULL);
+  assert(err == OMX_ErrorNone);
+
+  s->in_buf_headers = calloc(s->num_in_bufs, sizeof(OMX_BUFFERHEADERTYPE*));
+  for (int i=0; i<s->num_in_bufs; i++) {
+    err = OMX_AllocateBuffer(s->handle, &s->in_buf_headers[i], PORT_INDEX_IN, s,
+                             in_port.nBufferSize);
+    assert(err == OMX_ErrorNone);
+  }
+
+  s->out_buf_headers = calloc(s->num_out_bufs, sizeof(OMX_BUFFERHEADERTYPE*));
+  for (int i=0; i<s->num_out_bufs; i++) {
+    err = OMX_AllocateBuffer(s->handle, &s->out_buf_headers[i], PORT_INDEX_OUT, s,
+                             out_port.nBufferSize);
+    assert(err == OMX_ErrorNone);
+  }
+
+  wait_for_state(s, OMX_StateIdle);
+
+  err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateExecuting, NULL);
+  assert(err == OMX_ErrorNone);
+
+  wait_for_state(s, OMX_StateExecuting);
+
+  // give omx all the output buffers
+  for (int i = 0; i < s->num_out_bufs; i++) {
+    // printf("fill %p\n", s->out_buf_headers[i]);
+    err = OMX_FillThisBuffer(s->handle, s->out_buf_headers[i]);
+    assert(err == OMX_ErrorNone);
+  }
+
+  // fill the input free queue
+  for (int i = 0; i < s->num_in_bufs; i++) {
+    queue_push(&s->free_in, (void*)s->in_buf_headers[i]);
+  }
+}
+
+static void handle_out_buf(EncoderState *s, OMX_BUFFERHEADERTYPE *out_buf) {
+  int err;
+  uint8_t *buf_data = out_buf->pBuffer + out_buf->nOffset;
+
+  if (out_buf->nFlags & OMX_BUFFERFLAG_CODECCONFIG) {
+    if (s->codec_config_len < out_buf->nFilledLen) {
+      s->codec_config = realloc(s->codec_config, out_buf->nFilledLen);
+    }
+    s->codec_config_len = out_buf->nFilledLen;
+    memcpy(s->codec_config, buf_data, out_buf->nFilledLen);
+  }
+
+  if (s->stream_sock_raw) {
+    uint64_t current_time = nanos_since_boot();
+    uint64_t diff = current_time - out_buf->nTimeStamp*1000LL;
+    double msdiff = (double) diff / 1000000.0;
+    // printf("encoded latency to tsEof: %f\n", msdiff);
+    zmq_send(s->stream_sock_raw, &out_buf->nTimeStamp, sizeof(out_buf->nTimeStamp), ZMQ_SNDMORE);
+    zmq_send(s->stream_sock_raw, buf_data, out_buf->nFilledLen, 0);
+  }
+
+  if (s->of) {
+    //printf("write %d flags 0x%x\n", out_buf->nFilledLen, out_buf->nFlags);
+    fwrite(buf_data, out_buf->nFilledLen, 1, s->of);
+  }
+
+  if (s->remuxing) {
+    if (!s->wrote_codec_config && s->codec_config_len > 0) {
+      if (s->codec_ctx->extradata_size < s->codec_config_len) {
+        s->codec_ctx->extradata = realloc(s->codec_ctx->extradata, s->codec_config_len + AV_INPUT_BUFFER_PADDING_SIZE);
+      }
+      s->codec_ctx->extradata_size = s->codec_config_len;
+      memcpy(s->codec_ctx->extradata, s->codec_config, s->codec_config_len);
+      memset(s->codec_ctx->extradata + s->codec_ctx->extradata_size, 0, AV_INPUT_BUFFER_PADDING_SIZE);
+
+      err = avcodec_parameters_from_context(s->out_stream->codecpar, s->codec_ctx);
+      assert(err >= 0);
+      err = avformat_write_header(s->ofmt_ctx, NULL);
+      assert(err >= 0);
+
+      s->wrote_codec_config = true;
+    }
+
+    if (out_buf->nTimeStamp > 0) {
+      // input timestamps are in microseconds
+      AVRational in_timebase = {1, 1000000};
+
+      AVPacket pkt;
+      av_init_packet(&pkt);
+      pkt.data = buf_data;
+      pkt.size = out_buf->nFilledLen;
+      pkt.pts = pkt.dts = av_rescale_q_rnd(out_buf->nTimeStamp, in_timebase, s->ofmt_ctx->streams[0]->time_base, AV_ROUND_NEAR_INF|AV_ROUND_PASS_MINMAX);
+      pkt.duration = av_rescale_q(50*1000, in_timebase, s->ofmt_ctx->streams[0]->time_base);
+
+      if (out_buf->nFlags & OMX_BUFFERFLAG_SYNCFRAME) {
+        pkt.flags |= AV_PKT_FLAG_KEY;
+      }
+
+      err = av_write_frame(s->ofmt_ctx, &pkt);
+      if (err < 0) { LOGW("ts encoder write issue"); }
+
+      av_free_packet(&pkt);
+    }
+  }
+
+  // give omx back the buffer
+  err = OMX_FillThisBuffer(s->handle, out_buf);
+  assert(err == OMX_ErrorNone);
+}
+
+int encoder_encode_frame(EncoderState *s,
+                         const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
+                         int in_width, int in_height,
+                         int *frame_segment, VIPCBufExtra *extra) {
+  int err;
+  pthread_mutex_lock(&s->lock);
+
+  if (s->opening) {
+    encoder_open(s, s->next_path);
+    s->opening = false;
+  }
+
+  if (!s->open) {
+    pthread_mutex_unlock(&s->lock);
+    return -1;
+  }
+
+  // this sometimes freezes... put it outside the encoder lock so we can still trigger rotates...
+  // THIS IS A REALLY BAD IDEA, but apparently the race has to happen 30 times to trigger this
+  pthread_mutex_unlock(&s->lock);
+  OMX_BUFFERHEADERTYPE* in_buf = queue_pop(&s->free_in);
+  pthread_mutex_lock(&s->lock);
+
+  if (s->rotating) {
+    encoder_close(s);
+    encoder_open(s, s->next_path);
+    s->segment = s->next_segment;
+    s->rotating = false;
+  }
+
+  int ret = s->counter;
+
+  uint8_t *in_buf_ptr = in_buf->pBuffer;
+  // printf("in_buf ptr %p\n", in_buf_ptr);
+
+  uint8_t *in_y_ptr = in_buf_ptr;
+  int in_y_stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, s->width);
+  int in_uv_stride = VENUS_UV_STRIDE(COLOR_FMT_NV12, s->width);
+  // uint8_t *in_uv_ptr = in_buf_ptr + (s->width * s->height);
+  uint8_t *in_uv_ptr = in_buf_ptr + (in_y_stride * VENUS_Y_SCANLINES(COLOR_FMT_NV12, s->height));
+
+  if (s->downscale) {
+    I420Scale(y_ptr, in_width,
+              u_ptr, in_width/2,
+              v_ptr, in_width/2,
+              in_width, in_height,
+              s->y_ptr2, s->width,
+              s->u_ptr2, s->width/2,
+              s->v_ptr2, s->width/2,
+              s->width, s->height,
+              kFilterNone);
+    y_ptr = s->y_ptr2;
+    u_ptr = s->u_ptr2;
+    v_ptr = s->v_ptr2;
+  }
+  err = I420ToNV12(y_ptr, s->width,
+                   u_ptr, s->width/2,
+                   v_ptr, s->width/2,
+                   in_y_ptr, in_y_stride,
+                   in_uv_ptr, in_uv_stride,
+                   s->width, s->height);
+  assert(err == 0);
+
+  // in_buf->nFilledLen = (s->width*s->height) + (s->width*s->height/2);
+  in_buf->nFilledLen = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, s->width, s->height);
+  in_buf->nFlags = OMX_BUFFERFLAG_ENDOFFRAME;
+  in_buf->nOffset = 0;
+  in_buf->nTimeStamp = extra->timestamp_eof/1000LL;  // OMX_TICKS, in microseconds
+
+  err = OMX_EmptyThisBuffer(s->handle, in_buf);
+  assert(err == OMX_ErrorNone);
+
+  // pump output
+  while (true) {
+    OMX_BUFFERHEADERTYPE *out_buf = queue_try_pop(&s->done_out);
+    if (!out_buf) {
+      break;
+    }
+    handle_out_buf(s, out_buf);
+  }
+
+  s->dirty = true;
+
+  s->counter++;
+
+  if (frame_segment) {
+    *frame_segment = s->segment;
+  }
+
+  if (s->closing) {
+    encoder_close(s);
+    s->closing = false;
+  }
+
+  pthread_mutex_unlock(&s->lock);
+  return ret;
+}
+
+void encoder_open(EncoderState *s, const char* path) {
+  int err;
+
+  pthread_mutex_lock(&s->lock);
+
+  snprintf(s->vid_path, sizeof(s->vid_path), "%s/%s", path, s->filename);
+
+  if (s->remuxing) {
+    avformat_alloc_output_context2(&s->ofmt_ctx, NULL, NULL, s->vid_path);
+    assert(s->ofmt_ctx);
+
+    s->out_stream = avformat_new_stream(s->ofmt_ctx, NULL);
+    assert(s->out_stream);
+
+    // set codec correctly
+    av_register_all();
+
+    AVCodec *codec = NULL;
+    codec = avcodec_find_encoder(AV_CODEC_ID_H264);
+    assert(codec);
+
+    s->codec_ctx = avcodec_alloc_context3(codec);
+    assert(s->codec_ctx);
+    s->codec_ctx->width = s->width;
+    s->codec_ctx->height = s->height;
+    s->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
+    s->codec_ctx->time_base = (AVRational){ 1, s->fps };
+
+    err = avio_open(&s->ofmt_ctx->pb, s->vid_path, AVIO_FLAG_WRITE);
+    assert(err >= 0);
+
+    s->wrote_codec_config = false;
+  } else {
+    s->of = fopen(s->vid_path, "wb");
+    assert(s->of);
+    if (s->codec_config_len > 0) {
+      fwrite(s->codec_config, s->codec_config_len, 1, s->of);
+    }
+  }
+
+  // create camera lock file
+  snprintf(s->lock_path, sizeof(s->lock_path), "%s/%s.lock", path, s->filename);
+  int lock_fd = open(s->lock_path, O_RDWR | O_CREAT, 0777);
+  assert(lock_fd >= 0);
+  close(lock_fd);
+
+  s->open = true;
+  s->counter = 0;
+
+  pthread_mutex_unlock(&s->lock);
+}
+
+void encoder_close(EncoderState *s) {
+  int err;
+
+  pthread_mutex_lock(&s->lock);
+
+  if (s->open) {
+    if (s->dirty) {
+      // drain output only if there could be frames in the encoder
+
+      OMX_BUFFERHEADERTYPE* in_buf = queue_pop(&s->free_in);
+      in_buf->nFilledLen = 0;
+      in_buf->nOffset = 0;
+      in_buf->nFlags = OMX_BUFFERFLAG_EOS;
+      in_buf->nTimeStamp = 0;
+
+      err = OMX_EmptyThisBuffer(s->handle, in_buf);
+      assert(err == OMX_ErrorNone);
+
+      while (true) {
+        OMX_BUFFERHEADERTYPE *out_buf = queue_pop(&s->done_out);
+
+        handle_out_buf(s, out_buf);
+
+        if (out_buf->nFlags & OMX_BUFFERFLAG_EOS) {
+          break;
+        }
+      }
+      s->dirty = false;
+    }
+
+    if (s->remuxing) {
+      av_write_trailer(s->ofmt_ctx);
+      avio_closep(&s->ofmt_ctx->pb);
+      avformat_free_context(s->ofmt_ctx);
+    } else {
+      fclose(s->of);
+    }
+    unlink(s->lock_path);
+  }
+  s->open = false;
+
+  pthread_mutex_unlock(&s->lock);
+}
+
+void encoder_rotate(EncoderState *s, const char* new_path, int new_segment) {
+  pthread_mutex_lock(&s->lock);
+  snprintf(s->next_path, sizeof(s->next_path), "%s", new_path);
+  s->next_segment = new_segment;
+  if (s->open) {
+    if (s->next_segment == -1) {
+      s->closing = true;
+    } else {
+      s->rotating = true;
+    }
+  } else {
+    s->segment = s->next_segment;
+    s->opening = true;
+  }
+  pthread_mutex_unlock(&s->lock);
+}
+
+void encoder_destroy(EncoderState *s) {
+  int err;
+
+  assert(!s->open);
+
+  err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateIdle, NULL);
+  assert(err == OMX_ErrorNone);
+
+  wait_for_state(s, OMX_StateIdle);
+
+  err = OMX_SendCommand(s->handle, OMX_CommandStateSet, OMX_StateLoaded, NULL);
+  assert(err == OMX_ErrorNone);
+
+  for (int i=0; i<s->num_in_bufs; i++) {
+    err = OMX_FreeBuffer(s->handle, PORT_INDEX_IN, s->in_buf_headers[i]);
+    assert(err == OMX_ErrorNone);
+  }
+  free(s->in_buf_headers);
+
+  for (int i=0; i<s->num_out_bufs; i++) {
+    err = OMX_FreeBuffer(s->handle, PORT_INDEX_OUT, s->out_buf_headers[i]);
+    assert(err == OMX_ErrorNone);
+  }
+  free(s->out_buf_headers);
+
+  wait_for_state(s, OMX_StateLoaded);
+
+  err = OMX_FreeHandle(s->handle);
+  assert(err == OMX_ErrorNone);
+
+  if (s->downscale) {
+    free(s->y_ptr2);
+    free(s->u_ptr2);
+    free(s->v_ptr2);
+  }
+}
+
+#if 0
+
+// cd one/selfdrive/visiond
+// clang
+//   -fPIC -pie
+//   -std=gnu11 -O2 -g
+//   -o encoder
+//   -I ~/one/selfdrive
+//   -I ~/one/phonelibs/openmax/include
+//   -I ~/one/phonelibs/libyuv/include
+//   -lOmxVenc -lOmxCore -llog
+//   encoder.c
+//   buffering.c
+//   -L ~/one/phonelibs/libyuv/lib -l:libyuv.a
+
+int main() {
+  int err;
+
+  EncoderState state;
+  EncoderState *s = &state;
+  memset(s, 0, sizeof(*s));
+
+  int w = 1164;
+  int h = 874;
+
+  encoder_init(s, w, h, 20);
+  printf("inited\n");
+
+  encoder_open(s, "/sdcard/t1");
+
+  // uint8_t *tmpy = malloc(640*480);
+  // uint8_t *tmpu = malloc((640/2)*(480/2));
+  // uint8_t *tmpv = malloc((640/2)*(480/2));
+
+  // memset(tmpy, 0, 640*480);
+  // // memset(tmpu, 0xff, (640/2)*(480/2));
+  // memset(tmpv, 0, (640/2)*(480/2));
+
+// #if 0
+  // FILE *infile = fopen("/sdcard/camera_t2.yuv", "rb");
+  uint8_t *inbuf = malloc(w*h*3/2);
+  memset(inbuf, 0, w*h*3/2);
+
+  for (int i=0; i<20*3+5; i++) {
+
+    // fread(inbuf, w*h*3/2, 1, infile);
+
+    uint8_t *tmpy = inbuf;
+    uint8_t *tmpu = inbuf + w*h;
+    uint8_t *tmpv = inbuf + w*h + (w/2)*(h/2);
+
+    for (int y=0; y<h/2; y++) {
+      for (int x=0; x<w/2; x++) {
+        tmpu[y * (w/2) + x] = (i ^ y ^ x);
+      }
+    }
+
+    encoder_encode_frame(s, 20000*i, tmpy, tmpu, tmpv);
+  }
+// #endif
+
+  // while(1);
+
+  printf("done\n");
+
+  // encoder_close(s);
+
+  // printf("restart\n");
+  // fclose(s->of);
+  // s->of = fopen("/sdcard/tmpout2.hevc", "wb");
+  // if (s->codec_config) {
+  //   fwrite(s->codec_config, s->codec_config_len, 1, s->of);
+  // }
+  // encoder_open(s, "/sdcard/t1");
+
+  encoder_rotate(s, "/sdcard/t2");
+
+  for (int i=0; i<20*3+5; i++) {
+
+    // fread(inbuf, w*h*3/2, 1, infile);
+
+    uint8_t *tmpy = inbuf;
+    uint8_t *tmpu = inbuf + w*h;
+    uint8_t *tmpv = inbuf + w*h + (w/2)*(h/2);
+
+    for (int y=0; y<h/2; y++) {
+      for (int x=0; x<w/2; x++) {
+        tmpu[y * (w/2) + x] = (i ^ y ^ x);
+      }
+    }
+
+    encoder_encode_frame(s, 20000*i, tmpy, tmpu, tmpv);
+  }
+  encoder_close(s);
+
+  return 0;
+}
+#endif

selfdrive/loggerd/encoder.h

@@ -0,0 +1,86 @@
+#ifndef ENCODER_H
+#define ENCODER_H
+
+#include <stdio.h>
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <pthread.h>
+
+#include <OMX_Component.h>
+#include <libavformat/avformat.h>
+
+#include "common/cqueue.h"
+#include "common/visionipc.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct EncoderState {
+  pthread_mutex_t lock;
+  int width, height, fps;
+  const char* path;
+  char vid_path[1024];
+  char lock_path[1024];
+  bool open;
+  bool dirty;
+  int counter;
+  int segment;
+
+  bool rotating;
+  bool closing;
+  bool opening;
+  char next_path[1024];
+  int next_segment;
+
+  const char* filename;
+  FILE *of;
+
+  size_t codec_config_len;
+  uint8_t *codec_config;
+  bool wrote_codec_config;
+
+  pthread_mutex_t state_lock;
+  pthread_cond_t state_cv;
+  OMX_STATETYPE state;
+
+  OMX_HANDLETYPE handle;
+
+  int num_in_bufs;
+  OMX_BUFFERHEADERTYPE** in_buf_headers;
+
+  int num_out_bufs;
+  OMX_BUFFERHEADERTYPE** out_buf_headers;
+
+  Queue free_in;
+  Queue done_out;
+
+  void *stream_sock_raw;
+
+  AVFormatContext *ofmt_ctx;
+  AVCodecContext *codec_ctx;
+  AVStream *out_stream;
+  bool remuxing;
+
+  void *zmq_ctx;
+
+  bool downscale;
+  uint8_t *y_ptr2, *u_ptr2, *v_ptr2;
+} EncoderState;
+
+void encoder_init(EncoderState *s, const char* filename, int width, int height, int fps, int bitrate, bool h265, bool downscale);
+int encoder_encode_frame(EncoderState *s,
+                         const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr,
+                         int in_width, int in_height,
+                         int *frame_segment, VIPCBufExtra *extra);
+void encoder_open(EncoderState *s, const char* path);
+void encoder_rotate(EncoderState *s, const char* new_path, int new_segment);
+void encoder_close(EncoderState *s);
+void encoder_destroy(EncoderState *s);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/loggerd/ethernetsniffer.py

@@ -0,0 +1,19 @@
+#!/usr/bin/env python3
+import zmq
+import cereal.messaging as messaging
+from cereal.services import service_list
+import pcap
+
+def main(gctx=None):
+  ethernetData = messaging.pub_sock('ethernetData')
+
+  for ts, pkt in pcap.pcap('eth0'):
+    dat = messaging.new_message()
+    dat.init('ethernetData', 1)
+    dat.ethernetData[0].ts = ts
+    dat.ethernetData[0].pkt = str(pkt)
+    ethernetData.send(dat.to_bytes())
+
+if __name__ == "__main__":
+  main()
+

selfdrive/loggerd/frame_logger.h

@@ -0,0 +1,81 @@
+#ifndef FRAMELOGGER_H
+#define FRAMELOGGER_H
+
+#include <cstdint>
+
+#include <string>
+#include <mutex>
+
+class FrameLogger {
+public:
+  virtual ~FrameLogger() {}
+
+  virtual void Open(const std::string &path) = 0;
+  virtual void Close() = 0;
+
+  int LogFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr, int *frame_segment) {
+    std::lock_guard<std::recursive_mutex> guard(lock);
+    
+    if (opening) {
+      Open(next_path);
+      opening = false;
+    }
+
+    if (!is_open) return -1;
+
+    if (rotating) {
+      Close();
+      Open(next_path);
+      segment = next_segment;
+      rotating = false;
+    }
+
+    int ret = ProcessFrame(ts, y_ptr, u_ptr, v_ptr);
+
+    if (ret >= 0 && frame_segment) {
+      *frame_segment = segment;
+    }
+
+    if (closing) {
+      Close();
+      closing = false;
+    }
+
+    return ret;
+  }
+
+  void Rotate(const std::string &new_path, int new_segment) {
+    std::lock_guard<std::recursive_mutex> guard(lock);
+
+    next_path = new_path;
+    next_segment = new_segment;
+    if (is_open) {
+      if (next_segment == -1) {
+        closing = true;
+      } else {
+        rotating = true;
+      }
+    } else {
+      segment = next_segment;
+      opening = true;
+    }
+  }
+
+protected:
+
+  virtual int ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr) = 0;
+
+  std::recursive_mutex lock;
+
+  bool is_open = false;
+  int segment = -1;
+
+  std::string vid_path, lock_path;
+
+private:
+  int next_segment = -1;
+  bool opening = false, closing = false, rotating = false;
+  std::string next_path;
+};
+
+#endif

selfdrive/loggerd/include/msm_media_info.h

@@ -0,0 +1,819 @@
+#ifndef __MEDIA_INFO_H__
+#define __MEDIA_INFO_H__
+
+#ifndef MSM_MEDIA_ALIGN
+#define MSM_MEDIA_ALIGN(__sz, __align) (((__sz) + (__align-1)) & (~(__align-1)))
+#endif
+
+#ifndef MSM_MEDIA_ROUNDUP
+#define MSM_MEDIA_ROUNDUP(__sz, __r) (((__sz) + ((__r) - 1)) / (__r))
+#endif
+
+#ifndef MSM_MEDIA_MAX
+#define MSM_MEDIA_MAX(__a, __b) ((__a) > (__b)?(__a):(__b))
+#endif
+
+enum color_fmts {
+	/* Venus NV12:
+	 * YUV 4:2:0 image with a plane of 8 bit Y samples followed
+	 * by an interleaved U/V plane containing 8 bit 2x2 subsampled
+	 * colour difference samples.
+	 *
+	 * <-------- Y/UV_Stride -------->
+	 * <------- Width ------->
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  ^           ^
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  Height      |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |          Y_Scanlines
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  V           |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              V
+	 * U V U V U V U V U V U V . . . .  ^
+	 * U V U V U V U V U V U V . . . .  |
+	 * U V U V U V U V U V U V . . . .  |
+	 * U V U V U V U V U V U V . . . .  UV_Scanlines
+	 * . . . . . . . . . . . . . . . .  |
+	 * . . . . . . . . . . . . . . . .  V
+	 * . . . . . . . . . . . . . . . .  --> Buffer size alignment
+	 *
+	 * Y_Stride : Width aligned to 128
+	 * UV_Stride : Width aligned to 128
+	 * Y_Scanlines: Height aligned to 32
+	 * UV_Scanlines: Height/2 aligned to 16
+	 * Extradata: Arbitrary (software-imposed) padding
+	 * Total size = align((Y_Stride * Y_Scanlines
+	 *          + UV_Stride * UV_Scanlines
+	 *          + max(Extradata, Y_Stride * 8), 4096)
+	 */
+	COLOR_FMT_NV12,
+
+	/* Venus NV21:
+	 * YUV 4:2:0 image with a plane of 8 bit Y samples followed
+	 * by an interleaved V/U plane containing 8 bit 2x2 subsampled
+	 * colour difference samples.
+	 *
+	 * <-------- Y/UV_Stride -------->
+	 * <------- Width ------->
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  ^           ^
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  Height      |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |          Y_Scanlines
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  V           |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              V
+	 * V U V U V U V U V U V U . . . .  ^
+	 * V U V U V U V U V U V U . . . .  |
+	 * V U V U V U V U V U V U . . . .  |
+	 * V U V U V U V U V U V U . . . .  UV_Scanlines
+	 * . . . . . . . . . . . . . . . .  |
+	 * . . . . . . . . . . . . . . . .  V
+	 * . . . . . . . . . . . . . . . .  --> Padding & Buffer size alignment
+	 *
+	 * Y_Stride : Width aligned to 128
+	 * UV_Stride : Width aligned to 128
+	 * Y_Scanlines: Height aligned to 32
+	 * UV_Scanlines: Height/2 aligned to 16
+	 * Extradata: Arbitrary (software-imposed) padding
+	 * Total size = align((Y_Stride * Y_Scanlines
+	 *          + UV_Stride * UV_Scanlines
+	 *          + max(Extradata, Y_Stride * 8), 4096)
+	 */
+	COLOR_FMT_NV21,
+	/* Venus NV12_MVTB:
+	 * Two YUV 4:2:0 images/views one after the other
+	 * in a top-bottom layout, same as NV12
+	 * with a plane of 8 bit Y samples followed
+	 * by an interleaved U/V plane containing 8 bit 2x2 subsampled
+	 * colour difference samples.
+	 *
+	 *
+	 * <-------- Y/UV_Stride -------->
+	 * <------- Width ------->
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  ^           ^               ^
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  Height      |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |          Y_Scanlines      |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  V           |               |
+	 * . . . . . . . . . . . . . . . .              |             View_1
+	 * . . . . . . . . . . . . . . . .              |               |
+	 * . . . . . . . . . . . . . . . .              |               |
+	 * . . . . . . . . . . . . . . . .              V               |
+	 * U V U V U V U V U V U V . . . .  ^                           |
+	 * U V U V U V U V U V U V . . . .  |                           |
+	 * U V U V U V U V U V U V . . . .  |                           |
+	 * U V U V U V U V U V U V . . . .  UV_Scanlines                |
+	 * . . . . . . . . . . . . . . . .  |                           |
+	 * . . . . . . . . . . . . . . . .  V                           V
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  ^           ^               ^
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  Height      |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |          Y_Scanlines      |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  |           |               |
+	 * Y Y Y Y Y Y Y Y Y Y Y Y . . . .  V           |               |
+	 * . . . . . . . . . . . . . . . .              |             View_2
+	 * . . . . . . . . . . . . . . . .              |               |
+	 * . . . . . . . . . . . . . . . .              |               |
+	 * . . . . . . . . . . . . . . . .              V               |
+	 * U V U V U V U V U V U V . . . .  ^                           |
+	 * U V U V U V U V U V U V . . . .  |                           |
+	 * U V U V U V U V U V U V . . . .  |                           |
+	 * U V U V U V U V U V U V . . . .  UV_Scanlines                |
+	 * . . . . . . . . . . . . . . . .  |                           |
+	 * . . . . . . . . . . . . . . . .  V                           V
+	 * . . . . . . . . . . . . . . . .  --> Buffer size alignment
+	 *
+	 * Y_Stride : Width aligned to 128
+	 * UV_Stride : Width aligned to 128
+	 * Y_Scanlines: Height aligned to 32
+	 * UV_Scanlines: Height/2 aligned to 16
+	 * View_1 begin at: 0 (zero)
+	 * View_2 begin at: Y_Stride * Y_Scanlines + UV_Stride * UV_Scanlines
+	 * Extradata: Arbitrary (software-imposed) padding
+	 * Total size = align((2*(Y_Stride * Y_Scanlines)
+	 *          + 2*(UV_Stride * UV_Scanlines) + Extradata), 4096)
+	 */
+	COLOR_FMT_NV12_MVTB,
+	/* Venus NV12 UBWC:
+	 * Compressed Macro-tile format for NV12.
+	 * Contains 4 planes in the following order -
+	 * (A) Y_Meta_Plane
+	 * (B) Y_UBWC_Plane
+	 * (C) UV_Meta_Plane
+	 * (D) UV_UBWC_Plane
+	 *
+	 * Y_Meta_Plane consists of meta information to decode compressed
+	 * tile data in Y_UBWC_Plane.
+	 * Y_UBWC_Plane consists of Y data in compressed macro-tile format.
+	 * UBWC decoder block will use the Y_Meta_Plane data together with
+	 * Y_UBWC_Plane data to produce loss-less uncompressed 8 bit Y samples.
+	 *
+	 * UV_Meta_Plane consists of meta information to decode compressed
+	 * tile data in UV_UBWC_Plane.
+	 * UV_UBWC_Plane consists of UV data in compressed macro-tile format.
+	 * UBWC decoder block will use UV_Meta_Plane data together with
+	 * UV_UBWC_Plane data to produce loss-less uncompressed 8 bit 2x2
+	 * subsampled color difference samples.
+	 *
+	 * Each tile in Y_UBWC_Plane/UV_UBWC_Plane is independently decodable
+	 * and randomly accessible. There is no dependency between tiles.
+	 *
+	 * <----- Y_Meta_Stride ---->
+	 * <-------- Width ------>
+	 * M M M M M M M M M M M M . .      ^           ^
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      Height      |
+	 * M M M M M M M M M M M M . .      |         Meta_Y_Scanlines
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      V           |
+	 * . . . . . . . . . . . . . .                  |
+	 * . . . . . . . . . . . . . .                  |
+	 * . . . . . . . . . . . . . .      -------> Buffer size aligned to 4k
+	 * . . . . . . . . . . . . . .                  V
+	 * <--Compressed tile Y Stride--->
+	 * <------- Width ------->
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  ^           ^
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  Height      |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |        Macro_tile_Y_Scanlines
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  V           |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .  -------> Buffer size aligned to 4k
+	 * . . . . . . . . . . . . . . . .              V
+	 * <----- UV_Meta_Stride ---->
+	 * M M M M M M M M M M M M . .      ^
+	 * M M M M M M M M M M M M . .      |
+	 * M M M M M M M M M M M M . .      |
+	 * M M M M M M M M M M M M . .      M_UV_Scanlines
+	 * . . . . . . . . . . . . . .      |
+	 * . . . . . . . . . . . . . .      V
+	 * . . . . . . . . . . . . . .      -------> Buffer size aligned to 4k
+	 * <--Compressed tile UV Stride--->
+	 * U* V* U* V* U* V* U* V* . . . .  ^
+	 * U* V* U* V* U* V* U* V* . . . .  |
+	 * U* V* U* V* U* V* U* V* . . . .  |
+	 * U* V* U* V* U* V* U* V* . . . .  UV_Scanlines
+	 * . . . . . . . . . . . . . . . .  |
+	 * . . . . . . . . . . . . . . . .  V
+	 * . . . . . . . . . . . . . . . .  -------> Buffer size aligned to 4k
+	 *
+	 * Y_Stride = align(Width, 128)
+	 * UV_Stride = align(Width, 128)
+	 * Y_Scanlines = align(Height, 32)
+	 * UV_Scanlines = align(Height/2, 16)
+	 * Y_UBWC_Plane_size = align(Y_Stride * Y_Scanlines, 4096)
+	 * UV_UBWC_Plane_size = align(UV_Stride * UV_Scanlines, 4096)
+	 * Y_Meta_Stride = align(roundup(Width, Y_TileWidth), 64)
+	 * Y_Meta_Scanlines = align(roundup(Height, Y_TileHeight), 16)
+	 * Y_Meta_Plane_size = align(Y_Meta_Stride * Y_Meta_Scanlines, 4096)
+	 * UV_Meta_Stride = align(roundup(Width, UV_TileWidth), 64)
+	 * UV_Meta_Scanlines = align(roundup(Height, UV_TileHeight), 16)
+	 * UV_Meta_Plane_size = align(UV_Meta_Stride * UV_Meta_Scanlines, 4096)
+	 * Extradata = 8k
+	 *
+	 * Total size = align( Y_UBWC_Plane_size + UV_UBWC_Plane_size +
+	 *           Y_Meta_Plane_size + UV_Meta_Plane_size
+	 *           + max(Extradata, Y_Stride * 48), 4096)
+	 */
+	COLOR_FMT_NV12_UBWC,
+	/* Venus NV12 10-bit UBWC:
+	 * Compressed Macro-tile format for NV12.
+	 * Contains 4 planes in the following order -
+	 * (A) Y_Meta_Plane
+	 * (B) Y_UBWC_Plane
+	 * (C) UV_Meta_Plane
+	 * (D) UV_UBWC_Plane
+	 *
+	 * Y_Meta_Plane consists of meta information to decode compressed
+	 * tile data in Y_UBWC_Plane.
+	 * Y_UBWC_Plane consists of Y data in compressed macro-tile format.
+	 * UBWC decoder block will use the Y_Meta_Plane data together with
+	 * Y_UBWC_Plane data to produce loss-less uncompressed 10 bit Y samples.
+	 *
+	 * UV_Meta_Plane consists of meta information to decode compressed
+	 * tile data in UV_UBWC_Plane.
+	 * UV_UBWC_Plane consists of UV data in compressed macro-tile format.
+	 * UBWC decoder block will use UV_Meta_Plane data together with
+	 * UV_UBWC_Plane data to produce loss-less uncompressed 10 bit 2x2
+	 * subsampled color difference samples.
+	 *
+	 * Each tile in Y_UBWC_Plane/UV_UBWC_Plane is independently decodable
+	 * and randomly accessible. There is no dependency between tiles.
+	 *
+	 * <----- Y_Meta_Stride ----->
+	 * <-------- Width ------>
+	 * M M M M M M M M M M M M . .      ^           ^
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      Height      |
+	 * M M M M M M M M M M M M . .      |         Meta_Y_Scanlines
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      V           |
+	 * . . . . . . . . . . . . . .                  |
+	 * . . . . . . . . . . . . . .                  |
+	 * . . . . . . . . . . . . . .      -------> Buffer size aligned to 4k
+	 * . . . . . . . . . . . . . .                  V
+	 * <--Compressed tile Y Stride--->
+	 * <------- Width ------->
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  ^           ^
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  Height      |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |        Macro_tile_Y_Scanlines
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  |           |
+	 * Y* Y* Y* Y* Y* Y* Y* Y* . . . .  V           |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .  -------> Buffer size aligned to 4k
+	 * . . . . . . . . . . . . . . . .              V
+	 * <----- UV_Meta_Stride ---->
+	 * M M M M M M M M M M M M . .      ^
+	 * M M M M M M M M M M M M . .      |
+	 * M M M M M M M M M M M M . .      |
+	 * M M M M M M M M M M M M . .      M_UV_Scanlines
+	 * . . . . . . . . . . . . . .      |
+	 * . . . . . . . . . . . . . .      V
+	 * . . . . . . . . . . . . . .      -------> Buffer size aligned to 4k
+	 * <--Compressed tile UV Stride--->
+	 * U* V* U* V* U* V* U* V* . . . .  ^
+	 * U* V* U* V* U* V* U* V* . . . .  |
+	 * U* V* U* V* U* V* U* V* . . . .  |
+	 * U* V* U* V* U* V* U* V* . . . .  UV_Scanlines
+	 * . . . . . . . . . . . . . . . .  |
+	 * . . . . . . . . . . . . . . . .  V
+	 * . . . . . . . . . . . . . . . .  -------> Buffer size aligned to 4k
+	 *
+	 *
+	 * Y_Stride = align(Width * 4/3, 128)
+	 * UV_Stride = align(Width * 4/3, 128)
+	 * Y_Scanlines = align(Height, 32)
+	 * UV_Scanlines = align(Height/2, 16)
+	 * Y_UBWC_Plane_Size = align(Y_Stride * Y_Scanlines, 4096)
+	 * UV_UBWC_Plane_Size = align(UV_Stride * UV_Scanlines, 4096)
+	 * Y_Meta_Stride = align(roundup(Width, Y_TileWidth), 64)
+	 * Y_Meta_Scanlines = align(roundup(Height, Y_TileHeight), 16)
+	 * Y_Meta_Plane_size = align(Y_Meta_Stride * Y_Meta_Scanlines, 4096)
+	 * UV_Meta_Stride = align(roundup(Width, UV_TileWidth), 64)
+	 * UV_Meta_Scanlines = align(roundup(Height, UV_TileHeight), 16)
+	 * UV_Meta_Plane_size = align(UV_Meta_Stride * UV_Meta_Scanlines, 4096)
+	 * Extradata = 8k
+	 *
+	 * Total size = align(Y_UBWC_Plane_size + UV_UBWC_Plane_size +
+	 *           Y_Meta_Plane_size + UV_Meta_Plane_size
+	 *           + max(Extradata, Y_Stride * 48), 4096)
+	 */
+	COLOR_FMT_NV12_BPP10_UBWC,
+	/* Venus RGBA8888 format:
+	 * Contains 1 plane in the following order -
+	 * (A) RGBA plane
+	 *
+	 * <-------- RGB_Stride -------->
+	 * <------- Width ------->
+	 * R R R R R R R R R R R R . . . .  ^           ^
+	 * R R R R R R R R R R R R . . . .  |           |
+	 * R R R R R R R R R R R R . . . .  Height      |
+	 * R R R R R R R R R R R R . . . .  |       RGB_Scanlines
+	 * R R R R R R R R R R R R . . . .  |           |
+	 * R R R R R R R R R R R R . . . .  |           |
+	 * R R R R R R R R R R R R . . . .  |           |
+	 * R R R R R R R R R R R R . . . .  V           |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              V
+	 *
+	 * RGB_Stride = align(Width * 4, 128)
+	 * RGB_Scanlines = align(Height, 32)
+	 * RGB_Plane_size = align(RGB_Stride * RGB_Scanlines, 4096)
+	 * Extradata = 8k
+	 *
+	 * Total size = align(RGB_Plane_size + Extradata, 4096)
+	 */
+	COLOR_FMT_RGBA8888,
+	/* Venus RGBA8888 UBWC format:
+	 * Contains 2 planes in the following order -
+	 * (A) Meta plane
+	 * (B) RGBA plane
+	 *
+	 * <--- RGB_Meta_Stride ---->
+	 * <-------- Width ------>
+	 * M M M M M M M M M M M M . .      ^           ^
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      Height      |
+	 * M M M M M M M M M M M M . .      |       Meta_RGB_Scanlines
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      |           |
+	 * M M M M M M M M M M M M . .      V           |
+	 * . . . . . . . . . . . . . .                  |
+	 * . . . . . . . . . . . . . .                  |
+	 * . . . . . . . . . . . . . .      -------> Buffer size aligned to 4k
+	 * . . . . . . . . . . . . . .                  V
+	 * <-------- RGB_Stride -------->
+	 * <------- Width ------->
+	 * R R R R R R R R R R R R . . . .  ^           ^
+	 * R R R R R R R R R R R R . . . .  |           |
+	 * R R R R R R R R R R R R . . . .  Height      |
+	 * R R R R R R R R R R R R . . . .  |       RGB_Scanlines
+	 * R R R R R R R R R R R R . . . .  |           |
+	 * R R R R R R R R R R R R . . . .  |           |
+	 * R R R R R R R R R R R R . . . .  |           |
+	 * R R R R R R R R R R R R . . . .  V           |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .              |
+	 * . . . . . . . . . . . . . . . .    -------> Buffer size aligned to 4k
+	 * . . . . . . . . . . . . . . . .              V
+	 *
+	 * RGB_Stride = align(Width * 4, 128)
+	 * RGB_Scanlines = align(Height, 32)
+	 * RGB_Plane_size = align(RGB_Stride * RGB_Scanlines, 4096)
+	 * RGB_Meta_Stride = align(roundup(Width, RGB_TileWidth), 64)
+	 * RGB_Meta_Scanline = align(roundup(Height, RGB_TileHeight), 16)
+	 * RGB_Meta_Plane_size = align(RGB_Meta_Stride *
+	 *		RGB_Meta_Scanlines, 4096)
+	 * Extradata = 8k
+	 *
+	 * Total size = align(RGB_Meta_Plane_size + RGB_Plane_size +
+	 *		Extradata, 4096)
+	 */
+	COLOR_FMT_RGBA8888_UBWC,
+};
+
+static inline unsigned int VENUS_EXTRADATA_SIZE(int width, int height)
+{
+	(void)height;
+	(void)width;
+
+	/*
+	 * In the future, calculate the size based on the w/h but just
+	 * hardcode it for now since 16K satisfies all current usecases.
+	 */
+	return 16 * 1024;
+}
+
+static inline unsigned int VENUS_Y_STRIDE(int color_fmt, int width)
+{
+	unsigned int alignment, stride = 0;
+	if (!width)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV21:
+	case COLOR_FMT_NV12:
+	case COLOR_FMT_NV12_MVTB:
+	case COLOR_FMT_NV12_UBWC:
+		alignment = 128;
+		stride = MSM_MEDIA_ALIGN(width, alignment);
+		break;
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		alignment = 256;
+		stride = MSM_MEDIA_ALIGN(width, 192);
+		stride = MSM_MEDIA_ALIGN(stride * 4/3, alignment);
+		break;
+	default:
+		break;
+	}
+invalid_input:
+	return stride;
+}
+
+static inline unsigned int VENUS_UV_STRIDE(int color_fmt, int width)
+{
+	unsigned int alignment, stride = 0;
+	if (!width)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV21:
+	case COLOR_FMT_NV12:
+	case COLOR_FMT_NV12_MVTB:
+	case COLOR_FMT_NV12_UBWC:
+		alignment = 128;
+		stride = MSM_MEDIA_ALIGN(width, alignment);
+		break;
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		alignment = 256;
+		stride = MSM_MEDIA_ALIGN(width, 192);
+		stride = MSM_MEDIA_ALIGN(stride * 4/3, alignment);
+		break;
+	default:
+		break;
+	}
+invalid_input:
+	return stride;
+}
+
+static inline unsigned int VENUS_Y_SCANLINES(int color_fmt, int height)
+{
+	unsigned int alignment, sclines = 0;
+	if (!height)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV21:
+	case COLOR_FMT_NV12:
+	case COLOR_FMT_NV12_MVTB:
+	case COLOR_FMT_NV12_UBWC:
+		alignment = 32;
+		break;
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		alignment = 16;
+		break;
+	default:
+		return 0;
+	}
+	sclines = MSM_MEDIA_ALIGN(height, alignment);
+invalid_input:
+	return sclines;
+}
+
+static inline unsigned int VENUS_UV_SCANLINES(int color_fmt, int height)
+{
+	unsigned int alignment, sclines = 0;
+	if (!height)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV21:
+	case COLOR_FMT_NV12:
+	case COLOR_FMT_NV12_MVTB:
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		alignment = 16;
+		break;
+	case COLOR_FMT_NV12_UBWC:
+		alignment = 32;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	sclines = MSM_MEDIA_ALIGN(height / 2, alignment);
+
+invalid_input:
+	return sclines;
+}
+
+static inline unsigned int VENUS_Y_META_STRIDE(int color_fmt, int width)
+{
+	int y_tile_width = 0, y_meta_stride = 0;
+
+	if (!width)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV12_UBWC:
+		y_tile_width = 32;
+		break;
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		y_tile_width = 48;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	y_meta_stride = MSM_MEDIA_ROUNDUP(width, y_tile_width);
+	y_meta_stride = MSM_MEDIA_ALIGN(y_meta_stride, 64);
+
+invalid_input:
+	return y_meta_stride;
+}
+
+static inline unsigned int VENUS_Y_META_SCANLINES(int color_fmt, int height)
+{
+	int y_tile_height = 0, y_meta_scanlines = 0;
+
+	if (!height)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV12_UBWC:
+		y_tile_height = 8;
+		break;
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		y_tile_height = 4;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	y_meta_scanlines = MSM_MEDIA_ROUNDUP(height, y_tile_height);
+	y_meta_scanlines = MSM_MEDIA_ALIGN(y_meta_scanlines, 16);
+
+invalid_input:
+	return y_meta_scanlines;
+}
+
+static inline unsigned int VENUS_UV_META_STRIDE(int color_fmt, int width)
+{
+	int uv_tile_width = 0, uv_meta_stride = 0;
+
+	if (!width)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV12_UBWC:
+		uv_tile_width = 16;
+		break;
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		uv_tile_width = 24;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	uv_meta_stride = MSM_MEDIA_ROUNDUP(width / 2, uv_tile_width);
+	uv_meta_stride = MSM_MEDIA_ALIGN(uv_meta_stride, 64);
+
+invalid_input:
+	return uv_meta_stride;
+}
+
+static inline unsigned int VENUS_UV_META_SCANLINES(int color_fmt, int height)
+{
+	int uv_tile_height = 0, uv_meta_scanlines = 0;
+
+	if (!height)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV12_UBWC:
+		uv_tile_height = 8;
+		break;
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		uv_tile_height = 4;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	uv_meta_scanlines = MSM_MEDIA_ROUNDUP(height / 2, uv_tile_height);
+	uv_meta_scanlines = MSM_MEDIA_ALIGN(uv_meta_scanlines, 16);
+
+invalid_input:
+	return uv_meta_scanlines;
+}
+
+static inline unsigned int VENUS_RGB_STRIDE(int color_fmt, int width)
+{
+	unsigned int alignment = 0, stride = 0;
+	if (!width)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_RGBA8888:
+		alignment = 128;
+		break;
+	case COLOR_FMT_RGBA8888_UBWC:
+		alignment = 256;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	stride = MSM_MEDIA_ALIGN(width * 4, alignment);
+
+invalid_input:
+	return stride;
+}
+
+static inline unsigned int VENUS_RGB_SCANLINES(int color_fmt, int height)
+{
+	unsigned int alignment = 0, scanlines = 0;
+
+	if (!height)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_RGBA8888:
+		alignment = 32;
+		break;
+	case COLOR_FMT_RGBA8888_UBWC:
+		alignment = 16;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	scanlines = MSM_MEDIA_ALIGN(height, alignment);
+
+invalid_input:
+	return scanlines;
+}
+
+static inline unsigned int VENUS_RGB_META_STRIDE(int color_fmt, int width)
+{
+	int rgb_tile_width = 0, rgb_meta_stride = 0;
+
+	if (!width)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_RGBA8888_UBWC:
+		rgb_tile_width = 16;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	rgb_meta_stride = MSM_MEDIA_ROUNDUP(width, rgb_tile_width);
+	rgb_meta_stride = MSM_MEDIA_ALIGN(rgb_meta_stride, 64);
+
+invalid_input:
+	return rgb_meta_stride;
+}
+
+static inline unsigned int VENUS_RGB_META_SCANLINES(int color_fmt, int height)
+{
+	int rgb_tile_height = 0, rgb_meta_scanlines = 0;
+
+	if (!height)
+		goto invalid_input;
+
+	switch (color_fmt) {
+	case COLOR_FMT_RGBA8888_UBWC:
+		rgb_tile_height = 4;
+		break;
+	default:
+		goto invalid_input;
+	}
+
+	rgb_meta_scanlines = MSM_MEDIA_ROUNDUP(height, rgb_tile_height);
+	rgb_meta_scanlines = MSM_MEDIA_ALIGN(rgb_meta_scanlines, 16);
+
+invalid_input:
+	return rgb_meta_scanlines;
+}
+
+static inline unsigned int VENUS_BUFFER_SIZE(
+	int color_fmt, int width, int height)
+{
+	const unsigned int extra_size = VENUS_EXTRADATA_SIZE(width, height);
+	unsigned int uv_alignment = 0, size = 0;
+	unsigned int y_plane, uv_plane, y_stride,
+		uv_stride, y_sclines, uv_sclines;
+	unsigned int y_ubwc_plane = 0, uv_ubwc_plane = 0;
+	unsigned int y_meta_stride = 0, y_meta_scanlines = 0;
+	unsigned int uv_meta_stride = 0, uv_meta_scanlines = 0;
+	unsigned int y_meta_plane = 0, uv_meta_plane = 0;
+	unsigned int rgb_stride = 0, rgb_scanlines = 0;
+	unsigned int rgb_plane = 0, rgb_ubwc_plane = 0, rgb_meta_plane = 0;
+	unsigned int rgb_meta_stride = 0, rgb_meta_scanlines = 0;
+
+	if (!width || !height)
+		goto invalid_input;
+
+	y_stride = VENUS_Y_STRIDE(color_fmt, width);
+	uv_stride = VENUS_UV_STRIDE(color_fmt, width);
+	y_sclines = VENUS_Y_SCANLINES(color_fmt, height);
+	uv_sclines = VENUS_UV_SCANLINES(color_fmt, height);
+	rgb_stride = VENUS_RGB_STRIDE(color_fmt, width);
+	rgb_scanlines = VENUS_RGB_SCANLINES(color_fmt, height);
+
+	switch (color_fmt) {
+	case COLOR_FMT_NV21:
+	case COLOR_FMT_NV12:
+		uv_alignment = 4096;
+		y_plane = y_stride * y_sclines;
+		uv_plane = uv_stride * uv_sclines + uv_alignment;
+		size = y_plane + uv_plane +
+				MSM_MEDIA_MAX(extra_size, 8 * y_stride);
+		size = MSM_MEDIA_ALIGN(size, 4096);
+		break;
+	case COLOR_FMT_NV12_MVTB:
+		uv_alignment = 4096;
+		y_plane = y_stride * y_sclines;
+		uv_plane = uv_stride * uv_sclines + uv_alignment;
+		size = y_plane + uv_plane;
+		size = 2 * size + extra_size;
+		size = MSM_MEDIA_ALIGN(size, 4096);
+		break;
+	case COLOR_FMT_NV12_UBWC:
+	case COLOR_FMT_NV12_BPP10_UBWC:
+		y_ubwc_plane = MSM_MEDIA_ALIGN(y_stride * y_sclines, 4096);
+		uv_ubwc_plane = MSM_MEDIA_ALIGN(uv_stride * uv_sclines, 4096);
+		y_meta_stride = VENUS_Y_META_STRIDE(color_fmt, width);
+		y_meta_scanlines = VENUS_Y_META_SCANLINES(color_fmt, height);
+		y_meta_plane = MSM_MEDIA_ALIGN(
+				y_meta_stride * y_meta_scanlines, 4096);
+		uv_meta_stride = VENUS_UV_META_STRIDE(color_fmt, width);
+		uv_meta_scanlines = VENUS_UV_META_SCANLINES(color_fmt, height);
+		uv_meta_plane = MSM_MEDIA_ALIGN(uv_meta_stride *
+					uv_meta_scanlines, 4096);
+
+		size = y_ubwc_plane + uv_ubwc_plane + y_meta_plane +
+			uv_meta_plane +
+			MSM_MEDIA_MAX(extra_size + 8192, 48 * y_stride);
+		size = MSM_MEDIA_ALIGN(size, 4096);
+		break;
+	case COLOR_FMT_RGBA8888:
+		rgb_plane = MSM_MEDIA_ALIGN(rgb_stride  * rgb_scanlines, 4096);
+		size = rgb_plane;
+		size =  MSM_MEDIA_ALIGN(size, 4096);
+		break;
+	case COLOR_FMT_RGBA8888_UBWC:
+		rgb_ubwc_plane = MSM_MEDIA_ALIGN(rgb_stride * rgb_scanlines,
+							4096);
+		rgb_meta_stride = VENUS_RGB_META_STRIDE(color_fmt, width);
+		rgb_meta_scanlines = VENUS_RGB_META_SCANLINES(color_fmt,
+					height);
+		rgb_meta_plane = MSM_MEDIA_ALIGN(rgb_meta_stride *
+					rgb_meta_scanlines, 4096);
+		size = rgb_ubwc_plane + rgb_meta_plane;
+		size = MSM_MEDIA_ALIGN(size, 4096);
+		break;
+	default:
+		break;
+	}
+invalid_input:
+	return size;
+}
+
+static inline unsigned int VENUS_VIEW2_OFFSET(
+	int color_fmt, int width, int height)
+{
+	unsigned int offset = 0;
+	unsigned int y_plane, uv_plane, y_stride,
+		uv_stride, y_sclines, uv_sclines;
+	if (!width || !height)
+		goto invalid_input;
+
+	y_stride = VENUS_Y_STRIDE(color_fmt, width);
+	uv_stride = VENUS_UV_STRIDE(color_fmt, width);
+	y_sclines = VENUS_Y_SCANLINES(color_fmt, height);
+	uv_sclines = VENUS_UV_SCANLINES(color_fmt, height);
+	switch (color_fmt) {
+	case COLOR_FMT_NV12_MVTB:
+		y_plane = y_stride * y_sclines;
+		uv_plane = uv_stride * uv_sclines;
+		offset = y_plane + uv_plane;
+		break;
+	default:
+		break;
+	}
+invalid_input:
+	return offset;
+}
+
+#endif

selfdrive/loggerd/logger.c

@@ -0,0 +1,217 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <stdbool.h>
+#include <string.h>
+#include <assert.h>
+#include <time.h>
+#include <errno.h>
+
+#include <unistd.h>
+#include <sys/stat.h>
+
+#include <pthread.h>
+#include <bzlib.h>
+
+#include "common/swaglog.h"
+
+#include "logger.h"
+
+static int mkpath(char* file_path) {
+  assert(file_path && *file_path);
+  char* p;
+  for (p=strchr(file_path+1, '/'); p; p=strchr(p+1, '/')) {
+    *p = '\0';
+    if (mkdir(file_path, 0777)==-1) {
+      if (errno != EEXIST) {
+        *p = '/';
+        return -1;
+      }
+    }
+    *p = '/';
+  }
+  return 0;
+}
+
+void logger_init(LoggerState *s, const char* log_name, const uint8_t* init_data, size_t init_data_len, bool has_qlog) {
+  memset(s, 0, sizeof(*s));
+  if (init_data) {
+    s->init_data = (uint8_t*)malloc(init_data_len);
+    assert(s->init_data);
+    memcpy(s->init_data, init_data, init_data_len);
+    s->init_data_len = init_data_len;
+  }
+
+  umask(0);
+
+  pthread_mutex_init(&s->lock, NULL);
+
+  s->part = -1;
+  s->has_qlog = has_qlog;
+
+  time_t rawtime = time(NULL);
+  struct tm timeinfo;
+  localtime_r(&rawtime, &timeinfo);
+
+  strftime(s->route_name, sizeof(s->route_name),
+           "%Y-%m-%d--%H-%M-%S", &timeinfo);
+  snprintf(s->log_name, sizeof(s->log_name), "%s", log_name);
+}
+
+static LoggerHandle* logger_open(LoggerState *s, const char* root_path) {
+  int err;
+
+  LoggerHandle *h = NULL;
+  for (int i=0; i<LOGGER_MAX_HANDLES; i++) {
+    if (s->handles[i].refcnt == 0) {
+      h = &s->handles[i];
+      break;
+    }
+  }
+  assert(h);
+
+  snprintf(h->segment_path, sizeof(h->segment_path),
+          "%s/%s--%d", root_path, s->route_name, s->part);
+
+  snprintf(h->log_path, sizeof(h->log_path), "%s/%s.bz2", h->segment_path, s->log_name);
+  snprintf(h->qlog_path, sizeof(h->qlog_path), "%s/qlog.bz2", h->segment_path);
+  snprintf(h->lock_path, sizeof(h->lock_path), "%s.lock", h->log_path);
+
+  err = mkpath(h->log_path);
+  if (err) return NULL;
+
+  FILE* lock_file = fopen(h->lock_path, "wb");
+  if (lock_file == NULL) return NULL;
+  fclose(lock_file);
+
+  h->log_file = fopen(h->log_path, "wb");
+  if (h->log_file == NULL) goto fail;
+
+  if (s->has_qlog) {
+    h->qlog_file = fopen(h->qlog_path, "wb");
+    if (h->qlog_file == NULL) goto fail;
+  }
+
+  int bzerror;
+  h->bz_file = BZ2_bzWriteOpen(&bzerror, h->log_file, 9, 0, 30);
+  if (bzerror != BZ_OK) goto fail;
+
+  if (s->has_qlog) {
+    h->bz_qlog = BZ2_bzWriteOpen(&bzerror, h->qlog_file, 9, 0, 30);
+    if (bzerror != BZ_OK) goto fail;
+  }
+
+  if (s->init_data) {
+    BZ2_bzWrite(&bzerror, h->bz_file, s->init_data, s->init_data_len);
+    if (bzerror != BZ_OK) goto fail;
+
+    if (s->has_qlog) {
+      // init data goes in the qlog too
+      BZ2_bzWrite(&bzerror, h->bz_qlog, s->init_data, s->init_data_len);
+      if (bzerror != BZ_OK) goto fail;
+    }
+  }
+
+  pthread_mutex_init(&h->lock, NULL);
+  h->refcnt++;
+  return h;
+fail:
+  LOGE("logger failed to open files");
+  if (h->qlog_file) fclose(h->qlog_file);
+  if (h->log_file) fclose(h->log_file);
+  return NULL;
+}
+
+int logger_next(LoggerState *s, const char* root_path,
+                            char* out_segment_path, size_t out_segment_path_len,
+                            int* out_part) {
+  pthread_mutex_lock(&s->lock);
+  s->part++;
+
+  LoggerHandle* next_h = logger_open(s, root_path);
+  if (!next_h) {
+    pthread_mutex_unlock(&s->lock);
+    return -1;
+  }
+
+  if (s->cur_handle) {
+    lh_close(s->cur_handle);
+  }
+  s->cur_handle = next_h;
+
+  if (out_segment_path) {
+    snprintf(out_segment_path, out_segment_path_len, "%s", next_h->segment_path);
+  }
+  if (out_part) {
+    *out_part = s->part;
+  }
+
+  pthread_mutex_unlock(&s->lock);
+  return 0;
+}
+
+LoggerHandle* logger_get_handle(LoggerState *s) {
+  pthread_mutex_lock(&s->lock);
+  LoggerHandle* h = s->cur_handle;
+  if (h) {
+    pthread_mutex_lock(&h->lock);
+    h->refcnt++;
+    pthread_mutex_unlock(&h->lock);
+  }
+  pthread_mutex_unlock(&s->lock);
+  return h;
+}
+
+void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog) {
+  pthread_mutex_lock(&s->lock);
+  if (s->cur_handle) {
+    lh_log(s->cur_handle, data, data_size, in_qlog);
+  }
+  pthread_mutex_unlock(&s->lock);
+}
+
+void logger_close(LoggerState *s) {
+  pthread_mutex_lock(&s->lock);
+  free(s->init_data);
+  if (s->cur_handle) {
+    lh_close(s->cur_handle);
+  }
+  pthread_mutex_unlock(&s->lock);
+}
+
+void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog) {
+  pthread_mutex_lock(&h->lock);
+  assert(h->refcnt > 0);
+  int bzerror;
+  BZ2_bzWrite(&bzerror, h->bz_file, data, data_size);
+
+  if (in_qlog && h->bz_qlog != NULL) {
+    BZ2_bzWrite(&bzerror, h->bz_qlog, data, data_size);
+  }
+  pthread_mutex_unlock(&h->lock);
+}
+
+void lh_close(LoggerHandle* h) {
+  pthread_mutex_lock(&h->lock);
+  assert(h->refcnt > 0);
+  h->refcnt--;
+  if (h->refcnt == 0) {
+    if (h->bz_file){
+      int bzerror;
+      BZ2_bzWriteClose(&bzerror, h->bz_file, 0, NULL, NULL);
+      h->bz_file = NULL;
+    }
+    if (h->bz_qlog){
+      int bzerror;
+      BZ2_bzWriteClose(&bzerror, h->bz_qlog, 0, NULL, NULL);
+      h->bz_qlog = NULL;
+    }
+    if (h->qlog_file) fclose(h->qlog_file);
+    fclose(h->log_file);
+    unlink(h->lock_path);
+    pthread_mutex_unlock(&h->lock);
+    pthread_mutex_destroy(&h->lock);
+    return;
+  }
+  pthread_mutex_unlock(&h->lock);
+}

selfdrive/loggerd/logger.h

@@ -0,0 +1,59 @@
+#ifndef LOGGER_H
+#define LOGGER_H
+
+#include <stdio.h>
+#include <stdint.h>
+#include <pthread.h>
+#include <bzlib.h>
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define LOGGER_MAX_HANDLES 16
+
+typedef struct LoggerHandle {
+  pthread_mutex_t lock;
+  int refcnt;
+  char segment_path[4096];
+  char log_path[4096];
+  char lock_path[4096];
+  FILE* log_file;
+  BZFILE* bz_file;
+
+  FILE* qlog_file;
+  char qlog_path[4096];
+  BZFILE* bz_qlog;
+} LoggerHandle;
+
+typedef struct LoggerState {
+  pthread_mutex_t lock;
+
+  uint8_t* init_data;
+  size_t init_data_len;
+
+  int part;
+  char route_name[64];
+  char log_name[64];
+  bool has_qlog;
+
+  LoggerHandle handles[LOGGER_MAX_HANDLES];
+  LoggerHandle* cur_handle;
+} LoggerState;
+
+void logger_init(LoggerState *s, const char* log_name, const uint8_t* init_data, size_t init_data_len, bool has_qlog);
+int logger_next(LoggerState *s, const char* root_path,
+                            char* out_segment_path, size_t out_segment_path_len,
+                            int* out_part);
+LoggerHandle* logger_get_handle(LoggerState *s);
+void logger_close(LoggerState *s);
+void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog);
+
+void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog);
+void lh_close(LoggerHandle* h);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/loggerd/loggerd.cc

@@ -0,0 +1,738 @@
+#include <cstdio>
+#include <cstdlib>
+#include <cstdint>
+#include <cassert>
+#include <unistd.h>
+#include <signal.h>
+#include <errno.h>
+#include <poll.h>
+#include <string.h>
+#include <inttypes.h>
+#include <libyuv.h>
+#include <sys/resource.h>
+
+#include <string>
+#include <iostream>
+#include <fstream>
+#include <streambuf>
+#include <thread>
+#include <mutex>
+#include <condition_variable>
+#include <random>
+
+#include <ftw.h>
+
+#include <zmq.h>
+#include <yaml-cpp/yaml.h>
+#include <capnp/serialize.h>
+
+#ifdef QCOM
+#include <cutils/properties.h>
+#endif
+
+#include "common/version.h"
+#include "common/timing.h"
+#include "common/params.h"
+#include "common/swaglog.h"
+#include "common/visionipc.h"
+#include "common/utilpp.h"
+#include "common/util.h"
+
+#include "logger.h"
+#include "messaging.hpp"
+
+#ifndef QCOM
+// no encoder on PC
+#define DISABLE_ENCODER
+#endif
+
+
+#ifndef DISABLE_ENCODER
+#include "encoder.h"
+#include "raw_logger.h"
+#endif
+
+#include "cereal/gen/cpp/log.capnp.h"
+
+#define CAMERA_FPS 20
+#define SEGMENT_LENGTH 60
+#define LOG_ROOT "/data/media/0/realdata"
+#define ENABLE_LIDAR 0
+
+#define RAW_CLIP_LENGTH 100 // 5 seconds at 20fps
+#define RAW_CLIP_FREQUENCY (randrange(61, 8*60)) // once every ~4 minutes
+
+namespace {
+
+double randrange(double a, double b) {
+  static std::mt19937 gen(millis_since_boot());
+
+  std::uniform_real_distribution<> dist(a, b);
+  return dist(gen);
+}
+
+
+volatile sig_atomic_t do_exit = 0;
+static void set_do_exit(int sig) {
+  do_exit = 1;
+}
+struct LoggerdState {
+  Context *ctx;
+  LoggerState logger;
+
+  std::mutex lock;
+  std::condition_variable cv;
+  char segment_path[4096];
+  uint32_t last_frame_id;
+  uint32_t rotate_last_frame_id;
+  int rotate_segment;
+};
+LoggerdState s;
+
+#ifndef DISABLE_ENCODER
+void encoder_thread(bool is_streaming, bool raw_clips, bool front) {
+  int err;
+
+  if (front) {
+    char *value;
+    const int result = read_db_value(NULL, "RecordFront", &value, NULL);
+    if (result != 0) return;
+    if (value[0] != '1') { free(value); return; }
+    free(value);
+    LOGW("recording front camera");
+
+    set_thread_name("FrontCameraEncoder");
+  } else {
+    set_thread_name("RearCameraEncoder");
+  }
+
+  VisionStream stream;
+
+  bool encoder_inited = false;
+  EncoderState encoder;
+  EncoderState encoder_alt;
+  bool has_encoder_alt = false;
+
+  int encoder_segment = -1;
+  int cnt = 0;
+
+  PubSocket *idx_sock = PubSocket::create(s.ctx, front ? "frontEncodeIdx" : "encodeIdx");
+  assert(idx_sock != NULL);
+
+  LoggerHandle *lh = NULL;
+
+  while (!do_exit) {
+    VisionStreamBufs buf_info;
+    if (front) {
+      err = visionstream_init(&stream, VISION_STREAM_YUV_FRONT, false, &buf_info);
+    } else {
+      err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info);
+    }
+    if (err != 0) {
+      LOGD("visionstream connect fail");
+      usleep(100000);
+      continue;
+    }
+
+    if (!encoder_inited) {
+      LOGD("encoder init %dx%d", buf_info.width, buf_info.height);
+      encoder_init(&encoder, front ? "dcamera.hevc" : "fcamera.hevc", buf_info.width, buf_info.height, CAMERA_FPS, front ? 2500000 : 5000000, true, false);
+      if (!front) {
+        encoder_init(&encoder_alt, "qcamera.ts", 480, 360, CAMERA_FPS, 128000, false, true);
+        has_encoder_alt = true;
+      }
+      encoder_inited = true;
+      if (is_streaming) {
+        encoder.zmq_ctx = zmq_ctx_new();
+        encoder.stream_sock_raw = zmq_socket(encoder.zmq_ctx, ZMQ_PUB);
+        assert(encoder.stream_sock_raw);
+        zmq_bind(encoder.stream_sock_raw, "tcp://*:9002");
+      }
+    }
+
+    // dont log a raw clip in the first minute
+    double rawlogger_start_time = seconds_since_boot()+RAW_CLIP_FREQUENCY;
+    int rawlogger_clip_cnt = 0;
+    RawLogger *rawlogger = NULL;
+
+    if (raw_clips) {
+      rawlogger = new RawLogger("prcamera", buf_info.width, buf_info.height, CAMERA_FPS);
+    }
+
+    while (!do_exit) {
+      VIPCBufExtra extra;
+      VIPCBuf* buf = visionstream_get(&stream, &extra);
+      if (buf == NULL) {
+        LOG("visionstream get failed");
+        break;
+      }
+
+      uint64_t current_time = nanos_since_boot();
+      uint64_t diff = current_time - extra.timestamp_eof;
+      double msdiff = (double) diff / 1000000.0;
+      // printf("logger latency to tsEof: %f\n", msdiff);
+
+      uint8_t *y = (uint8_t*)buf->addr;
+      uint8_t *u = y + (buf_info.width*buf_info.height);
+      uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2);
+
+      {
+        bool should_rotate = false;
+        std::unique_lock<std::mutex> lk(s.lock);
+        if (!front) {
+          // wait if log camera is older on back camera
+          while ( extra.frame_id > s.last_frame_id //if the log camera is older, wait for it to catch up.
+                 && (extra.frame_id-s.last_frame_id) < 8 // but if its too old then there probably was a discontinuity (visiond restarted)
+                 && !do_exit) {
+            s.cv.wait(lk);
+          }
+          should_rotate = extra.frame_id > s.rotate_last_frame_id && encoder_segment < s.rotate_segment;
+        } else {
+          // front camera is best effort
+          should_rotate = encoder_segment < s.rotate_segment;
+        }
+        if (do_exit) break;
+
+        // rotate the encoder if the logger is on a newer segment
+        if (should_rotate) {
+          LOG("rotate encoder to %s", s.segment_path);
+
+          encoder_rotate(&encoder, s.segment_path, s.rotate_segment);
+          if (has_encoder_alt) {
+            encoder_rotate(&encoder_alt, s.segment_path, s.rotate_segment);
+          }
+
+          if (raw_clips) {
+            rawlogger->Rotate(s.segment_path, s.rotate_segment);
+          }
+
+          encoder_segment = s.rotate_segment;
+          if (lh) {
+            lh_close(lh);
+          }
+          lh = logger_get_handle(&s.logger);
+        }
+      }
+
+      {
+        // encode hevc
+        int out_segment = -1;
+        int out_id = encoder_encode_frame(&encoder,
+                                          y, u, v,
+                                          buf_info.width, buf_info.height,
+                                          &out_segment, &extra);
+
+        if (has_encoder_alt) {
+          int out_segment_alt = -1;
+          encoder_encode_frame(&encoder_alt,
+                               y, u, v,
+                               buf_info.width, buf_info.height,
+                               &out_segment_alt, &extra);
+        }
+
+        // publish encode index
+        capnp::MallocMessageBuilder msg;
+        cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+        event.setLogMonoTime(nanos_since_boot());
+        auto eidx = event.initEncodeIdx();
+        eidx.setFrameId(extra.frame_id);
+        eidx.setType(front ? cereal::EncodeIndex::Type::FRONT : cereal::EncodeIndex::Type::FULL_H_E_V_C);
+        eidx.setEncodeId(cnt);
+        eidx.setSegmentNum(out_segment);
+        eidx.setSegmentId(out_id);
+
+        auto words = capnp::messageToFlatArray(msg);
+        auto bytes = words.asBytes();
+        if (idx_sock->send((char*)bytes.begin(), bytes.size()) < 0) {
+          printf("err sending encodeIdx pkt: %s\n", strerror(errno));
+        }
+        if (lh) {
+          lh_log(lh, bytes.begin(), bytes.size(), false);
+        }
+      }
+
+      if (raw_clips) {
+        double ts = seconds_since_boot();
+        if (ts > rawlogger_start_time) {
+          // encode raw if in clip
+          int out_segment = -1;
+          int out_id = rawlogger->LogFrame(cnt, y, u, v, &out_segment);
+
+          if (rawlogger_clip_cnt == 0) {
+            LOG("starting raw clip in seg %d", out_segment);
+          }
+
+          // publish encode index
+          capnp::MallocMessageBuilder msg;
+          cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+          event.setLogMonoTime(nanos_since_boot());
+          auto eidx = event.initEncodeIdx();
+          eidx.setFrameId(extra.frame_id);
+          eidx.setType(cereal::EncodeIndex::Type::FULL_LOSSLESS_CLIP);
+          eidx.setEncodeId(cnt);
+          eidx.setSegmentNum(out_segment);
+          eidx.setSegmentId(out_id);
+
+          auto words = capnp::messageToFlatArray(msg);
+          auto bytes = words.asBytes();
+          if (lh) {
+            lh_log(lh, bytes.begin(), bytes.size(), false);
+          }
+
+          // close rawlogger if clip ended
+          rawlogger_clip_cnt++;
+          if (rawlogger_clip_cnt >= RAW_CLIP_LENGTH) {
+            rawlogger->Close();
+
+            rawlogger_clip_cnt = 0;
+            rawlogger_start_time = ts+RAW_CLIP_FREQUENCY;
+
+            LOG("ending raw clip in seg %d, next in %.1f sec", out_segment, rawlogger_start_time-ts);
+          }
+        }
+      }
+
+      cnt++;
+    }
+
+    if (lh) {
+      lh_close(lh);
+      lh = NULL;
+    }
+
+    if (raw_clips) {
+      rawlogger->Close();
+      delete rawlogger;
+    }
+
+    visionstream_destroy(&stream);
+  }
+
+  delete idx_sock;
+
+  if (encoder_inited) {
+    LOG("encoder destroy");
+    encoder_close(&encoder);
+    encoder_destroy(&encoder);
+  }
+
+  if (has_encoder_alt) {
+    LOG("encoder alt destroy");
+    encoder_close(&encoder_alt);
+    encoder_destroy(&encoder_alt);
+  }
+}
+#endif
+
+#if ENABLE_LIDAR
+
+#include <netinet/in.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+
+#define VELODYNE_DATA_PORT 2368
+#define VELODYNE_TELEMETRY_PORT 8308
+
+#define MAX_LIDAR_PACKET 2048
+
+int lidar_thread() {
+  // increase kernel max buffer size
+  system("sysctl -w net.core.rmem_max=26214400");
+  set_thread_name("lidar");
+
+  int sock;
+  if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+    perror("cannot create socket");
+    return -1;
+  }
+
+  int a = 26214400;
+  if (setsockopt(sock, SOL_SOCKET, SO_RCVBUF, &a, sizeof(int)) == -1) {
+    perror("cannot set socket opts");
+    return -1;
+  }
+
+  struct sockaddr_in addr;
+  memset(&addr, 0, sizeof(struct sockaddr_in));
+  addr.sin_family = AF_INET;
+  addr.sin_port = htons(VELODYNE_DATA_PORT);
+  inet_aton("192.168.5.11", &(addr.sin_addr));
+
+  if (bind(sock, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
+    perror("cannot bind LIDAR socket");
+    return -1;
+  }
+
+  capnp::byte buf[MAX_LIDAR_PACKET];
+
+  while (!do_exit) {
+    // receive message
+    struct sockaddr from;
+    socklen_t fromlen = sizeof(from);
+    int cnt = recvfrom(sock, (void *)buf, MAX_LIDAR_PACKET, 0, &from, &fromlen);
+    if (cnt <= 0) {
+      printf("bug in lidar recieve!\n");
+      continue;
+    }
+
+    // create message for log
+    capnp::MallocMessageBuilder msg;
+    auto event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
+    auto lidar_pts = event.initLidarPts();
+
+    // copy in the buffer
+    // TODO: can we remove this copy? does it matter?
+    kj::ArrayPtr<capnp::byte> bufferPtr = kj::arrayPtr(buf, cnt);
+    lidar_pts.setPkt(bufferPtr);
+
+    // log it
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    logger_log(&s.logger, bytes.begin(), bytes.size());
+  }
+  return 0;
+}
+#endif
+
+}
+
+void append_property(const char* key, const char* value, void *cookie) {
+  std::vector<std::pair<std::string, std::string> > *properties =
+    (std::vector<std::pair<std::string, std::string> > *)cookie;
+
+  properties->push_back(std::make_pair(std::string(key), std::string(value)));
+}
+
+kj::Array<capnp::word> gen_init_data() {
+  capnp::MallocMessageBuilder msg;
+  auto event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+  auto init = event.initInitData();
+
+  init.setDeviceType(cereal::InitData::DeviceType::NEO);
+  init.setVersion(capnp::Text::Reader(COMMA_VERSION));
+
+  std::ifstream cmdline_stream("/proc/cmdline");
+  std::vector<std::string> kernel_args;
+  std::string buf;
+  while (cmdline_stream >> buf) {
+    kernel_args.push_back(buf);
+  }
+
+  auto lkernel_args = init.initKernelArgs(kernel_args.size());
+  for (int i=0; i<kernel_args.size(); i++) {
+    lkernel_args.set(i, kernel_args[i]);
+  }
+
+  init.setKernelVersion(util::read_file("/proc/version"));
+
+#ifdef QCOM
+  {
+    std::vector<std::pair<std::string, std::string> > properties;
+    property_list(append_property, (void*)&properties);
+
+    auto lentries = init.initAndroidProperties().initEntries(properties.size());
+    for (int i=0; i<properties.size(); i++) {
+      auto lentry = lentries[i];
+      lentry.setKey(properties[i].first);
+      lentry.setValue(properties[i].second);
+    }
+  }
+#endif
+
+  const char* dongle_id = getenv("DONGLE_ID");
+  if (dongle_id) {
+    init.setDongleId(std::string(dongle_id));
+  }
+
+  const char* clean = getenv("CLEAN");
+  if (!clean) {
+    init.setDirty(true);
+  }
+
+  char* git_commit = NULL;
+  read_db_value(NULL, "GitCommit", &git_commit, NULL);
+  if (git_commit) {
+    init.setGitCommit(capnp::Text::Reader(git_commit));
+  }
+
+  char* git_branch = NULL;
+  read_db_value(NULL, "GitBranch", &git_branch, NULL);
+  if (git_branch) {
+    init.setGitBranch(capnp::Text::Reader(git_branch));
+  }
+
+  char* git_remote = NULL;
+  read_db_value(NULL, "GitRemote", &git_remote, NULL);
+  if (git_remote) {
+    init.setGitRemote(capnp::Text::Reader(git_remote));
+  }
+
+  char* passive = NULL;
+  read_db_value(NULL, "Passive", &passive, NULL);
+  init.setPassive(passive && strlen(passive) && passive[0] == '1');
+
+
+  {
+    // log params
+    std::map<std::string, std::string> params;
+    read_db_all(NULL, &params);
+    auto lparams = init.initParams().initEntries(params.size());
+    int i = 0;
+    for (auto& kv : params) {
+      auto lentry = lparams[i];
+      lentry.setKey(kv.first);
+      lentry.setValue(kv.second);
+      i++;
+    }
+  }
+
+
+  auto words = capnp::messageToFlatArray(msg);
+
+  if (git_commit) {
+    free((void*)git_commit);
+  }
+
+  if (git_branch) {
+    free((void*)git_branch);
+  }
+
+  if (git_remote) {
+    free((void*)git_remote);
+  }
+
+  if (passive) {
+    free((void*)passive);
+  }
+
+  return words;
+}
+
+static int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupeflag) {
+  const char* dot = strrchr(fpath, '.');
+  if (dot && strcmp(dot, ".lock") == 0) {
+    unlink(fpath);
+  }
+  return 0;
+}
+
+static void clear_locks() {
+  ftw(LOG_ROOT, clear_locks_fn, 16);
+}
+
+static void bootlog() {
+  int err;
+
+  {
+    auto words = gen_init_data();
+    auto bytes = words.asBytes();
+    logger_init(&s.logger, "bootlog", bytes.begin(), bytes.size(), false);
+  }
+
+  err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment);
+  assert(err == 0);
+  LOGW("bootlog to %s", s.segment_path);
+
+  {
+    capnp::MallocMessageBuilder msg;
+    auto event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
+
+    auto boot = event.initBoot();
+
+    boot.setWallTimeNanos(nanos_since_epoch());
+
+    std::string lastKmsg = util::read_file("/sys/fs/pstore/console-ramoops");
+    boot.setLastKmsg(capnp::Data::Reader((const kj::byte*)lastKmsg.data(), lastKmsg.size()));
+
+    std::string lastPmsg = util::read_file("/sys/fs/pstore/pmsg-ramoops-0");
+    boot.setLastPmsg(capnp::Data::Reader((const kj::byte*)lastPmsg.data(), lastPmsg.size()));
+
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    logger_log(&s.logger, bytes.begin(), bytes.size(), false);
+  }
+
+  logger_close(&s.logger);
+}
+
+int main(int argc, char** argv) {
+  int err;
+
+  if (argc > 1 && strcmp(argv[1], "--bootlog") == 0) {
+    bootlog();
+    return 0;
+  }
+
+  setpriority(PRIO_PROCESS, 0, -12);
+
+  clear_locks();
+
+  signal(SIGINT, (sighandler_t)set_do_exit);
+  signal(SIGTERM, (sighandler_t)set_do_exit);
+
+  s.ctx = Context::create();
+  Poller * poller = Poller::create();
+
+  std::string exe_dir = util::dir_name(util::readlink("/proc/self/exe"));
+  std::string service_list_path = exe_dir + "/../../cereal/service_list.yaml";
+
+  // subscribe to all services
+
+  SubSocket *frame_sock = NULL;
+  std::vector<SubSocket*> socks;
+
+  std::map<SubSocket*, int> qlog_counter;
+  std::map<SubSocket*, int> qlog_freqs;
+
+  YAML::Node service_list = YAML::LoadFile(service_list_path);
+  for (const auto& it : service_list) {
+    auto name = it.first.as<std::string>();
+    bool should_log = it.second[1].as<bool>();
+    int qlog_freq = it.second[3] ? it.second[3].as<int>() : 0;
+
+    if (should_log) {
+      SubSocket * sock = SubSocket::create(s.ctx, name);
+      assert(sock != NULL);
+
+      poller->registerSocket(sock);
+      socks.push_back(sock);
+
+      if (name == "frame") {
+        frame_sock = sock;
+      }
+
+      qlog_counter[sock] = (qlog_freq == 0) ? -1 : 0;
+      qlog_freqs[sock] = qlog_freq;
+    }
+  }
+
+
+  {
+    auto words = gen_init_data();
+    auto bytes = words.asBytes();
+    logger_init(&s.logger, "rlog", bytes.begin(), bytes.size(), true);
+  }
+
+  bool is_streaming = false;
+  bool is_logging = true;
+
+  if (argc > 1 && strcmp(argv[1], "--stream") == 0) {
+    is_streaming = true;
+  } else if (argc > 1 && strcmp(argv[1], "--only-stream") == 0) {
+    is_streaming = true;
+    is_logging = false;
+  }
+
+  if (is_logging) {
+    err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment);
+    assert(err == 0);
+    LOGW("logging to %s", s.segment_path);
+  }
+
+  double start_ts = seconds_since_boot();
+  double last_rotate_ts = start_ts;
+
+#ifndef DISABLE_ENCODER
+  // rear camera
+  std::thread encoder_thread_handle(encoder_thread, is_streaming, false, false);
+
+  // front camera
+  std::thread front_encoder_thread_handle(encoder_thread, false, false, true);
+#endif
+
+#if ENABLE_LIDAR
+  std::thread lidar_thread_handle(lidar_thread);
+#endif
+
+  uint64_t msg_count = 0;
+  uint64_t bytes_count = 0;
+
+  while (!do_exit) {
+    for (auto sock : poller->poll(100 * 1000)){
+      while (true) {
+        Message * msg = sock->receive(true);
+        if (msg == NULL){
+          break;
+        }
+
+        uint8_t* data = (uint8_t*)msg->getData();
+        size_t len = msg->getSize();
+
+        if (sock == frame_sock) {
+          // track camera frames to sync to encoder
+          auto amsg = kj::heapArray<capnp::word>((len / sizeof(capnp::word)) + 1);
+          memcpy(amsg.begin(), data, len);
+
+          capnp::FlatArrayMessageReader cmsg(amsg);
+          cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+          if (event.isFrame()) {
+            std::unique_lock<std::mutex> lk(s.lock);
+            s.last_frame_id = event.getFrame().getFrameId();
+            lk.unlock();
+            s.cv.notify_all();
+          }
+        }
+
+        logger_log(&s.logger, data, len, qlog_counter[sock] == 0);
+        delete msg;
+
+        if (qlog_counter[sock] != -1) {
+          //printf("%p: %d/%d\n", socks[i], qlog_counter[socks[i]], qlog_freqs[socks[i]]);
+          qlog_counter[sock]++;
+          qlog_counter[sock] %= qlog_freqs[sock];
+        }
+
+        bytes_count += len;
+        msg_count++;
+      }
+    }
+
+    double ts = seconds_since_boot();
+    if (ts - last_rotate_ts > SEGMENT_LENGTH) {
+      // rotate the log
+
+      last_rotate_ts += SEGMENT_LENGTH;
+
+      std::lock_guard<std::mutex> guard(s.lock);
+      s.rotate_last_frame_id = s.last_frame_id;
+
+      if (is_logging) {
+        err = logger_next(&s.logger, LOG_ROOT, s.segment_path, sizeof(s.segment_path), &s.rotate_segment);
+        assert(err == 0);
+        LOGW("rotated to %s", s.segment_path);
+      }
+    }
+
+    if ((msg_count%1000) == 0) {
+      LOGD("%lu messages, %.2f msg/sec, %.2f KB/sec", msg_count, msg_count*1.0/(ts-start_ts), bytes_count*0.001/(ts-start_ts));
+    }
+  }
+
+  LOGW("joining threads");
+  s.cv.notify_all();
+
+
+#ifndef DISABLE_ENCODER
+  front_encoder_thread_handle.join();
+  encoder_thread_handle.join();
+  LOGW("encoder joined");
+#endif
+
+#if ENABLE_LIDAR
+  lidar_thread_handle.join();
+  LOGW("lidar joined");
+#endif
+
+  logger_close(&s.logger);
+
+  for (auto s : socks){
+    delete s;
+  }
+
+  delete s.ctx;
+  return 0;
+}

selfdrive/loggerd/raw_logger.cc

@@ -0,0 +1,163 @@
+#include <cstdio>
+#include <cstdlib>
+#include <cassert>
+
+#include <fcntl.h>
+#include <unistd.h>
+
+#define __STDC_CONSTANT_MACROS
+
+extern "C" {
+#include <libavutil/imgutils.h>
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+}
+
+#include "common/swaglog.h"
+#include "common/utilpp.h"
+
+#include "raw_logger.h"
+
+RawLogger::RawLogger(const std::string &afilename, int awidth, int aheight, int afps)
+  : filename(afilename),
+    width(awidth),
+    height(aheight),
+    fps(afps) {
+
+  int err = 0;
+
+  av_register_all();
+  codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF);
+  // codec = avcodec_find_encoder(AV_CODEC_ID_FFV1);
+  assert(codec);
+
+  codec_ctx = avcodec_alloc_context3(codec);
+  assert(codec_ctx);
+  codec_ctx->width = width;
+  codec_ctx->height = height;
+  codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
+
+  // codec_ctx->thread_count = 2;
+
+  // ffv1enc doesn't respect AV_PICTURE_TYPE_I. make every frame a key frame for now.
+  // codec_ctx->gop_size = 0;
+
+  codec_ctx->time_base = (AVRational){ 1, fps };
+
+  err = avcodec_open2(codec_ctx, codec, NULL);
+  assert(err >= 0);
+
+  frame = av_frame_alloc();
+  assert(frame);
+  frame->format = codec_ctx->pix_fmt;
+  frame->width = width;
+  frame->height = height;
+  frame->linesize[0] = width;
+  frame->linesize[1] = width/2;
+  frame->linesize[2] = width/2;
+}
+
+RawLogger::~RawLogger() {
+  av_frame_free(&frame);
+  avcodec_close(codec_ctx);
+  av_free(codec_ctx);
+}
+
+void RawLogger::Open(const std::string &path) {
+  int err = 0;
+
+  std::lock_guard<std::recursive_mutex> guard(lock);
+
+  vid_path = util::string_format("%s/%s.mkv", path.c_str(), filename.c_str());
+
+  // create camera lock file
+  lock_path = util::string_format("%s/%s.lock", path.c_str(), filename.c_str());
+
+  LOG("open %s\n", lock_path.c_str());
+
+  int lock_fd = open(lock_path.c_str(), O_RDWR | O_CREAT, 0777);
+  assert(lock_fd >= 0);
+  close(lock_fd);
+
+  format_ctx = NULL;
+  avformat_alloc_output_context2(&format_ctx, NULL, NULL, vid_path.c_str());
+  assert(format_ctx);
+
+  stream = avformat_new_stream(format_ctx, codec);
+  // AVStream *stream = avformat_new_stream(format_ctx, NULL);
+  assert(stream);
+  stream->id = 0;
+  stream->time_base = (AVRational){ 1, fps };
+  // codec_ctx->time_base = stream->time_base;
+
+  err = avcodec_parameters_from_context(stream->codecpar, codec_ctx);
+  assert(err >= 0);
+
+  err = avio_open(&format_ctx->pb, vid_path.c_str(), AVIO_FLAG_WRITE);
+  assert(err >= 0);
+
+  err = avformat_write_header(format_ctx, NULL);
+  assert(err >= 0);
+
+  is_open = true;
+  counter = 0;
+}
+
+void RawLogger::Close() {
+  int err = 0;
+
+  std::lock_guard<std::recursive_mutex> guard(lock);
+
+  if (!is_open) return;
+
+  err = av_write_trailer(format_ctx);
+  assert(err == 0);
+
+  avcodec_close(stream->codec);
+
+  err = avio_closep(&format_ctx->pb);
+  assert(err == 0);
+
+  avformat_free_context(format_ctx);
+  format_ctx = NULL;
+
+  unlink(lock_path.c_str());
+  is_open = false;
+}
+
+int RawLogger::ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr) {
+  int err = 0;
+
+  AVPacket pkt;
+  av_init_packet(&pkt);
+  pkt.data = NULL;
+  pkt.size = 0;
+
+  frame->data[0] = (uint8_t*)y_ptr;
+  frame->data[1] = (uint8_t*)u_ptr;
+  frame->data[2] = (uint8_t*)v_ptr;
+  frame->pts = ts;
+
+  int ret = counter;
+
+  int got_output = 0;
+  err = avcodec_encode_video2(codec_ctx, &pkt, frame, &got_output);
+  if (err) {
+    LOGE("encoding error\n");
+    ret = -1;
+  } else if (got_output) {
+
+    av_packet_rescale_ts(&pkt, codec_ctx->time_base, stream->time_base);
+    pkt.stream_index = 0;
+
+    err = av_interleaved_write_frame(format_ctx, &pkt);
+    if (err < 0) {
+      LOGE("encoder writer error\n");
+      ret = -1;
+    } else {
+      counter++;
+    }
+  }
+
+  return ret;
+}

selfdrive/loggerd/raw_logger.h

@@ -0,0 +1,43 @@
+#ifndef FFV1LOGGER_H
+#define FFV1LOGGER_H
+
+#include <cstdio>
+#include <cstdlib>
+
+#include <string>
+#include <vector>
+#include <mutex>
+#include <condition_variable>
+
+extern "C" {
+#include <libavutil/imgutils.h>
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+}
+
+#include "frame_logger.h"
+
+class RawLogger : public FrameLogger {
+public:
+  RawLogger(const std::string &filename, int awidth, int aheight, int afps);
+  ~RawLogger();
+
+  int ProcessFrame(uint64_t ts, const uint8_t *y_ptr, const uint8_t *u_ptr, const uint8_t *v_ptr);
+  void Open(const std::string &path);
+  void Close();
+
+private:
+  std::string filename;
+  int width, height, fps;
+  int counter = 0;
+
+  AVCodec *codec = NULL;
+  AVCodecContext *codec_ctx = NULL;
+
+  AVStream *stream = NULL;
+  AVFormatContext *format_ctx = NULL;
+
+  AVFrame *frame = NULL;
+};
+
+#endif

selfdrive/loggerd/tests/Makefile

@@ -0,0 +1,35 @@
+CC = clang
+CXX = clang++
+
+PHONELIBS = ../../../phonelibs
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args \
+             -Wno-deprecated-declarations
+
+CFLAGS = -std=gnu11 -g -fPIC -O2 $(WARN_FLAGS)
+CXXFLAGS = -std=c++11 -g -fPIC -O2 $(WARN_FLAGS)
+
+FFMPEG_LIBS = -lavformat \
+              -lavcodec \
+              -lswscale \
+              -lavutil \
+              -lz
+
+OBJS = testraw.o \
+       ../RawLogger.o \
+       ../../common/visionipc.o
+
+testraw: $(OBJS)
+	$(CXX) -fPIC -o '$@' $^ -L/usr/lib $(FFMPEG_LIBS)
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) \
+         -I../ \
+         -I../../ \
+         -I../../../ \
+         -c -o '$@' '$<'

selfdrive/loggerd/tests/fill_eon.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+"""Script to fill up EON with fake data"""
+
+import os
+
+from selfdrive.loggerd.config import ROOT, get_available_percent
+from selfdrive.loggerd.tests.loggerd_tests_common import create_random_file
+
+
+if __name__ == "__main__":
+  segment_idx = 0
+  while True:
+    seg_name = "1970-01-01--00-00-00--%d" % segment_idx
+    seg_path = os.path.join(ROOT, seg_name)
+
+    print(seg_path)
+
+    create_random_file(os.path.join(seg_path, 'fcamera.hevc'), 36)
+    create_random_file(os.path.join(seg_path, 'rlog.bz2'), 2)
+
+    segment_idx += 1
+
+    # Fill up to 99 percent
+    available_percent = get_available_percent()
+    if available_percent < 1.0:
+        break

selfdrive/loggerd/tests/loggerd_tests_common.py

@@ -0,0 +1,81 @@
+import os
+import errno
+import shutil
+import random
+import tempfile
+import unittest
+
+import selfdrive.loggerd.uploader as uploader
+
+def create_random_file(file_path, size_mb, lock=False):
+    try:
+      os.mkdir(os.path.dirname(file_path))
+    except OSError:
+      pass
+
+    lock_path = file_path + ".lock"
+    os.close(os.open(lock_path, os.O_CREAT | os.O_EXCL))
+
+    chunks = 128
+    chunk_bytes = int(size_mb * 1024 * 1024 / chunks)
+    data = os.urandom(chunk_bytes)
+
+    with open(file_path, 'wb') as f:
+      for _ in range(chunks):
+        f.write(data)
+
+    if not lock:
+        os.remove(lock_path)
+
+class MockResponse():
+  def __init__(self, text):
+    self.text = text
+
+class MockApi():
+  def __init__(self, dongle_id):
+    pass
+
+  def get(self, *args, **kwargs):
+    return MockResponse('{"url": "http://localhost/does/not/exist", "headers": {}}')
+
+  def get_token(self):
+    return "fake-token"
+
+class MockParams():
+  def __init__(self):
+    self.params = {
+      "DongleId": b"0000000000000000",
+      "IsUploadRawEnabled": b"1",
+    }
+
+  def get(self, k):
+    return self.params[k]
+
+class UploaderTestCase(unittest.TestCase):
+  f_type = "UNKNOWN"
+
+  def setUp(self):
+    self.root = tempfile.mkdtemp()
+    uploader.ROOT = self.root  # Monkey patch root dir
+    uploader.Api = MockApi
+    uploader.Params = MockParams
+    uploader.fake_upload = 1
+    uploader.is_on_hotspot = lambda *args: False
+    uploader.is_on_wifi = lambda *args: True
+    self.seg_num = random.randint(1, 300)
+    self.seg_format = "2019-04-18--12-52-54--{}"
+    self.seg_format2 = "2019-05-18--11-22-33--{}"
+    self.seg_dir = self.seg_format.format(self.seg_num)
+
+  def tearDown(self):
+    try:
+      shutil.rmtree(self.root)
+    except OSError as e:
+      if e.errno != errno.ENOENT:
+        raise
+
+  def make_file_with_data(self, f_dir, fn, size_mb=.1, lock=False):
+    file_path = os.path.join(self.root, f_dir, fn)
+    create_random_file(file_path, size_mb, lock)
+
+    return file_path

selfdrive/loggerd/tests/test_deleter.py

@@ -0,0 +1,108 @@
+import os
+import time
+import threading
+import unittest
+from collections import namedtuple
+
+import selfdrive.loggerd.deleter as deleter
+from common.timeout import Timeout, TimeoutException
+
+from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase
+
+Stats = namedtuple("Stats", ['f_bavail', 'f_blocks', 'f_frsize'])
+
+
+class TestDeleter(UploaderTestCase):
+  def fake_statvfs(self, d):
+    return self.fake_stats
+
+  def setUp(self):
+    self.f_type = "fcamera.hevc"
+    super(TestDeleter, self).setUp()
+    self.fake_stats = Stats(f_bavail=0, f_blocks=10, f_frsize=4096)
+    deleter.os.statvfs = self.fake_statvfs
+    deleter.ROOT = self.root
+
+  def tearDown(self):
+    super(TestDeleter, self).tearDown()
+
+  def start_thread(self):
+    self.end_event = threading.Event()
+    self.del_thread = threading.Thread(target=deleter.deleter_thread, args=[self.end_event])
+    self.del_thread.daemon = True
+    self.del_thread.start()
+
+  def join_thread(self):
+    self.end_event.set()
+    self.del_thread.join()
+
+  def test_delete(self):
+    f_path = self.make_file_with_data(self.seg_dir, self.f_type, 1)
+
+    self.start_thread()
+
+    with Timeout(5, "Timeout waiting for file to be deleted"):
+      while os.path.exists(f_path):
+        time.sleep(0.01)
+    self.join_thread()
+
+    self.assertFalse(os.path.exists(f_path), "File not deleted")
+
+  def test_delete_files_in_create_order(self):
+    f_path_1 = self.make_file_with_data(self.seg_dir, self.f_type)
+    time.sleep(1)
+    self.seg_num += 1
+    self.seg_dir = self.seg_format.format(self.seg_num)
+    f_path_2 = self.make_file_with_data(self.seg_dir, self.f_type)
+
+    self.start_thread()
+
+    with Timeout(5, "Timeout waiting for file to be deleted"):
+      while os.path.exists(f_path_1) and os.path.exists(f_path_2):
+        time.sleep(0.01)
+
+    self.join_thread()
+
+    self.assertFalse(os.path.exists(f_path_1), "Older file not deleted")
+
+    self.assertTrue(os.path.exists(f_path_2), "Newer file deleted before older file")
+
+  def test_no_delete_when_available_space(self):
+    f_path = self.make_file_with_data(self.seg_dir, self.f_type)
+
+    block_size = 4096
+    available = (10 * 1024 * 1024 * 1024) / block_size  # 10GB free
+    self.fake_stats = Stats(f_bavail=available, f_blocks=10, f_frsize=block_size)
+
+    self.start_thread()
+
+    try:
+      with Timeout(2, "Timeout waiting for file to be deleted"):
+        while os.path.exists(f_path):
+          time.sleep(0.01)
+    except TimeoutException:
+      pass
+    finally:
+      self.join_thread()
+
+    self.assertTrue(os.path.exists(f_path), "File deleted with available space")
+
+  def test_no_delete_with_lock_file(self):
+    f_path = self.make_file_with_data(self.seg_dir, self.f_type, lock=True)
+
+    self.start_thread()
+
+    try:
+      with Timeout(2, "Timeout waiting for file to be deleted"):
+        while os.path.exists(f_path):
+          time.sleep(0.01)
+    except TimeoutException:
+      pass
+    finally:
+      self.join_thread()
+
+    self.assertTrue(os.path.exists(f_path), "File deleted when locked")
+
+
+if __name__ == "__main__":
+  unittest.main()

selfdrive/loggerd/tests/test_uploader.py

@@ -0,0 +1,116 @@
+import os
+import time
+import threading
+import logging
+import json
+
+from selfdrive.swaglog import cloudlog
+import selfdrive.loggerd.uploader as uploader
+
+from common.timeout import Timeout
+
+from selfdrive.loggerd.tests.loggerd_tests_common import UploaderTestCase
+
+class TestLogHandler(logging.Handler):
+  def __init__(self):
+    logging.Handler.__init__(self)
+    self.reset()
+
+  def reset(self):
+    self.upload_order = list()
+
+  def emit(self, record):
+    try:
+      j = json.loads(record.message)
+      if j["event"] == "upload_success":
+        self.upload_order.append(j["key"])
+    except BaseException:
+      pass
+
+log_handler = TestLogHandler()
+cloudlog.addHandler(log_handler)
+
+class TestUploader(UploaderTestCase):
+  def setUp(self):
+    super(TestUploader, self).setUp()
+    log_handler.reset()
+
+  def tearDown(self):
+    super(TestUploader, self).tearDown()
+
+  def start_thread(self):
+    self.end_event = threading.Event()
+    self.up_thread = threading.Thread(target=uploader.uploader_fn, args=[self.end_event])
+    self.up_thread.daemon = True
+    self.up_thread.start()
+
+  def join_thread(self):
+    self.end_event.set()
+    self.up_thread.join()
+
+  def gen_files(self, lock=False):
+    f_paths = list()
+    for t in ["bootlog.bz2", "qlog.bz2", "rlog.bz2", "dcamera.hevc", "fcamera.hevc"]:
+      f_paths.append(self.make_file_with_data(self.seg_dir, t, 1, lock=lock))
+    return f_paths
+
+  def gen_order(self, seg1, seg2):
+    keys = [f"{self.seg_format.format(i)}/qlog.bz2" for i in seg1]
+    keys += [f"{self.seg_format2.format(i)}/qlog.bz2" for i in seg2]
+    for i in seg1:
+      keys += [f"{self.seg_format.format(i)}/{f}" for f in ['rlog.bz2','fcamera.hevc','dcamera.hevc']]
+    for i in seg2:
+      keys += [f"{self.seg_format2.format(i)}/{f}" for f in ['rlog.bz2','fcamera.hevc','dcamera.hevc']]
+    keys += [f"{self.seg_format.format(i)}/bootlog.bz2" for i in seg1]
+    keys += [f"{self.seg_format2.format(i)}/bootlog.bz2" for i in seg2]
+    return keys
+
+  def test_upload(self):
+    f_paths = self.gen_files(lock=False)
+
+    self.start_thread()
+
+    with Timeout(5, "Timeout waiting for file to be uploaded"):
+      while len(os.listdir(self.root)):
+        time.sleep(0.01)
+    self.join_thread()
+
+    for f_path in f_paths:
+      self.assertFalse(os.path.exists(f_path), "All files not uploaded")
+    exp_order = self.gen_order([self.seg_num], [])
+    self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order")
+
+  def test_upload_files_in_create_order(self):
+    f_paths = list()
+    seg1_nums = [0,1,2,10,20]
+    for i in seg1_nums:
+      self.seg_dir = self.seg_format.format(i)
+      f_paths += self.gen_files()
+    seg2_nums = [5,50,51]
+    for i in seg2_nums:
+      self.seg_dir = self.seg_format2.format(i)
+      f_paths += self.gen_files()
+
+    self.start_thread()
+
+    with Timeout(5, "Timeout waiting for file to be upload"):
+      while len(os.listdir(self.root)):
+        time.sleep(0.01)
+
+    self.join_thread()
+
+    for f_path in f_paths:
+      self.assertFalse(os.path.exists(f_path), "All files not uploaded")
+    exp_order = self.gen_order(seg1_nums, seg2_nums)
+    self.assertTrue(log_handler.upload_order == exp_order, "Files uploaded in wrong order")
+
+  def test_no_upload_with_lock_file(self):
+    f_paths = self.gen_files(lock=True)
+
+    self.start_thread()
+    # allow enough time that files should have been uploaded if they would be uploaded
+    time.sleep(5)
+    self.join_thread()
+
+    for f_path in f_paths:
+      self.assertTrue(os.path.exists(f_path), "File upload when locked")

selfdrive/loggerd/tests/testraw.cc

@@ -0,0 +1,57 @@
+#include <cstdio>
+#include <cstdlib>
+#include <unistd.h>
+
+#include <zmq.h>
+
+#include "common/visionipc.h"
+#include "common/timing.h"
+
+#include "RawLogger.h"
+
+int main() {
+  int err;
+
+  VisionStream stream;
+
+  VisionStreamBufs buf_info;
+  while (true) {
+    err = visionstream_init(&stream, VISION_STREAM_YUV, false, &buf_info);
+    if (err != 0) {
+      printf("visionstream fail\n");
+      usleep(100000);
+    }
+    break;
+  }
+
+  RawLogger vidlogger("prcamera", buf_info.width, buf_info.height, 20);
+  vidlogger.Open("o1");
+
+  for (int cnt=0; cnt<200; cnt++) {
+    VIPCBufExtra extra;
+    VIPSBuf* buf = visionstream_get(&stream, &extra);
+    if (buf == NULL) {
+      printf("visionstream get failed\n");
+      break;
+    }
+
+    if (cnt == 100) {
+      vidlogger.Rotate("o2", 2);
+    }
+
+    uint8_t *y = (uint8_t*)buf->addr;
+    uint8_t *u = y + (buf_info.width*buf_info.height);
+    uint8_t *v = u + (buf_info.width/2)*(buf_info.height/2);
+
+    double t1 = millis_since_boot();
+    vidlogger.LogFrame(cnt, y, u, v, NULL);
+    double t2 = millis_since_boot();
+    printf("%d %.2f\n", cnt, (t2-t1));
+  }
+
+  vidlogger.Close();
+
+  visionstream_destroy(&stream);
+
+  return 0;
+}

selfdrive/loggerd/uploader.py

@@ -0,0 +1,271 @@
+#!/usr/bin/env python3
+import os
+import re
+import time
+import json
+import random
+import ctypes
+import inspect
+import requests
+import traceback
+import threading
+import subprocess
+
+from selfdrive.swaglog import cloudlog
+from selfdrive.loggerd.config import ROOT
+
+from common import android
+from common.params import Params
+from common.api import Api
+
+fake_upload = os.getenv("FAKEUPLOAD") is not None
+
+def raise_on_thread(t, exctype):
+  for ctid, tobj in threading._active.items():
+    if tobj is t:
+      tid = ctid
+      break
+  else:
+    raise Exception("Could not find thread")
+
+  '''Raises an exception in the threads with id tid'''
+  if not inspect.isclass(exctype):
+    raise TypeError("Only types can be raised (not instances)")
+
+  res = ctypes.pythonapi.PyThreadState_SetAsyncExc(ctypes.c_long(tid),
+                                                   ctypes.py_object(exctype))
+  if res == 0:
+    raise ValueError("invalid thread id")
+  elif res != 1:
+    # "if it returns a number greater than one, you're in trouble,
+    # and you should call it again with exc=NULL to revert the effect"
+    ctypes.pythonapi.PyThreadState_SetAsyncExc(tid, 0)
+    raise SystemError("PyThreadState_SetAsyncExc failed")
+
+def get_directory_sort(d):
+  return list(map(lambda s: s.rjust(10, '0'), d.rsplit('--', 1)))
+
+def listdir_by_creation(d):
+  try:
+    paths = os.listdir(d)
+    paths = sorted(paths, key=get_directory_sort)
+    return paths
+  except OSError:
+    cloudlog.exception("listdir_by_creation failed")
+    return list()
+
+def clear_locks(root):
+  for logname in os.listdir(root):
+    path = os.path.join(root, logname)
+    try:
+      for fname in os.listdir(path):
+        if fname.endswith(".lock"):
+          os.unlink(os.path.join(path, fname))
+    except OSError:
+      cloudlog.exception("clear_locks failed")
+
+def is_on_wifi():
+  # ConnectivityManager.getActiveNetworkInfo()
+  try:
+    result = android.parse_service_call_string(android.service_call(["connectivity", "2"]))
+    if result is None:
+      return True
+    return 'WIFI' in result
+  except AttributeError:
+    return False
+
+def is_on_hotspot():
+  try:
+    result = subprocess.check_output(["ifconfig", "wlan0"], stderr=subprocess.STDOUT, encoding='utf8')
+    result = re.findall(r"inet addr:((\d+\.){3}\d+)", result)[0][0]
+
+    is_android = result.startswith('192.168.43.')
+    is_ios = result.startswith('172.20.10.')
+    is_entune = result.startswith('10.0.2.')
+
+    return (is_android or is_ios or is_entune)
+  except:
+    return False
+
+class Uploader():
+  def __init__(self, dongle_id, root):
+    self.dongle_id = dongle_id
+    self.api = Api(dongle_id)
+    self.root = root
+
+    self.upload_thread = None
+
+    self.last_resp = None
+    self.last_exc = None
+
+    self.immediate_priority = {"qlog.bz2": 0, "qcamera.ts": 1}
+    self.high_priority = {"rlog.bz2": 0, "fcamera.hevc": 1, "dcamera.hevc": 2}
+
+  def clean_dirs(self):
+    try:
+      for logname in os.listdir(self.root):
+        path = os.path.join(self.root, logname)
+        # remove empty directories
+        if not os.listdir(path):
+          os.rmdir(path)
+    except OSError:
+      cloudlog.exception("clean_dirs failed")
+
+  def get_upload_sort(self, name):
+    if name in self.immediate_priority:
+      return self.immediate_priority[name]
+    if name in self.high_priority:
+      return self.high_priority[name] + 100
+    return 1000
+
+  def gen_upload_files(self):
+    if not os.path.isdir(self.root):
+      return
+    for logname in listdir_by_creation(self.root):
+      path = os.path.join(self.root, logname)
+      try:
+        names = os.listdir(path)
+      except OSError:
+        continue
+      if any(name.endswith(".lock") for name in names):
+        continue
+
+      for name in sorted(names, key=self.get_upload_sort):
+        key = os.path.join(logname, name)
+        fn = os.path.join(path, name)
+
+        yield (name, key, fn)
+
+  def next_file_to_upload(self, with_raw):
+    upload_files = list(self.gen_upload_files())
+    # try to upload qlog files first
+    for name, key, fn in upload_files:
+      if name in self.immediate_priority:
+        return (key, fn)
+
+    if with_raw:
+      # then upload the full log files, rear and front camera files
+      for name, key, fn in upload_files:
+        if name in self.high_priority:
+          return (key, fn)
+
+      # then upload other files
+      for name, key, fn in upload_files:
+        if not name.endswith('.lock') and not name.endswith(".tmp"):
+          return (key, fn)
+
+    return None
+
+  def do_upload(self, key, fn):
+    try:
+      url_resp = self.api.get("v1.3/"+self.dongle_id+"/upload_url/", timeout=10, path=key, access_token=self.api.get_token())
+      url_resp_json = json.loads(url_resp.text)
+      url = url_resp_json['url']
+      headers = url_resp_json['headers']
+      cloudlog.info("upload_url v1.3 %s %s", url, str(headers))
+
+      if fake_upload:
+        cloudlog.info("*** WARNING, THIS IS A FAKE UPLOAD TO %s ***" % url)
+        class FakeResponse():
+          def __init__(self):
+            self.status_code = 200
+        self.last_resp = FakeResponse()
+      else:
+        with open(fn, "rb") as f:
+          self.last_resp = requests.put(url, data=f, headers=headers, timeout=10)
+    except Exception as e:
+      self.last_exc = (e, traceback.format_exc())
+      raise
+
+  def normal_upload(self, key, fn):
+    self.last_resp = None
+    self.last_exc = None
+
+    try:
+      self.do_upload(key, fn)
+    except Exception:
+      pass
+
+    return self.last_resp
+
+  def upload(self, key, fn):
+    try:
+      sz = os.path.getsize(fn)
+    except OSError:
+      cloudlog.exception("upload: getsize failed")
+      return False
+
+    cloudlog.event("upload", key=key, fn=fn, sz=sz)
+
+    cloudlog.info("checking %r with size %r", key, sz)
+
+    if sz == 0:
+      # can't upload files of 0 size
+      os.unlink(fn) # delete the file
+      success = True
+    else:
+      cloudlog.info("uploading %r", fn)
+      stat = self.normal_upload(key, fn)
+      if stat is not None and stat.status_code in (200, 201):
+        cloudlog.event("upload_success", key=key, fn=fn, sz=sz)
+
+        # delete the file
+        try:
+          os.unlink(fn)
+        except OSError:
+          cloudlog.event("delete_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz)
+
+        success = True
+      else:
+        cloudlog.event("upload_failed", stat=stat, exc=self.last_exc, key=key, fn=fn, sz=sz)
+        success = False
+
+    self.clean_dirs()
+
+    return success
+
+def uploader_fn(exit_event):
+  cloudlog.info("uploader_fn")
+
+  params = Params()
+  dongle_id = params.get("DongleId").decode('utf8')
+
+  if dongle_id is None:
+    cloudlog.info("uploader missing dongle_id")
+    raise Exception("uploader can't start without dongle id")
+
+  uploader = Uploader(dongle_id, ROOT)
+
+  backoff = 0.1
+  while True:
+    allow_raw_upload = (params.get("IsUploadRawEnabled") != b"0")
+    on_hotspot = is_on_hotspot()
+    on_wifi = is_on_wifi()
+    should_upload = on_wifi and not on_hotspot
+
+    if exit_event.is_set():
+      return
+
+    d = uploader.next_file_to_upload(with_raw=allow_raw_upload and should_upload)
+    if d is None:
+      time.sleep(5)
+      continue
+
+    key, fn = d
+
+    cloudlog.event("uploader_netcheck", is_on_hotspot=on_hotspot, is_on_wifi=on_wifi)
+    cloudlog.info("to upload %r", d)
+    success = uploader.upload(key, fn)
+    if success:
+      backoff = 0.1
+    else:
+      cloudlog.info("backoff %r", backoff)
+      time.sleep(backoff + random.uniform(0, backoff))
+      backoff = min(backoff*2, 120)
+    cloudlog.info("upload done, success=%r", success)
+
+def main(gctx=None):
+  uploader_fn(threading.Event())
+
+if __name__ == "__main__":
+  main()

selfdrive/mapd/default_speeds.json

@@ -0,0 +1,106 @@
+{
+  "_comment": "These speeds are from https://wiki.openstreetmap.org/wiki/Speed_limits  Special cases have been stripped",
+  "AR:urban": "40",
+  "AR:urban:primary": "60",
+  "AR:urban:secondary": "60",
+  "AR:rural": "110",
+  "AT:urban": "50",
+  "AT:rural": "100",
+  "AT:trunk": "100",
+  "AT:motorway": "130",
+  "BE:urban": "50",
+  "BE-VLG:rural": "70",
+  "BE-WAL:rural": "90",
+  "BE:trunk": "120",
+  "BE:motorway": "120",
+  "CH:urban[1]": "50",
+  "CH:rural": "80",
+  "CH:trunk": "100",
+  "CH:motorway": "120",
+  "CZ:pedestrian_zone": "20",
+  "CZ:living_street": "20",
+  "CZ:urban": "50",
+  "CZ:urban_trunk": "80",
+  "CZ:urban_motorway": "80",
+  "CZ:rural": "90",
+  "CZ:trunk": "110",
+  "CZ:motorway": "130",
+  "DK:urban": "50",
+  "DK:rural": "80",
+  "DK:motorway": "130",
+  "DE:living_street": "7",
+  "DE:residential": "30",
+  "DE:urban": "50",
+  "DE:rural": "100",
+  "DE:trunk": "none",
+  "DE:motorway": "none",
+  "FI:urban": "50",
+  "FI:rural": "80",
+  "FI:trunk": "100",
+  "FI:motorway": "120",
+  "FR:urban": "50",
+  "FR:rural": "80",
+  "FR:trunk": "110",
+  "FR:motorway": "130",
+  "GR:urban": "50",
+  "GR:rural": "90",
+  "GR:trunk": "110",
+  "GR:motorway": "130",
+  "HU:urban": "50",
+  "HU:rural": "90",
+  "HU:trunk": "110",
+  "HU:motorway": "130",
+  "IT:urban": "50",
+  "IT:rural": "90",
+  "IT:trunk": "110",
+  "IT:motorway": "130",
+  "JP:national": "60",
+  "JP:motorway": "100",
+  "LT:living_street": "20",
+  "LT:urban": "50",
+  "LT:rural": "90",
+  "LT:trunk": "120",
+  "LT:motorway": "130",
+  "PL:living_street": "20",
+  "PL:urban": "50",
+  "PL:rural": "90",
+  "PL:trunk": "100",
+  "PL:motorway": "140",
+  "RO:urban": "50",
+  "RO:rural": "90",
+  "RO:trunk": "100",
+  "RO:motorway": "130",
+  "RU:living_street": "20",
+  "RU:urban": "60",
+  "RU:rural": "90",
+  "RU:motorway": "110",
+  "SK:urban": "50",
+  "SK:rural": "90",
+  "SK:trunk": "90",
+  "SK:motorway": "90",
+  "SI:urban": "50",
+  "SI:rural": "90",
+  "SI:trunk": "110",
+  "SI:motorway": "130",
+  "ES:living_street": "20",
+  "ES:urban": "50",
+  "ES:rural": "50",
+  "ES:trunk": "90",
+  "ES:motorway": "120",
+  "SE:urban": "50",
+  "SE:rural": "70",
+  "SE:trunk": "90",
+  "SE:motorway": "110",
+  "GB:nsl_restricted": "30 mph",
+  "GB:nsl_single": "60 mph",
+  "GB:nsl_dual": "70 mph",
+  "GB:motorway": "70 mph",
+  "UA:urban": "50",
+  "UA:rural": "90",
+  "UA:trunk": "110",
+  "UA:motorway": "130",
+  "UZ:living_street": "30",
+  "UZ:urban": "70",
+  "UZ:rural": "100",
+  "UZ:motorway": "110"
+}

selfdrive/mapd/default_speeds_generator.py

@@ -0,0 +1,240 @@
+#!/usr/bin/env python3
+import json
+
+DEFAULT_OUTPUT_FILENAME = "default_speeds_by_region.json"
+
+def main(filename = DEFAULT_OUTPUT_FILENAME):
+  countries = []
+
+  """
+  --------------------------------------------------
+      US - United State of America
+  --------------------------------------------------
+  """
+  US = Country("US") # First step, create the country using the ISO 3166 two letter code
+  countries.append(US) # Second step, add the country to countries list
+
+  """ Default rules """
+  # Third step, add some default rules for the country
+  # Speed limit rules are based on OpenStreetMaps (OSM) tags.
+  # The dictionary {...} defines the tag_name: value
+  # if a road in OSM has a tag with the name tag_name and this value, the speed limit listed below will be applied.
+  # The text at the end is the speed limit (use no unit for km/h)
+  # Rules apply in the order in which they are written for each country
+  # Rules for specific regions (states) take priority over country rules
+  # If you modify existing country rules, you must update all existing states without that rule to use the old rule
+  US.add_rule({"highway": "motorway"}, "65 mph") # On US roads with the tag highway and value motorway, the speed limit will default to  65 mph
+  US.add_rule({"highway": "trunk"}, "55 mph")
+  US.add_rule({"highway": "primary"}, "55 mph")
+  US.add_rule({"highway": "secondary"}, "45 mph")
+  US.add_rule({"highway": "tertiary"}, "35 mph")
+  US.add_rule({"highway": "unclassified"}, "55 mph")
+  US.add_rule({"highway": "residential"}, "25 mph")
+  US.add_rule({"highway": "service"}, "25 mph")
+  US.add_rule({"highway": "motorway_link"}, "55 mph")
+  US.add_rule({"highway": "trunk_link"}, "55 mph")
+  US.add_rule({"highway": "primary_link"}, "55 mph")
+  US.add_rule({"highway": "secondary_link"}, "45 mph")
+  US.add_rule({"highway": "tertiary_link"}, "35 mph")
+  US.add_rule({"highway": "living_street"}, "15 mph")
+
+  """ States """
+  new_york = US.add_region("New York") # Fourth step, add a state/region to country
+  new_york.add_rule({"highway": "primary"}, "45 mph") # Fifth step , add rules to the state. See the text above for how to write rules
+  new_york.add_rule({"highway": "secondary"}, "55 mph")
+  new_york.add_rule({"highway": "tertiary"}, "55 mph")
+  new_york.add_rule({"highway": "residential"}, "30 mph")
+  new_york.add_rule({"highway": "primary_link"}, "45 mph")
+  new_york.add_rule({"highway": "secondary_link"}, "55 mph")
+  new_york.add_rule({"highway": "tertiary_link"}, "55 mph")
+  # All if not written by the state, the rules will default to the country rules
+
+  #california = US.add_region("California")
+  # California uses only the default US rules
+
+  michigan = US.add_region("Michigan")
+  michigan.add_rule({"highway": "motorway"}, "70 mph")
+
+  oregon = US.add_region("Oregon")
+  oregon.add_rule({"highway": "motorway"}, "55 mph")
+  oregon.add_rule({"highway": "secondary"}, "35 mph")
+  oregon.add_rule({"highway": "tertiary"}, "30 mph")
+  oregon.add_rule({"highway": "service"}, "15 mph")
+  oregon.add_rule({"highway": "secondary_link"}, "35 mph")
+  oregon.add_rule({"highway": "tertiary_link"}, "30 mph")
+
+  south_dakota = US.add_region("South Dakota")
+  south_dakota.add_rule({"highway": "motorway"}, "80 mph")
+  south_dakota.add_rule({"highway": "trunk"}, "70 mph")
+  south_dakota.add_rule({"highway": "primary"}, "65 mph")
+  south_dakota.add_rule({"highway": "trunk_link"}, "70 mph")
+  south_dakota.add_rule({"highway": "primary_link"}, "65 mph")
+
+  wisconsin = US.add_region("Wisconsin")
+  wisconsin.add_rule({"highway": "trunk"}, "65 mph")
+  wisconsin.add_rule({"highway": "tertiary"}, "45 mph")
+  wisconsin.add_rule({"highway": "unclassified"}, "35 mph")
+  wisconsin.add_rule({"highway": "trunk_link"}, "65 mph")
+  wisconsin.add_rule({"highway": "tertiary_link"}, "45 mph")
+
+  """
+  --------------------------------------------------
+      AU - Australia
+  --------------------------------------------------
+  """
+  AU = Country("AU")
+  countries.append(AU)
+
+  """ Default rules """
+  AU.add_rule({"highway": "motorway"}, "100")
+  AU.add_rule({"highway": "trunk"}, "80")
+  AU.add_rule({"highway": "primary"}, "80")
+  AU.add_rule({"highway": "secondary"}, "50")
+  AU.add_rule({"highway": "tertiary"}, "50")
+  AU.add_rule({"highway": "unclassified"}, "80")
+  AU.add_rule({"highway": "residential"}, "50")
+  AU.add_rule({"highway": "service"}, "40")
+  AU.add_rule({"highway": "motorway_link"}, "90")
+  AU.add_rule({"highway": "trunk_link"}, "80")
+  AU.add_rule({"highway": "primary_link"}, "80")
+  AU.add_rule({"highway": "secondary_link"}, "50")
+  AU.add_rule({"highway": "tertiary_link"}, "50")
+  AU.add_rule({"highway": "living_street"}, "30")
+
+  """
+  --------------------------------------------------
+      CA - Canada
+  --------------------------------------------------
+  """
+  CA = Country("CA")
+  countries.append(CA)
+
+  """ Default rules """
+  CA.add_rule({"highway": "motorway"}, "100")
+  CA.add_rule({"highway": "trunk"}, "80")
+  CA.add_rule({"highway": "primary"}, "80")
+  CA.add_rule({"highway": "secondary"}, "50")
+  CA.add_rule({"highway": "tertiary"}, "50")
+  CA.add_rule({"highway": "unclassified"}, "80")
+  CA.add_rule({"highway": "residential"}, "40")
+  CA.add_rule({"highway": "service"}, "40")
+  CA.add_rule({"highway": "motorway_link"}, "90")
+  CA.add_rule({"highway": "trunk_link"}, "80")
+  CA.add_rule({"highway": "primary_link"}, "80")
+  CA.add_rule({"highway": "secondary_link"}, "50")
+  CA.add_rule({"highway": "tertiary_link"}, "50")
+  CA.add_rule({"highway": "living_street"}, "20")
+
+
+  """
+  --------------------------------------------------
+      DE - Germany
+  --------------------------------------------------
+  """
+  DE = Country("DE")
+  countries.append(DE)
+
+  """ Default rules """
+  DE.add_rule({"highway": "motorway"}, "none")
+  DE.add_rule({"highway": "living_street"}, "10")
+  DE.add_rule({"highway": "residential"}, "30")
+  DE.add_rule({"zone:traffic": "DE:rural"}, "100")
+  DE.add_rule({"zone:traffic": "DE:urban"}, "50")
+  DE.add_rule({"zone:maxspeed": "DE:30"}, "30")
+  DE.add_rule({"zone:maxspeed": "DE:urban"}, "50")
+  DE.add_rule({"zone:maxspeed": "DE:rural"}, "100")
+  DE.add_rule({"zone:maxspeed": "DE:motorway"}, "none")
+  DE.add_rule({"bicycle_road": "yes"}, "30")
+  
+
+  """
+  --------------------------------------------------
+      EE - Estonia
+  --------------------------------------------------
+  """
+  EE = Country("EE")
+  countries.append(EE)
+
+  """ Default rules """
+  EE.add_rule({"highway": "motorway"}, "90")
+  EE.add_rule({"highway": "trunk"}, "90")
+  EE.add_rule({"highway": "primary"}, "90")
+  EE.add_rule({"highway": "secondary"}, "50")
+  EE.add_rule({"highway": "tertiary"}, "50")
+  EE.add_rule({"highway": "unclassified"}, "90")
+  EE.add_rule({"highway": "residential"}, "40")
+  EE.add_rule({"highway": "service"}, "40")
+  EE.add_rule({"highway": "motorway_link"}, "90")
+  EE.add_rule({"highway": "trunk_link"}, "70")
+  EE.add_rule({"highway": "primary_link"}, "70")
+  EE.add_rule({"highway": "secondary_link"}, "50")
+  EE.add_rule({"highway": "tertiary_link"}, "50")
+  EE.add_rule({"highway": "living_street"}, "20")
+
+
+  """ --- DO NOT MODIFY CODE BELOW THIS LINE --- """
+  """ --- ADD YOUR COUNTRY OR STATE ABOVE --- """
+
+  # Final step
+  write_json(countries, filename)
+
+def write_json(countries, filename = DEFAULT_OUTPUT_FILENAME):
+  out_dict = {}
+  for country in countries:
+    out_dict.update(country.jsonify())
+  json_string = json.dumps(out_dict, indent=2)
+  with open(filename, "wb") as f:
+    f.write(json_string)
+
+
+class Region():
+  ALLOWABLE_TAG_KEYS = ["highway", "zone:traffic", "bicycle_road", "zone:maxspeed"]
+  ALLOWABLE_HIGHWAY_TYPES = ["motorway", "trunk", "primary", "secondary", "tertiary", "unclassified", "residential", "service", "motorway_link", "trunk_link", "primary_link", "secondary_link", "tertiary_link", "living_street"]
+  def __init__(self, name):
+    self.name = name
+    self.rules = []
+
+  def add_rule(self, tag_conditions, speed):
+    new_rule = {}
+    if not isinstance(tag_conditions, dict):
+      raise TypeError("Rule tag conditions must be dictionary")
+    if not all(tag_key in self.ALLOWABLE_TAG_KEYS for tag_key in tag_conditions):
+      raise ValueError("Rule tag keys must be in allowable tag kesy") # If this is by mistake, please update ALLOWABLE_TAG_KEYS
+    if 'highway' in tag_conditions:
+      if not tag_conditions['highway'] in self.ALLOWABLE_HIGHWAY_TYPES:
+        raise ValueError("Invalid Highway type {}".format(tag_conditions["highway"]))
+    new_rule['tags'] = tag_conditions
+    try:
+      new_rule['speed'] = str(speed)
+    except ValueError:
+      raise ValueError("Rule speed must be string")
+    self.rules.append(new_rule)
+
+  def jsonify(self):
+    ret_dict = {}
+    ret_dict[self.name] = self.rules
+    return ret_dict
+
+class Country(Region):
+  ALLOWABLE_COUNTRY_CODES = ["AF","AX","AL","DZ","AS","AD","AO","AI","AQ","AG","AR","AM","AW","AU","AT","AZ","BS","BH","BD","BB","BY","BE","BZ","BJ","BM","BT","BO","BQ","BA","BW","BV","BR","IO","BN","BG","BF","BI","KH","CM","CA","CV","KY","CF","TD","CL","CN","CX","CC","CO","KM","CG","CD","CK","CR","CI","HR","CU","CW","CY","CZ","DK","DJ","DM","DO","EC","EG","SV","GQ","ER","EE","ET","FK","FO","FJ","FI","FR","GF","PF","TF","GA","GM","GE","DE","GH","GI","GR","GL","GD","GP","GU","GT","GG","GN","GW","GY","HT","HM","VA","HN","HK","HU","IS","IN","ID","IR","IQ","IE","IM","IL","IT","JM","JP","JE","JO","KZ","KE","KI","KP","KR","KW","KG","LA","LV","LB","LS","LR","LY","LI","LT","LU","MO","MK","MG","MW","MY","MV","ML","MT","MH","MQ","MR","MU","YT","MX","FM","MD","MC","MN","ME","MS","MA","MZ","MM","NA","NR","NP","NL","NC","NZ","NI","NE","NG","NU","NF","MP","NO","OM","PK","PW","PS","PA","PG","PY","PE","PH","PN","PL","PT","PR","QA","RE","RO","RU","RW","BL","SH","KN","LC","MF","PM","VC","WS","SM","ST","SA","SN","RS","SC","SL","SG","SX","SK","SI","SB","SO","ZA","GS","SS","ES","LK","SD","SR","SJ","SZ","SE","CH","SY","TW","TJ","TZ","TH","TL","TG","TK","TO","TT","TN","TR","TM","TC","TV","UG","UA","AE","GB","US","UM","UY","UZ","VU","VE","VN","VG","VI","WF","EH","YE","ZM","ZW"]
+  def __init__(self, ISO_3166_alpha_2):
+    Region.__init__(self, ISO_3166_alpha_2)
+    if ISO_3166_alpha_2 not in self.ALLOWABLE_COUNTRY_CODES:
+      raise ValueError("Not valid IOS 3166 country code")
+    self.regions = {}
+
+  def add_region(self, name):
+    self.regions[name] = Region(name)
+    return self.regions[name]
+
+  def jsonify(self):
+    ret_dict = {}
+    ret_dict[self.name] = {}
+    for r_name, region in self.regions.items():
+      ret_dict[self.name].update(region.jsonify())
+    ret_dict[self.name]['Default'] = self.rules
+    return ret_dict
+
+
+if __name__ == '__main__':
+  main()

selfdrive/mapd/mapd.py

@@ -0,0 +1,296 @@
+#!/usr/bin/env python3
+
+# Add phonelibs openblas to LD_LIBRARY_PATH if import fails
+from common.basedir import BASEDIR
+try:
+  from scipy import spatial
+except ImportError as e:
+  import os
+  import sys
+
+
+  openblas_path = os.path.join(BASEDIR, "phonelibs/openblas/")
+  os.environ['LD_LIBRARY_PATH'] += ':' + openblas_path
+
+  args = [sys.executable]
+  args.extend(sys.argv)
+  os.execv(sys.executable, args)
+
+DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json"
+from selfdrive.mapd import default_speeds_generator
+default_speeds_generator.main(DEFAULT_SPEEDS_BY_REGION_JSON_FILE)
+
+import os
+import sys
+import time
+import zmq
+import threading
+import numpy as np
+import overpy
+from collections import defaultdict
+
+from common.params import Params
+from common.transformations.coordinates import geodetic2ecef
+from cereal.services import service_list
+import cereal.messaging as messaging
+from selfdrive.mapd.mapd_helpers import MAPS_LOOKAHEAD_DISTANCE, Way, circle_through_points
+import selfdrive.crash as crash
+from selfdrive.version import version, dirty
+
+
+OVERPASS_API_URL = "https://overpass.kumi.systems/api/interpreter"
+OVERPASS_HEADERS = {
+    'User-Agent': 'NEOS (comma.ai)',
+    'Accept-Encoding': 'gzip'
+}
+
+last_gps = None
+query_lock = threading.Lock()
+last_query_result = None
+last_query_pos = None
+cache_valid = False
+
+def build_way_query(lat, lon, radius=50):
+  """Builds a query to find all highways within a given radius around a point"""
+  pos = "  (around:%f,%f,%f)" % (radius, lat, lon)
+  lat_lon = "(%f,%f)" % (lat, lon)
+  q = """(
+  way
+  """ + pos + """
+  [highway][highway!~"^(footway|path|bridleway|steps|cycleway|construction|bus_guideway|escape)$"];
+  >;);out;""" + """is_in""" + lat_lon + """;area._[admin_level~"[24]"];
+  convert area ::id = id(), admin_level = t['admin_level'],
+  name = t['name'], "ISO3166-1:alpha2" = t['ISO3166-1:alpha2'];out;
+  """
+  return q
+
+
+def query_thread():
+  global last_query_result, last_query_pos, cache_valid
+  api = overpy.Overpass(url=OVERPASS_API_URL, headers=OVERPASS_HEADERS, timeout=10.)
+
+  while True:
+    time.sleep(1)
+    if last_gps is not None:
+      fix_ok = last_gps.flags & 1
+      if not fix_ok:
+        continue
+
+      if last_query_pos is not None:
+        cur_ecef = geodetic2ecef((last_gps.latitude, last_gps.longitude, last_gps.altitude))
+        prev_ecef = geodetic2ecef((last_query_pos.latitude, last_query_pos.longitude, last_query_pos.altitude))
+        dist = np.linalg.norm(cur_ecef - prev_ecef)
+        if dist < 1000: #updated when we are 1km from the edge of the downloaded circle
+          continue
+
+        if dist > 3000:
+          cache_valid = False
+
+      q = build_way_query(last_gps.latitude, last_gps.longitude, radius=3000)
+      try:
+        new_result = api.query(q)
+
+        # Build kd-tree
+        nodes = []
+        real_nodes = []
+        node_to_way = defaultdict(list)
+        location_info = {}
+
+        for n in new_result.nodes:
+          nodes.append((float(n.lat), float(n.lon), 0))
+          real_nodes.append(n)
+
+        for way in new_result.ways:
+          for n in way.nodes:
+            node_to_way[n.id].append(way)
+
+        for area in new_result.areas:
+          if area.tags.get('admin_level', '') == "2":
+            location_info['country'] = area.tags.get('ISO3166-1:alpha2', '')
+          if area.tags.get('admin_level', '') == "4":
+            location_info['region'] = area.tags.get('name', '')
+
+        nodes = np.asarray(nodes)
+        nodes = geodetic2ecef(nodes)
+        tree = spatial.cKDTree(nodes)
+
+        query_lock.acquire()
+        last_query_result = new_result, tree, real_nodes, node_to_way, location_info
+        last_query_pos = last_gps
+        cache_valid = True
+        query_lock.release()
+
+      except Exception as e:
+        print(e)
+        query_lock.acquire()
+        last_query_result = None
+        query_lock.release()
+
+
+def mapsd_thread():
+  global last_gps
+
+  gps_sock = messaging.sub_sock('gpsLocation', conflate=True)
+  gps_external_sock = messaging.sub_sock('gpsLocationExternal', conflate=True)
+  map_data_sock = messaging.pub_sock('liveMapData')
+
+  cur_way = None
+  curvature_valid = False
+  curvature = None
+  upcoming_curvature = 0.
+  dist_to_turn = 0.
+  road_points = None
+
+  while True:
+    gps = messaging.recv_one(gps_sock)
+    gps_ext = messaging.recv_one_or_none(gps_external_sock)
+
+    if gps_ext is not None:
+      gps = gps_ext.gpsLocationExternal
+    else:
+      gps = gps.gpsLocation
+
+    last_gps = gps
+
+    fix_ok = gps.flags & 1
+    if not fix_ok or last_query_result is None or not cache_valid:
+      cur_way = None
+      curvature = None
+      curvature_valid = False
+      upcoming_curvature = 0.
+      dist_to_turn = 0.
+      road_points = None
+      map_valid = False
+    else:
+      map_valid = True
+      lat = gps.latitude
+      lon = gps.longitude
+      heading = gps.bearing
+      speed = gps.speed
+
+      query_lock.acquire()
+      cur_way = Way.closest(last_query_result, lat, lon, heading, cur_way)
+      if cur_way is not None:
+        pnts, curvature_valid = cur_way.get_lookahead(lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
+
+        xs = pnts[:, 0]
+        ys = pnts[:, 1]
+        road_points = [float(x) for x in xs], [float(y) for y in ys]
+
+        if speed < 10:
+          curvature_valid = False
+        if curvature_valid and pnts.shape[0] <= 3:
+          curvature_valid = False
+
+        # The curvature is valid when at least MAPS_LOOKAHEAD_DISTANCE of road is found
+        if curvature_valid:
+          # Compute the curvature for each point
+          with np.errstate(divide='ignore'):
+            circles = [circle_through_points(*p) for p in zip(pnts, pnts[1:], pnts[2:])]
+            circles = np.asarray(circles)
+            radii = np.nan_to_num(circles[:, 2])
+            radii[radii < 10] = np.inf
+            curvature = 1. / radii
+
+          # Index of closest point
+          closest = np.argmin(np.linalg.norm(pnts, axis=1))
+          dist_to_closest = pnts[closest, 0]  # We can use x distance here since it should be close
+
+          # Compute distance along path
+          dists = list()
+          dists.append(0)
+          for p, p_prev in zip(pnts, pnts[1:, :]):
+            dists.append(dists[-1] + np.linalg.norm(p - p_prev))
+          dists = np.asarray(dists)
+          dists = dists - dists[closest] + dist_to_closest
+          dists = dists[1:-1]
+
+          close_idx = np.logical_and(dists > 0, dists < 500)
+          dists = dists[close_idx]
+          curvature = curvature[close_idx]
+
+          if len(curvature):
+            # TODO: Determine left or right turn
+            curvature = np.nan_to_num(curvature)
+
+            # Outlier rejection
+            new_curvature = np.percentile(curvature, 90, interpolation='lower')
+
+            k = 0.6
+            upcoming_curvature = k * upcoming_curvature + (1 - k) * new_curvature
+            in_turn_indices = curvature > 0.8 * new_curvature
+
+            if np.any(in_turn_indices):
+              dist_to_turn = np.min(dists[in_turn_indices])
+            else:
+              dist_to_turn = 999
+          else:
+            upcoming_curvature = 0.
+            dist_to_turn = 999
+
+      query_lock.release()
+
+    dat = messaging.new_message()
+    dat.init('liveMapData')
+
+    if last_gps is not None:
+      dat.liveMapData.lastGps = last_gps
+
+    if cur_way is not None:
+      dat.liveMapData.wayId = cur_way.id
+
+      # Speed limit
+      max_speed = cur_way.max_speed()
+      if max_speed is not None:
+        dat.liveMapData.speedLimitValid = True
+        dat.liveMapData.speedLimit = max_speed
+
+        # TODO: use the function below to anticipate upcoming speed limits
+        #max_speed_ahead, max_speed_ahead_dist = cur_way.max_speed_ahead(max_speed, lat, lon, heading, MAPS_LOOKAHEAD_DISTANCE)
+        #if max_speed_ahead is not None and max_speed_ahead_dist is not None:
+        #  dat.liveMapData.speedLimitAheadValid = True
+        #  dat.liveMapData.speedLimitAhead = float(max_speed_ahead)
+        #  dat.liveMapData.speedLimitAheadDistance = float(max_speed_ahead_dist)
+
+
+      advisory_max_speed = cur_way.advisory_max_speed()
+      if advisory_max_speed is not None:
+        dat.liveMapData.speedAdvisoryValid = True
+        dat.liveMapData.speedAdvisory = advisory_max_speed
+
+      # Curvature
+      dat.liveMapData.curvatureValid = curvature_valid
+      dat.liveMapData.curvature = float(upcoming_curvature)
+      dat.liveMapData.distToTurn = float(dist_to_turn)
+      if road_points is not None:
+        dat.liveMapData.roadX, dat.liveMapData.roadY = road_points
+      if curvature is not None:
+        dat.liveMapData.roadCurvatureX = [float(x) for x in dists]
+        dat.liveMapData.roadCurvature = [float(x) for x in curvature]
+
+    dat.liveMapData.mapValid = map_valid
+
+    map_data_sock.send(dat.to_bytes())
+
+
+def main(gctx=None):
+  params = Params()
+  dongle_id = params.get("DongleId")
+  crash.bind_user(id=dongle_id)
+  crash.bind_extra(version=version, dirty=dirty, is_eon=True)
+  crash.install()
+
+  main_thread = threading.Thread(target=mapsd_thread)
+  main_thread.daemon = True
+  main_thread.start()
+
+  q_thread = threading.Thread(target=query_thread)
+  q_thread.daemon = True
+  q_thread.start()
+
+  while True:
+    time.sleep(0.1)
+
+
+if __name__ == "__main__":
+  main()

selfdrive/mapd/mapd_helpers.py

@@ -0,0 +1,364 @@
+import math
+import json
+import numpy as np
+from datetime import datetime
+from common.basedir import BASEDIR
+from selfdrive.config import Conversions as CV
+from common.transformations.coordinates import LocalCoord, geodetic2ecef
+
+LOOKAHEAD_TIME = 10.
+MAPS_LOOKAHEAD_DISTANCE = 50 * LOOKAHEAD_TIME
+
+DEFAULT_SPEEDS_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds.json"
+DEFAULT_SPEEDS = {}
+with open(DEFAULT_SPEEDS_JSON_FILE, "rb") as f:
+  DEFAULT_SPEEDS = json.loads(f.read())
+
+DEFAULT_SPEEDS_BY_REGION_JSON_FILE = BASEDIR + "/selfdrive/mapd/default_speeds_by_region.json"
+DEFAULT_SPEEDS_BY_REGION = {}
+with open(DEFAULT_SPEEDS_BY_REGION_JSON_FILE, "rb") as f:
+  DEFAULT_SPEEDS_BY_REGION = json.loads(f.read())
+
+def circle_through_points(p1, p2, p3):
+  """Fits a circle through three points
+  Formulas from: http://www.ambrsoft.com/trigocalc/circle3d.htm"""
+  x1, y1, _ = p1
+  x2, y2, _ = p2
+  x3, y3, _ = p3
+
+  A = x1 * (y2 - y3) - y1 * (x2 - x3) + x2 * y3 - x3 * y2
+  B = (x1**2 + y1**2) * (y3 - y2) + (x2**2 + y2**2) * (y1 - y3) + (x3**2 + y3**2) * (y2 - y1)
+  C = (x1**2 + y1**2) * (x2 - x3) + (x2**2 + y2**2) * (x3 - x1) + (x3**2 + y3**2) * (x1 - x2)
+  D = (x1**2 + y1**2) * (x3 * y2 - x2 * y3) + (x2**2 + y2**2) * (x1 * y3 - x3 * y1) + (x3**2 + y3**2) * (x2 * y1 - x1 * y2)
+
+  return (-B / (2 * A), - C / (2 * A), np.sqrt((B**2 + C**2 - 4 * A * D) / (4 * A**2)))
+
+def parse_speed_unit(max_speed):
+  """Converts a maxspeed string to m/s based on the unit present in the input.
+  OpenStreetMap defaults to kph if no unit is present. """
+
+  if not max_speed:
+    return None
+
+  conversion = CV.KPH_TO_MS
+  if 'mph' in max_speed:
+    max_speed = max_speed.replace(' mph', '')
+    conversion = CV.MPH_TO_MS
+  try:
+    return float(max_speed) * conversion
+  except ValueError:
+    return None
+
+def parse_speed_tags(tags):
+  """Parses tags on a way to find the maxspeed string"""
+  max_speed = None
+
+  if 'maxspeed' in tags:
+    max_speed = tags['maxspeed']
+
+  if 'maxspeed:conditional' in tags:
+    try:
+      max_speed_cond, cond = tags['maxspeed:conditional'].split(' @ ')
+      cond = cond[1:-1]
+
+      start, end = cond.split('-')
+      now = datetime.now()  # TODO: Get time and timezone from gps fix so this will work correctly on replays
+      start = datetime.strptime(start, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
+      end = datetime.strptime(end, "%H:%M").replace(year=now.year, month=now.month, day=now.day)
+
+      if start <= now <= end:
+        max_speed = max_speed_cond
+    except ValueError:
+      pass
+
+  if not max_speed and 'source:maxspeed' in tags:
+    max_speed = DEFAULT_SPEEDS.get(tags['source:maxspeed'], None)
+  if not max_speed and 'maxspeed:type' in tags:
+    max_speed = DEFAULT_SPEEDS.get(tags['maxspeed:type'], None)
+
+  max_speed = parse_speed_unit(max_speed)
+  return max_speed
+
+def geocode_maxspeed(tags, location_info):
+  max_speed = None
+  try:
+    geocode_country = location_info.get('country', '')
+    geocode_region = location_info.get('region', '')
+
+    country_rules = DEFAULT_SPEEDS_BY_REGION.get(geocode_country, {})
+    country_defaults = country_rules.get('Default', [])
+    for rule in country_defaults:
+      rule_valid = all(
+        tag_name in tags
+        and tags[tag_name] == value
+        for tag_name, value in rule['tags'].items()
+      )
+      if rule_valid:
+        max_speed = rule['speed']
+        break #stop searching country
+
+    region_rules = country_rules.get(geocode_region, [])
+    for rule in region_rules:
+      rule_valid = all(
+        tag_name in tags
+        and tags[tag_name] == value
+        for tag_name, value in rule['tags'].items()
+      )
+      if rule_valid:
+        max_speed = rule['speed']
+        break #stop searching region
+  except KeyError:
+    pass
+  max_speed = parse_speed_unit(max_speed)
+  return max_speed
+
+class Way:
+  def __init__(self, way, query_results):
+    self.id = way.id
+    self.way = way
+    self.query_results = query_results
+
+    points = list()
+
+    for node in self.way.get_nodes(resolve_missing=False):
+      points.append((float(node.lat), float(node.lon), 0.))
+
+    self.points = np.asarray(points)
+
+  @classmethod
+  def closest(cls, query_results, lat, lon, heading, prev_way=None):
+    results, tree, real_nodes, node_to_way, location_info = query_results
+
+    cur_pos = geodetic2ecef((lat, lon, 0))
+    nodes = tree.query_ball_point(cur_pos, 500)
+
+    # If no nodes within 500m, choose closest one
+    if not nodes:
+      nodes = [tree.query(cur_pos)[1]]
+
+    ways = []
+    for n in nodes:
+      real_node = real_nodes[n]
+      ways += node_to_way[real_node.id]
+    ways = set(ways)
+
+    closest_way = None
+    best_score = None
+    for way in ways:
+      way = Way(way, query_results)
+      points = way.points_in_car_frame(lat, lon, heading)
+
+      on_way = way.on_way(lat, lon, heading, points)
+      if not on_way:
+        continue
+
+      # Create mask of points in front and behind
+      x = points[:, 0]
+      y = points[:, 1]
+      angles = np.arctan2(y, x)
+      front = np.logical_and((-np.pi / 2) < angles,
+                                angles < (np.pi / 2))
+      behind = np.logical_not(front)
+
+      dists = np.linalg.norm(points, axis=1)
+
+      # Get closest point behind the car
+      dists_behind = np.copy(dists)
+      dists_behind[front] = np.NaN
+      closest_behind = points[np.nanargmin(dists_behind)]
+
+      # Get closest point in front of the car
+      dists_front = np.copy(dists)
+      dists_front[behind] = np.NaN
+      closest_front = points[np.nanargmin(dists_front)]
+
+      # fit line: y = a*x + b
+      x1, y1, _ = closest_behind
+      x2, y2, _ = closest_front
+      a = (y2 - y1) / max((x2 - x1), 1e-5)
+      b = y1 - a * x1
+
+      # With a factor of 60 a 20m offset causes the same error as a 20 degree heading error
+      # (A 20 degree heading offset results in an a of about 1/3)
+      score = abs(a) * 60. + abs(b)
+
+      # Prefer same type of road
+      if prev_way is not None:
+        if way.way.tags.get('highway', '') == prev_way.way.tags.get('highway', ''):
+          score *= 0.5
+
+      if closest_way is None or score < best_score:
+        closest_way = way
+        best_score = score
+
+    # Normal score is < 5
+    if best_score > 50:
+      return None
+
+    return closest_way
+
+  def __str__(self):
+    return "%s %s" % (self.id, self.way.tags)
+
+  def max_speed(self):
+    """Extracts the (conditional) speed limit from a way"""
+    if not self.way:
+      return None
+
+    max_speed = parse_speed_tags(self.way.tags)
+    if not max_speed:
+      location_info = self.query_results[4]
+      max_speed = geocode_maxspeed(self.way.tags, location_info)
+
+    return max_speed
+
+  def max_speed_ahead(self, current_speed_limit, lat, lon, heading, lookahead):
+    """Look ahead for a max speed"""
+    if not self.way:
+      return None
+
+    speed_ahead = None
+    speed_ahead_dist = None
+    lookahead_ways = 5
+    way = self
+    for i in range(lookahead_ways):
+      way_pts = way.points_in_car_frame(lat, lon, heading)
+
+      # Check current lookahead distance
+      max_dist = np.linalg.norm(way_pts[-1, :])
+
+      if max_dist > 2 * lookahead:
+        break
+
+      if 'maxspeed' in way.way.tags:
+        spd = parse_speed_tags(way.way.tags)
+        if not spd:
+          location_info = self.query_results[4]
+          spd = geocode_maxspeed(way.way.tags, location_info)
+        if spd < current_speed_limit:
+          speed_ahead = spd
+          min_dist = np.linalg.norm(way_pts[1, :])
+          speed_ahead_dist = min_dist
+          break
+      # Find next way
+      way = way.next_way()
+      if not way:
+        break
+
+    return speed_ahead, speed_ahead_dist
+
+  def advisory_max_speed(self):
+    if not self.way:
+      return None
+
+    tags = self.way.tags
+    adv_speed = None
+
+    if 'maxspeed:advisory' in tags:
+      adv_speed = tags['maxspeed:advisory']
+      adv_speed = parse_speed_unit(adv_speed)
+    return adv_speed
+
+  def on_way(self, lat, lon, heading, points=None):
+    if points is None:
+      points = self.points_in_car_frame(lat, lon, heading)
+    x = points[:, 0]
+    return np.min(x) < 0. and np.max(x) > 0.
+
+  def closest_point(self, lat, lon, heading, points=None):
+    if points is None:
+      points = self.points_in_car_frame(lat, lon, heading)
+    i = np.argmin(np.linalg.norm(points, axis=1))
+    return points[i]
+
+  def distance_to_closest_node(self, lat, lon, heading, points=None):
+    if points is None:
+      points = self.points_in_car_frame(lat, lon, heading)
+    return np.min(np.linalg.norm(points, axis=1))
+
+  def points_in_car_frame(self, lat, lon, heading):
+    lc = LocalCoord.from_geodetic([lat, lon, 0.])
+
+    # Build rotation matrix
+    heading = math.radians(-heading + 90)
+    c, s = np.cos(heading), np.sin(heading)
+    rot = np.array([[c, s, 0.], [-s, c, 0.], [0., 0., 1.]])
+
+    # Convert to local coordinates
+    points_carframe = lc.geodetic2ned(self.points).T
+
+    # Rotate with heading of car
+    points_carframe = np.dot(rot, points_carframe[(1, 0, 2), :]).T
+
+    return points_carframe
+
+  def next_way(self, backwards=False):
+    results, tree, real_nodes, node_to_way, location_info = self.query_results
+
+    if backwards:
+      node = self.way.nodes[0]
+    else:
+      node = self.way.nodes[-1]
+
+    ways = node_to_way[node.id]
+
+    way = None
+    try:
+      # Simple heuristic to find next way
+      ways = [w for w in ways if w.id != self.id]
+      ways = [w for w in ways if w.nodes[0] == node]
+
+      # Filter on highway tag
+      acceptable_tags = list()
+      cur_tag = self.way.tags['highway']
+      acceptable_tags.append(cur_tag)
+      if cur_tag == 'motorway_link':
+        acceptable_tags.append('motorway')
+        acceptable_tags.append('trunk')
+        acceptable_tags.append('primary')
+      ways = [w for w in ways if w.tags['highway'] in acceptable_tags]
+
+      # Filter on number of lanes
+      cur_num_lanes = int(self.way.tags['lanes'])
+      if len(ways) > 1:
+        ways_same_lanes = [w for w in ways if int(w.tags['lanes']) == cur_num_lanes]
+        if len(ways_same_lanes) == 1:
+          ways = ways_same_lanes
+      if len(ways) > 1:
+        ways = [w for w in ways if int(w.tags['lanes']) > cur_num_lanes]
+      if len(ways) == 1:
+        way = Way(ways[0], self.query_results)
+
+    except (KeyError, ValueError):
+      pass
+
+    return way
+
+  def get_lookahead(self, lat, lon, heading, lookahead):
+    pnts = None
+    way = self
+    valid = False
+
+    for i in range(5):
+      # Get new points and append to list
+      new_pnts = way.points_in_car_frame(lat, lon, heading)
+
+      if pnts is None:
+        pnts = new_pnts
+      else:
+        pnts = np.vstack([pnts, new_pnts])
+
+      # Check current lookahead distance
+      max_dist = np.linalg.norm(pnts[-1, :])
+      if max_dist > lookahead:
+        valid = True
+
+      if max_dist > 2 * lookahead:
+        break
+
+      # Find next way
+      way = way.next_way()
+      if not way:
+        break
+
+    return pnts, valid

selfdrive/modeld/SConscript

@@ -0,0 +1,36 @@
+Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
+lenv = env.Clone()
+
+libs = [messaging, common, 'OpenCL', 'SNPE', 'capnp', 'zmq', 'kj', 'yuv', gpucommon, visionipc]
+
+common_src = [
+  "models/commonmodel.c",
+  "runners/snpemodel.cc",
+  "transforms/loadyuv.c",
+  "transforms/transform.c"]
+
+if arch == "aarch64":
+  libs += ['gsl', 'CB', 'gnustl_shared']
+else:
+  libs += ['symphony-cpu', 'pthread']
+
+  if FindFile('libtensorflow.so', env['LIBPATH']):
+    # for tensorflow support
+    common_src += ['runners/tfmodel.cc']
+    libs += ['tensorflow']
+    # tell runners to use it
+    lenv['CFLAGS'].append("-DUSE_TF_MODEL")
+    lenv['CXXFLAGS'].append("-DUSE_TF_MODEL")
+
+common = lenv.Object(common_src)
+
+lenv.Program('_monitoringd', [
+    "monitoringd.cc",
+    "models/monitoring.cc",
+  ]+common, LIBS=libs)
+
+lenv.Program('_modeld', [
+    "modeld.cc",
+    "models/driving.cc",
+  ]+common, LIBS=libs)
+

selfdrive/modeld/constants.py

@@ -0,0 +1,6 @@
+MAX_DISTANCE = 140.
+LANE_OFFSET = 1.8
+MAX_REL_V = 10.
+
+LEAD_X_SCALE = 10
+LEAD_Y_SCALE = 10

selfdrive/modeld/modeld

@@ -0,0 +1,4 @@
+#!/bin/sh
+export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/:/home/batman/one/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH"
+exec ./_modeld
+

selfdrive/modeld/modeld.cc

@@ -0,0 +1,247 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "common/visionbuf.h"
+#include "common/visionipc.h"
+#include "common/swaglog.h"
+
+#include "models/driving.h"
+
+volatile sig_atomic_t do_exit = 0;
+
+static void set_do_exit(int sig) {
+  do_exit = 1;
+}
+
+// globals
+bool run_model;
+mat3 cur_transform;
+pthread_mutex_t transform_lock;
+
+void* live_thread(void *arg) {
+  int err;
+  set_thread_name("live");
+
+  Context * c = Context::create();
+  SubSocket * live_calibration_sock = SubSocket::create(c, "liveCalibration");
+  assert(live_calibration_sock != NULL);
+
+  Poller * poller = Poller::create({live_calibration_sock});
+
+  /*
+     import numpy as np
+     from common.transformations.model import medmodel_frame_from_road_frame
+     medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
+     ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
+  */
+  Eigen::Matrix<float, 3, 3> ground_from_medmodel_frame;
+  ground_from_medmodel_frame <<
+    0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
+    -1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
+    -1.84808520e-20, 9.00738606e-04,-4.28751576e-02;
+
+  Eigen::Matrix<float, 3, 3> eon_intrinsics;
+  eon_intrinsics <<
+    910.0, 0.0, 582.0,
+    0.0, 910.0, 437.0,
+    0.0,   0.0,   1.0;
+
+  while (!do_exit) {
+    for (auto sock : poller->poll(10)){
+      Message * msg = sock->receive();
+
+      auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
+      memcpy(amsg.begin(), msg->getData(), msg->getSize());
+
+      capnp::FlatArrayMessageReader cmsg(amsg);
+      cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+      if (event.isLiveCalibration()) {
+        pthread_mutex_lock(&transform_lock);
+
+        auto extrinsic_matrix = event.getLiveCalibration().getExtrinsicMatrix();
+        Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen;
+        for (int i = 0; i < 4*3; i++){
+          extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
+        }
+
+        auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen;
+        Eigen::Matrix<float, 3, 3> camera_frame_from_ground;
+        camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0);
+        camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
+        camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3);
+
+        auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame;
+
+        for (int i=0; i<3*3; i++) {
+          cur_transform.v[i] = warp_matrix(i / 3, i % 3);
+        }
+
+        run_model = true;
+        pthread_mutex_unlock(&transform_lock);
+      }
+
+      delete msg;
+    }
+
+  }
+
+
+  return NULL;
+}
+
+int main(int argc, char **argv) {
+  int err;
+  set_realtime_priority(1);
+
+  // start calibration thread
+  pthread_t live_thread_handle;
+  err = pthread_create(&live_thread_handle, NULL, live_thread, NULL);
+  assert(err == 0);
+
+  // messaging
+  Context *msg_context = Context::create();
+  PubSocket *model_sock = PubSocket::create(msg_context, "model");
+  PubSocket *posenet_sock = PubSocket::create(msg_context, "cameraOdometry");
+  SubSocket *pathplan_sock = SubSocket::create(msg_context, "pathPlan", "127.0.0.1", true);
+
+  assert(model_sock != NULL);
+  assert(posenet_sock != NULL);
+  assert(pathplan_sock != NULL);
+
+  // cl init
+  cl_device_id device_id;
+  cl_context context;
+  cl_command_queue q;
+  {
+    // TODO: refactor this
+    cl_platform_id platform_id[2];
+    cl_uint num_devices;
+    cl_uint num_platforms;
+
+    err = clGetPlatformIDs(sizeof(platform_id)/sizeof(cl_platform_id), platform_id, &num_platforms);
+    assert(err == 0);
+    
+    #ifdef QCOM
+      int clPlatform = 0;
+    #else
+      // don't use nvidia on pc, it's broken
+      // TODO: write this nicely
+      int clPlatform = num_platforms-1;
+    #endif
+
+    char cBuffer[1024];
+    clGetPlatformInfo(platform_id[clPlatform], CL_PLATFORM_NAME, sizeof(cBuffer), &cBuffer, NULL);
+    LOGD("got %d opencl platform(s), using %s", num_platforms, cBuffer);
+
+    err = clGetDeviceIDs(platform_id[clPlatform], CL_DEVICE_TYPE_DEFAULT, 1,
+                         &device_id, &num_devices);
+    assert(err == 0);
+
+    context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err);
+    assert(err == 0);
+
+    q = clCreateCommandQueue(context, device_id, 0, &err);
+    assert(err == 0);
+  }
+
+  // init the models
+  ModelState model;
+  model_init(&model, device_id, context, true);
+  LOGW("models loaded, modeld starting");
+
+  // debayering does a 2x downscale
+  mat3 yuv_transform = transform_scale_buffer((mat3){{
+    1.0, 0.0, 0.0,
+    0.0, 1.0, 0.0,
+    0.0, 0.0, 1.0,
+  }}, 0.5);
+
+  // loop
+  VisionStream stream;
+  while (!do_exit) {
+    VisionStreamBufs buf_info;
+    err = visionstream_init(&stream, VISION_STREAM_YUV, true, &buf_info);
+    if (err) {
+      printf("visionstream connect fail\n");
+      usleep(100000);
+      continue;
+    }
+    LOGW("connected with buffer size: %d", buf_info.buf_len);
+
+    // one frame in memory
+    cl_mem yuv_cl;
+    VisionBuf yuv_ion = visionbuf_allocate_cl(buf_info.buf_len, device_id, context, &yuv_cl);
+
+    double last = 0;
+    int desire = -1;
+    while (!do_exit) {
+      VIPCBuf *buf;
+      VIPCBufExtra extra;
+      buf = visionstream_get(&stream, &extra);
+      if (buf == NULL) {
+        printf("visionstream get failed\n");
+        break;
+      }
+
+      pthread_mutex_lock(&transform_lock);
+      mat3 transform = cur_transform;
+      const bool run_model_this_iter = run_model;
+      pthread_mutex_unlock(&transform_lock);
+
+      Message *msg = pathplan_sock->receive(true);
+      if (msg != NULL) {
+        // TODO: copy and pasted from camerad/main.cc
+        auto amsg = kj::heapArray<capnp::word>((msg->getSize() / sizeof(capnp::word)) + 1);
+        memcpy(amsg.begin(), msg->getData(), msg->getSize());
+
+        capnp::FlatArrayMessageReader cmsg(amsg);
+        cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+        // TODO: path planner timeout?
+        desire = ((int)event.getPathPlan().getDesire()) - 1;
+        delete msg;
+      }
+
+      double mt1 = 0, mt2 = 0;
+      if (run_model_this_iter) {
+        float vec_desire[DESIRE_SIZE] = {0};
+        if (desire >= 0 && desire < DESIRE_SIZE) {
+          vec_desire[desire] = 1.0;
+        }
+
+        mat3 model_transform = matmul3(yuv_transform, transform);
+
+        mt1 = millis_since_boot();
+
+        // TODO: don't make copies!
+        memcpy(yuv_ion.addr, buf->addr, buf_info.buf_len);
+
+        ModelDataRaw model_buf =
+            model_eval_frame(&model, q, yuv_cl, buf_info.width, buf_info.height,
+                             model_transform, NULL, vec_desire);
+        mt2 = millis_since_boot();
+
+        model_publish(model_sock, extra.frame_id, model_buf, extra.timestamp_eof);
+        posenet_publish(posenet_sock, extra.frame_id, model_buf, extra.timestamp_eof);
+
+        LOGD("model process: %.2fms, from last %.2fms", mt2-mt1, mt1-last);
+        last = mt1;
+      }
+
+    }
+    visionbuf_free(&yuv_ion);
+  }
+
+  visionstream_destroy(&stream);
+
+  delete model_sock;
+  
+  model_free(&model);
+
+  LOG("joining live_thread");
+  err = pthread_join(live_thread_handle, NULL);
+  assert(err == 0);
+
+  return 0;
+}

selfdrive/modeld/models/commonmodel.c

@@ -0,0 +1,68 @@
+#include "commonmodel.h"
+
+#include <czmq.h>
+#include "cereal/gen/c/log.capnp.h"
+#include "common/mat.h"
+#include "common/timing.h"
+
+void frame_init(ModelFrame* frame, int width, int height,
+                      cl_device_id device_id, cl_context context) {
+  int err;
+  frame->device_id = device_id;
+  frame->context = context;
+
+  transform_init(&frame->transform, context, device_id);
+  frame->transformed_width = width;
+  frame->transformed_height = height;
+
+  frame->transformed_y_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
+                                       frame->transformed_width*frame->transformed_height, NULL, &err);
+  assert(err == 0);
+  frame->transformed_u_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
+                                       (frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
+  assert(err == 0);
+  frame->transformed_v_cl = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
+                                       (frame->transformed_width/2)*(frame->transformed_height/2), NULL, &err);
+  assert(err == 0);
+
+  frame->net_input_size = ((width*height*3)/2)*sizeof(float);
+  frame->net_input = clCreateBuffer(frame->context, CL_MEM_READ_WRITE,
+                                frame->net_input_size, (void*)NULL, &err);
+  assert(err == 0);
+
+  loadyuv_init(&frame->loadyuv, context, device_id, frame->transformed_width, frame->transformed_height);
+}
+
+float *frame_prepare(ModelFrame* frame, cl_command_queue q,
+                           cl_mem yuv_cl, int width, int height,
+                           mat3 transform) {
+  int err;
+  int i = 0;
+  transform_queue(&frame->transform, q,
+                  yuv_cl, width, height,
+                  frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
+                  frame->transformed_width, frame->transformed_height,
+                  transform);
+  loadyuv_queue(&frame->loadyuv, q,
+                frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,
+                frame->net_input);
+  float *net_input_buf = (float *)clEnqueueMapBuffer(q, frame->net_input, CL_TRUE,
+                                            CL_MAP_READ, 0, frame->net_input_size,
+                                            0, NULL, NULL, &err);
+  clFinish(q);
+  return net_input_buf;
+}
+
+void frame_free(ModelFrame* frame) {
+  transform_destroy(&frame->transform);
+  loadyuv_destroy(&frame->loadyuv);
+}
+
+
+float sigmoid(float input) {
+  return 1 / (1 + expf(-input));
+}
+
+float softplus(float input) {
+  return log1p(expf(input));
+}

selfdrive/modeld/models/commonmodel.h

@@ -0,0 +1,42 @@
+#ifndef COMMONMODEL_H
+#define COMMONMODEL_H
+
+#include <CL/cl.h>
+
+#include "common/mat.h"
+#include "transforms/transform.h"
+#include "transforms/loadyuv.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+float softplus(float input);
+float sigmoid(float input);
+
+typedef struct ModelFrame {
+  cl_device_id device_id;
+  cl_context context;
+
+  // input
+  Transform transform;
+  int transformed_width, transformed_height;
+  cl_mem transformed_y_cl, transformed_u_cl, transformed_v_cl;
+  LoadYUVState loadyuv;
+  cl_mem net_input;
+  size_t net_input_size;
+} ModelFrame;
+
+void frame_init(ModelFrame* frame, int width, int height,
+                      cl_device_id device_id, cl_context context);
+float *frame_prepare(ModelFrame* frame, cl_command_queue q,
+                           cl_mem yuv_cl, int width, int height,
+                           mat3 transform);
+void frame_free(ModelFrame* frame);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+

selfdrive/modeld/models/driving.cc

@@ -0,0 +1,287 @@
+#include <string.h>
+#include <assert.h>
+#include <fcntl.h>
+#include <unistd.h>
+#include "common/timing.h"
+#include "driving.h"
+
+
+#define PATH_IDX 0
+#define LL_IDX PATH_IDX + MODEL_PATH_DISTANCE*2
+#define RL_IDX LL_IDX + MODEL_PATH_DISTANCE*2 + 1
+#define LEAD_IDX RL_IDX + MODEL_PATH_DISTANCE*2 + 1
+#define LONG_X_IDX LEAD_IDX + MDN_GROUP_SIZE*LEAD_MDN_N + SELECTION 
+#define LONG_V_IDX LONG_X_IDX + TIME_DISTANCE*2
+#define LONG_A_IDX LONG_V_IDX + TIME_DISTANCE*2
+#define META_IDX LONG_A_IDX + TIME_DISTANCE*2
+#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE
+#define OUTPUT_SIZE  POSE_IDX + POSE_SIZE
+#ifdef TEMPORAL
+  #define TEMPORAL_SIZE 512
+#else
+  #define TEMPORAL_SIZE 0
+#endif
+
+// #define DUMP_YUV
+
+Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
+
+void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) {
+  frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
+  s->input_frames = (float*)calloc(MODEL_FRAME_SIZE * 2, sizeof(float));
+  
+  const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
+  s->output = (float*)calloc(output_size, sizeof(float));
+  
+  s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME);
+
+#ifdef TEMPORAL
+  assert(temporal);
+  s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
+#endif
+
+#ifdef DESIRE
+  s->desire = (float*)malloc(DESIRE_SIZE * sizeof(float));
+  for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = 0.0;
+  s->m->addDesire(s->desire, DESIRE_SIZE);
+#endif
+
+  // Build Vandermonde matrix
+  for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
+    for(int j = 0; j < POLYFIT_DEGREE - 1; j++) {
+      vander(i, j) = pow(i, POLYFIT_DEGREE-j-1);
+    }
+  }
+}
+
+
+
+ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
+                           cl_mem yuv_cl, int width, int height,
+                           mat3 transform, void* sock, float *desire_in) {
+#ifdef DESIRE
+  if (desire_in != NULL) {
+    for (int i = 0; i < DESIRE_SIZE; i++) s->desire[i] = desire_in[i];
+  }
+#endif
+
+  //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
+
+  float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform);
+  memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE);
+  memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE);
+  s->m->execute(s->input_frames);
+
+  #ifdef DUMP_YUV
+    FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
+    fwrite(new_frame_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file);
+    fclose(dump_yuv_file);
+    assert(1==2);
+  #endif
+
+  // net outputs
+  ModelDataRaw net_outputs;
+  net_outputs.path = &s->output[PATH_IDX];
+  net_outputs.left_lane = &s->output[LL_IDX];
+  net_outputs.right_lane = &s->output[RL_IDX];
+  net_outputs.lead = &s->output[LEAD_IDX];
+  net_outputs.long_x = &s->output[LONG_X_IDX];
+  net_outputs.long_v = &s->output[LONG_V_IDX];
+  net_outputs.long_a = &s->output[LONG_A_IDX];
+  net_outputs.meta = &s->output[META_IDX];
+  net_outputs.pose = &s->output[POSE_IDX];
+  return net_outputs;
+}
+
+void model_free(ModelState* s) {
+  free(s->output);
+  free(s->input_frames);
+  frame_free(&s->frame);
+  delete s->m;
+}
+
+void poly_fit(float *in_pts, float *in_stds, float *out) {
+  // References to inputs
+  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > pts(in_pts, MODEL_PATH_DISTANCE);
+  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > std(in_stds, MODEL_PATH_DISTANCE);
+  Eigen::Map<Eigen::Matrix<float, POLYFIT_DEGREE - 1, 1> > p(out, POLYFIT_DEGREE - 1);
+
+  float y0 = pts[0];
+  pts = pts.array() - y0;
+
+  // Build Least Squares equations
+  Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> lhs = vander.array().colwise() / std.array();
+  Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> rhs = pts.array() / std.array();
+
+  // Improve numerical stability
+  Eigen::Matrix<float, POLYFIT_DEGREE - 1, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
+  lhs = lhs * scale.asDiagonal();
+
+  // Solve inplace
+  Eigen::ColPivHouseholderQR<Eigen::Ref<Eigen::MatrixXf> > qr(lhs);
+  p = qr.solve(rhs);
+
+  // Apply scale to output
+  p = p.transpose() * scale.asDiagonal();
+  out[3] = y0;
+}
+
+
+void fill_path(cereal::ModelData::PathData::Builder path, const float * data, bool has_prob, const float offset) {
+  float points_arr[MODEL_PATH_DISTANCE];
+  float stds_arr[MODEL_PATH_DISTANCE];
+  float poly_arr[POLYFIT_DEGREE];
+  float std;
+  float prob;
+
+  for (int i=0; i<MODEL_PATH_DISTANCE; i++) {
+    points_arr[i] = data[i] + offset;
+    stds_arr[i] = softplus(data[MODEL_PATH_DISTANCE + i]);
+  }
+  if (has_prob) {
+    prob =  sigmoid(data[MODEL_PATH_DISTANCE*2]);
+  } else {
+    prob = 1.0;
+  }
+  std = softplus(data[MODEL_PATH_DISTANCE]);
+  poly_fit(points_arr, stds_arr, poly_arr);
+
+  if (std::getenv("DEBUG")){
+    kj::ArrayPtr<const float> stds(&stds_arr[0], ARRAYSIZE(stds_arr));
+    path.setStds(stds);
+
+    kj::ArrayPtr<const float> points(&points_arr[0], ARRAYSIZE(points_arr));
+    path.setPoints(points);
+  }
+
+  kj::ArrayPtr<const float> poly(&poly_arr[0], ARRAYSIZE(poly_arr));
+  path.setPoly(poly);
+  path.setProb(prob);
+  path.setStd(std);
+}
+
+void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, int mdn_max_idx) {
+  const double x_scale = 10.0;
+  const double y_scale = 10.0;
+ 
+  lead.setProb(sigmoid(data[LEAD_MDN_N*MDN_GROUP_SIZE]));
+  lead.setDist(x_scale * data[mdn_max_idx*MDN_GROUP_SIZE]);
+  lead.setStd(x_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS]));
+  lead.setRelY(y_scale * data[mdn_max_idx*MDN_GROUP_SIZE + 1]);
+  lead.setRelYStd(y_scale * softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 1]));
+  lead.setRelVel(data[mdn_max_idx*MDN_GROUP_SIZE + 2]);
+  lead.setRelVelStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 2]));
+  lead.setRelA(data[mdn_max_idx*MDN_GROUP_SIZE + 3]);
+  lead.setRelAStd(softplus(data[mdn_max_idx*MDN_GROUP_SIZE + MDN_VALS + 3]));
+}
+
+void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) {
+  meta.setEngagedProb(meta_data[0]);
+  meta.setGasDisengageProb(meta_data[1]);
+  meta.setBrakeDisengageProb(meta_data[2]);
+  meta.setSteerOverrideProb(meta_data[3]);
+  kj::ArrayPtr<const float> desire_pred(&meta_data[OTHER_META_SIZE], DESIRE_PRED_SIZE);
+  meta.setDesirePrediction(desire_pred);
+}
+
+void fill_longi(cereal::ModelData::LongitudinalData::Builder longi, const float * long_v_data, const float * long_a_data) {
+  // just doing 10 vals, 1 every sec for now
+  float speed_arr[TIME_DISTANCE/10];
+  float accel_arr[TIME_DISTANCE/10];
+  for (int i=0; i<TIME_DISTANCE/10; i++) {
+    speed_arr[i] = long_v_data[i*10];
+    accel_arr[i] = long_a_data[i*10];
+  }
+  kj::ArrayPtr<const float> speed(&speed_arr[0], ARRAYSIZE(speed_arr));
+  longi.setSpeeds(speed);
+  kj::ArrayPtr<const float> accel(&accel_arr[0], ARRAYSIZE(accel_arr));
+  longi.setAccelerations(accel);
+}
+
+void model_publish(PubSocket *sock, uint32_t frame_id,
+                   const ModelDataRaw net_outputs, uint64_t timestamp_eof) {
+    // make msg
+    capnp::MallocMessageBuilder msg;
+    cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
+  
+    auto framed = event.initModel();
+    framed.setFrameId(frame_id);
+    framed.setTimestampEof(timestamp_eof);
+
+    auto lpath = framed.initPath();
+    fill_path(lpath, net_outputs.path, false, 0);
+    auto left_lane = framed.initLeftLane();
+    fill_path(left_lane, net_outputs.left_lane, true, 1.8);
+    auto right_lane = framed.initRightLane();
+    fill_path(right_lane, net_outputs.right_lane, true, -1.8);
+    auto longi = framed.initLongitudinal();
+    fill_longi(longi, net_outputs.long_v, net_outputs.long_a);
+
+
+    // Find the distribution that corresponds to the current lead
+    int mdn_max_idx = 0;
+    for (int i=1; i<LEAD_MDN_N; i++) {
+      if (net_outputs.lead[i*MDN_GROUP_SIZE + 8] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 8]) {
+        mdn_max_idx = i;
+      }
+    }
+    auto lead = framed.initLead();
+    fill_lead(lead, net_outputs.lead, mdn_max_idx);
+    // Find the distribution that corresponds to the lead in 2s
+    mdn_max_idx = 0;
+    for (int i=1; i<LEAD_MDN_N; i++) {
+      if (net_outputs.lead[i*MDN_GROUP_SIZE + 9] > net_outputs.lead[mdn_max_idx*MDN_GROUP_SIZE + 9]) {
+        mdn_max_idx = i;
+      }
+    }
+    auto lead_future = framed.initLeadFuture();
+    fill_lead(lead_future, net_outputs.lead, mdn_max_idx);
+
+
+    auto meta = framed.initMeta();
+    fill_meta(meta, net_outputs.meta);
+
+
+    // send message
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    sock->send((char*)bytes.begin(), bytes.size());
+  }
+
+void posenet_publish(PubSocket *sock, uint32_t frame_id,
+                   const ModelDataRaw net_outputs, uint64_t timestamp_eof) {
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+
+  float trans_arr[3];
+  float trans_std_arr[3];
+  float rot_arr[3];
+  float rot_std_arr[3];
+
+  for (int i =0; i < 3; i++) {
+    trans_arr[i] = net_outputs.pose[i];
+    trans_std_arr[i] = softplus(net_outputs.pose[6 + i]) + 1e-6;
+
+    rot_arr[i] = M_PI * net_outputs.pose[3 + i] / 180.0;
+    rot_std_arr[i] = M_PI * (softplus(net_outputs.pose[9 + i]) + 1e-6) / 180.0;
+  }
+
+  auto posenetd = event.initCameraOdometry();
+  kj::ArrayPtr<const float> trans_vs(&trans_arr[0], 3);
+  posenetd.setTrans(trans_vs);
+  kj::ArrayPtr<const float> rot_vs(&rot_arr[0], 3);
+  posenetd.setRot(rot_vs);
+  kj::ArrayPtr<const float> trans_std_vs(&trans_std_arr[0], 3);
+  posenetd.setTransStd(trans_std_vs);
+  kj::ArrayPtr<const float> rot_std_vs(&rot_std_arr[0], 3);
+  posenetd.setRotStd(rot_std_vs);
+  
+  posenetd.setTimestampEof(timestamp_eof);
+  posenetd.setFrameId(frame_id);
+
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  sock->send((char*)bytes.begin(), bytes.size());
+  }

selfdrive/modeld/models/driving.h

@@ -0,0 +1,81 @@
+#ifndef MODEL_H
+#define MODEL_H
+
+// gate this here
+#define TEMPORAL
+#define DESIRE
+
+#ifdef DESIRE
+  #define DESIRE_SIZE 8
+#endif
+
+#ifdef QCOM
+#include <eigen3/Eigen/Dense>
+#else
+#include <Eigen/Dense>
+#endif
+
+#include "common/mat.h"
+#include "common/util.h"
+
+#include "commonmodel.h"
+#include "runners/run.h"
+
+#include "cereal/gen/cpp/log.capnp.h"
+#include <czmq.h>
+#include <capnp/serialize.h>
+#include "messaging.hpp"
+
+#define MODEL_WIDTH 512
+#define MODEL_HEIGHT 256
+#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
+#define MODEL_NAME "supercombo_dlc"
+
+#define MODEL_PATH_DISTANCE 192
+#define POLYFIT_DEGREE 4
+#define SPEED_PERCENTILES 10
+#define DESIRE_PRED_SIZE 32
+#define OTHER_META_SIZE 4
+#define LEAD_MDN_N 5 // probs for 5 groups
+#define MDN_VALS 4 // output xyva for each lead group
+#define SELECTION 3 //output 3 group (lead now, in 2s and 6s)
+#define MDN_GROUP_SIZE 11
+#define TIME_DISTANCE 100
+#define POSE_SIZE 12
+
+struct ModelDataRaw {
+    float *path;
+    float *left_lane;
+    float *right_lane;
+    float *lead;
+    float *long_x;
+    float *long_v;
+    float *long_a;
+    float *meta;
+    float *pose;
+  };
+
+
+typedef struct ModelState {
+  ModelFrame frame;
+  float *output;
+  float *input_frames;
+  RunModel *m;
+#ifdef DESIRE
+  float *desire;
+#endif
+} ModelState;
+
+void model_init(ModelState* s, cl_device_id device_id,
+                cl_context context, int temporal);
+ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
+                           cl_mem yuv_cl, int width, int height,
+                           mat3 transform, void* sock, float *desire_in);
+void model_free(ModelState* s);
+void poly_fit(float *in_pts, float *in_stds, float *out);
+
+void model_publish(PubSocket* sock, uint32_t frame_id,
+                   const ModelDataRaw data, uint64_t timestamp_eof);
+void posenet_publish(PubSocket* sock, uint32_t frame_id,
+                   const ModelDataRaw data, uint64_t timestamp_eof);
+#endif

selfdrive/modeld/models/monitoring.cc

@@ -0,0 +1,163 @@
+#include <string.h>
+#include "monitoring.h"
+#include "common/mat.h"
+#include "common/timing.h"
+
+// #include <fastcv.h>
+#include <libyuv.h>
+
+#define MODEL_WIDTH 160
+#define MODEL_HEIGHT 320
+#define FULL_W 426
+
+void monitoring_init(MonitoringState* s) {
+  s->m = new DefaultRunModel("../../models/monitoring_model_q.dlc", (float*)&s->output, OUTPUT_SIZE, USE_DSP_RUNTIME);
+}
+
+MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height) {
+
+  uint8_t *raw_buf = (uint8_t*) stream_buf;
+  uint8_t *raw_y_buf = raw_buf;
+  uint8_t *raw_u_buf = raw_y_buf + (width * height);
+  uint8_t *raw_v_buf = raw_u_buf + ((width/2) * (height/2));
+
+  int cropped_width = height/2;
+  int cropped_height = height;
+
+  int resized_width = MODEL_WIDTH;
+  int resized_height = MODEL_HEIGHT;
+
+  uint8_t *cropped_buf = new uint8_t[cropped_width*cropped_height*3/2];
+  uint8_t *cropped_y_buf = cropped_buf;
+  uint8_t *cropped_u_buf = cropped_y_buf + (cropped_width * cropped_height);
+  uint8_t *cropped_v_buf = cropped_u_buf + ((cropped_width/2) * (cropped_height/2));
+
+  if (true) {
+    for (int r = 0; r < height/2; r++) {
+      memcpy(cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width + (width - cropped_width), cropped_width);
+      memcpy(cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width + (width - cropped_width), cropped_width);
+      memcpy(cropped_u_buf + r*cropped_width/2, raw_u_buf + r*width/2 + ((width/2) - (cropped_width/2)), cropped_width/2);
+      memcpy(cropped_v_buf + r*cropped_width/2, raw_v_buf + r*width/2 + ((width/2) - (cropped_width/2)), cropped_width/2);
+    }
+  } else {
+    // not tested
+    uint8_t *premirror_cropped_buf = new uint8_t[cropped_width*cropped_height*3/2];
+    uint8_t *premirror_cropped_y_buf = premirror_cropped_buf;
+    uint8_t *premirror_cropped_u_buf = premirror_cropped_y_buf + (cropped_width * cropped_height);
+    uint8_t *premirror_cropped_v_buf = premirror_cropped_u_buf + ((cropped_width/2) * (cropped_height/2));
+    for (int r = 0; r < height/2; r++) {
+      memcpy(premirror_cropped_y_buf + 2*r*cropped_width, raw_y_buf + 2*r*width, cropped_width);
+      memcpy(premirror_cropped_y_buf + (2*r+1)*cropped_width, raw_y_buf + (2*r+1)*width, cropped_width);
+      memcpy(premirror_cropped_u_buf + r*cropped_width/2, raw_u_buf + r*width/2, cropped_width/2);
+      memcpy(premirror_cropped_v_buf + r*cropped_width/2, raw_v_buf + r*width/2, cropped_width/2);
+    }
+    libyuv::I420Mirror(premirror_cropped_y_buf, cropped_width,
+                       premirror_cropped_u_buf, cropped_width/2,
+                       premirror_cropped_v_buf, cropped_width/2,
+                       cropped_y_buf, cropped_width,
+                       cropped_u_buf, cropped_width/2,
+                       cropped_v_buf, cropped_width/2,
+                       cropped_width, cropped_height);
+  }
+
+  uint8_t *resized_buf = new uint8_t[resized_width*resized_height*3/2];
+  uint8_t *resized_y_buf = resized_buf;
+  uint8_t *resized_u_buf = resized_y_buf + (resized_width * resized_height);
+  uint8_t *resized_v_buf = resized_u_buf + ((resized_width/2) * (resized_height/2));
+
+  libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear;
+  libyuv::I420Scale(cropped_y_buf, cropped_width,
+                    cropped_u_buf, cropped_width/2,
+                    cropped_v_buf, cropped_width/2,
+                    cropped_width, cropped_height,
+                    resized_y_buf, resized_width,
+                    resized_u_buf, resized_width/2,
+                    resized_v_buf, resized_width/2,
+                    resized_width, resized_height,
+                    mode);
+
+  int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
+
+  float *net_input_buf = new float[yuv_buf_len];
+  // one shot conversion, O(n) anyway
+  // yuvframe2tensor, normalize
+  for (int r = 0; r < MODEL_HEIGHT/2; r++) {
+    for (int c = 0; c < MODEL_WIDTH/2; c++) {
+      // Y_ul
+      net_input_buf[(c*MODEL_HEIGHT/2) + r] = (resized_buf[(2*r*resized_width) + (2*c)] - 128.f) * 0.0078125f;
+      // Y_ur
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + ((MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width) + (2*c+1)] - 128.f) * 0.0078125f;
+      // Y_dl
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width+1) + (2*c)] - 128.f) * 0.0078125f;
+      // Y_dr
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(2*r*resized_width+1) + (2*c+1)] - 128.f) * 0.0078125f;
+      // U
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(resized_width*resized_height) + (r*resized_width/2) + c] - 128.f) * 0.0078125f;
+      // V
+      net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = (resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + (r*resized_width/2) + c] - 128.f) * 0.0078125f;
+    }
+  }
+
+  // FILE *dump_yuv_file = fopen("/sdcard/rawdump.yuv", "wb");
+  // fwrite(raw_buf, height*width*3/2, sizeof(uint8_t), dump_yuv_file);
+  // fclose(dump_yuv_file);
+
+  // FILE *dump_yuv_file2 = fopen("/sdcard/inputdump.yuv", "wb");
+  // fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2);
+  // fclose(dump_yuv_file2);
+
+  delete[] cropped_buf;
+  delete[] resized_buf;
+  s->m->execute(net_input_buf);
+  delete[] net_input_buf;
+
+  MonitoringResult ret = {0};
+  memcpy(&ret.face_orientation, &s->output[0], sizeof ret.face_orientation);
+  memcpy(&ret.face_orientation_meta, &s->output[6], sizeof ret.face_orientation_meta);
+  memcpy(&ret.face_position, &s->output[3], sizeof ret.face_position);
+  memcpy(&ret.face_position_meta, &s->output[9], sizeof ret.face_position_meta);
+  memcpy(&ret.face_prob, &s->output[12], sizeof ret.face_prob);
+  memcpy(&ret.left_eye_prob, &s->output[21], sizeof ret.left_eye_prob);
+  memcpy(&ret.right_eye_prob, &s->output[30], sizeof ret.right_eye_prob);
+  memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob);
+  memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob);
+  ret.face_orientation_meta[0] = softplus(ret.face_orientation_meta[0]);
+  ret.face_orientation_meta[1] = softplus(ret.face_orientation_meta[1]);
+  ret.face_orientation_meta[2] = softplus(ret.face_orientation_meta[2]);
+  ret.face_position_meta[0] = softplus(ret.face_position_meta[0]);
+  ret.face_position_meta[1] = softplus(ret.face_position_meta[1]);
+  return ret;
+}
+
+void monitoring_publish(PubSocket* sock, uint32_t frame_id, const MonitoringResult res) {
+  // make msg
+  capnp::MallocMessageBuilder msg;
+  cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+  event.setLogMonoTime(nanos_since_boot());
+
+  auto framed = event.initDriverMonitoring();
+  framed.setFrameId(frame_id);
+
+  kj::ArrayPtr<const float> face_orientation(&res.face_orientation[0], ARRAYSIZE(res.face_orientation));
+  kj::ArrayPtr<const float> face_orientation_std(&res.face_orientation_meta[0], ARRAYSIZE(res.face_orientation_meta));
+  kj::ArrayPtr<const float> face_position(&res.face_position[0], ARRAYSIZE(res.face_position));
+  kj::ArrayPtr<const float> face_position_std(&res.face_position_meta[0], ARRAYSIZE(res.face_position_meta));
+  framed.setFaceOrientation(face_orientation);
+  framed.setFaceOrientationStd(face_orientation_std);
+  framed.setFacePosition(face_position);
+  framed.setFacePositionStd(face_position_std);
+  framed.setFaceProb(res.face_prob);
+  framed.setLeftEyeProb(res.left_eye_prob);
+  framed.setRightEyeProb(res.right_eye_prob);
+  framed.setLeftBlinkProb(res.left_blink_prob);
+  framed.setRightBlinkProb(res.right_blink_prob);
+
+  // send message
+  auto words = capnp::messageToFlatArray(msg);
+  auto bytes = words.asBytes();
+  sock->send((char*)bytes.begin(), bytes.size());
+}
+
+void monitoring_free(MonitoringState* s) {
+  delete s->m;
+}

selfdrive/modeld/models/monitoring.h

@@ -0,0 +1,44 @@
+#ifndef MONITORING_H
+#define MONITORING_H
+
+#include "common/util.h"
+#include "commonmodel.h"
+#include "runners/run.h"
+
+#include "cereal/gen/cpp/log.capnp.h"
+#include <capnp/serialize.h>
+#include "messaging.hpp"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#define OUTPUT_SIZE 33
+
+typedef struct MonitoringResult {
+  float face_orientation[3];
+  float face_orientation_meta[3];
+  float face_position[2];
+  float face_position_meta[2];
+  float face_prob;
+  float left_eye_prob;
+  float right_eye_prob;
+  float left_blink_prob;
+  float right_blink_prob;
+} MonitoringResult;
+
+typedef struct MonitoringState {
+  RunModel *m;
+  float output[OUTPUT_SIZE];
+} MonitoringState;
+
+void monitoring_init(MonitoringState* s);
+MonitoringResult monitoring_eval_frame(MonitoringState* s, void* stream_buf, int width, int height);
+void monitoring_publish(PubSocket *sock, uint32_t frame_id, const MonitoringResult res);
+void monitoring_free(MonitoringState* s);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

selfdrive/modeld/monitoringd

@@ -0,0 +1,5 @@
+#!/bin/sh
+export LD_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8:/home/batman/one/phonelibs/snpe/x86_64-linux-clang:$LD_LIBRARY_PATH"
+export ADSP_LIBRARY_PATH="/data/pythonpath/phonelibs/snpe/aarch64-android-clang3.8/"
+exec ./_monitoringd
+

selfdrive/modeld/monitoringd.cc

@@ -0,0 +1,80 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <signal.h>
+#include <cassert>
+
+#include "common/visionbuf.h"
+#include "common/visionipc.h"
+#include "common/swaglog.h"
+
+#include "models/monitoring.h"
+
+#ifndef PATH_MAX
+#include <linux/limits.h>
+#endif
+
+
+volatile sig_atomic_t do_exit = 0;
+
+static void set_do_exit(int sig) {
+  do_exit = 1;
+}
+
+int main(int argc, char **argv) {
+  int err;
+  set_realtime_priority(1);
+
+  // messaging
+  Context *msg_context = Context::create();
+  PubSocket *monitoring_sock = PubSocket::create(msg_context, "driverMonitoring");
+
+  // init the models
+  MonitoringState monitoring;
+  monitoring_init(&monitoring);
+
+  // loop
+  VisionStream stream;
+  while (!do_exit) {
+    VisionStreamBufs buf_info;
+    err = visionstream_init(&stream, VISION_STREAM_YUV_FRONT, true, &buf_info);
+    if (err) {
+      printf("visionstream connect fail\n");
+      usleep(100000);
+      continue;
+    }
+    LOGW("connected with buffer size: %d", buf_info.buf_len);
+
+    double last = 0;
+    while (!do_exit) {
+      VIPCBuf *buf;
+      VIPCBufExtra extra;
+      buf = visionstream_get(&stream, &extra);
+      if (buf == NULL) {
+        printf("visionstream get failed\n");
+        break;
+      }
+      //printf("frame_id: %d %dx%d\n", extra.frame_id, buf_info.width, buf_info.height);
+
+      double t1 = millis_since_boot();
+
+      MonitoringResult res = monitoring_eval_frame(&monitoring, buf->addr, buf_info.width, buf_info.height);
+
+      double t2 = millis_since_boot();
+
+      // send dm packet
+      monitoring_publish(monitoring_sock, extra.frame_id, res);
+
+      LOGD("monitoring process: %.2fms, from last %.2fms", t2-t1, t1-last);
+      last = t1;
+    }
+
+  }
+
+  visionstream_destroy(&stream);
+
+  delete monitoring_sock;
+  monitoring_free(&monitoring);
+
+  return 0;
+}

selfdrive/modeld/runners/run.h

@@ -0,0 +1,18 @@
+#ifndef RUN_H
+#define RUN_H
+
+#include "runmodel.h"
+#include "snpemodel.h"
+
+#ifdef QCOM
+  #define DefaultRunModel SNPEModel
+#else
+  #ifdef USE_TF_MODEL
+    #include "tfmodel.h"
+    #define DefaultRunModel TFModel
+  #else
+    #define DefaultRunModel SNPEModel
+  #endif
+#endif
+
+#endif

selfdrive/modeld/runners/runmodel.h

@@ -0,0 +1,12 @@
+#ifndef RUNMODEL_H
+#define RUNMODEL_H
+
+class RunModel {
+public:
+  virtual void addRecurrent(float *state, int state_size) {}
+  virtual void addDesire(float *state, int state_size) {}
+  virtual void execute(float *net_input_buf) {}
+};
+
+#endif
+

selfdrive/modeld/runners/snpemodel.cc

@@ -0,0 +1,126 @@
+#include <cassert>
+#include <stdlib.h>
+#include "common/util.h"
+#include "snpemodel.h"
+
+void PrintErrorStringAndExit() {
+  const char* const errStr = zdl::DlSystem::getLastErrorString();
+  std::cerr << zdl::DlSystem::getLastErrorString() << std::endl;
+  std::exit(EXIT_FAILURE);
+}
+
+SNPEModel::SNPEModel(const char *path, float *output, size_t output_size, int runtime) {
+#ifdef QCOM
+  zdl::DlSystem::Runtime_t Runtime;
+  if (runtime==USE_GPU_RUNTIME) {
+    Runtime = zdl::DlSystem::Runtime_t::GPU;
+  } else if (runtime==USE_DSP_RUNTIME) {
+    Runtime = zdl::DlSystem::Runtime_t::DSP;
+  } else {
+    Runtime = zdl::DlSystem::Runtime_t::CPU;
+  }
+  assert(zdl::SNPE::SNPEFactory::isRuntimeAvailable(Runtime));
+#endif
+  size_t model_size;
+  model_data = (uint8_t *)read_file(path, &model_size);
+  assert(model_data);
+
+  // load model
+  std::unique_ptr<zdl::DlContainer::IDlContainer> container = zdl::DlContainer::IDlContainer::open(model_data, model_size);
+  if (!container) { PrintErrorStringAndExit(); }
+  printf("loaded model with size: %u\n", model_size);
+
+  // create model runner
+  zdl::SNPE::SNPEBuilder snpeBuilder(container.get());
+  while (!snpe) {
+#ifdef QCOM
+    snpe = snpeBuilder.setOutputLayers({})
+                      .setRuntimeProcessor(Runtime)
+                      .setUseUserSuppliedBuffers(true)
+                      .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE)
+                      .build();
+#else
+    snpe = snpeBuilder.setOutputLayers({})
+                      .setUseUserSuppliedBuffers(true)
+                      .setPerformanceProfile(zdl::DlSystem::PerformanceProfile_t::HIGH_PERFORMANCE)
+                      .build();
+#endif
+    if (!snpe) std::cerr << zdl::DlSystem::getLastErrorString() << std::endl;
+  }
+
+  // get input and output names
+  const auto &strListi_opt = snpe->getInputTensorNames();
+  if (!strListi_opt) throw std::runtime_error("Error obtaining Input tensor names");
+  const auto &strListi = *strListi_opt;
+  //assert(strListi.size() == 1);
+  const char *input_tensor_name = strListi.at(0);
+
+  const auto &strListo_opt = snpe->getOutputTensorNames();
+  if (!strListo_opt) throw std::runtime_error("Error obtaining Output tensor names");
+  const auto &strListo = *strListo_opt;
+  assert(strListo.size() == 1);
+  const char *output_tensor_name = strListo.at(0);
+
+  printf("model: %s -> %s\n", input_tensor_name, output_tensor_name);
+
+  zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat;
+  zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory();
+
+  // create input buffer
+  {
+    const auto &inputDims_opt = snpe->getInputDimensions(input_tensor_name);
+    const zdl::DlSystem::TensorShape& bufferShape = *inputDims_opt;
+    std::vector<size_t> strides(bufferShape.rank());
+    strides[strides.size() - 1] = sizeof(float);
+    size_t product = 1;
+    for (size_t i = 0; i < bufferShape.rank(); i++) product *= bufferShape[i];
+    size_t stride = strides[strides.size() - 1];
+    for (size_t i = bufferShape.rank() - 1; i > 0; i--) {
+      stride *= bufferShape[i];
+      strides[i-1] = stride;
+    }
+    printf("input product is %u\n", product);
+    inputBuffer = ubFactory.createUserBuffer(NULL, product*sizeof(float), strides, &userBufferEncodingFloat);
+
+    inputMap.add(input_tensor_name, inputBuffer.get());
+  }
+
+  // create output buffer
+  {
+    std::vector<size_t> outputStrides = {output_size * sizeof(float), sizeof(float)};
+    outputBuffer = ubFactory.createUserBuffer(output, output_size * sizeof(float), outputStrides, &userBufferEncodingFloat);
+    outputMap.add(output_tensor_name, outputBuffer.get());
+  }
+}
+
+void SNPEModel::addRecurrent(float *state, int state_size) {
+  recurrentBuffer = this->addExtra(state, state_size, 2);
+}
+
+void SNPEModel::addDesire(float *state, int state_size) {
+  desireBuffer = this->addExtra(state, state_size, 1);
+}
+
+std::unique_ptr<zdl::DlSystem::IUserBuffer> SNPEModel::addExtra(float *state, int state_size, int idx) {
+  // get input and output names
+  const auto &strListi_opt = snpe->getInputTensorNames();
+  if (!strListi_opt) throw std::runtime_error("Error obtaining Input tensor names");
+  const auto &strListi = *strListi_opt;
+  const char *input_tensor_name = strListi.at(idx);
+  printf("adding index %d: %s\n", idx, input_tensor_name);
+
+  zdl::DlSystem::UserBufferEncodingFloat userBufferEncodingFloat;
+  zdl::DlSystem::IUserBufferFactory& ubFactory = zdl::SNPE::SNPEFactory::getUserBufferFactory();
+  std::vector<size_t> retStrides = {state_size * sizeof(float), sizeof(float)};
+  auto ret = ubFactory.createUserBuffer(state, state_size * sizeof(float), retStrides, &userBufferEncodingFloat);
+  inputMap.add(input_tensor_name, ret.get());
+  return ret;
+}
+
+void SNPEModel::execute(float *net_input_buf) {
+  assert(inputBuffer->setBufferAddress(net_input_buf));
+  if (!snpe->execute(inputMap, outputMap)) {
+    PrintErrorStringAndExit();
+  }
+}
+

selfdrive/modeld/runners/snpemodel.h

@@ -0,0 +1,51 @@
+#ifndef SNPEMODEL_H
+#define SNPEMODEL_H
+
+#include <SNPE/SNPE.hpp>
+#include <SNPE/SNPEBuilder.hpp>
+#include <SNPE/SNPEFactory.hpp>
+#include <DlContainer/IDlContainer.hpp>
+#include <DlSystem/DlError.hpp>
+#include <DlSystem/ITensor.hpp>
+#include <DlSystem/ITensorFactory.hpp>
+#include <DlSystem/IUserBuffer.hpp>
+#include <DlSystem/IUserBufferFactory.hpp>
+
+#include "runmodel.h"
+
+#define USE_CPU_RUNTIME 0
+#define USE_GPU_RUNTIME 1
+#define USE_DSP_RUNTIME 2
+
+class SNPEModel : public RunModel {
+public:
+  SNPEModel(const char *path, float *output, size_t output_size, int runtime);
+  ~SNPEModel() {
+    if (model_data) free(model_data);
+  }
+  void addRecurrent(float *state, int state_size);
+  void addDesire(float *state, int state_size);
+  void execute(float *net_input_buf);
+private:
+  uint8_t *model_data = NULL;
+
+  // snpe model stuff
+  std::unique_ptr<zdl::SNPE::SNPE> snpe;
+
+  // snpe input stuff
+  zdl::DlSystem::UserBufferMap inputMap;
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> inputBuffer;
+
+  // snpe output stuff
+  zdl::DlSystem::UserBufferMap outputMap;
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> outputBuffer;
+  float *output;
+
+  // recurrent and desire
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> addExtra(float *state, int state_size, int idx);
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> recurrentBuffer;
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> desireBuffer;
+};
+
+#endif
+

selfdrive/modeld/runners/tfmodel.cc

@@ -0,0 +1,160 @@
+#include "tfmodel.h"
+#include <string>
+#include <string.h>
+#include <stdlib.h>
+#include <stdexcept>
+#include "common/util.h"
+#include "common/swaglog.h"
+#include <cassert>
+
+void TFModel::status_check() const {
+  if (TF_GetCode(this->status) != TF_OK) {
+    throw std::runtime_error(TF_Message(status));
+  }
+}
+
+TF_Tensor *TFModel::allocate_tensor_for_output(TF_Output out, float *dat) {
+  int num_dims = TF_GraphGetTensorNumDims(graph, out, status);
+  status_check();
+  int64_t *dims = new int64_t[num_dims];
+  TF_GraphGetTensorShape(graph, out, dims, num_dims, status);
+  status_check();
+  dims[0] = 1;
+
+  int total = 1;
+  for (int i = 0; i < num_dims; i++) total *= dims[i];
+  //printf("dims %d total %d wdat %p\n", num_dims, total, dat);
+
+  // don't deallocate the buffers
+  auto d = [](void* ddata, size_t, void* arg) {};
+  TF_Tensor *ret = TF_NewTensor(TF_FLOAT, dims, num_dims, (void*)dat, sizeof(float)*total, d, NULL);
+
+  //TF_Tensor *ret = TF_AllocateTensor(TF_FLOAT, dims, num_dims, sizeof(float)*total);
+  //memcpy(TF_TensorData(ret), dat, sizeof(float)*total);
+
+  assert(ret);
+  delete[] dims;
+
+  return ret;
+}
+
+TFModel::TFModel(const char *path, float *_output, size_t _output_size, int runtime) {
+  // load model
+  {
+    TF_Buffer* buf;
+    size_t model_size;
+    char tmp[1024];
+    snprintf(tmp, sizeof(tmp), "%s.pb", path);
+    LOGD("loading model %s", tmp);
+    uint8_t *model_data = (uint8_t *)read_file(tmp, &model_size);
+    assert(model_data);
+    buf = TF_NewBuffer();
+    buf->data = model_data;
+    buf->length = model_size;
+    buf->data_deallocator = [](void *data, size_t) { free(data); };
+    LOGD("loaded model of size %d", model_size);
+
+    // import graph
+    status = TF_NewStatus();
+    graph = TF_NewGraph();
+    TF_ImportGraphDefOptions *opts = TF_NewImportGraphDefOptions();
+    // TODO: fix the GPU, currently it hangs if you set this to /gpu:0
+    //TF_ImportGraphDefOptionsSetDefaultDevice(opts, "/cpu:0");
+    TF_GraphImportGraphDef(graph, buf, opts, status);
+    TF_DeleteImportGraphDefOptions(opts);
+    TF_DeleteBuffer(buf);
+    status_check();
+    LOGD("imported graph");
+  }
+
+  // set up session
+  TF_SessionOptions* sess_opts = TF_NewSessionOptions();
+
+  // don't use all GPU memory
+  /*uint8_t config[15] = {0x32, 0xb, 0x9, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x20, 0x1, 0x38, 0x1};
+  double gpu_memory_fraction = 0.2;
+  auto bytes = reinterpret_cast<std::uint8_t*>(&gpu_memory_fraction);
+  for (std::size_t i = 0; i < sizeof(gpu_memory_fraction); ++i) {
+    config[i + 3] = bytes[i];
+  }
+  TF_SetConfig(sess_opts, config, sizeof(config), status);
+  status_check();*/
+
+  // make session
+  session = TF_NewSession(graph, sess_opts, status);
+  TF_DeleteSessionOptions(sess_opts);
+  status_check();
+
+  // find tensors
+  // TODO: make this generic
+  input_operation = {TF_GraphOperationByName(graph, "lambda/div"), 0};
+  if (input_operation.oper == NULL) {
+    input_operation = {TF_GraphOperationByName(graph, "vision_lambda/div"), 0};
+  }
+  assert(input_operation.oper != NULL);
+
+  output_operation = {TF_GraphOperationByName(graph, "outputs/outputs/Identity"), 0};
+  if (output_operation.oper == NULL) {
+    output_operation = {TF_GraphOperationByName(graph, "outputs/concat"), 0};
+  }
+  assert(output_operation.oper != NULL);
+
+  // output tensor is good to bind now
+  output = _output;
+  output_size = _output_size;
+}
+
+TFModel::~TFModel() {
+  TF_DeleteSession(session, status);
+  status_check();
+  TF_DeleteGraph(graph);
+  TF_DeleteStatus(status);
+}
+
+void TFModel::addRecurrent(float *state, int state_size) {
+  rnn_operation.oper = TF_GraphOperationByName(graph, "rnn_state");
+  rnn_operation.index = 0;
+  assert(rnn_operation.oper != NULL);
+
+  rnn_input_buf = state;
+}
+
+void TFModel::addDesire(float *state, int state_size) {
+  desire_operation.oper = TF_GraphOperationByName(graph, "desire");
+  desire_operation.index = 0;
+  assert(desire_operation.oper != NULL);
+
+  desire_input_buf = state;
+}
+
+void TFModel::execute(float *net_input_buf) {
+  TF_Tensor *input_tensor = allocate_tensor_for_output(input_operation, net_input_buf);
+  assert(input_tensor);
+  TF_Tensor *output_tensor = NULL;
+
+  if (rnn_input_buf == NULL) {
+    TF_SessionRun(session, NULL,
+      &input_operation, &input_tensor, 1,
+      &output_operation, &output_tensor, 1,
+      NULL, 0, NULL, status);
+  } else {
+    //printf("%f %f %f\n", net_input_buf[0], rnn_input_buf[0], desire_input_buf[0]);
+    TF_Tensor *rnn_tensor = allocate_tensor_for_output(rnn_operation, rnn_input_buf);
+    TF_Tensor *desire_tensor = allocate_tensor_for_output(desire_operation, desire_input_buf);
+    TF_Output io[] = {input_operation, rnn_operation, desire_operation};
+    TF_Tensor* id[] = {input_tensor, rnn_tensor, desire_tensor};
+    TF_SessionRun(session, NULL,
+      io, id, 3,
+      &output_operation, &output_tensor, 1,
+      NULL, 0, NULL, status);
+    TF_DeleteTensor(rnn_tensor);
+    TF_DeleteTensor(desire_tensor);
+  }
+  TF_DeleteTensor(input_tensor);
+  status_check();
+  assert(output_tensor);
+  memcpy((void*)output, TF_TensorData(output_tensor), output_size*sizeof(float));
+  TF_DeleteTensor(output_tensor);
+}
+
+

selfdrive/modeld/runners/tfmodel.h

@@ -0,0 +1,39 @@
+#ifndef TFMODEL_H
+#define TFMODEL_H
+
+#include <stdlib.h>
+#include "runmodel.h"
+
+#include "tensorflow/c/c_api.h"
+
+struct TFState;
+
+class TFModel : public RunModel {
+public:
+  TFModel(const char *path, float *output, size_t output_size, int runtime);
+	~TFModel();
+  void addRecurrent(float *state, int state_size);
+  void addDesire(float *state, int state_size);
+  void execute(float *net_input_buf);
+private:
+  void status_check() const;
+  TF_Tensor *allocate_tensor_for_output(TF_Output out, float *dat);
+
+  float *output;
+  size_t output_size;
+
+  TF_Session* session;
+  TF_Graph* graph;
+  TF_Status* status;
+
+  TF_Output input_operation;
+  TF_Output rnn_operation;
+  TF_Output desire_operation;
+  TF_Output output_operation;
+
+  float *rnn_input_buf = NULL;
+  float *desire_input_buf = NULL;
+};
+
+#endif
+

selfdrive/modeld/test/polyfit/.gitignore

@@ -0,0 +1 @@
+main

selfdrive/modeld/test/polyfit/Makefile

@@ -0,0 +1,15 @@
+PHONELIBS = ../../../../phonelibs
+
+EIGEN_FLAGS = -I$(PHONELIBS)/eigen
+
+CXXFLAGS += $(EIGEN_FLAGS)
+LDFLAGS += -lm
+
+.PHONY: clean
+
+main: main.cc data.h
+	g++ -O2 $(EIGEN_FLAGS) -o main main.cc -lm
+
+
+clean:
+	rm -f main

selfdrive/modeld/test/polyfit/data.h

@@ -0,0 +1,8 @@
+#pragma once
+
+#define MODEL_PATH_DISTANCE 192
+#define POLYFIT_DEGREE 4
+
+float pts[MODEL_PATH_DISTANCE] = {3.1261718, 3.1642578, 3.0548828, 3.1125, 3.190625, 3.01875, 2.9816406, 3.1222656, 2.9728515, 2.9826171, 3.034375, 3.0392578, 3.1642578, 3.0792968, 3.0011718, 3.0705078, 2.9904296, 3.0089843, 3.0597656, 3.0978515, 2.9210937, 2.9992187, 2.9474609, 2.9621093, 2.9289062, 2.89375, 2.7975585, 2.9015625, 2.8175781, 2.9132812, 2.8175781, 2.7501953, 2.8332031, 2.8166015, 2.7638671, 2.8878906, 2.7599609, 2.6999023, 2.6720703, 2.6398437, 2.7243164, 2.6120117, 2.6588867, 2.5558593, 2.5978515, 2.5485351, 2.4269531, 2.5001953, 2.4855468, 2.4367187, 2.2973144, 2.2812011, 2.2890136, 2.39375, 2.2836425, 2.3815429, 2.2138183, 2.1964843, 2.1840332, 2.1759765, 2.0421875, 2.1034667, 2.0281494, 2.0880859, 1.9706542, 1.9276855, 1.8522155, 1.8991821, 1.7780273, 1.8180053, 1.8326843, 1.8270385, 1.7182128, 1.6439941, 1.5360839, 1.68385, 1.4584472, 1.5955322, 1.6002929, 1.4157226, 1.4704101, 1.2936523, 1.2990234, 1.4281738, 1.4357421, 1.409375, 1.2511718, 1.2194335, 1.1554687, 1.043164, 1.0954101, 1.0392578, 1.0895507, 1.0880859, 0.897168, 0.83369142, 0.86494142, 0.87763673, 0.85322267, 0.72968751, 0.57832032, 0.73066407, 0.78828126, 0.69160157, 0.64375, 0.5919922, 0.5529297, 0.52070314, 0.60957032, 0.51093751, 0.3576172, 0.49921876, 0.284375, 0.21992187, 0.25214845, 0.30683595, 0.30976564, 0.2716797, 0.22089843, 0.25507814, 0.084179685, 0.071484372, 0.1828125, 0.15644531, 0.13789062, 0.054882813, 0.021679688, -0.091601565, -0.0203125, -0.13359375, -0.037890624, -0.29765624, -0.15605469, -0.30351561, -0.055468749, -0.22148438, -0.246875, -0.31718749, -0.25468749, -0.35234374, -0.16484375, -0.56523436, -0.56523436, -0.39921874, -0.58671874, -0.45585936, -0.50859374, -0.44023436, -0.42656249, -0.56328124, -0.70195311, -0.403125, -0.76445311, -0.98710936, -0.7625, -0.75273436, -0.825, -0.996875, -0.86210936, -0.99492186, -0.85625, -0.88359374, -0.97148436, -1.0320313, -1.1609375, -1.1296875, -1.0203125, -1.0691407, -1.2371094, -1.1277344, -1.2214844, -1.1921875, -1.2996094, -1.2917969, -1.3699219, -1.434375, -1.3699219, -1.3601563, -1.5730469, -1.3152344, -1.4851563, -1.48125, -1.5925782, -1.746875, -1.5847657, -1.6003907, -1.5984375, -1.7703125, -1.8328125, -1.8152344, -1.9714844, -1.9421875};
+
+float stds[MODEL_PATH_DISTANCE] = {1.0945262, 1.156862, 1.0777057, 1.1501777, 1.234844, 1.0140595, 1.2004665, 1.1926303, 1.1269455, 1.0362904, 0.98873031, 0.88530254, 1.0078473, 0.93637651, 0.90959895, 0.86409503, 0.86353016, 0.74534553, 0.78025728, 0.88014913, 0.75756663, 0.77129823, 0.75581717, 0.79222, 0.84098673, 0.79402477, 0.85648865, 0.80315614, 0.77346581, 0.73097658, 0.72557795, 0.72930044, 0.666103, 0.77142948, 0.704379, 0.6806078, 0.67680347, 0.71318036, 0.72244918, 0.66123307, 0.62547487, 0.67786956, 0.68404138, 0.70508122, 0.62400025, 0.72325015, 0.73942852, 0.67811751, 0.70370805, 0.65040058, 0.6870054, 0.66093785, 0.666103, 0.70040214, 0.65300173, 0.69714534, 0.65825552, 0.64833081, 0.6464982, 0.75850725, 0.69627059, 0.71659386, 0.69307244, 0.61554217, 0.62015557, 0.61998636, 0.67650336, 0.68142927, 0.6278621, 0.612294, 0.62592906, 0.63736153, 0.74233508, 0.69297016, 0.69621509, 0.67229682, 0.64879686, 0.72361159, 0.70229048, 0.60928106, 0.62712252, 0.66923952, 0.65802008, 0.68361813, 0.61587888, 0.63348651, 0.60727841, 0.64873856, 0.68847752, 0.58432156, 0.61683363, 0.63311422, 0.64981711, 0.57369792, 0.62604266, 0.62162364, 0.62066346, 0.62808979, 0.58524042, 0.63537884, 0.65367514, 0.63900274, 0.61089778, 0.62513435, 0.6470505, 0.63952166, 0.5937764, 0.64310449, 0.64330715, 0.64322031, 0.64632386, 0.60827911, 0.58887208, 0.61959165, 0.70725286, 0.64287293, 0.62326396, 0.65896219, 0.55610275, 0.6658656, 0.65681434, 0.583188, 0.6311124, 0.559652, 0.71419227, 0.62490743, 0.66699386, 0.62032485, 0.663036, 0.61414057, 0.66179425, 0.59399503, 0.65203643, 0.67839557, 0.63698763, 0.617452, 0.61022842, 0.7398752, 0.65657932, 0.68718743, 0.67901206, 0.66126263, 0.69949967, 0.70709819, 0.713336, 0.68130863, 0.68652785, 0.67028236, 0.7626031, 0.65259206, 0.72977453, 0.66049516, 0.64261246, 0.66906089, 0.69762796, 0.73719794, 0.69081914, 0.69849437, 0.72435051, 0.62354708, 0.68812829, 0.7193296, 0.66211933, 0.69278532, 0.7518425, 0.69661695, 0.672491, 0.71539241, 0.7369433, 0.66120356, 0.79088491, 0.77491313, 0.79442614, 0.7878198, 0.78881842, 0.70690477, 0.80707121, 0.78768665, 0.7215547, 0.75226194, 0.72196257, 0.765799, 0.77267712, 0.75844234, 0.81038833, 0.81188059, 0.79864907, 0.816436, 0.845298, 0.85074174, 0.73668873, 0.83516812};

selfdrive/modeld/test/polyfit/main.cc

@@ -0,0 +1,52 @@
+#include <iostream>
+#include <cmath>
+
+#include <Eigen/Dense>
+
+#include "data.h"
+
+Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> vander;
+
+void poly_init(){
+  // Build Vandermonde matrix
+  for(int i = 0; i < MODEL_PATH_DISTANCE; i++) {
+    for(int j = 0; j < POLYFIT_DEGREE; j++) {
+      vander(i, j) = pow(i, POLYFIT_DEGREE-j-1);
+    }
+  }
+}
+
+void poly_fit(float *in_pts, float *in_stds, float *out) {
+  // References to inputs
+  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > pts(in_pts, MODEL_PATH_DISTANCE);
+  Eigen::Map<Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> > std(in_stds, MODEL_PATH_DISTANCE);
+  Eigen::Map<Eigen::Matrix<float, POLYFIT_DEGREE, 1> > p(out, POLYFIT_DEGREE);
+
+  // Build Least Squares equations
+  Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE> lhs = vander.array().colwise() / std.array();
+  Eigen::Matrix<float, MODEL_PATH_DISTANCE, 1> rhs = pts.array() / std.array();
+
+  Eigen::Matrix<float, POLYFIT_DEGREE, 1> scale = 1. / (lhs.array()*lhs.array()).sqrt().colwise().sum();
+  lhs = lhs * scale.asDiagonal();
+
+  // Solve inplace
+  Eigen::ColPivHouseholderQR<Eigen::Ref<Eigen::MatrixXf> > qr(lhs);
+  p = qr.solve(rhs);
+
+  p = p.transpose() * scale.asDiagonal();
+}
+
+int main(void) {
+  poly_init();
+
+
+  float poly[4];
+  poly_fit(pts, stds, poly);
+
+  std::cout << "[";
+  std::cout << poly[0] << ",";
+  std::cout << poly[1] << ",";
+  std::cout << poly[2] << ",";
+  std::cout << poly[3];
+  std::cout << "]" << std::endl;
+}

selfdrive/modeld/test/polyfit/test.py

@@ -0,0 +1,24 @@
+import numpy as np
+
+pts = np.array([3.1261718, 3.1642578, 3.0548828, 3.1125, 3.190625, 3.01875, 2.9816406, 3.1222656, 2.9728515, 2.9826171, 3.034375, 3.0392578, 3.1642578, 3.0792968, 3.0011718, 3.0705078, 2.9904296, 3.0089843, 3.0597656, 3.0978515, 2.9210937, 2.9992187, 2.9474609, 2.9621093, 2.9289062, 2.89375, 2.7975585, 2.9015625, 2.8175781, 2.9132812, 2.8175781, 2.7501953, 2.8332031, 2.8166015, 2.7638671, 2.8878906, 2.7599609, 2.6999023, 2.6720703, 2.6398437, 2.7243164, 2.6120117, 2.6588867, 2.5558593, 2.5978515, 2.5485351, 2.4269531, 2.5001953, 2.4855468, 2.4367187, 2.2973144, 2.2812011, 2.2890136, 2.39375, 2.2836425, 2.3815429, 2.2138183, 2.1964843, 2.1840332, 2.1759765, 2.0421875, 2.1034667, 2.0281494, 2.0880859, 1.9706542, 1.9276855, 1.8522155, 1.8991821, 1.7780273, 1.8180053, 1.8326843, 1.8270385, 1.7182128, 1.6439941, 1.5360839, 1.68385, 1.4584472, 1.5955322, 1.6002929, 1.4157226, 1.4704101, 1.2936523, 1.2990234, 1.4281738, 1.4357421, 1.409375, 1.2511718, 1.2194335, 1.1554687, 1.043164, 1.0954101, 1.0392578, 1.0895507, 1.0880859, 0.897168, 0.83369142, 0.86494142, 0.87763673, 0.85322267, 0.72968751, 0.57832032, 0.73066407, 0.78828126, 0.69160157, 0.64375, 0.5919922, 0.5529297, 0.52070314, 0.60957032, 0.51093751, 0.3576172, 0.49921876, 0.284375, 0.21992187, 0.25214845, 0.30683595, 0.30976564, 0.2716797, 0.22089843, 0.25507814, 0.084179685, 0.071484372, 0.1828125, 0.15644531, 0.13789062, 0.054882813, 0.021679688, -0.091601565, -0.0203125, -0.13359375, -0.037890624, -0.29765624, -0.15605469, -0.30351561, -0.055468749, -0.22148438, -0.246875, -0.31718749, -0.25468749, -0.35234374, -0.16484375, -0.56523436, -0.56523436, -0.39921874, -0.58671874, -0.45585936, -0.50859374, -0.44023436, -0.42656249, -0.56328124, -0.70195311, -0.403125, -0.76445311, -0.98710936, -0.7625, -0.75273436, -0.825, -0.996875, -0.86210936, -0.99492186, -0.85625, -0.88359374, -0.97148436, -1.0320313, -1.1609375, -1.1296875, -1.0203125, -1.0691407, -1.2371094, -1.1277344, -1.2214844, -1.1921875, -1.2996094, -1.2917969, -1.3699219, -1.434375, -1.3699219, -1.3601563, -1.5730469, -1.3152344, -1.4851563, -1.48125, -1.5925782, -1.746875, -1.5847657, -1.6003907, -1.5984375, -1.7703125, -1.8328125, -1.8152344, -1.9714844, -1.9421875])
+
+stds = np.array([1.0945262, 1.156862, 1.0777057, 1.1501777, 1.234844, 1.0140595, 1.2004665, 1.1926303, 1.1269455, 1.0362904, 0.98873031, 0.88530254, 1.0078473, 0.93637651, 0.90959895, 0.86409503, 0.86353016, 0.74534553, 0.78025728, 0.88014913, 0.75756663, 0.77129823, 0.75581717, 0.79222, 0.84098673, 0.79402477, 0.85648865, 0.80315614, 0.77346581, 0.73097658, 0.72557795, 0.72930044, 0.666103, 0.77142948, 0.704379, 0.6806078, 0.67680347, 0.71318036, 0.72244918, 0.66123307, 0.62547487, 0.67786956, 0.68404138, 0.70508122, 0.62400025, 0.72325015, 0.73942852, 0.67811751, 0.70370805, 0.65040058, 0.6870054, 0.66093785, 0.666103, 0.70040214, 0.65300173, 0.69714534, 0.65825552, 0.64833081, 0.6464982, 0.75850725, 0.69627059, 0.71659386, 0.69307244, 0.61554217, 0.62015557, 0.61998636, 0.67650336, 0.68142927, 0.6278621, 0.612294, 0.62592906, 0.63736153, 0.74233508, 0.69297016, 0.69621509, 0.67229682, 0.64879686, 0.72361159, 0.70229048, 0.60928106, 0.62712252, 0.66923952, 0.65802008, 0.68361813, 0.61587888, 0.63348651, 0.60727841, 0.64873856, 0.68847752, 0.58432156, 0.61683363, 0.63311422, 0.64981711, 0.57369792, 0.62604266, 0.62162364, 0.62066346, 0.62808979, 0.58524042, 0.63537884, 0.65367514, 0.63900274, 0.61089778, 0.62513435, 0.6470505, 0.63952166, 0.5937764, 0.64310449, 0.64330715, 0.64322031, 0.64632386, 0.60827911, 0.58887208, 0.61959165, 0.70725286, 0.64287293, 0.62326396, 0.65896219, 0.55610275, 0.6658656, 0.65681434, 0.583188, 0.6311124, 0.559652, 0.71419227, 0.62490743, 0.66699386, 0.62032485, 0.663036, 0.61414057, 0.66179425, 0.59399503, 0.65203643, 0.67839557, 0.63698763, 0.617452, 0.61022842, 0.7398752, 0.65657932, 0.68718743, 0.67901206, 0.66126263, 0.69949967, 0.70709819, 0.713336, 0.68130863, 0.68652785, 0.67028236, 0.7626031, 0.65259206, 0.72977453, 0.66049516, 0.64261246, 0.66906089, 0.69762796, 0.73719794, 0.69081914, 0.69849437, 0.72435051, 0.62354708, 0.68812829, 0.7193296, 0.66211933, 0.69278532, 0.7518425, 0.69661695, 0.672491, 0.71539241, 0.7369433, 0.66120356, 0.79088491, 0.77491313, 0.79442614, 0.7878198, 0.78881842, 0.70690477, 0.80707121, 0.78768665, 0.7215547, 0.75226194, 0.72196257, 0.765799, 0.77267712, 0.75844234, 0.81038833, 0.81188059, 0.79864907, 0.816436, 0.845298, 0.85074174, 0.73668873, 0.83516812])
+
+order = 3
+
+x = np.arange(0, len(pts))
+print(np.polyfit(x, pts, order, w=1/stds))
+
+# Do polyfit manually
+w = 1.0 / stds
+lhs = np.vander(x, order+1).astype(np.float64)
+rhs = pts
+
+lhs *= np.atleast_2d(w).T
+rhs *= w
+
+scale = np.sqrt((lhs*lhs).sum(axis=0))
+lhs = lhs / scale
+c, resids, rank, s = np.linalg.lstsq(lhs, rhs, rcond=None)
+c = (c.T/scale).T
+print(c)

selfdrive/modeld/test/snpe_benchmark/.gitignore

@@ -0,0 +1 @@
+benchmark

selfdrive/modeld/test/snpe_benchmark/benchmark.cc

@@ -0,0 +1,190 @@
+#include <SNPE/SNPE.hpp>
+#include <SNPE/SNPEBuilder.hpp>
+#include <SNPE/SNPEFactory.hpp>
+#include <DlContainer/IDlContainer.hpp>
+#include <DlSystem/DlError.hpp>
+#include <DlSystem/ITensor.hpp>
+#include <DlSystem/ITensorFactory.hpp>
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+using namespace std;
+
+int64_t timespecDiff(struct timespec *timeA_p, struct timespec *timeB_p) {
+  return ((timeA_p->tv_sec * 1000000000) + timeA_p->tv_nsec) - ((timeB_p->tv_sec * 1000000000) + timeB_p->tv_nsec);
+}
+
+void PrintErrorStringAndExit() {
+  cout << "ERROR!" << endl;
+  const char* const errStr = zdl::DlSystem::getLastErrorString();
+  std::cerr << errStr << std::endl;
+  std::exit(EXIT_FAILURE);
+}
+
+
+zdl::DlSystem::Runtime_t checkRuntime() {
+  static zdl::DlSystem::Version_t Version = zdl::SNPE::SNPEFactory::getLibraryVersion();
+  static zdl::DlSystem::Runtime_t Runtime;
+  std::cout << "SNPE Version: " << Version.asString().c_str() << std::endl; //Print Version number
+  if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::DSP)) {
+    std::cout << "Using DSP runtime" << std::endl;
+    Runtime = zdl::DlSystem::Runtime_t::DSP;
+  } else if (zdl::SNPE::SNPEFactory::isRuntimeAvailable(zdl::DlSystem::Runtime_t::GPU)) {
+    std::cout << "Using GPU runtime" << std::endl;
+    Runtime = zdl::DlSystem::Runtime_t::GPU;
+  } else {
+    std::cout << "Using cpu runtime" << std::endl;
+    Runtime = zdl::DlSystem::Runtime_t::CPU;
+  }
+  return Runtime;
+}
+
+void test(char *filename) {
+  static zdl::DlSystem::Runtime_t runtime = checkRuntime();
+  std::unique_ptr<zdl::DlContainer::IDlContainer> container;
+  container = zdl::DlContainer::IDlContainer::open(filename);
+
+  if (!container) { PrintErrorStringAndExit(); }
+  cout << "start build" << endl;
+  std::unique_ptr<zdl::SNPE::SNPE> snpe;
+  {
+    snpe = NULL;
+    zdl::SNPE::SNPEBuilder snpeBuilder(container.get());
+    snpe = snpeBuilder.setOutputLayers({})
+      .setRuntimeProcessor(runtime)
+      .setUseUserSuppliedBuffers(false)
+      //.setDebugMode(true)
+      .build();
+    if (!snpe) {
+      cout << "ERROR!" << endl;
+      const char* const errStr = zdl::DlSystem::getLastErrorString();
+      std::cerr << errStr << std::endl;
+    }
+    cout << "ran snpeBuilder" << endl;
+  }
+
+  const auto &strList_opt = snpe->getInputTensorNames();
+  if (!strList_opt) throw std::runtime_error("Error obtaining input tensor names");
+
+  cout << "get input tensor names done" << endl;
+  const auto &strList = *strList_opt;
+  static zdl::DlSystem::TensorMap inputTensorMap;
+  static zdl::DlSystem::TensorMap outputTensorMap;
+  vector<std::unique_ptr<zdl::DlSystem::ITensor> > inputs;
+  for (int i = 0; i < strList.size(); i++) {
+    cout << "input name: " << strList.at(i) << endl;
+
+    const auto &inputDims_opt = snpe->getInputDimensions(strList.at(i));
+    const auto &inputShape = *inputDims_opt;
+    inputs.push_back(zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape));
+    inputTensorMap.add(strList.at(i), inputs[i].get());
+  }
+
+  struct timespec start, end;
+  cout << "**** starting benchmark ****" << endl;
+  for (int i = 0; i < 10; i++) {
+    clock_gettime(CLOCK_MONOTONIC, &start);
+    assert(snpe->execute(inputTensorMap, outputTensorMap));
+    clock_gettime(CLOCK_MONOTONIC, &end);
+    uint64_t timeElapsed = timespecDiff(&end, &start);
+    printf("time: %f ms\n", timeElapsed*1.0/1e6);
+  }
+}
+
+void get_testframe(int index, std::unique_ptr<zdl::DlSystem::ITensor> &input) {
+  FILE * pFile;
+  string filepath="/data/ipt/quantize_samples/sample_input_"+std::to_string(index);
+  pFile = fopen(filepath.c_str(),"rb");
+  int length = 1*6*160*320*4;
+  float * frame_buffer = new float[length/4]; // 32/8
+  fread(frame_buffer, length, 1, pFile);
+  // std::cout << *(frame_buffer+length/4-1) << std::endl;
+  std::copy(frame_buffer, frame_buffer+(length/4), input->begin());
+}
+
+void SaveITensor(const std::string& path, const zdl::DlSystem::ITensor* tensor)
+{
+   std::ofstream os(path, std::ofstream::binary);
+   if (!os)
+   {
+      std::cerr << "Failed to open output file for writing: " << path << "\n";
+      std::exit(EXIT_FAILURE);
+   }
+   for ( auto it = tensor->cbegin(); it != tensor->cend(); ++it )
+   {
+      float f = *it;
+      if (!os.write(reinterpret_cast<char*>(&f), sizeof(float)))
+      {
+         std::cerr << "Failed to write data to: " << path << "\n";
+         std::exit(EXIT_FAILURE);
+      }
+   }
+}
+
+void testrun(char* modelfile) {
+  static zdl::DlSystem::Runtime_t runtime = checkRuntime();
+  std::unique_ptr<zdl::DlContainer::IDlContainer> container;
+  container = zdl::DlContainer::IDlContainer::open(modelfile);
+
+  if (!container) { PrintErrorStringAndExit(); }
+  cout << "start build" << endl;
+  std::unique_ptr<zdl::SNPE::SNPE> snpe;
+  {
+    snpe = NULL;
+    zdl::SNPE::SNPEBuilder snpeBuilder(container.get());
+    snpe = snpeBuilder.setOutputLayers({})
+      .setRuntimeProcessor(runtime)
+      .setUseUserSuppliedBuffers(false)
+      //.setDebugMode(true)
+      .build();
+    if (!snpe) {
+      cout << "ERROR!" << endl;
+      const char* const errStr = zdl::DlSystem::getLastErrorString();
+      std::cerr << errStr << std::endl;
+    }
+    cout << "ran snpeBuilder" << endl;
+  }
+
+  const auto &strList_opt = snpe->getInputTensorNames();
+  if (!strList_opt) throw std::runtime_error("Error obtaining input tensor names");
+  cout << "get input tensor names done" << endl;
+
+  const auto &strList = *strList_opt;
+  static zdl::DlSystem::TensorMap inputTensorMap;
+  static zdl::DlSystem::TensorMap outputTensorMap;
+
+  assert (strList.size() == 1);
+  const auto &inputDims_opt = snpe->getInputDimensions(strList.at(0));
+  const auto &inputShape = *inputDims_opt;
+  std::cout << "winkwink" << std::endl;
+
+  for (int i=0;i<10000;i++) {
+    std::unique_ptr<zdl::DlSystem::ITensor> input;
+    input = zdl::SNPE::SNPEFactory::getTensorFactory().createTensor(inputShape);
+    get_testframe(i,input);
+    snpe->execute(input.get(), outputTensorMap);
+    zdl::DlSystem::StringList tensorNames = outputTensorMap.getTensorNames();
+    std::for_each( tensorNames.begin(), tensorNames.end(), [&](const char* name) {
+      std::ostringstream path;
+      path << "/data/opt/Result_" << std::to_string(i) << ".raw";
+      auto tensorPtr = outputTensorMap.getTensor(name);
+      SaveITensor(path.str(), tensorPtr);
+    });
+  }
+}
+
+int main(int argc, char* argv[]) {
+  if (argc < 2) {
+    printf("usage: %s <filename>\n", argv[0]);
+    return -1;
+  }
+
+  if (argc == 2) {
+    while(1) test(argv[1]);
+  } else if (argc == 3) {
+    testrun(argv[1]);
+  }
+  return 0;
+}
+

selfdrive/modeld/test/snpe_benchmark/benchmark.sh

@@ -0,0 +1,3 @@
+#!/bin/sh -e
+clang++ -I ~/one/phonelibs/snpe/include/ -lSNPE -lsymphony-cpu -lsymphonypower benchmark.cc -o benchmark
+./benchmark $1

selfdrive/modeld/test/tf_test/build.sh

@@ -0,0 +1,2 @@
+#!/bin/bash
+clang++ -I /home/batman/one/external/tensorflow/include/ -L /home/batman/one/external/tensorflow/lib -Wl,-rpath=/home/batman/one/external/tensorflow/lib main.cc -ltensorflow

selfdrive/modeld/test/tf_test/main.cc

@@ -0,0 +1,69 @@
+#include <stdio.h>
+#include <stdlib.h>
+#include <assert.h>
+#include "tensorflow/c/c_api.h"
+
+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;
+}
+
+static void DeallocateBuffer(void* data, size_t) {
+  free(data);
+}
+
+int main(int argc, char* argv[]) {
+  TF_Buffer* buf;
+	TF_Graph* graph;
+	TF_Status* status;
+	char *path = argv[1];
+
+  // load model
+  {
+    size_t model_size;
+    char tmp[1024];
+    snprintf(tmp, sizeof(tmp), "%s.pb", path);
+    printf("loading model %s\n", tmp);
+    uint8_t *model_data = (uint8_t *)read_file(tmp, &model_size);
+    buf = TF_NewBuffer();
+    buf->data = model_data;
+    buf->length = model_size;
+    buf->data_deallocator = DeallocateBuffer;
+    printf("loaded model of size %d\n", model_size);
+  }
+
+  // import graph
+  status = TF_NewStatus();
+  graph = TF_NewGraph();
+  TF_ImportGraphDefOptions *opts = TF_NewImportGraphDefOptions();
+  TF_GraphImportGraphDef(graph, buf, opts, status);
+  TF_DeleteImportGraphDefOptions(opts);
+  TF_DeleteBuffer(buf);
+  if (TF_GetCode(status) != TF_OK) {
+    printf("FAIL: %s\n", TF_Message(status));
+  } else {
+    printf("SUCCESS\n");
+  }
+}

selfdrive/modeld/test/tf_test/pb_loader.py

@@ -0,0 +1,9 @@
+#!/usr/bin/env python3
+import sys
+import tensorflow as tf
+
+with open(sys.argv[1], "rb") as f:
+  graph_def = tf.compat.v1.GraphDef()
+  graph_def.ParseFromString(f.read())
+  #tf.io.write_graph(graph_def, '', sys.argv[1]+".try")
+

selfdrive/modeld/transforms/loadyuv.c

@@ -0,0 +1,82 @@
+#include <string.h>
+#include <assert.h>
+
+#include "clutil.h"
+
+#include "loadyuv.h"
+
+void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height) {
+  int err = 0;
+  memset(s, 0, sizeof(*s));
+
+  s->width = width;
+  s->height = height;
+
+  char args[1024];
+  snprintf(args, sizeof(args),
+           "-cl-fast-relaxed-math -cl-denorms-are-zero "
+           "-DTRANSFORMED_WIDTH=%d -DTRANSFORMED_HEIGHT=%d",
+           width, height);
+  cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "transforms/loadyuv.cl", args);
+
+  s->loadys_krnl = clCreateKernel(prg, "loadys", &err);
+  assert(err == 0);
+  s->loaduv_krnl = clCreateKernel(prg, "loaduv", &err);
+  assert(err == 0);
+
+  // done with this
+  err = clReleaseProgram(prg);
+  assert(err == 0);
+}
+
+void loadyuv_destroy(LoadYUVState* s) {
+  int err = 0;
+
+  err = clReleaseKernel(s->loadys_krnl);
+  assert(err == 0);
+  err = clReleaseKernel(s->loaduv_krnl);
+  assert(err == 0);
+}
+
+void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
+                   cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
+                   cl_mem out_cl) {
+  int err = 0;
+
+  err = clSetKernelArg(s->loadys_krnl, 0, sizeof(cl_mem), &y_cl);
+  assert(err == 0);
+  err = clSetKernelArg(s->loadys_krnl, 1, sizeof(cl_mem), &out_cl);
+  assert(err == 0);
+
+  const size_t loadys_work_size = (s->width*s->height)/8;
+  err = clEnqueueNDRangeKernel(q, s->loadys_krnl, 1, NULL,
+                               &loadys_work_size, NULL, 0, 0, NULL);
+  assert(err == 0);
+
+  const size_t loaduv_work_size = ((s->width/2)*(s->height/2))/8;
+  cl_int loaduv_out_off = (s->width*s->height);
+
+  err = clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &u_cl);
+  assert(err == 0);
+  err = clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl);
+  assert(err == 0);
+  err = clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off);
+  assert(err == 0);
+
+  err = clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
+                               &loaduv_work_size, NULL, 0, 0, NULL);
+  assert(err == 0);
+
+  loaduv_out_off += (s->width/2)*(s->height/2);
+
+  err = clSetKernelArg(s->loaduv_krnl, 0, sizeof(cl_mem), &v_cl);
+  assert(err == 0);
+  err = clSetKernelArg(s->loaduv_krnl, 1, sizeof(cl_mem), &out_cl);
+  assert(err == 0);
+  err = clSetKernelArg(s->loaduv_krnl, 2, sizeof(cl_int), &loaduv_out_off);
+  assert(err == 0);
+
+  err = clEnqueueNDRangeKernel(q, s->loaduv_krnl, 1, NULL,
+                               &loaduv_work_size, NULL, 0, 0, NULL);
+  assert(err == 0);
+}

selfdrive/modeld/transforms/loadyuv.cl

@@ -0,0 +1,43 @@
+#define UV_SIZE ((TRANSFORMED_WIDTH/2)*(TRANSFORMED_HEIGHT/2))
+
+__kernel void loadys(__global uchar8 const * const Y,
+                     __global float * out)
+{
+    const int gid = get_global_id(0);
+    const int ois = gid * 8;
+    const int oy = ois / TRANSFORMED_WIDTH;
+    const int ox = ois % TRANSFORMED_WIDTH;
+
+    const uchar8 ys = Y[gid];
+
+    // y = (x - 128) / 128
+    const float8 ysf = (convert_float8(ys) - 128.f) * 0.0078125f;
+
+    // 02
+    // 13
+
+    __global float* outy0;
+    __global float* outy1;
+    if ((oy & 1) == 0) {
+      outy0 = out; //y0
+      outy1 = out + UV_SIZE*2; //y2
+    } else {
+      outy0 = out + UV_SIZE; //y1
+      outy1 = out + UV_SIZE*3; //y3
+    }
+
+    vstore4(ysf.s0246, 0, outy0 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
+    vstore4(ysf.s1357, 0, outy1 + (oy/2) * (TRANSFORMED_WIDTH/2) + ox/2);
+}
+
+__kernel void loaduv(__global uchar8 const * const in,
+                     __global float8 * out,
+                     int out_offset)
+{
+  const int gid = get_global_id(0);
+  const uchar8 inv = in[gid];
+
+  // y = (x - 128) / 128
+  const float8 outv  = (convert_float8(inv) - 128.f) * 0.0078125f;
+  out[gid + out_offset / 8] = outv;
+}

selfdrive/modeld/transforms/loadyuv.h

@@ -0,0 +1,34 @@
+#ifndef LOADYUV_H
+#define LOADYUV_H
+
+#include <inttypes.h>
+#include <stdbool.h>
+
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+  int width, height;
+  cl_kernel loadys_krnl, loaduv_krnl;
+} LoadYUVState;
+
+void loadyuv_init(LoadYUVState* s, cl_context ctx, cl_device_id device_id, int width, int height);
+
+void loadyuv_destroy(LoadYUVState* s);
+
+void loadyuv_queue(LoadYUVState* s, cl_command_queue q,
+                   cl_mem y_cl, cl_mem u_cl, cl_mem v_cl,
+                   cl_mem out_cl);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // LOADYUV_H

selfdrive/modeld/transforms/transform.c

@@ -0,0 +1,149 @@
+#include <string.h>
+#include <assert.h>
+
+#include "clutil.h"
+
+#include "transform.h"
+
+void transform_init(Transform* s, cl_context ctx, cl_device_id device_id) {
+  int err = 0;
+  memset(s, 0, sizeof(*s));
+
+  cl_program prg = CLU_LOAD_FROM_FILE(ctx, device_id, "transforms/transform.cl", "");
+
+  s->krnl = clCreateKernel(prg, "warpPerspective", &err);
+  assert(err == 0);
+
+  // done with this
+  err = clReleaseProgram(prg);
+  assert(err == 0);
+
+  s->m_y_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err);
+  assert(err == 0);
+
+  s->m_uv_cl = clCreateBuffer(ctx, CL_MEM_READ_WRITE, 3*3*sizeof(float), NULL, &err);
+  assert(err == 0);
+}
+
+void transform_destroy(Transform* s) {
+  int err = 0;
+
+  err = clReleaseMemObject(s->m_y_cl);
+  assert(err == 0);
+  err = clReleaseMemObject(s->m_uv_cl);
+  assert(err == 0);
+
+  err = clReleaseKernel(s->krnl);
+  assert(err == 0);
+}
+
+void transform_queue(Transform* s,
+                     cl_command_queue q,
+                     cl_mem in_yuv, int in_width, int in_height,
+                     cl_mem out_y, cl_mem out_u, cl_mem out_v,
+                     int out_width, int out_height,
+                     mat3 projection) {
+  int err = 0;
+  const int zero = 0;
+
+  // sampled using pixel center origin
+  // (because thats how fastcv and opencv does it)
+
+  mat3 projection_y = projection;
+
+  // in and out uv is half the size of y.
+  mat3 projection_uv = transform_scale_buffer(projection, 0.5);
+
+  err = clEnqueueWriteBuffer(q, s->m_y_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_y.v, 0, NULL, NULL);
+  assert(err == 0);
+  err = clEnqueueWriteBuffer(q, s->m_uv_cl, CL_TRUE, 0, 3*3*sizeof(float), (void*)projection_uv.v, 0, NULL, NULL);
+  assert(err == 0);
+
+  const int in_y_width = in_width;
+  const int in_y_height = in_height;
+  const int in_uv_width = in_width/2;
+  const int in_uv_height = in_height/2;
+  const int in_y_offset = 0;
+  const int in_u_offset = in_y_offset + in_y_width*in_y_height;
+  const int in_v_offset = in_u_offset + in_uv_width*in_uv_height;
+
+  const int out_y_width = out_width;
+  const int out_y_height = out_height;
+  const int out_uv_width = out_width/2;
+  const int out_uv_height = out_height/2;
+
+  err = clSetKernelArg(s->krnl, 0, sizeof(cl_mem), &in_yuv);
+  assert(err == 0);
+
+  err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_y_width);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_y_offset);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_y_height);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_y_width);
+  assert(err == 0);
+
+  err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_y);
+  assert(err == 0);
+
+  err = clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_y_width);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_y_height);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_y_width);
+  assert(err == 0);
+
+  err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_y_cl);
+  assert(err == 0);
+
+  const size_t work_size_y[2] = {out_y_width, out_y_height};
+
+  err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
+                              (const size_t*)&work_size_y, NULL, 0, 0, NULL);
+  assert(err == 0);
+
+
+  const size_t work_size_uv[2] = {out_uv_width, out_uv_height};
+
+  err = clSetKernelArg(s->krnl, 1, sizeof(cl_int), &in_uv_width);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_u_offset);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 3, sizeof(cl_int), &in_uv_height);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 4, sizeof(cl_int), &in_uv_width);
+  assert(err == 0);
+
+  err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_u);
+  assert(err == 0);
+
+  err = clSetKernelArg(s->krnl, 6, sizeof(cl_int), &out_uv_width);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 7, sizeof(cl_int), &zero);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 8, sizeof(cl_int), &out_uv_height);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 9, sizeof(cl_int), &out_uv_width);
+  assert(err == 0);
+
+  err = clSetKernelArg(s->krnl, 10, sizeof(cl_mem), &s->m_uv_cl);
+  assert(err == 0);
+
+  err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
+                              (const size_t*)&work_size_uv, NULL, 0, 0, NULL);
+  assert(err == 0);
+
+
+  err = clSetKernelArg(s->krnl, 2, sizeof(cl_int), &in_v_offset);
+  assert(err == 0);
+  err = clSetKernelArg(s->krnl, 5, sizeof(cl_mem), &out_v);
+  assert(err == 0);
+
+
+  err = clEnqueueNDRangeKernel(q, s->krnl, 2, NULL,
+                              (const size_t*)&work_size_uv, NULL, 0, 0, NULL);
+  assert(err == 0);
+}

selfdrive/modeld/transforms/transform.cl

@@ -0,0 +1,54 @@
+#define INTER_BITS 5
+#define INTER_TAB_SIZE (1 << INTER_BITS)
+#define INTER_SCALE 1.f / INTER_TAB_SIZE
+
+#define INTER_REMAP_COEF_BITS 15
+#define INTER_REMAP_COEF_SCALE (1 << INTER_REMAP_COEF_BITS)
+
+__kernel void warpPerspective(__global const uchar * src,
+                              int src_step, int src_offset, int src_rows, int src_cols,
+                              __global uchar * dst,
+                              int dst_step, int dst_offset, int dst_rows, int dst_cols,
+                              __constant float * M)
+{
+    int dx = get_global_id(0);
+    int dy = get_global_id(1);
+
+    if (dx < dst_cols && dy < dst_rows)
+    {
+        float X0 = M[0] * dx + M[1] * dy + M[2];
+        float Y0 = M[3] * dx + M[4] * dy + M[5];
+        float W = M[6] * dx + M[7] * dy + M[8];
+        W = W != 0.0f ? INTER_TAB_SIZE / W : 0.0f;
+        int X = rint(X0 * W), Y = rint(Y0 * W);
+
+        short sx = convert_short_sat(X >> INTER_BITS);
+        short sy = convert_short_sat(Y >> INTER_BITS);
+        short ay = (short)(Y & (INTER_TAB_SIZE - 1));
+        short ax = (short)(X & (INTER_TAB_SIZE - 1));
+
+        int v0 = (sx >= 0 && sx < src_cols && sy >= 0 && sy < src_rows) ?
+            convert_int(src[mad24(sy, src_step, src_offset + sx)]) : 0;
+        int v1 = (sx+1 >= 0 && sx+1 < src_cols && sy >= 0 && sy < src_rows) ?
+            convert_int(src[mad24(sy, src_step, src_offset + (sx+1))]) : 0;
+        int v2 = (sx >= 0 && sx < src_cols && sy+1 >= 0 && sy+1 < src_rows) ?
+            convert_int(src[mad24(sy+1, src_step, src_offset + sx)]) : 0;
+        int v3 = (sx+1 >= 0 && sx+1 < src_cols && sy+1 >= 0 && sy+1 < src_rows) ?
+            convert_int(src[mad24(sy+1, src_step, src_offset + (sx+1))]) : 0;
+
+        float taby = 1.f/INTER_TAB_SIZE*ay;
+        float tabx = 1.f/INTER_TAB_SIZE*ax;
+
+        int dst_index = mad24(dy, dst_step, dst_offset + dx);
+
+        int itab0 = convert_short_sat_rte( (1.0f-taby)*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
+        int itab1 = convert_short_sat_rte( (1.0f-taby)*tabx * INTER_REMAP_COEF_SCALE );
+        int itab2 = convert_short_sat_rte( taby*(1.0f-tabx) * INTER_REMAP_COEF_SCALE );
+        int itab3 = convert_short_sat_rte( taby*tabx * INTER_REMAP_COEF_SCALE );
+
+        int val = v0 * itab0 +  v1 * itab1 + v2 * itab2 + v3 * itab3;
+
+        uchar pix = convert_uchar_sat((val + (1 << (INTER_REMAP_COEF_BITS-1))) >> INTER_REMAP_COEF_BITS);
+        dst[dst_index] = pix;
+    }
+}

selfdrive/modeld/transforms/transform.h

@@ -0,0 +1,38 @@
+#ifndef TRANSFORM_H
+#define TRANSFORM_H
+
+#include <inttypes.h>
+#include <stdbool.h>
+
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#include "common/mat.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct {
+  cl_kernel krnl;
+  cl_mem m_y_cl, m_uv_cl;
+} Transform;
+
+void transform_init(Transform* s, cl_context ctx, cl_device_id device_id);
+
+void transform_destroy(Transform* transform);
+
+void transform_queue(Transform* s, cl_command_queue q,
+                     cl_mem yuv, int in_width, int in_height,
+                     cl_mem out_y, cl_mem out_u, cl_mem out_v,
+                     int out_width, int out_height,
+                     mat3 projection);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // TRANSFORM_H

selfdrive/modeld/visiontest.c

@@ -0,0 +1,160 @@
+#include <assert.h>
+#include <stdio.h>
+#include <string.h>
+
+#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
+#ifdef __APPLE__
+#include <OpenCL/cl.h>
+#else
+#include <CL/cl.h>
+#endif
+
+#include "clutil.h"
+#include "transforms/transform.h"
+
+typedef struct {
+  int disable_model;
+  Transform transform;
+
+  int in_width;
+  int in_height;
+  int out_width;
+  int out_height;
+
+  cl_context context;
+  cl_command_queue command_queue;
+  cl_device_id device_id;
+
+  size_t in_yuv_size;
+  cl_mem in_yuv_cl;
+
+  cl_mem out_y_cl, out_u_cl, out_v_cl;
+} VisionTest;
+
+void initialize_opencl(VisionTest* visiontest) {
+  // init cl
+  /* Get Platform and Device Info */
+  cl_platform_id platform_ids[16] = {0};
+  cl_uint num_platforms;
+  int err = clGetPlatformIDs(16, platform_ids, &num_platforms);
+  if (err != 0) {
+    fprintf(stderr, "cl error: %d\n", err);
+  }
+  assert(err == 0);
+
+  // try to find a CPU device
+  cl_device_id device_id = NULL;
+  for (int i=0; i<num_platforms; i++) {
+    cl_uint num_devices_unused;
+    err = clGetDeviceIDs(platform_ids[i], CL_DEVICE_TYPE_CPU, 1, &device_id,
+                         &num_devices_unused);
+    if (err == 0) break;
+  }
+  if (err != 0) {
+    fprintf(stderr, "cl error: %d\n", err);
+  }
+  assert(err == 0);
+
+  visiontest->context = clCreateContext(NULL, 1, &device_id, NULL, NULL, &err);
+  assert(err == 0);
+
+  visiontest->device_id = device_id;
+}
+
+VisionTest* visiontest_create(int temporal_model, int disable_model,
+                              int input_width, int input_height,
+                              int model_input_width, int model_input_height) {
+  int err = 0;
+
+  VisionTest* const vt = calloc(1, sizeof(*vt));
+  assert(vt);
+
+  vt->disable_model = disable_model;
+  vt->in_width = input_width;
+  vt->in_height = input_height;
+  vt->out_width = model_input_width;
+  vt->out_height = model_input_height;
+
+  initialize_opencl(vt);
+
+  transform_init(&vt->transform, vt->context, vt->device_id);
+
+
+  assert((vt->in_width%2) == 0 && (vt->in_height%2) == 0);
+  vt->in_yuv_size = vt->in_width*vt->in_height*3/2;
+  vt->in_yuv_cl = clCreateBuffer(vt->context, CL_MEM_READ_WRITE,
+                                 vt->in_yuv_size, NULL, &err);
+  assert(err == 0);
+
+  vt->out_y_cl = clCreateBuffer(vt->context, CL_MEM_READ_WRITE,
+                                vt->out_width*vt->out_width, NULL, &err);
+  assert(err == 0);
+  vt->out_u_cl = clCreateBuffer(vt->context, CL_MEM_READ_WRITE,
+                                 vt->out_width*vt->out_width/4, NULL, &err);
+  assert(err == 0);
+  vt->out_v_cl = clCreateBuffer(vt->context, CL_MEM_READ_WRITE,
+                                 vt->out_width*vt->out_width/4, NULL, &err);
+  assert(err == 0);
+
+  vt->command_queue = clCreateCommandQueue(vt->context, vt->device_id, 0, &err);
+  assert(err == 0);
+
+  return vt;
+}
+
+void visiontest_destroy(VisionTest* vt) {
+  transform_destroy(&vt->transform);
+
+  int err = 0;
+
+  err = clReleaseMemObject(vt->in_yuv_cl);
+  assert(err == 0);
+  err = clReleaseMemObject(vt->out_y_cl);
+  assert(err == 0);
+  err = clReleaseMemObject(vt->out_u_cl);
+  assert(err == 0);
+  err = clReleaseMemObject(vt->out_v_cl);
+  assert(err == 0);
+
+  err = clReleaseCommandQueue(vt->command_queue);
+  assert(err == 0);
+
+  err = clReleaseContext(vt->context);
+  assert(err == 0);
+
+  free(vt);
+}
+
+void visiontest_transform(VisionTest* vt, const uint8_t* yuv_data,
+                          uint8_t* out_y, uint8_t* out_u, uint8_t* out_v,
+                          const float* transform) {
+  int err = 0;
+
+  err = clEnqueueWriteBuffer(vt->command_queue, vt->in_yuv_cl, CL_FALSE,
+                             0, vt->in_yuv_size, yuv_data, 0, NULL, NULL);
+  assert(err == 0);
+
+  mat3 transform_m = *(const mat3*)transform;
+
+  transform_queue(&vt->transform, vt->command_queue,
+                  vt->in_yuv_cl, vt->in_width, vt->in_height,
+                  vt->out_y_cl, vt->out_u_cl, vt->out_v_cl,
+                  vt->out_width, vt->out_height,
+                  transform_m);
+
+  err = clEnqueueReadBuffer(vt->command_queue, vt->out_y_cl, CL_FALSE,
+                            0, vt->out_width*vt->out_height, out_y,
+                            0, NULL, NULL);
+  assert(err == 0);
+  err = clEnqueueReadBuffer(vt->command_queue, vt->out_u_cl, CL_FALSE,
+                            0, vt->out_width*vt->out_height/4, out_u,
+                            0, NULL, NULL);
+  assert(err == 0);
+  err = clEnqueueReadBuffer(vt->command_queue, vt->out_v_cl, CL_FALSE,
+                            0, vt->out_width*vt->out_height/4, out_v,
+                            0, NULL, NULL);
+  assert(err == 0);
+
+  clFinish(vt->command_queue);
+}
+

selfdrive/modeld/visiontest.mk

@@ -0,0 +1,105 @@
+CC:=clang
+CXX:=clang++
+OPT_FLAGS:=-O2 -g -ggdb3
+
+UNAME_S := $(shell uname -s)
+ifeq ($(UNAME_S),Linux)
+	SHARED_FLAGS=-Wl,--whole-archive $^ -Wl,--no-whole-archive
+endif
+ifeq ($(UNAME_S),Darwin)
+	SHARED_FLAGS=-Wl,-force_load $^
+endif
+
+PHONELIBS := ../../phonelibs
+BASEDIR := ../..
+
+WARN_FLAGS = -Werror=implicit-function-declaration \
+             -Werror=incompatible-pointer-types \
+             -Werror=int-conversion \
+             -Werror=return-type \
+             -Werror=format-extra-args
+
+CFLAGS = -std=gnu11 -g -fPIC $(OPT_FLAGS) $(WARN_FLAGS)
+CXXFLAGS = -std=c++14 -fPIC $(OPT_FLAGS) $(WARN_FLAGS)
+
+EIGEN_FLAGS = -I$(PHONELIBS)/eigen
+
+CEREAL_LIBS = $(BASEDIR)/cereal/libmessaging.a
+
+OPENCV_LIBS = -lopencv_video -lopencv_core -lopencv_imgproc
+
+ifeq ($(UNAME_S),Darwin)
+  VT_LDFLAGS += $(PHONELIBS)/capnp-c/mac/lib/libcapnp_c.a \
+                 $(PHONELIBS)/zmq/mac/lib/libczmq.a \
+                 $(PHONELIBS)/zmq/mac/lib/libzmq.a \
+                -framework OpenCL
+
+  OPENCV_LIBS += -L/usr/local/opt/opencv@2/lib
+  OPENCV_FLAGS += -I/usr/local/opt/opencv@2/include
+
+else
+  VT_LDFLAGS += $(CEREAL_LIBS) \
+								-L/system/vendor/lib64 \
+                -L$(BASEDIR)/external/zmq/lib/ \
+								-l:libczmq.a -l:libzmq.a \
+                -lOpenCL
+endif
+
+.PHONY: all visiontest clean test
+all: visiontest
+
+libvisiontest_inputs := visiontest.c \
+                        transforms/transform.c \
+                        transforms/loadyuv.c \
+                        ../common/clutil.c \
+                        $(BASEDIR)/selfdrive/common/util.c \
+                        $(CEREAL_OBJS)
+
+visiontest: libvisiontest.so
+all-tests := $(addsuffix .test, $(basename $(wildcard test_*)))
+
+%.o: %.cc
+	@echo "[ CXX ] $@"
+	$(CXX) $(CXXFLAGS) -MMD \
+		-I. -I.. -I../.. \
+		-Wall \
+		-I$(BASEDIR)/ -I$(BASEDIR)/selfdrive -I$(BASEDIR)/selfdrive/common \
+		$(EIGEN_FLAGS) \
+    $(OPENCV_FLAGS) \
+    $(CEREAL_CXXFLAGS) \
+		-c -o '$@' '$<'
+
+%.o: %.c
+	@echo "[ CXX ] $@"
+	$(CC) $(CFLAGS) -MMD \
+		-I. -I.. -I../.. \
+		-Wall \
+		-I$(BASEDIR)/ -I$(BASEDIR)/selfdrive -I$(BASEDIR)/selfdrive/common \
+    $(CEREAL_CFLAGS) \
+		-c -o '$@' '$<'
+
+libvisiontest.so: $(libvisiontest_inputs)
+	$(eval $@_TMP := $(shell mktemp))
+	$(CC) -std=gnu11 -shared -fPIC -O2 -g \
+		-Werror=implicit-function-declaration -Werror=incompatible-pointer-types \
+		-Werror=int-conversion -Wno-pointer-to-int-cast \
+		-I. -DCLU_NO_CACHE \
+		$^ -o $($@_TMP) \
+		-I$(PHONELIBS)/opencl/include \
+		-I$(BASEDIR)/selfdrive/common \
+		$(CEREAL_CXXFLAGS) \
+		$(CEREAL_CFLAGS) \
+		-I$(BASEDIR)/external/zmq/include \
+		-I$(BASEDIR)/ -I$(BASEDIR)/selfdrive \
+		-lstdc++ \
+		$(VT_LDFLAGS) \
+		-lm -lpthread
+	mv $($@_TMP) $@
+
+test : $(all-tests)
+
+test_%.test : test_%
+	@./'$<' || echo FAIL
+
+clean:
+	rm -rf *.o *.so *.a

selfdrive/modeld/visiontest.py

@@ -0,0 +1,135 @@
+import os
+import subprocess
+from cffi import FFI
+from common.basedir import BASEDIR
+
+# Initialize visiontest. Ignore output.
+_visiond_dir = os.path.dirname(os.path.abspath(__file__))
+_libvisiontest = "libvisiontest.so"
+try:  # bacause this crashes somtimes when running pipeline
+  subprocess.check_output(["make", "-C", _visiond_dir, "-f",
+                         os.path.join(_visiond_dir, "visiontest.mk"),
+                         _libvisiontest])
+except:
+  pass
+
+class VisionTest():
+  """A version of the vision model that can be run on a desktop.
+
+     WARNING: This class is not thread safe. VisionTest objects cannot be
+              created or used on multiple threads simultaneously.
+  """
+
+  ffi = FFI()
+  ffi.cdef("""
+  typedef unsigned char uint8_t;
+
+  struct VisionTest;
+  typedef struct VisionTest VisionTest;
+
+  VisionTest* visiontest_create(int temporal_model, int disable_model,
+                                int input_width, int input_height,
+                                int model_input_width, int model_input_height);
+  void visiontest_destroy(VisionTest* visiontest);
+
+  void visiontest_transform(VisionTest* vt, const uint8_t* yuv_data,
+                            uint8_t* out_y, uint8_t* out_u, uint8_t* out_v,
+                            const float* transform);
+  """)
+
+  clib = ffi.dlopen(os.path.join(_visiond_dir, _libvisiontest))
+
+  def __init__(self, input_size, model_input_size, model):
+    """Create a wrapper around visiond for off-device python code.
+
+       Inputs:
+        input_size: The size of YUV images passed to transform.
+        model_input_size: The size of YUV images passed to the model.
+        model: The name of the model to use. "temporal", "yuv", or None to disable the
+               model (used to disable OpenCL).
+    """
+    self._input_size = input_size
+    self._model_input_size = model_input_size
+
+    if model is None:
+      disable_model = 1
+      temporal_model = 0
+    elif model == "yuv":
+      disable_model = 0
+      temporal_model = 0
+    elif model == "temporal":
+      disable_model = 0
+      temporal_model = 1
+    else:
+      raise ValueError("Bad model name: {}".format(model))
+
+    prevdir = os.getcwd()
+    os.chdir(_visiond_dir) # tmp hack to find kernels
+    os.environ['BASEDIR'] = BASEDIR
+    self._visiontest_c = self.clib.visiontest_create(
+      temporal_model, disable_model, self._input_size[0], self._input_size[1],
+      self._model_input_size[0], self._model_input_size[1])
+    os.chdir(prevdir)
+
+  @property
+  def input_size(self):
+    return self._input_size
+
+  @property
+  def model_input_size(self):
+    return self._model_input_size
+
+  def transform(self, yuv_data, transform):
+    y_len = self.model_input_size[0] * self.model_input_size[1]
+    t_y_ptr = bytearray(y_len)
+    t_u_ptr = bytearray(y_len // 4)
+    t_v_ptr = bytearray(y_len // 4)
+
+    self.transform_output_buffer(yuv_data, t_y_ptr, t_u_ptr, t_v_ptr,
+                                 transform)
+
+    return t_y_ptr, t_u_ptr, t_v_ptr
+
+  def transform_contiguous(self, yuv_data, transform):
+    y_ol = self.model_input_size[0] * self.model_input_size[1]
+    uv_ol = y_ol // 4
+    result = bytearray(y_ol * 3 // 2)
+    result_view = memoryview(result)
+    t_y_ptr = result_view[:y_ol]
+    t_u_ptr = result_view[y_ol:y_ol + uv_ol]
+    t_v_ptr = result_view[y_ol + uv_ol:]
+
+    self.transform_output_buffer(yuv_data, t_y_ptr, t_u_ptr, t_v_ptr,
+                                 transform)
+    return result
+
+
+  def transform_output_buffer(self, yuv_data, y_out, u_out, v_out,
+                              transform):
+    assert len(yuv_data) == self.input_size[0] * self.input_size[1] * 3/2
+
+    cast = self.ffi.cast
+    from_buffer = self.ffi.from_buffer
+    yuv_ptr = cast("unsigned char*", from_buffer(yuv_data))
+    transform_ptr = self.ffi.new("float[]", transform)
+
+    y_out_ptr = cast("unsigned char*", from_buffer(y_out))
+    u_out_ptr = cast("unsigned char*", from_buffer(u_out))
+    v_out_ptr = cast("unsigned char*", from_buffer(v_out))
+
+    self.clib.visiontest_transform(self._visiontest_c, yuv_ptr, y_out_ptr,
+                                   u_out_ptr, v_out_ptr, transform_ptr)
+
+  def close(self):
+    self.clib.visiontest_destroy(self._visiontest_c)
+    self._visiontest_c = None
+
+  def __enter__(self):
+    return self
+
+  def __exit__(self, type, value, traceback):
+    self.close()
+
+
+if __name__ == "__main__":
+  VisionTest((560, 304), (320, 160), "temporal")

selfdrive/proclogd/SConscript

@@ -0,0 +1,2 @@
+Import('env', 'messaging')
+env.Program('proclogd.cc', LIBS=[messaging, 'pthread', 'zmq', 'czmq', 'capnp', 'kj'])

selfdrive/proclogd/proclogd.cc

@@ -0,0 +1,247 @@
+#include <cstdio>
+#include <cstdlib>
+#include <climits>
+#include <cassert>
+
+#include <unistd.h>
+#include <dirent.h>
+
+#include <vector>
+#include <string>
+#include <memory>
+#include <utility>
+#include <sstream>
+#include <fstream>
+#include <algorithm>
+#include <functional>
+#include <unordered_map>
+
+#include "messaging.hpp"
+#include <capnp/serialize.h>
+#include "cereal/gen/cpp/log.capnp.h"
+
+#include "common/timing.h"
+#include "common/utilpp.h"
+
+namespace {
+
+struct ProcCache {
+  std::string name;
+  std::vector<std::string> cmdline;
+  std::string exe;
+};
+
+}
+
+int main() {
+  int err;
+
+  Context * c = Context::create();
+  PubSocket * publisher = PubSocket::create(c, "procLog");
+  assert(publisher != NULL);
+
+  double jiffy = sysconf(_SC_CLK_TCK);
+  size_t page_size = sysconf(_SC_PAGE_SIZE);
+
+  std::unordered_map<pid_t, ProcCache> proc_cache;
+
+  while (1) {
+
+    capnp::MallocMessageBuilder msg;
+    cereal::Event::Builder event = msg.initRoot<cereal::Event>();
+    event.setLogMonoTime(nanos_since_boot());
+    auto procLog = event.initProcLog();
+
+    auto orphanage = msg.getOrphanage();
+
+    // stat
+    {
+      std::vector<capnp::Orphan<cereal::ProcLog::CPUTimes>> otimes;
+
+      std::ifstream sstat("/proc/stat");
+      std::string stat_line;
+      while (std::getline(sstat, stat_line)) {
+        if (util::starts_with(stat_line, "cpu ")) {
+          // cpu total
+        } else if (util::starts_with(stat_line, "cpu")) {
+          // specific cpu
+          int id;
+          unsigned long utime, ntime, stime, itime;
+          unsigned long iowtime, irqtime, sirqtime;
+
+          int count = sscanf(stat_line.data(), "cpu%d %lu %lu %lu %lu %lu %lu %lu",
+            &id, &utime, &ntime, &stime, &itime, &iowtime, &irqtime, &sirqtime);
+
+          auto ltimeo = orphanage.newOrphan<cereal::ProcLog::CPUTimes>();
+          auto ltime = ltimeo.get();
+          ltime.setCpuNum(id);
+          ltime.setUser(utime / jiffy);
+          ltime.setNice(ntime / jiffy);
+          ltime.setSystem(stime / jiffy);
+          ltime.setIdle(itime / jiffy);
+          ltime.setIowait(iowtime / jiffy);
+          ltime.setIrq(irqtime / jiffy);
+          ltime.setSoftirq(irqtime / jiffy);
+
+          otimes.push_back(std::move(ltimeo));
+
+        } else {
+          break;
+        }
+      }
+
+      auto ltimes = procLog.initCpuTimes(otimes.size());
+      for (size_t i = 0; i < otimes.size(); i++) {
+        ltimes.adoptWithCaveats(i, std::move(otimes[i]));
+      }
+    }
+
+    // meminfo
+    {
+      auto mem = procLog.initMem();
+
+      std::ifstream smem("/proc/meminfo");
+      std::string mem_line;
+
+      uint64_t mem_total = 0, mem_free = 0, mem_available = 0, mem_buffers = 0;
+      uint64_t mem_cached = 0, mem_active = 0, mem_inactive = 0, mem_shared = 0;
+
+      while (std::getline(smem, mem_line)) {
+        if (util::starts_with(mem_line, "MemTotal:")) sscanf(mem_line.data(), "MemTotal: %lu kB", &mem_total);
+        else if (util::starts_with(mem_line, "MemFree:")) sscanf(mem_line.data(), "MemFree: %lu kB", &mem_free);
+        else if (util::starts_with(mem_line, "MemAvailable:")) sscanf(mem_line.data(), "MemAvailable: %lu kB", &mem_available);
+        else if (util::starts_with(mem_line, "Buffers:")) sscanf(mem_line.data(), "Buffers: %lu kB", &mem_buffers);
+        else if (util::starts_with(mem_line, "Cached:")) sscanf(mem_line.data(), "Cached: %lu kB", &mem_cached);
+        else if (util::starts_with(mem_line, "Active:")) sscanf(mem_line.data(), "Active: %lu kB", &mem_active);
+        else if (util::starts_with(mem_line, "Inactive:")) sscanf(mem_line.data(), "Inactive: %lu kB", &mem_inactive);
+        else if (util::starts_with(mem_line, "Shmem:")) sscanf(mem_line.data(), "Shmem: %lu kB", &mem_shared);
+      }
+
+      mem.setTotal(mem_total * 1024);
+      mem.setFree(mem_free * 1024);
+      mem.setAvailable(mem_available * 1024);
+      mem.setBuffers(mem_buffers * 1024);
+      mem.setCached(mem_cached * 1024);
+      mem.setActive(mem_active * 1024);
+      mem.setInactive(mem_inactive * 1024);
+      mem.setShared(mem_shared * 1024);
+    }
+
+    // processes
+    {
+      std::vector<capnp::Orphan<cereal::ProcLog::Process>> oprocs;
+      struct dirent *de = NULL;
+      DIR *d = opendir("/proc");
+      assert(d);
+      while ((de = readdir(d))) {
+        if (!isdigit(de->d_name[0])) continue;
+        pid_t pid = atoi(de->d_name);
+
+
+        auto lproco = orphanage.newOrphan<cereal::ProcLog::Process>();
+        auto lproc = lproco.get();
+
+        lproc.setPid(pid);
+
+        char tcomm[PATH_MAX] = {0};
+
+        {
+          std::string stat = util::read_file(util::string_format("/proc/%d/stat", pid));
+
+          char state;
+
+          int ppid;
+          unsigned long utime, stime;
+          long cutime, cstime, priority, nice, num_threads;
+          unsigned long long starttime;
+          unsigned long vms, rss;
+          int processor;
+
+          int count = sscanf(stat.data(),
+            "%*d (%1024[^)]) %c %d %*d %*d %*d %*d %*d %*d %*d %*d %*d "
+             "%lu %lu %ld %ld %ld %ld %ld %*d %lld "
+             "%lu %lu %*d %*d %*d %*d %*d %*d %*d "
+             "%*d %*d %*d %*d %*d %*d %*d %d",
+            tcomm, &state, &ppid,
+            &utime, &stime, &cutime, &cstime, &priority, &nice, &num_threads, &starttime,
+            &vms, &rss, &processor);
+
+          if (count != 14) continue;
+
+          lproc.setState(state);
+          lproc.setPpid(ppid);
+          lproc.setCpuUser(utime / jiffy);
+          lproc.setCpuSystem(stime / jiffy);
+          lproc.setCpuChildrenUser(cutime / jiffy);
+          lproc.setCpuChildrenSystem(cstime / jiffy);
+          lproc.setPriority(priority);
+          lproc.setNice(nice);
+          lproc.setNumThreads(num_threads);
+          lproc.setStartTime(starttime / jiffy);
+          lproc.setMemVms(vms);
+          lproc.setMemRss((uint64_t)rss * page_size);
+          lproc.setProcessor(processor);
+        }
+
+        std::string name(tcomm);
+        lproc.setName(name);
+
+        // populate other things from cache
+        auto cache_it = proc_cache.find(pid);
+        ProcCache cache;
+        if (cache_it != proc_cache.end()) {
+          cache = cache_it->second;
+        }
+        if (cache_it == proc_cache.end() || cache.name != name) {
+          cache = (ProcCache){
+            .name = name,
+            .exe = util::readlink(util::string_format("/proc/%d/exe", pid)),
+          };
+
+          // null-delimited cmdline arguments to vector
+          std::string cmdline_s = util::read_file(util::string_format("/proc/%d/cmdline", pid));
+          const char* cmdline_p = cmdline_s.c_str();
+          const char* cmdline_ep = cmdline_p + cmdline_s.size();
+
+          // strip trailing null bytes
+          while ((cmdline_ep-1) > cmdline_p && *(cmdline_ep-1) == 0) {
+            cmdline_ep--;
+          }
+
+          while (cmdline_p < cmdline_ep) {
+            std::string arg(cmdline_p);
+            cache.cmdline.push_back(arg);
+            cmdline_p += arg.size() + 1;
+          }
+
+          proc_cache[pid] = cache;
+        }
+
+        auto lcmdline = lproc.initCmdline(cache.cmdline.size());
+        for (size_t i = 0; i < lcmdline.size(); i++) {
+          lcmdline.set(i, cache.cmdline[i]);
+        }
+        lproc.setExe(cache.exe);
+
+        oprocs.push_back(std::move(lproco));
+      }
+      closedir(d);
+
+      auto lprocs = procLog.initProcs(oprocs.size());
+      for (size_t i = 0; i < oprocs.size(); i++) {
+        lprocs.adoptWithCaveats(i, std::move(oprocs[i]));
+      }
+    }
+
+    auto words = capnp::messageToFlatArray(msg);
+    auto bytes = words.asBytes();
+    publisher->send((char*)bytes.begin(), bytes.size());
+
+    usleep(2000000); // 2 secs
+  }
+
+  delete c;
+  delete publisher;
+
+  return 0;
+}