loggerd: move to system/ (#27534)

old-commit-hash: 94eb215980

system/loggerd/encoder/encoder.cc

@@ -0,0 +1,82 @@
+#include <cassert>
+#include "system/loggerd/encoder/encoder.h"
+
+VideoEncoder::~VideoEncoder() {}
+
+void VideoEncoder::publisher_init() {
+  // publish
+  service_name = this->type == DriverCam ? "driverEncodeData" :
+    (this->type == WideRoadCam ? "wideRoadEncodeData" :
+    (this->in_width == this->out_width ? "roadEncodeData" : "qRoadEncodeData"));
+  pm.reset(new PubMaster({service_name}));
+}
+
+void VideoEncoder::publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra,
+                                     unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat) {
+  // broadcast packet
+  MessageBuilder msg;
+  auto event = msg.initEvent(true);
+  auto edat = (e->type == DriverCam) ? event.initDriverEncodeData() :
+    ((e->type == WideRoadCam) ? event.initWideRoadEncodeData() :
+    (e->in_width == e->out_width ? event.initRoadEncodeData() : event.initQRoadEncodeData()));
+  auto edata = edat.initIdx();
+  struct timespec ts;
+  timespec_get(&ts, TIME_UTC);
+  edat.setUnixTimestampNanos((uint64_t)ts.tv_sec*1000000000 + ts.tv_nsec);
+  edata.setFrameId(extra.frame_id);
+  edata.setTimestampSof(extra.timestamp_sof);
+  edata.setTimestampEof(extra.timestamp_eof);
+  edata.setType(e->codec);
+  edata.setEncodeId(e->cnt++);
+  edata.setSegmentNum(segment_num);
+  edata.setSegmentId(idx);
+  edata.setFlags(flags);
+  edata.setLen(dat.size());
+  edat.setData(dat);
+  if (flags & V4L2_BUF_FLAG_KEYFRAME) edat.setHeader(header);
+
+  auto words = new kj::Array<capnp::word>(capnp::messageToFlatArray(msg));
+  auto bytes = words->asBytes();
+  e->pm->send(e->service_name, bytes.begin(), bytes.size());
+  if (e->write) {
+    e->to_write.push(words);
+  } else {
+    delete words;
+  }
+}
+
+// TODO: writing should be moved to loggerd
+void VideoEncoder::write_handler(VideoEncoder *e, const char *path) {
+  VideoWriter writer(path, e->filename, e->codec != cereal::EncodeIndex::Type::FULL_H_E_V_C, e->out_width, e->out_height, e->fps, e->codec);
+
+  bool first = true;
+  kj::Array<capnp::word>* out_buf;
+  while ((out_buf = e->to_write.pop())) {
+    capnp::FlatArrayMessageReader cmsg(*out_buf);
+    cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
+
+    auto edata = (e->type == DriverCam) ? event.getDriverEncodeData() :
+      ((e->type == WideRoadCam) ? event.getWideRoadEncodeData() :
+      (e->in_width == e->out_width ? event.getRoadEncodeData() : event.getQRoadEncodeData()));
+    auto idx = edata.getIdx();
+    auto flags = idx.getFlags();
+
+    if (first) {
+      assert(flags & V4L2_BUF_FLAG_KEYFRAME);
+      auto header = edata.getHeader();
+      writer.write((uint8_t *)header.begin(), header.size(), idx.getTimestampEof()/1000, true, false);
+      first = false;
+    }
+
+    // dangerous cast from const, but should be fine
+    auto data = edata.getData();
+    if (data.size() > 0) {
+      writer.write((uint8_t *)data.begin(), data.size(), idx.getTimestampEof()/1000, false, flags & V4L2_BUF_FLAG_KEYFRAME);
+    }
+
+    // free the data
+    delete out_buf;
+  }
+
+  // VideoWriter is freed on out of scope
+}

system/loggerd/encoder/encoder.h

@@ -0,0 +1,62 @@
+#pragma once
+
+#include <cassert>
+#include <cstdint>
+#include <thread>
+
+#include "cereal/messaging/messaging.h"
+#include "cereal/visionipc/visionipc.h"
+#include "common/queue.h"
+#include "system/loggerd/video_writer.h"
+#include "system/camerad/cameras/camera_common.h"
+
+#define V4L2_BUF_FLAG_KEYFRAME 8
+
+class VideoEncoder {
+public:
+  VideoEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps,
+              int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write)
+  : filename(filename), type(type), in_width(in_width), in_height(in_height), fps(fps),
+    bitrate(bitrate), codec(codec), out_width(out_width), out_height(out_height), write(write) { }
+  virtual ~VideoEncoder();
+  virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0;
+  virtual void encoder_open(const char* path) = 0;
+  virtual void encoder_close() = 0;
+
+  void publisher_init();
+  static void publisher_publish(VideoEncoder *e, int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr<capnp::byte> header, kj::ArrayPtr<capnp::byte> dat);
+
+  void writer_open(const char* path) {
+    if (this->write) write_handler_thread = std::thread(VideoEncoder::write_handler, this, path);
+  }
+
+  void writer_close() {
+    if (this->write) {
+      to_write.push(NULL);
+      write_handler_thread.join();
+    }
+    assert(to_write.empty());
+  }
+
+protected:
+  bool write;
+  const char* filename;
+  int in_width, in_height;
+  int out_width, out_height, fps;
+  int bitrate;
+  cereal::EncodeIndex::Type codec;
+  CameraType type;
+
+private:
+  // total frames encoded
+  int cnt = 0;
+
+  // publishing
+  std::unique_ptr<PubMaster> pm;
+  const char *service_name;
+
+  // writing support
+  static void write_handler(VideoEncoder *e, const char *path);
+  std::thread write_handler_thread;
+  SafeQueue<kj::Array<capnp::word>* > to_write;
+};

system/loggerd/encoder/ffmpeg_encoder.cc

@@ -0,0 +1,153 @@
+#pragma clang diagnostic ignored "-Wdeprecated-declarations"
+
+#include "system/loggerd/encoder/ffmpeg_encoder.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+
+#include <cassert>
+#include <cstdio>
+#include <cstdlib>
+
+#define __STDC_CONSTANT_MACROS
+
+#include "libyuv.h"
+
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libavutil/imgutils.h>
+}
+
+#include "common/swaglog.h"
+#include "common/util.h"
+
+const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0;
+
+void FfmpegEncoder::encoder_init() {
+  frame = av_frame_alloc();
+  assert(frame);
+  frame->format = AV_PIX_FMT_YUV420P;
+  frame->width = out_width;
+  frame->height = out_height;
+  frame->linesize[0] = out_width;
+  frame->linesize[1] = out_width/2;
+  frame->linesize[2] = out_width/2;
+
+  convert_buf.resize(in_width * in_height * 3 / 2);
+
+  if (in_width != out_width || in_height != out_height) {
+    downscale_buf.resize(out_width * out_height * 3 / 2);
+  }
+
+  publisher_init();
+}
+
+FfmpegEncoder::~FfmpegEncoder() {
+  encoder_close();
+  av_frame_free(&frame);
+}
+
+void FfmpegEncoder::encoder_open(const char* path) {
+  const AVCodec *codec = avcodec_find_encoder(AV_CODEC_ID_FFVHUFF);
+
+  this->codec_ctx = avcodec_alloc_context3(codec);
+  assert(this->codec_ctx);
+  this->codec_ctx->width = frame->width;
+  this->codec_ctx->height = frame->height;
+  this->codec_ctx->pix_fmt = AV_PIX_FMT_YUV420P;
+  this->codec_ctx->time_base = (AVRational){ 1, fps };
+  int err = avcodec_open2(this->codec_ctx, codec, NULL);
+  assert(err >= 0);
+
+  writer_open(path);
+  is_open = true;
+  segment_num++;
+  counter = 0;
+}
+
+void FfmpegEncoder::encoder_close() {
+  if (!is_open) return;
+
+  writer_close();
+  avcodec_free_context(&codec_ctx);
+  is_open = false;
+}
+
+int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
+  assert(buf->width == this->in_width);
+  assert(buf->height == this->in_height);
+
+  uint8_t *cy = convert_buf.data();
+  uint8_t *cu = cy + in_width * in_height;
+  uint8_t *cv = cu + (in_width / 2) * (in_height / 2);
+  libyuv::NV12ToI420(buf->y, buf->stride,
+                     buf->uv, buf->stride,
+                     cy, in_width,
+                     cu, in_width/2,
+                     cv, in_width/2,
+                     in_width, in_height);
+
+  if (downscale_buf.size() > 0) {
+    uint8_t *out_y = downscale_buf.data();
+    uint8_t *out_u = out_y + frame->width * frame->height;
+    uint8_t *out_v = out_u + (frame->width / 2) * (frame->height / 2);
+    libyuv::I420Scale(cy, in_width,
+                      cu, in_width/2,
+                      cv, in_width/2,
+                      in_width, in_height,
+                      out_y, frame->width,
+                      out_u, frame->width/2,
+                      out_v, frame->width/2,
+                      frame->width, frame->height,
+                      libyuv::kFilterNone);
+    frame->data[0] = out_y;
+    frame->data[1] = out_u;
+    frame->data[2] = out_v;
+  } else {
+    frame->data[0] = cy;
+    frame->data[1] = cu;
+    frame->data[2] = cv;
+  }
+  frame->pts = counter*50*1000; // 50ms per frame
+
+  int ret = counter;
+
+  int err = avcodec_send_frame(this->codec_ctx, frame);
+  if (err < 0) {
+    LOGE("avcodec_send_frame error %d", err);
+    ret = -1;
+  }
+
+  AVPacket pkt;
+  av_init_packet(&pkt);
+  pkt.data = NULL;
+  pkt.size = 0;
+  while (ret >= 0) {
+    err = avcodec_receive_packet(this->codec_ctx, &pkt);
+    if (err == AVERROR_EOF) {
+      break;
+    } else if (err == AVERROR(EAGAIN)) {
+      // Encoder might need a few frames on startup to get started. Keep going
+      ret = 0;
+      break;
+    } else if (err < 0) {
+      LOGE("avcodec_receive_packet error %d", err);
+      ret = -1;
+      break;
+    }
+
+    if (env_debug_encoder) {
+      printf("%20s got %8d bytes flags %8x idx %4d id %8d\n", this->filename, pkt.size, pkt.flags, counter, extra->frame_id);
+    }
+
+    publisher_publish(this, segment_num, counter, *extra,
+      (pkt.flags & AV_PKT_FLAG_KEY) ? V4L2_BUF_FLAG_KEYFRAME : 0,
+      kj::arrayPtr<capnp::byte>(pkt.data, (size_t)0), // TODO: get the header
+      kj::arrayPtr<capnp::byte>(pkt.data, pkt.size));
+
+    counter++;
+  }
+  av_packet_unref(&pkt);
+  return ret;
+}

system/loggerd/encoder/ffmpeg_encoder.h

@@ -0,0 +1,37 @@
+#pragma once
+
+#include <cstdio>
+#include <cstdlib>
+#include <string>
+#include <vector>
+
+extern "C" {
+#include <libavcodec/avcodec.h>
+#include <libavformat/avformat.h>
+#include <libavutil/imgutils.h>
+}
+
+#include "system/loggerd/encoder/encoder.h"
+#include "system/loggerd/loggerd.h"
+
+class FfmpegEncoder : public VideoEncoder {
+ public:
+  FfmpegEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps,
+                int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) :
+                VideoEncoder(filename, type, in_width, in_height, fps, bitrate, cereal::EncodeIndex::Type::BIG_BOX_LOSSLESS, out_width, out_height, write) { encoder_init(); }
+  ~FfmpegEncoder();
+  void encoder_init();
+  int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
+  void encoder_open(const char* path);
+  void encoder_close();
+
+private:
+  int segment_num = -1;
+  int counter = 0;
+  bool is_open = false;
+
+  AVCodecContext *codec_ctx;
+  AVFrame *frame = NULL;
+  std::vector<uint8_t> convert_buf;
+  std::vector<uint8_t> downscale_buf;
+};

system/loggerd/encoder/v4l_encoder.cc

@@ -0,0 +1,311 @@
+#include <cassert>
+#include <sys/ioctl.h>
+#include <poll.h>
+
+#include "system/loggerd/encoder/v4l_encoder.h"
+#include "common/util.h"
+#include "common/timing.h"
+
+#include "libyuv.h"
+#include "msm_media_info.h"
+
+// has to be in this order
+#include "v4l2-controls.h"
+#include <linux/videodev2.h>
+#define V4L2_QCOM_BUF_FLAG_CODECCONFIG 0x00020000
+#define V4L2_QCOM_BUF_FLAG_EOS 0x02000000
+
+// echo 0x7fffffff > /sys/kernel/debug/msm_vidc/debug_level
+const int env_debug_encoder = (getenv("DEBUG_ENCODER") != NULL) ? atoi(getenv("DEBUG_ENCODER")) : 0;
+
+#define checked_ioctl(x,y,z) { int _ret = HANDLE_EINTR(ioctl(x,y,z)); if (_ret!=0) { LOGE("checked_ioctl failed %d %lx %p", x, y, z); } assert(_ret==0); }
+
+static void dequeue_buffer(int fd, v4l2_buf_type buf_type, unsigned int *index=NULL, unsigned int *bytesused=NULL, unsigned int *flags=NULL, struct timeval *timestamp=NULL) {
+  v4l2_plane plane = {0};
+  v4l2_buffer v4l_buf = {
+    .type = buf_type,
+    .memory = V4L2_MEMORY_USERPTR,
+    .m = { .planes = &plane, },
+    .length = 1,
+  };
+  checked_ioctl(fd, VIDIOC_DQBUF, &v4l_buf);
+
+  if (index) *index = v4l_buf.index;
+  if (bytesused) *bytesused = v4l_buf.m.planes[0].bytesused;
+  if (flags) *flags = v4l_buf.flags;
+  if (timestamp) *timestamp = v4l_buf.timestamp;
+  assert(v4l_buf.m.planes[0].data_offset == 0);
+}
+
+static void queue_buffer(int fd, v4l2_buf_type buf_type, unsigned int index, VisionBuf *buf, struct timeval timestamp={}) {
+  v4l2_plane plane = {
+    .length = (unsigned int)buf->len,
+    .m = { .userptr = (unsigned long)buf->addr, },
+    .bytesused = (uint32_t)buf->len,
+    .reserved = {(unsigned int)buf->fd}
+  };
+
+  v4l2_buffer v4l_buf = {
+    .type = buf_type,
+    .index = index,
+    .memory = V4L2_MEMORY_USERPTR,
+    .m = { .planes = &plane, },
+    .length = 1,
+    .flags = V4L2_BUF_FLAG_TIMESTAMP_COPY,
+    .timestamp = timestamp
+  };
+
+  checked_ioctl(fd, VIDIOC_QBUF, &v4l_buf);
+}
+
+static void request_buffers(int fd, v4l2_buf_type buf_type, unsigned int count) {
+  struct v4l2_requestbuffers reqbuf = {
+    .type = buf_type,
+    .memory = V4L2_MEMORY_USERPTR,
+    .count = count
+  };
+  checked_ioctl(fd, VIDIOC_REQBUFS, &reqbuf);
+}
+
+void V4LEncoder::dequeue_handler(V4LEncoder *e) {
+  std::string dequeue_thread_name = "dq-"+std::string(e->filename);
+  util::set_thread_name(dequeue_thread_name.c_str());
+
+  e->segment_num++;
+  uint32_t idx = -1;
+  bool exit = false;
+
+  // POLLIN is capture, POLLOUT is frame
+  struct pollfd pfd;
+  pfd.events = POLLIN | POLLOUT;
+  pfd.fd = e->fd;
+
+  // save the header
+  kj::Array<capnp::byte> header;
+
+  while (!exit) {
+    int rc = poll(&pfd, 1, 1000);
+    if (!rc) { LOGE("encoder dequeue poll timeout"); continue; }
+
+    if (env_debug_encoder >= 2) {
+      printf("%20s poll %x at %.2f ms\n", e->filename, pfd.revents, millis_since_boot());
+    }
+
+    int frame_id = -1;
+    if (pfd.revents & POLLIN) {
+      unsigned int bytesused, flags, index;
+      struct timeval timestamp;
+      dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, &index, &bytesused, &flags, &timestamp);
+      e->buf_out[index].sync(VISIONBUF_SYNC_FROM_DEVICE);
+      uint8_t *buf = (uint8_t*)e->buf_out[index].addr;
+      int64_t ts = timestamp.tv_sec * 1000000 + timestamp.tv_usec;
+
+      // eof packet, we exit
+      if (flags & V4L2_QCOM_BUF_FLAG_EOS) {
+        exit = true;
+      } else if (flags & V4L2_QCOM_BUF_FLAG_CODECCONFIG) {
+        // save header
+        header = kj::heapArray<capnp::byte>(buf, bytesused);
+      } else {
+        VisionIpcBufExtra extra = e->extras.pop();
+        assert(extra.timestamp_eof/1000 == ts); // stay in sync
+        frame_id = extra.frame_id;
+        ++idx;
+        e->publisher_publish(e, e->segment_num, idx, extra, flags, header, kj::arrayPtr<capnp::byte>(buf, bytesused));
+      }
+
+      if (env_debug_encoder) {
+        printf("%20s got(%d) %6d bytes flags %8x idx %3d/%4d id %8d ts %ld lat %.2f ms (%lu frames free)\n",
+          e->filename, index, bytesused, flags, e->segment_num, idx, frame_id, ts, millis_since_boot()-(ts/1000.), e->free_buf_in.size());
+      }
+
+      // requeue the buffer
+      queue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, index, &e->buf_out[index]);
+    }
+
+    if (pfd.revents & POLLOUT) {
+      unsigned int index;
+      dequeue_buffer(e->fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, &index);
+      e->free_buf_in.push(index);
+    }
+  }
+}
+
+void V4LEncoder::encoder_init() {
+  fd = open("/dev/v4l/by-path/platform-aa00000.qcom_vidc-video-index1", O_RDWR|O_NONBLOCK);
+  assert(fd >= 0);
+
+  struct v4l2_capability cap;
+  checked_ioctl(fd, VIDIOC_QUERYCAP, &cap);
+  LOGD("opened encoder device %s %s = %d", cap.driver, cap.card, fd);
+  assert(strcmp((const char *)cap.driver, "msm_vidc_driver") == 0);
+  assert(strcmp((const char *)cap.card, "msm_vidc_venc") == 0);
+
+  struct v4l2_format fmt_out = {
+    .type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE,
+    .fmt = {
+      .pix_mp = {
+        // downscales are free with v4l
+        .width = (unsigned int)out_width,
+        .height = (unsigned int)out_height,
+        .pixelformat = (codec == cereal::EncodeIndex::Type::FULL_H_E_V_C) ? V4L2_PIX_FMT_HEVC : V4L2_PIX_FMT_H264,
+        .field = V4L2_FIELD_ANY,
+        .colorspace = V4L2_COLORSPACE_DEFAULT,
+      }
+    }
+  };
+  checked_ioctl(fd, VIDIOC_S_FMT, &fmt_out);
+
+  v4l2_streamparm streamparm = {
+    .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+    .parm = {
+      .output = {
+        // TODO: more stuff here? we don't know
+        .timeperframe = {
+          .numerator = 1,
+          .denominator = 20
+        }
+      }
+    }
+  };
+  checked_ioctl(fd, VIDIOC_S_PARM, &streamparm);
+
+  struct v4l2_format fmt_in = {
+    .type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE,
+    .fmt = {
+      .pix_mp = {
+        .width = (unsigned int)in_width,
+        .height = (unsigned int)in_height,
+        .pixelformat = V4L2_PIX_FMT_NV12,
+        .field = V4L2_FIELD_ANY,
+        .colorspace = V4L2_COLORSPACE_470_SYSTEM_BG,
+      }
+    }
+  };
+  checked_ioctl(fd, VIDIOC_S_FMT, &fmt_in);
+
+  LOGD("in buffer size %d, out buffer size %d",
+    fmt_in.fmt.pix_mp.plane_fmt[0].sizeimage,
+    fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage);
+
+  // shared ctrls
+  {
+    struct v4l2_control ctrls[] = {
+      { .id = V4L2_CID_MPEG_VIDEO_HEADER_MODE, .value = V4L2_MPEG_VIDEO_HEADER_MODE_SEPARATE},
+      { .id = V4L2_CID_MPEG_VIDEO_BITRATE, .value = bitrate},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL, .value = V4L2_CID_MPEG_VIDC_VIDEO_RATE_CONTROL_VBR_CFR},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_PRIORITY, .value = V4L2_MPEG_VIDC_VIDEO_PRIORITY_REALTIME_DISABLE},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_IDR_PERIOD, .value = 1},
+    };
+    for (auto ctrl : ctrls) {
+      checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
+    }
+  }
+
+  if (codec == cereal::EncodeIndex::Type::FULL_H_E_V_C) {
+    struct v4l2_control ctrls[] = {
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_PROFILE, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_PROFILE_MAIN},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_HEVC_TIER_LEVEL, .value = V4L2_MPEG_VIDC_VIDEO_HEVC_LEVEL_HIGH_TIER_LEVEL_5},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 29},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0},
+    };
+    for (auto ctrl : ctrls) {
+      checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
+    }
+  } else {
+    struct v4l2_control ctrls[] = {
+      { .id = V4L2_CID_MPEG_VIDEO_H264_PROFILE, .value = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH},
+      { .id = V4L2_CID_MPEG_VIDEO_H264_LEVEL, .value = V4L2_MPEG_VIDEO_H264_LEVEL_UNKNOWN},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_P_FRAMES, .value = 14},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_NUM_B_FRAMES, .value = 0},
+      { .id = V4L2_CID_MPEG_VIDEO_H264_ENTROPY_MODE, .value = V4L2_MPEG_VIDEO_H264_ENTROPY_MODE_CABAC},
+      { .id = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL, .value = V4L2_CID_MPEG_VIDC_VIDEO_H264_CABAC_MODEL_0},
+      { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_MODE, .value = 0},
+      { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_ALPHA, .value = 0},
+      { .id = V4L2_CID_MPEG_VIDEO_H264_LOOP_FILTER_BETA, .value = 0},
+      { .id = V4L2_CID_MPEG_VIDEO_MULTI_SLICE_MODE, .value = 0},
+    };
+    for (auto ctrl : ctrls) {
+      checked_ioctl(fd, VIDIOC_S_CTRL, &ctrl);
+    }
+  }
+
+  // allocate buffers
+  request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, BUF_OUT_COUNT);
+  request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, BUF_IN_COUNT);
+
+  // start encoder
+  v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  checked_ioctl(fd, VIDIOC_STREAMON, &buf_type);
+  buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  checked_ioctl(fd, VIDIOC_STREAMON, &buf_type);
+
+  // queue up output buffers
+  for (unsigned int i = 0; i < BUF_OUT_COUNT; i++) {
+    buf_out[i].allocate(fmt_out.fmt.pix_mp.plane_fmt[0].sizeimage);
+    queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, i, &buf_out[i]);
+  }
+  // queue up input buffers
+  for (unsigned int i = 0; i < BUF_IN_COUNT; i++) {
+    free_buf_in.push(i);
+  }
+
+  publisher_init();
+}
+
+void V4LEncoder::encoder_open(const char* path) {
+  dequeue_handler_thread = std::thread(V4LEncoder::dequeue_handler, this);
+  writer_open(path);
+  this->is_open = true;
+  this->counter = 0;
+}
+
+int V4LEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) {
+  struct timeval timestamp {
+    .tv_sec = (long)(extra->timestamp_eof/1000000000),
+    .tv_usec = (long)((extra->timestamp_eof/1000) % 1000000),
+  };
+
+  // reserve buffer
+  int buffer_in = free_buf_in.pop();
+
+  // push buffer
+  extras.push(*extra);
+  //buf->sync(VISIONBUF_SYNC_TO_DEVICE);
+  queue_buffer(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, buffer_in, buf, timestamp);
+
+  return this->counter++;
+}
+
+void V4LEncoder::encoder_close() {
+  if (this->is_open) {
+    // pop all the frames before closing, then put the buffers back
+    for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.pop();
+    for (int i = 0; i < BUF_IN_COUNT; i++) free_buf_in.push(i);
+    // no frames, stop the encoder
+    struct v4l2_encoder_cmd encoder_cmd = { .cmd = V4L2_ENC_CMD_STOP };
+    checked_ioctl(fd, VIDIOC_ENCODER_CMD, &encoder_cmd);
+    // join waits for V4L2_QCOM_BUF_FLAG_EOS
+    dequeue_handler_thread.join();
+    assert(extras.empty());
+    writer_close();
+  }
+  this->is_open = false;
+}
+
+V4LEncoder::~V4LEncoder() {
+  encoder_close();
+  v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE;
+  checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type);
+  request_buffers(fd, V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE, 0);
+  buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE;
+  checked_ioctl(fd, VIDIOC_STREAMOFF, &buf_type);
+  request_buffers(fd, V4L2_BUF_TYPE_VIDEO_CAPTURE_MPLANE, 0);
+  close(fd);
+
+  for (int i = 0; i < BUF_OUT_COUNT; i++) {
+    if (buf_out[i].free() != 0) {
+      LOGE("Failed to free buffer");
+    }
+  }
+}

system/loggerd/encoder/v4l_encoder.h

@@ -0,0 +1,33 @@
+#pragma once
+
+#include "common/queue.h"
+#include "system/loggerd/encoder/encoder.h"
+
+#define BUF_IN_COUNT 7
+#define BUF_OUT_COUNT 6
+
+class V4LEncoder : public VideoEncoder {
+public:
+  V4LEncoder(const char* filename, CameraType type, int in_width, int in_height, int fps,
+             int bitrate, cereal::EncodeIndex::Type codec, int out_width, int out_height, bool write) :
+             VideoEncoder(filename, type, in_width, in_height, fps, bitrate, codec, out_width, out_height, write) { encoder_init(); }
+  ~V4LEncoder();
+  void encoder_init();
+  int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra);
+  void encoder_open(const char* path);
+  void encoder_close();
+private:
+  int fd;
+
+  bool is_open = false;
+  int segment_num = -1;
+  int counter = 0;
+
+  SafeQueue<VisionIpcBufExtra> extras;
+
+  static void dequeue_handler(V4LEncoder *e);
+  std::thread dequeue_handler_thread;
+
+  VisionBuf buf_out[BUF_OUT_COUNT];
+  SafeQueue<unsigned int> free_buf_in;
+};