camerad: IFE debayer support (#33720)

* ife is up

* split out cdm, bps needs this too

* straight to vipc buffer

* start reducing the diff

* support both

* disable for now

* cleanup

---------

Co-authored-by: Comma Device <device@comma.ai>

msgq_repo

@@ -1 +1 @@
-Subproject commit a3b51b311fa45b0f67eb087bb5c1ef2a79e6ed91
+Subproject commit 3e17f865bbd3bbabb3841227ed0774c8fb74efef

system/camerad/SConscript

@@ -3,7 +3,7 @@ Import('env', 'arch', 'messaging', 'common', 'gpucommon', 'visionipc')
 libs = ['pthread', common, 'jpeg', 'OpenCL', messaging, visionipc, gpucommon]
 
 camera_obj = env.Object(['cameras/camera_qcom2.cc', 'cameras/camera_common.cc', 'cameras/spectra.cc',
-                         'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
+                         'cameras/cdm.cc', 'sensors/ar0231.cc', 'sensors/ox03c10.cc', 'sensors/os04c10.cc'])
 env.Program('camerad', ['main.cc', camera_obj], LIBS=libs)
 
 if GetOption("extras") and arch == "x86_64":

system/camerad/cameras/camera_common.cc

@@ -7,7 +7,6 @@
 
 #include "common/clutil.h"
 #include "common/swaglog.h"
-#include "third_party/linux/include/msm_media_info.h"
 
 #include "system/camerad/cameras/spectra.h"
 
@@ -65,16 +64,13 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera *
 
   const SensorInfo *sensor = cam->sensor.get();
 
-  camera_bufs = std::make_unique<VisionBuf[]>(frame_buf_count);
+  is_raw = cam->is_raw;
   camera_bufs_raw = std::make_unique<VisionBuf[]>(frame_buf_count);
-  camera_bufs_metadata = std::make_unique<FrameMetadata[]>(frame_buf_count);
+  frame_metadata = std::make_unique<FrameMetadata[]>(frame_buf_count);
 
   // RAW + final frames from ISP
   const int raw_frame_size = (sensor->frame_height + sensor->extra_height) * sensor->frame_stride;
   for (int i = 0; i < frame_buf_count; i++) {
-    camera_bufs[i].allocate(cam->yuv_size);
-    camera_bufs[i].init_cl(device_id, context);
-
     camera_bufs_raw[i].allocate(raw_frame_size);
     camera_bufs_raw[i].init_cl(device_id, context);
   }
@@ -95,7 +91,6 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera *
 
 CameraBuf::~CameraBuf() {
   for (int i = 0; i < frame_buf_count; i++) {
-    camera_bufs[i].free();
     camera_bufs_raw[i].free();
   }
   if (imgproc) delete imgproc;
@@ -104,18 +99,22 @@ CameraBuf::~CameraBuf() {
 bool CameraBuf::acquire(int expo_time) {
   if (!safe_queue.try_pop(cur_buf_idx, 50)) return false;
 
-  if (camera_bufs_metadata[cur_buf_idx].frame_id == -1) {
+  if (frame_metadata[cur_buf_idx].frame_id == -1) {
     LOGE("no frame data? wtf");
     return false;
   }
 
-  cur_frame_data = camera_bufs_metadata[cur_buf_idx];
+  cur_frame_data = frame_metadata[cur_buf_idx];
-  cur_yuv_buf = vipc_server->get_buffer(stream_type);
   cur_camera_buf = &camera_bufs_raw[cur_buf_idx];
-
+  if (is_raw) {
-  double start_time = millis_since_boot();
+    cur_yuv_buf = vipc_server->get_buffer(stream_type);
-  imgproc->runKernel(camera_bufs_raw[cur_buf_idx].buf_cl, cur_yuv_buf->buf_cl, out_img_width, out_img_height, expo_time);
+
-  cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0;
+    double start_time = millis_since_boot();
+    imgproc->runKernel(camera_bufs_raw[cur_buf_idx].buf_cl, cur_yuv_buf->buf_cl, out_img_width, out_img_height, expo_time);
+    cur_frame_data.processing_time = (millis_since_boot() - start_time) / 1000.0;
+  } else {
+    cur_yuv_buf = vipc_server->get_buffer(stream_type, cur_buf_idx);
+  }
 
   VisionIpcBufExtra extra = {
     cur_frame_data.frame_id,

system/camerad/cameras/camera_common.h

@@ -24,20 +24,21 @@ class ImgProc;
 
 class CameraBuf {
 private:
-  VisionIpcServer *vipc_server;
   ImgProc *imgproc = nullptr;
-  VisionStreamType stream_type;
   int cur_buf_idx;
   SafeQueue<int> safe_queue;
   int frame_buf_count;
+  bool is_raw;
 
 public:
+  VisionIpcServer *vipc_server;
+  VisionStreamType stream_type;
+
   FrameMetadata cur_frame_data;
   VisionBuf *cur_yuv_buf;
   VisionBuf *cur_camera_buf;
-  std::unique_ptr<VisionBuf[]> camera_bufs;
   std::unique_ptr<VisionBuf[]> camera_bufs_raw;
-  std::unique_ptr<FrameMetadata[]> camera_bufs_metadata;
+  std::unique_ptr<FrameMetadata[]> frame_metadata;
   int out_img_width, out_img_height;
 
   CameraBuf() = default;

system/camerad/cameras/camera_qcom2.cc

@@ -50,7 +50,7 @@ public:
 
   float fl_pix = 0;
 
-  CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config) {};
+  CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config, true /*config.camera_num == 2*/) {};
   ~CameraState();
   void init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
   void update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain);
@@ -285,7 +285,7 @@ void camerad_thread() {
   std::vector<std::unique_ptr<CameraState>> cams;
   for (const auto &config : {ROAD_CAMERA_CONFIG, WIDE_ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG}) {
     auto cam = std::make_unique<CameraState>(&m, config);
-    cam->init(&v, device_id ,ctx);
+    cam->init(&v, device_id, ctx);
     cams.emplace_back(std::move(cam));
   }
 

system/camerad/cameras/cdm.cc

@@ -0,0 +1,33 @@
+#include "cdm.h"
+
+int write_cont(uint8_t *dst, uint32_t reg, std::vector<uint32_t> vals) {
+  struct cdm_regcontinuous_cmd *cmd = (struct cdm_regcontinuous_cmd*)dst;
+  cmd->cmd = CAM_CDM_CMD_REG_CONT;
+  cmd->count = vals.size();
+  cmd->offset = reg;
+  cmd->reserved0 = 0;
+  cmd->reserved1 = 0;
+
+  uint32_t *vd = (uint32_t*)(dst + sizeof(struct cdm_regcontinuous_cmd));
+  for (int i = 0; i < vals.size(); i++) {
+    *vd = vals[i];
+    vd++;
+  }
+
+  return sizeof(struct cdm_regcontinuous_cmd) + vals.size()*sizeof(uint32_t);
+}
+
+int write_random(uint8_t *dst, std::vector<uint32_t> vals) {
+  struct cdm_regrandom_cmd *cmd = (struct cdm_regrandom_cmd*)dst;
+  cmd->cmd = CAM_CDM_CMD_REG_RANDOM;
+  cmd->count = vals.size() / 2;
+  cmd->reserved = 0;
+
+  uint32_t *vd = (uint32_t*)(dst + sizeof(struct cdm_regrandom_cmd));
+  for (int i = 0; i < vals.size(); i++) {
+    *vd = vals[i];
+    vd++;
+  }
+
+  return sizeof(struct cdm_regrandom_cmd) + vals.size()*sizeof(uint32_t);
+}

system/camerad/cameras/cdm.h

@@ -1,3 +1,15 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <vector>
+#include <memory>
+
+// our helpers
+int write_random(uint8_t *dst, std::vector<uint32_t> vals);
+int write_cont(uint8_t *dst, uint32_t reg, std::vector<uint32_t> vals);
+
 // from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h}
 
 enum cam_cdm_command {

system/camerad/cameras/ife.h

@@ -0,0 +1,767 @@
+#include "cdm.h"
+
+int build_initial_config(uint8_t *dst) {
+  uint8_t *start = dst;
+
+  // constants, some kind of HW quirk?
+  dst += write_random(dst, {
+    0x2c, 0xffffffff,
+    0x30, 0xffffffff,
+    0x34, 0xffffffff,
+    0x38, 0xffffffff,
+    0x3c, 0xffffffff,
+  });
+
+  dst += write_cont(dst, 0x478, {
+    0x00000004,
+    0x004000c0,
+  });
+
+  dst += write_cont(dst, 0x488, {
+    0x00000000,
+    0x00000000,
+    0x00000f0f,
+  });
+
+  dst += write_cont(dst, 0x49c, {
+    0x00000001,
+  });
+
+  dst += write_cont(dst, 0xce4, {
+    0x00000000,
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x4dc, {
+    0x00000000,
+    0x04050b84,
+    0x13031a82,
+    0x22022981,
+    0x3100387f,
+    0x04010b80,
+    0x13001a80,
+    0x2200297f,
+    0x30ff387f,
+    0x04050b84,
+    0x13031a82,
+    0x22022981,
+    0x3100387f,
+    0x04010b80,
+    0x13001a80,
+    0x2200297f,
+    0x30ff387f,
+    0x04050b84,
+    0x13031a82,
+    0x22022981,
+    0x3100387f,
+    0x04010b80,
+    0x13001a80,
+    0x2200297f,
+    0x30ff387f,
+    0x04050b84,
+    0x13031a82,
+    0x22022981,
+    0x3100387f,
+    0x04010b80,
+    0x13001a80,
+    0x2200297f,
+    0x30ff387f,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 248
+    .length = 287
+    .reserved = 33
+    .cmd = 11
+    .addr = 0
+    .DMIAddr = 3108
+    .DMISel = 9
+  */
+
+  dst += write_cont(dst, 0x560, {
+    0x00000001,
+    0x04440444,
+    0x04450445,
+    0x04440444,
+    0x04450445,
+    0x000000ca,
+    0x0000009c,
+  });
+
+  dst += write_cont(dst, 0x5e8, {
+    0x06363005,
+  });
+
+  dst += write_cont(dst, 0x5f4, {
+    0x00000000,
+    0x00000000,
+    0x00000000,
+    0x00000000,
+    0x3b3839a0,
+    0x003f8040,
+    0x00000000,
+    0x00000000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00000009,
+    0x00400808,
+    0x00000044,
+    0x004000a0,
+    0x0a0d00a6,
+    0x0a0d00a6,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 392
+    .length = 255
+    .reserved = 33
+    .cmd = 10
+    .addr = 0
+    .DMIAddr = 3108
+    .DMISel = 12
+  */
+
+  dst += write_cont(dst, 0x6bc, {
+    0x0b3c0000,
+    0x00670067,
+    0xd3b1300c,
+    0x13b1300c,
+    0x00670067,
+    0xd3b1300c,
+    0x13b1300c,
+    0xec4e4000,
+    0x0100c003,
+    0xec4e4000,
+    0x0100c003,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 444
+    .length = 883
+    .reserved = 33
+    .cmd = 10
+    .addr = 0
+    .DMIAddr = 3108
+    .DMISel = 14
+  */
+  /* TODO
+  cdm_dmi_cmd_t 444
+    .length = 883
+    .reserved = 33
+    .cmd = 10
+    .addr = 0
+    .DMIAddr = 3108
+    .DMISel = 15
+  */
+
+  dst += write_cont(dst, 0x6fc, {
+    0x00bf0080,
+    0x00000106,
+    0x00000000,
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x6f8, {
+    0x00000100,
+  });
+
+  dst += write_cont(dst, 0x71c, {
+    0x00008000,
+    0x08000066,
+  });
+
+  dst += write_cont(dst, 0x760, {
+    0x00800080,
+    0x00000000,
+    0x00000000,
+    0x00000000,
+    0x00800080,
+    0x00000000,
+    0x00000000,
+    0x00000000,
+    0x00800080,
+    0x00000000,
+    0x00000000,
+    0x00000000,
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x794, {
+    0x00000000,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 568
+    .length = 511
+    .reserved = 33
+    .cmd = 11
+    .addr = 0
+    .DMIAddr = 3108
+    .DMISel = 24
+  */
+
+  dst += write_cont(dst, 0x798, {
+    0x00000000,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 580
+    .length = 255
+    .reserved = 33
+    .cmd = 10
+    .addr = 0
+    .DMIAddr = 3108
+    .DMISel = 26
+  */
+  /* TODO
+  cdm_dmi_cmd_t 580
+    .length = 255
+    .reserved = 33
+    .cmd = 10
+    .addr = 0
+    .DMIAddr = 3108
+    .DMISel = 28
+  */
+  /* TODO
+  cdm_dmi_cmd_t 580
+    .length = 255
+    .reserved = 33
+    .cmd = 10
+    .addr = 0
+    .DMIAddr = 3108
+    .DMISel = 30
+  */
+
+  dst += write_cont(dst, 0xf30, {
+    0x00750259,
+    0x00000132,
+    0x00000000,
+    0x03ff0000,
+    0x01fe1eae,
+    0x00001f54,
+    0x02000000,
+    0x03ff0000,
+    0x1fad1e55,
+    0x000001fe,
+    0x02000000,
+    0x03ff0000,
+  });
+
+  dst += write_cont(dst, 0xa3c, {
+    0x00000003,
+    0x07870787,
+    0x30036666,
+    0x00000000,
+    0x00000000,
+    0x00000787,
+    0x04b704b7,
+    0x30036666,
+    0x00000000,
+    0x00000000,
+    0x000004b7,
+  });
+
+  dst += write_cont(dst, 0xa68, {
+    0x00000003,
+    0x03c30787,
+    0x3006cccc,
+    0x00000000,
+    0x00000000,
+    0x00000787,
+    0x025b04b7,
+    0x3006cccc,
+    0x00000000,
+    0x00000000,
+    0x00000787,
+  });
+
+
+  dst += write_cont(dst, 0xe10, {
+    0x000004b7,
+    0x00000787,
+  });
+
+  dst += write_cont(dst, 0xe30, {
+    0x0000025b,
+    0x00000787,
+  });
+
+  dst += write_cont(dst, 0xe18, {
+    0x0ff00000,
+    0x00000016,
+  });
+
+  dst += write_cont(dst, 0xe38, {
+    0x0ff00000,
+    0x00000017,
+  });
+
+  dst += write_cont(dst, 0xd84, {
+    0x000004b7,
+    0x00000787,
+  });
+
+  dst += write_cont(dst, 0xda4, {
+    0x000004b7,
+    0x00000787,
+  });
+
+  dst += write_cont(dst, 0xd60, {
+    0x04380300,
+    0x09016c7d,
+    0x021c0300,
+  });
+
+  dst += write_cont(dst, 0xd98, {
+    0x0ff00000,
+    0x00000016,
+  });
+
+  dst += write_cont(dst, 0xdb8, {
+    0x0ff00000,
+    0x00000017,
+  });
+
+  dst += write_cont(dst, 0xd6c, {
+    0x00000300,
+  });
+
+  dst += write_cont(dst, 0xd70, {
+    0x010e0f00,
+    0x09016c7d,
+    0x00870f00,
+  });
+
+  dst += write_cont(dst, 0xd7c, {
+    0x00000f00,
+  });
+
+  dst += write_cont(dst, 0x40, {
+    0x00000586,
+  });
+
+  dst += write_cont(dst, 0x48, {
+    0x0000000e,
+  });
+
+  dst += write_cont(dst, 0x4c, {
+    0x00000019,
+  });
+
+  dst += write_cont(dst, 0xe4c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xe6c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xe0c, {
+    0x00000e00,
+  });
+
+  dst += write_cont(dst, 0xe2c, {
+    0x00000e00,
+  });
+
+  dst += write_cont(dst, 0xd8c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdac, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdcc, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdec, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x44, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xaac, {
+    0x00000040,
+  });
+
+  dst += write_cont(dst, 0xf00, {
+    0x00000000,
+  });
+
+  //hexdump(start, dst - start);
+  return dst - start;
+}
+
+int build_first_update(uint8_t *dst) {
+  uint8_t *start = dst;
+
+  dst += write_random(dst, {
+    0x2c, 0xffffffff,
+    0x30, 0xffffffff,
+    0x34, 0xffffffff,
+    0x38, 0xffffffff,
+    0x3c, 0xffffffff,
+  });
+
+  dst += write_cont(dst, 0x4dc, {
+    0x00000001,
+    0x04050b84,
+    0x13031a82,
+    0x22022981,
+    0x3100387f,
+    0x04010b80,
+    0x13001a80,
+    0x2200297f,
+    0x30ff387f,
+    0x04050b84,
+    0x13031a82,
+    0x22022981,
+    0x3100387f,
+    0x04010b80,
+    0x13001a80,
+    0x2200297f,
+    0x30ff387f,
+    0x04050b84,
+    0x13031a82,
+    0x22022981,
+    0x3100387f,
+    0x04010b80,
+    0x13001a80,
+    0x2200297f,
+    0x30ff387f,
+    0x04050b84,
+    0x13031a82,
+    0x22022981,
+    0x3100387f,
+    0x04010b80,
+    0x13001a80,
+    0x2200297f,
+    0x30ff387f,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 184
+    .length = 287
+    .reserved = 33
+    .cmd = 11
+    .addr = 832
+    .DMIAddr = 3108
+    .DMISel = 10
+  */
+
+  dst += write_cont(dst, 0x560, {
+    0x00000001,
+    0x04440444,
+    0x04450445,
+    0x04440444,
+    0x04450445,
+    0x000000ca,
+    0x0000009c,
+  });
+
+  dst += write_cont(dst, 0x5c4, {
+    0x00000000,
+    0x00001000,
+    0x00001000,
+    0x00001000,
+    0x00001000,
+    0x00800080,
+    0x00802040,
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x5e8, {
+    0x06363007,
+  });
+
+  dst += write_cont(dst, 0x5f4, {
+    0x00000000,
+    0x00000000,
+    0x00000000,
+    0x00000000,
+    0x3b3839a0,
+    0x003f8040,
+    0x00000000,
+    0x00000000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00078000,
+    0x00000009,
+    0x00400808,
+    0x00000044,
+    0x004000a0,
+    0x0a0d00a6,
+    0x0a0d00a6,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 368
+    .length = 255
+    .reserved = 33
+    .cmd = 10
+    .addr = 5344
+    .DMIAddr = 3108
+    .DMISel = 13
+  */
+
+  dst += write_cont(dst, 0x6fc, {
+    0x00800080,
+    0x00000080,
+    0x00000000,
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x6f8, {
+    0x00000100,
+  });
+
+  dst += write_cont(dst, 0x71c, {
+    0x00008000,
+    0x08000066,
+  });
+
+  dst += write_cont(dst, 0x794, {
+    0x00000001,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 432
+    .length = 511
+    .reserved = 33
+    .cmd = 11
+    .addr = 832
+    .DMIAddr = 3108
+    .DMISel = 25
+  */
+
+  dst += write_cont(dst, 0x798, {
+    0x00000007,
+  });
+  /* TODO
+  cdm_dmi_cmd_t 444
+    .length = 255
+    .reserved = 33
+    .cmd = 10
+    .addr = 5344
+    .DMIAddr = 3108
+    .DMISel = 27
+  */
+  /* TODO
+  cdm_dmi_cmd_t 444
+    .length = 255
+    .reserved = 33
+    .cmd = 10
+    .addr = 5344
+    .DMIAddr = 3108
+    .DMISel = 29
+  */
+  /* TODO
+  cdm_dmi_cmd_t 444
+    .length = 255
+    .reserved = 33
+    .cmd = 10
+    .addr = 5344
+    .DMIAddr = 3108
+    .DMISel = 31
+  */
+
+  dst += write_cont(dst, 0xd84, {
+    0x000004b7,
+    0x00000787,
+  });
+
+  dst += write_cont(dst, 0xda4, {
+    0x000004b7,
+    0x00000787,
+  });
+
+  dst += write_cont(dst, 0xd6c, {
+    0x00000300,
+  });
+
+  dst += write_cont(dst, 0xd70, {
+    0x02640f00,
+    0x09016c7d,
+    0x01320f00,
+  });
+
+  dst += write_cont(dst, 0xd7c, {
+    0x00000f00,
+  });
+
+  dst += write_cont(dst, 0x40, {
+    0x00000444,
+  });
+
+  dst += write_cont(dst, 0x48, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x4c, {
+    0x00000019,
+  });
+
+  dst += write_cont(dst, 0xe4c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xe6c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xe0c, {
+    0x00000e00,
+  });
+
+  dst += write_cont(dst, 0xe2c, {
+    0x00000e00,
+  });
+
+  dst += write_cont(dst, 0xd8c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdac, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdcc, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdec, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x44, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xaac, {
+    0x00000040,
+  });
+
+  dst += write_cont(dst, 0xf00, {
+    0x00000000,
+  });
+
+  return dst - start;
+}
+
+int build_update(uint8_t *dst) {
+  uint8_t *start = dst;
+
+  dst += write_random(dst, {
+    0x2c, 0xffffffff,
+    0x30, 0xffffffff,
+    0x34, 0xffffffff,
+    0x38, 0xffffffff,
+    0x3c, 0xffffffff,
+  });
+
+  dst += write_cont(dst, 0x560, {
+    0x00000001,
+    0x04440444,
+    0x04450445,
+    0x04440444,
+    0x04450445,
+    0x000000ca,
+    0x0000009c,
+  });
+
+  dst += write_cont(dst, 0x6fc, {
+    0x00800080,
+    0x00000080,
+    0x00000000,
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xd84, {
+    0x000004b7,
+    0x00000787,
+  });
+
+  dst += write_cont(dst, 0xda4, {
+    0x000004b7,
+    0x00000787,
+  });
+
+  dst += write_cont(dst, 0xd6c, {
+    0x00000300,
+  });
+
+  dst += write_cont(dst, 0xd70, {
+    0x02640f00,
+    0x09016c7d,
+    0x01320f00,
+  });
+
+  dst += write_cont(dst, 0xd7c, {
+    0x00000f00,
+  });
+
+  dst += write_cont(dst, 0x40, {
+    0x00000444,
+  });
+
+  dst += write_cont(dst, 0x48, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x4c, {
+    0x00000019,
+  });
+
+  dst += write_cont(dst, 0xe4c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xe6c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xe0c, {
+    0x00000e00,
+  });
+
+  dst += write_cont(dst, 0xe2c, {
+    0x00000e00,
+  });
+
+  dst += write_cont(dst, 0xd8c, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdac, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdcc, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xdec, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0x44, {
+    0x00000000,
+  });
+
+  dst += write_cont(dst, 0xaac, {
+    0x00000040,
+  });
+
+  dst += write_cont(dst, 0xf00, {
+    0x00000000,
+  });
+
+  return dst - start;
+}

system/camerad/cameras/spectra.cc

@@ -15,28 +15,13 @@
 
 #include "common/util.h"
 #include "common/swaglog.h"
+#include "system/camerad/cameras/ife.h"
 #include "system/camerad/cameras/spectra.h"
+#include "third_party/linux/include/msm_media_info.h"
 
 // For debugging:
 // echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl
 
-int write_cont(uint8_t *dst, uint32_t reg, std::vector<uint32_t> vals) {
-  struct cdm_regcontinuous_cmd *cmd = (struct cdm_regcontinuous_cmd*)dst;
-  cmd->cmd = CAM_CDM_CMD_REG_CONT;
-  cmd->count = vals.size();
-  cmd->offset = reg;
-  cmd->reserved0 = 0;
-  cmd->reserved1 = 0;
-
-  uint32_t *vd = (uint32_t*)(dst + sizeof(struct cdm_regcontinuous_cmd));
-  for (int i = 0; i < vals.size(); i++) {
-    *vd = vals[i];
-    vd++;
-  }
-
-  return sizeof(struct cdm_regcontinuous_cmd) + vals.size()*sizeof(uint32_t);
-}
-
 // ************** low level camera helpers ****************
 
 int do_cam_control(int fd, int op_code, void *handle, int size) {
@@ -251,10 +236,11 @@ void SpectraMaster::init() {
 
 // *** SpectraCamera ***
 
-SpectraCamera::SpectraCamera(SpectraMaster *master, const CameraConfig &config)
+SpectraCamera::SpectraCamera(SpectraMaster *master, const CameraConfig &config, bool raw)
   : m(master),
-    enabled(config.enabled) ,
+    enabled(config.enabled),
-    cc(config) {
+    cc(config),
+    is_raw(raw) {
   mm.init(m->video0_fd);
 }
 
@@ -288,9 +274,12 @@ void SpectraCamera::camera_open(VisionIpcServer *v, cl_device_id device_id, cl_c
   uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, sensor->frame_height);
   uv_offset = stride*y_height;
   yuv_size = uv_offset + stride*uv_height;
-  // TODO: for when the frames are coming out of the IFE directly
+  if (!is_raw) {
-  //uv_offset = ALIGNED_SIZE(stride*y_height, 0x1000);
+    uv_offset = ALIGNED_SIZE(uv_offset, 0x1000);
-  //yuv_size = uv_offset + ALIGNED_SIZE(stride*uv_height, 0x1000);
+    yuv_size = uv_offset + ALIGNED_SIZE(stride*uv_height, 0x1000);
+  }
+  assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, sensor->frame_width));
+  assert(y_height/2 == uv_height);
 
   open = true;
   configISP();
@@ -490,19 +479,12 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
   }
   pkt->header.size = size;
 
-  // *** kmd cmd buf ***
-  {
-    pkt->kmd_cmd_buf_index = 0;
-    pkt->kmd_cmd_buf_offset = 0;
-  }
-
   // *** cmd buf ***
   {
     struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
     pkt->num_cmd_buf = 2;
 
     // *** first command ***
-    // TODO: support MMU
     buf_desc[0].size = ife_cmd.size;
     buf_desc[0].length = 0;
     buf_desc[0].type = CAM_CMD_BUF_DIRECT;
@@ -510,6 +492,20 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     buf_desc[0].mem_handle = ife_cmd.handle;
     buf_desc[0].offset = ife_cmd.aligned_size()*idx;
 
+    // stream of IFE register writes
+    if (!is_raw) {
+      if (init) {
+        buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset);
+      } else if (request_id == 1) {
+        buf_desc[0].length = build_first_update((unsigned char*)ife_cmd.ptr + buf_desc[0].offset);
+      } else {
+        buf_desc[0].length = build_update((unsigned char*)ife_cmd.ptr + buf_desc[0].offset);
+      }
+    }
+
+    pkt->kmd_cmd_buf_offset = buf_desc[0].length;
+    pkt->kmd_cmd_buf_index = 0;
+
     // *** second command ***
     // parsed by cam_isp_packet_generic_blob_handler
     struct isp_packet {
@@ -530,14 +526,15 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8;
     static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20);
     tmp.resource_hfr = {
-      .num_ports = 1,  // 10 for YUV (but I don't think we need them)
+      .num_ports = 1,
       .port_hfr_config[0] = {
-        .resource_type = CAM_ISP_IFE_OUT_RES_RDI_0, // CAM_ISP_IFE_OUT_RES_FULL for YUV
+        .resource_type = static_cast<uint32_t>(is_raw ? CAM_ISP_IFE_OUT_RES_RDI_0 : CAM_ISP_IFE_OUT_RES_FULL),
         .subsample_pattern = 1,
         .subsample_period = 0,
         .framedrop_pattern = 1,
         .framedrop_period = 0,
-      }};
+      }
+    };
 
     tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG;
     tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8;
@@ -586,27 +583,54 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf;
 
     struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset);
-    io_cfg[0].offsets[0] = 0;
-    io_cfg[0].mem_handle[0] = buf_handle_raw[idx];
 
-    io_cfg[0].planes[0] = (struct cam_plane_cfg){
+    if (is_raw) {
-      .width = sensor->frame_width,
+      io_cfg[0].mem_handle[0] = buf_handle_raw[idx];
-      .height = sensor->frame_height + sensor->extra_height,
+      io_cfg[0].planes[0] = (struct cam_plane_cfg){
-      .plane_stride = sensor->frame_stride,
+        .width = sensor->frame_width,
-      .slice_height = sensor->frame_height + sensor->extra_height,
+        .height = sensor->frame_height,
-    };
+        .plane_stride = stride,
-    io_cfg[0].format = sensor->mipi_format;
+        .slice_height = y_height,
-    io_cfg[0].color_space = CAM_COLOR_SPACE_BASE;
+      };
-    io_cfg[0].color_pattern = 0x5;
+      io_cfg[0].format = sensor->mipi_format;
-    io_cfg[0].bpp = (sensor->mipi_format == CAM_FORMAT_MIPI_RAW_10 ? 0xa : 0xc);
+      io_cfg[0].color_space = CAM_COLOR_SPACE_BASE;
-    io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;
+      io_cfg[0].color_pattern = 0x5;
-    io_cfg[0].fence = sync_objs[idx];
+      io_cfg[0].bpp = (sensor->mipi_format == CAM_FORMAT_MIPI_RAW_10 ? 0xa : 0xc);
-    io_cfg[0].direction = CAM_BUF_OUTPUT;
+      io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;
-    io_cfg[0].subsample_pattern = 0x1;
+      io_cfg[0].fence = sync_objs[idx];
-    io_cfg[0].framedrop_pattern = 0x1;
+      io_cfg[0].direction = CAM_BUF_OUTPUT;
+      io_cfg[0].subsample_pattern = 0x1;
+      io_cfg[0].framedrop_pattern = 0x1;
+    } else {
+      io_cfg[0].mem_handle[0] = buf_handle_yuv[idx];
+      io_cfg[0].mem_handle[1] = buf_handle_yuv[idx];
+      io_cfg[0].planes[0] = (struct cam_plane_cfg){
+        .width = sensor->frame_width,
+        .height = sensor->frame_height,
+        .plane_stride = stride,
+        .slice_height = y_height,
+      };
+      io_cfg[0].planes[1] = (struct cam_plane_cfg){
+        .width = sensor->frame_width,
+        .height = sensor->frame_height/2,
+        .plane_stride = stride,
+        .slice_height = uv_height,
+      };
+      io_cfg[0].offsets[1] = uv_offset;
+      io_cfg[0].format = CAM_FORMAT_NV12;
+      io_cfg[0].color_space = 0;
+      io_cfg[0].color_pattern = 0x0;
+      io_cfg[0].bpp = 0;
+      io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_FULL;
+      io_cfg[0].fence = sync_objs[idx];
+      io_cfg[0].direction = CAM_BUF_OUTPUT;
+      io_cfg[0].subsample_pattern = 0x1;
+      io_cfg[0].framedrop_pattern = 0x1;
+    }
   }
 
   // *** patches ***
+  // sets up the kernel driver to do address translation for the IFE
   {
     pkt->num_patches = 0;
     pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs;
@@ -631,7 +655,7 @@ void SpectraCamera::enqueue_buffer(int i, bool dp) {
       // TODO: handle frame drop cleanly
     }
 
-    buf.camera_bufs_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
+    buf.frame_metadata[i].timestamp_eof = (uint64_t)nanos_since_boot(); // set true eof
     if (dp) buf.queue(i);
 
     // destroy old output fence
@@ -699,11 +723,13 @@ void SpectraCamera::camera_map_bufs() {
     LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs_raw[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
     buf_handle_raw[i] = mem_mgr_map_cmd.out.buf_handle;
 
+    // TODO: this needs to match camera bufs length
     // final processed images
-    mem_mgr_map_cmd.fd = buf.camera_bufs[i].fd;
+    VisionBuf *vb = buf.vipc_server->get_buffer(buf.stream_type, i);
+    mem_mgr_map_cmd.fd = vb->fd;
     ret = do_cam_control(m->video0_fd, CAM_REQ_MGR_MAP_BUF, &mem_mgr_map_cmd, sizeof(mem_mgr_map_cmd));
-    LOGD("map buf req: (fd: %d) 0x%x %d", buf.camera_bufs_raw[i].fd, mem_mgr_map_cmd.out.buf_handle, ret);
+    LOGD("map buf req: (fd: %d) 0x%x %d", vb->fd, mem_mgr_map_cmd.out.buf_handle, ret);
-    buf_handle[i] = mem_mgr_map_cmd.out.buf_handle;
+    buf_handle_yuv[i] = mem_mgr_map_cmd.out.buf_handle;
   }
   enqueue_req_multi(1, FRAME_BUF_COUNT, 0);
 }
@@ -786,13 +812,19 @@ void SpectraCamera::configISP() {
     // ISP outputs
     .num_out_res = 0x1,
     .data[0] = (struct cam_isp_out_port_info){
-      .res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
+      .res_type = CAM_ISP_IFE_OUT_RES_FULL,
-      .format = sensor->mipi_format,
+      .format = CAM_FORMAT_NV12,
       .width = sensor->frame_width,
       .height = sensor->frame_height + sensor->extra_height,
       .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
     },
   };
+
+  if (is_raw) {
+    in_port_info.data[0].res_type = CAM_ISP_IFE_OUT_RES_RDI_0;
+    in_port_info.data[0].format = sensor->mipi_format;
+  }
+
   struct cam_isp_resource isp_resource = {
     .resource_id = CAM_ISP_RES_ID_PORT,
     .handle_type = CAM_HANDLE_USER_POINTER,
@@ -968,7 +1000,8 @@ void SpectraCamera::camera_close() {
     LOGD("release csiphy: %d", ret);
 
     for (int i = 0; i < FRAME_BUF_COUNT; i++) {
-      release(m->video0_fd, buf_handle[i]);
+      release(m->video0_fd, buf_handle_yuv[i]);
+      release(m->video0_fd, buf_handle_raw[i]);
     }
     LOGD("released buffers");
   }
@@ -1013,7 +1046,7 @@ void SpectraCamera::handle_camera_event(const cam_req_mgr_message *event_data) {
     frame_id_last = main_id;
     request_id_last = real_id;
 
-    auto &meta_data = buf.camera_bufs_metadata[buf_idx];
+    auto &meta_data = buf.frame_metadata[buf_idx];
     meta_data.frame_id = main_id - idx_offset;
     meta_data.request_id = real_id;
     meta_data.timestamp_sof = timestamp;

system/camerad/cameras/spectra.h

@@ -88,7 +88,7 @@ public:
 
 class SpectraCamera {
 public:
-  SpectraCamera(SpectraMaster *master, const CameraConfig &config);
+  SpectraCamera(SpectraMaster *master, const CameraConfig &config, bool raw);
   ~SpectraCamera();
 
   void camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
@@ -147,8 +147,7 @@ public:
   SpectraBuf bps_iq;
   SpectraBuf bps_striping;
 
-  int buf0_handle = 0;
+  int buf_handle_yuv[FRAME_BUF_COUNT] = {};
-  int buf_handle[FRAME_BUF_COUNT] = {};
   int buf_handle_raw[FRAME_BUF_COUNT] = {};
   int sync_objs[FRAME_BUF_COUNT] = {};
   int sync_objs_bps_out[FRAME_BUF_COUNT] = {};
@@ -158,6 +157,8 @@ public:
   uint64_t idx_offset = 0;
   bool skipped = true;
 
+  bool is_raw;
+
   CameraBuf buf;
   MemoryManager mm;
   SpectraMaster *m;

system/camerad/test/debug.sh

@@ -10,7 +10,7 @@ echo 0 | sudo tee /sys/module/cam_debug_util/parameters/debug_mdl
 sudo dmesg -C
 scons -u -j8 --minimal .
 export DEBUG_FRAMES=1
-export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1
+#export DISABLE_ROAD=1 DISABLE_WIDE_ROAD=1
-#export DISABLE_DRIVER=1
+export DISABLE_DRIVER=1
 #export LOGPRINT=debug
 ./camerad