IQ.Pilot Release Commit @ 0798119

system/camerad/cameras/camera_common.cc

@@ -0,0 +1,110 @@
+#include "system/camerad/cameras/camera_common.h"
+
+#include <cassert>
+#include <string>
+
+#include "common/swaglog.h"
+#include "system/camerad/cameras/spectra.h"
+
+
+void CameraBuf::init(cl_device_id device_id, cl_context context, SpectraCamera *cam, VisionIpcServer * v, int frame_cnt, VisionStreamType type) {
+  vipc_server = v;
+  stream_type = type;
+  frame_buf_count = frame_cnt;
+
+  const SensorInfo *sensor = cam->sensor.get();
+
+  // RAW frames from ISP
+  if (cam->cc.output_type != ISP_IFE_PROCESSED) {
+    camera_bufs_raw = std::make_unique<VisionBuf[]>(frame_buf_count);
+
+    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_raw[i].allocate(raw_frame_size);
+      camera_bufs_raw[i].init_cl(device_id, context);
+    }
+    LOGD("allocated %d CL buffers", frame_buf_count);
+  }
+
+  vipc_server->create_buffers_with_sizes(stream_type, VIPC_BUFFER_COUNT, out_img_width, out_img_height, cam->yuv_size, cam->stride, cam->uv_offset);
+  LOGD("created %d YUV vipc buffers with size %dx%d", VIPC_BUFFER_COUNT, cam->stride, cam->y_height);
+}
+
+CameraBuf::~CameraBuf() {
+  if (camera_bufs_raw != nullptr) {
+    for (int i = 0; i < frame_buf_count; i++) {
+      camera_bufs_raw[i].free();
+    }
+  }
+}
+
+void CameraBuf::sendFrameToVipc() {
+  assert(cur_buf_idx >=0 && cur_buf_idx < frame_buf_count);
+
+  if (camera_bufs_raw) {
+    cur_camera_buf = &camera_bufs_raw[cur_buf_idx];
+  }
+
+  cur_yuv_buf = vipc_server->get_buffer(stream_type, cur_buf_idx);
+
+  VisionIpcBufExtra extra = {
+    cur_frame_data.frame_id,
+    cur_frame_data.timestamp_sof,
+    cur_frame_data.timestamp_eof,
+  };
+  cur_yuv_buf->set_frame_id(cur_frame_data.frame_id);
+  vipc_server->send(cur_yuv_buf, &extra);
+}
+
+// common functions
+
+kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b) {
+  const uint8_t *dat = (const uint8_t *)b->cur_camera_buf->addr;
+
+  kj::Array<uint8_t> frame_image = kj::heapArray<uint8_t>(b->cur_camera_buf->len);
+  uint8_t *resized_dat = frame_image.begin();
+
+  memcpy(resized_dat, dat, b->cur_camera_buf->len);
+
+  return kj::mv(frame_image);
+}
+
+float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip) {
+  int lum_med;
+  uint32_t lum_binning[256] = {0};
+  const uint8_t *pix_ptr = b->cur_yuv_buf->y;
+
+  unsigned int lum_total = 0;
+  for (int y = ae_xywh.y; y < ae_xywh.y + ae_xywh.h; y += y_skip) {
+    for (int x = ae_xywh.x; x < ae_xywh.x + ae_xywh.w; x += x_skip) {
+      uint8_t lum = pix_ptr[(y * b->out_img_width) + x];
+      lum_binning[lum]++;
+      lum_total += 1;
+    }
+  }
+
+  // Find mean lumimance value
+  unsigned int lum_cur = 0;
+  for (lum_med = 255; lum_med >= 0; lum_med--) {
+    lum_cur += lum_binning[lum_med];
+
+    if (lum_cur >= lum_total / 2) {
+      break;
+    }
+  }
+
+  return lum_med / 256.0;
+}
+
+int open_v4l_by_name_and_index(const char name[], int index, int flags) {
+  for (int v4l_index = 0; /**/; ++v4l_index) {
+    std::string v4l_name = util::read_file(util::string_format("/sys/class/video4linux/v4l-subdev%d/name", v4l_index));
+    if (v4l_name.empty()) return -1;
+    if (v4l_name.find(name) == 0) {
+      if (index == 0) {
+        return HANDLE_EINTR(open(util::string_format("/dev/v4l-subdev%d", v4l_index).c_str(), flags));
+      }
+      index--;
+    }
+  }
+}

system/camerad/cameras/camera_common.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include <memory>
+
+#include "cereal/messaging/messaging.h"
+#include "msgq/visionipc/visionipc_server.h"
+#include "common/util.h"
+
+
+const int VIPC_BUFFER_COUNT = 18;
+
+typedef struct FrameMetadata {
+  uint32_t frame_id;
+  uint32_t request_id;
+  uint64_t timestamp_sof;
+  uint64_t timestamp_eof;
+  float processing_time;
+} FrameMetadata;
+
+class SpectraCamera;
+
+class CameraBuf {
+private:
+  int frame_buf_count;
+
+public:
+  VisionIpcServer *vipc_server;
+  VisionStreamType stream_type;
+
+  int cur_buf_idx;
+  FrameMetadata cur_frame_data;
+  VisionBuf *cur_yuv_buf;
+  VisionBuf *cur_camera_buf;
+  std::unique_ptr<VisionBuf[]> camera_bufs_raw;
+  uint32_t out_img_width, out_img_height;
+
+  CameraBuf() = default;
+  ~CameraBuf();
+  void init(cl_device_id device_id, cl_context context, SpectraCamera *cam, VisionIpcServer * v, int frame_cnt, VisionStreamType type);
+  void sendFrameToVipc();
+};
+
+void camerad_thread();
+kj::Array<uint8_t> get_raw_frame_image(const CameraBuf *b);
+float calculate_exposure_value(const CameraBuf *b, Rect ae_xywh, int x_skip, int y_skip);
+int open_v4l_by_name_and_index(const char name[], int index = 0, int flags = O_RDWR | O_NONBLOCK);

system/camerad/cameras/camera_qcom2.cc

@@ -0,0 +1,323 @@
+#include "system/camerad/cameras/camera_common.h"
+#include "system/camerad/cameras/spectra.h"
+
+#include <poll.h>
+#include <sys/ioctl.h>
+
+#include <algorithm>
+#include <cassert>
+#include <cerrno>
+#include <cmath>
+#include <cstring>
+#include <string>
+#include <vector>
+
+#ifdef __TICI__
+#include "CL/cl_ext_qcom.h"
+#else
+#define CL_PRIORITY_HINT_HIGH_QCOM NULL
+#define CL_CONTEXT_PRIORITY_HINT_QCOM NULL
+#endif
+
+#include "media/cam_sensor_cmn_header.h"
+
+#include "common/clutil.h"
+#include "common/params.h"
+#include "common/swaglog.h"
+
+
+ExitHandler do_exit;
+
+// for debugging
+const bool env_debug_frames = getenv("DEBUG_FRAMES") != nullptr;
+const bool env_log_raw_frames = getenv("LOG_RAW_FRAMES") != nullptr;
+const bool env_ctrl_exp_from_params = getenv("CTRL_EXP_FROM_PARAMS") != nullptr;
+
+
+class CameraState {
+public:
+  SpectraCamera camera;
+  int exposure_time = 5;
+  bool dc_gain_enabled = false;
+  int dc_gain_weight = 0;
+  int gain_idx = 0;
+  float analog_gain_frac = 0;
+
+  float cur_ev[3] = {};
+  float best_ev_score = 0;
+  int new_exp_g = 0;
+  int new_exp_t = 0;
+
+  Rect ae_xywh = {};
+  float measured_grey_fraction = 0;
+  float target_grey_fraction = 0.125;
+
+  float fl_pix = 0;
+  std::unique_ptr<PubMaster> pm;
+
+  CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config) {};
+  ~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);
+  void set_camera_exposure(float grey_frac);
+  void set_exposure_rect();
+  void sendState();
+
+  float get_gain_factor() const {
+    return (1 + dc_gain_weight * (camera.sensor->dc_gain_factor-1) / camera.sensor->dc_gain_max_weight);
+  }
+};
+
+void CameraState::init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx) {
+  camera.camera_open(v, device_id, ctx);
+
+  if (!camera.enabled) return;
+
+  fl_pix = camera.cc.focal_len / camera.sensor->pixel_size_mm / camera.sensor->out_scale;
+  set_exposure_rect();
+
+  dc_gain_weight = camera.sensor->dc_gain_min_weight;
+  gain_idx = camera.sensor->analog_gain_rec_idx;
+  cur_ev[0] = cur_ev[1] = cur_ev[2] = get_gain_factor() * camera.sensor->sensor_analog_gains[gain_idx] * exposure_time;
+
+  pm = std::make_unique<PubMaster>(std::vector{camera.cc.publish_name});
+}
+
+CameraState::~CameraState() {}
+
+void CameraState::set_exposure_rect() {
+  // set areas for each camera, shouldn't be changed
+  std::vector<std::pair<Rect, float>> ae_targets = {
+    // (Rect, F)
+    std::make_pair((Rect){96, 400, 1734, 524}, 567.0),  // wide
+    std::make_pair((Rect){96, 160, 1734, 986}, 2648.0), // road
+    std::make_pair((Rect){96, 242, 1736, 906}, 567.0)   // driver
+  };
+  int h_ref = 1208;
+  /*
+    exposure target intrinsics is
+    [
+      [F, 0, 0.5*ae_xywh[2]]
+      [0, F, 0.5*H-ae_xywh[1]]
+      [0, 0, 1]
+    ]
+  */
+  auto ae_target = ae_targets[camera.cc.camera_num];
+  Rect xywh_ref = ae_target.first;
+  float fl_ref = ae_target.second;
+
+  ae_xywh = (Rect){
+    std::max(0, (int)camera.buf.out_img_width / 2 - (int)(fl_pix / fl_ref * xywh_ref.w / 2)),
+    std::max(0, (int)camera.buf.out_img_height / 2 - (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y))),
+    std::min((int)(fl_pix / fl_ref * xywh_ref.w), (int)camera.buf.out_img_width / 2 + (int)(fl_pix / fl_ref * xywh_ref.w / 2)),
+    std::min((int)(fl_pix / fl_ref * xywh_ref.h), (int)camera.buf.out_img_height / 2 + (int)(fl_pix / fl_ref * (h_ref / 2 - xywh_ref.y)))
+  };
+}
+
+void CameraState::update_exposure_score(float desired_ev, int exp_t, int exp_g_idx, float exp_gain) {
+  float score = camera.sensor->getExposureScore(desired_ev, exp_t, exp_g_idx, exp_gain, gain_idx);
+  if (score < best_ev_score) {
+    new_exp_t = exp_t;
+    new_exp_g = exp_g_idx;
+    best_ev_score = score;
+  }
+}
+
+void CameraState::set_camera_exposure(float grey_frac) {
+  if (!camera.enabled) return;
+  std::vector<double> target_grey_minimums = {0.1, 0.1, 0.125}; // wide, road, driver
+
+  const float dt = 0.05;
+
+  const float ts_grey = 10.0;
+  const float ts_ev = 0.05;
+
+  const float k_grey = (dt / ts_grey) / (1.0 + dt / ts_grey);
+  const float k_ev = (dt / ts_ev) / (1.0 + dt / ts_ev);
+
+  // It takes 3 frames for the commanded exposure settings to take effect. The first frame is already started by the time
+  // we reach this function, the other 2 are due to the register buffering in the sensor.
+  // Therefore we use the target EV from 3 frames ago, the grey fraction that was just measured was the result of that control action.
+  // TODO: Lower latency to 2 frames, by using the histogram outputted by the sensor we can do AE before the debayering is complete
+
+  const auto &sensor = camera.sensor;
+  // Offset idx by one to not get stuck in self loop
+  const float cur_ev_ = cur_ev[(camera.buf.cur_frame_data.frame_id - 1) % 3] * sensor->ev_scale;
+
+  // Scale target grey between min and 0.4 depending on lighting conditions
+  float new_target_grey = std::clamp(0.4 - 0.3 * log2(1.0 + sensor->target_grey_factor*cur_ev_) / log2(6000.0), target_grey_minimums[camera.cc.camera_num], 0.4);
+  float target_grey = (1.0 - k_grey) * target_grey_fraction + k_grey * new_target_grey;
+
+  float desired_ev = std::clamp(cur_ev_ / sensor->ev_scale * target_grey / grey_frac, sensor->min_ev, sensor->max_ev);
+  float k = (1.0 - k_ev) / 3.0;
+  desired_ev = (k * cur_ev[0]) + (k * cur_ev[1]) + (k * cur_ev[2]) + (k_ev * desired_ev);
+
+  best_ev_score = 1e6;
+  new_exp_g = 0;
+  new_exp_t = 0;
+
+  // Hysteresis around high conversion gain
+  // We usually want this on since it results in lower noise, but turn off in very bright day scenes
+  bool enable_dc_gain = dc_gain_enabled;
+  if (!enable_dc_gain && target_grey < sensor->dc_gain_on_grey) {
+    enable_dc_gain = true;
+    dc_gain_weight = sensor->dc_gain_min_weight;
+  } else if (enable_dc_gain && target_grey > sensor->dc_gain_off_grey) {
+    enable_dc_gain = false;
+    dc_gain_weight = sensor->dc_gain_max_weight;
+  }
+
+  if (enable_dc_gain && dc_gain_weight < sensor->dc_gain_max_weight) {dc_gain_weight += 1;}
+  if (!enable_dc_gain && dc_gain_weight > sensor->dc_gain_min_weight) {dc_gain_weight -= 1;}
+
+  std::string gain_bytes, time_bytes;
+  if (env_ctrl_exp_from_params) {
+    static Params params;
+    gain_bytes = params.get("CameraDebugExpGain");
+    time_bytes = params.get("CameraDebugExpTime");
+  }
+
+  if (gain_bytes.size() > 0 && time_bytes.size() > 0) {
+    // Override gain and exposure time
+    gain_idx = std::stoi(gain_bytes);
+    exposure_time = std::stoi(time_bytes);
+
+    new_exp_g = gain_idx;
+    new_exp_t = exposure_time;
+    enable_dc_gain = false;
+  } else {
+    // Simple brute force optimizer to choose sensor parameters to reach desired EV
+    int min_g = std::max(gain_idx - 1, sensor->analog_gain_min_idx);
+    int max_g = std::min(gain_idx + 1, sensor->analog_gain_max_idx);
+    for (int g = min_g; g <= max_g; g++) {
+      float gain = sensor->sensor_analog_gains[g] * get_gain_factor();
+
+      // Compute optimal time for given gain
+      int t = std::clamp(int(std::round(desired_ev / gain)), sensor->exposure_time_min, sensor->exposure_time_max);
+
+      // Only go below recommended gain when absolutely necessary to not overexpose
+      if (g < sensor->analog_gain_rec_idx && t > 20 && g < gain_idx) {
+        continue;
+      }
+
+      update_exposure_score(desired_ev, t, g, gain);
+    }
+  }
+
+  measured_grey_fraction = grey_frac;
+  target_grey_fraction = target_grey;
+
+  analog_gain_frac = sensor->sensor_analog_gains[new_exp_g];
+  gain_idx = new_exp_g;
+  exposure_time = new_exp_t;
+  dc_gain_enabled = enable_dc_gain;
+
+  float gain = analog_gain_frac * get_gain_factor();
+  cur_ev[camera.buf.cur_frame_data.frame_id % 3] = exposure_time * gain;
+
+  // LOGE("ae - camera %d, cur_t %.5f, sof %.5f, dt %.5f", camera.cc.camera_num, 1e-9 * nanos_since_boot(), 1e-9 * camera.buf.cur_frame_data.timestamp_sof, 1e-9 * (nanos_since_boot() - camera.buf.cur_frame_data.timestamp_sof));
+
+  auto exp_reg_array = sensor->getExposureRegisters(exposure_time, new_exp_g, dc_gain_enabled);
+  camera.sensors_i2c(exp_reg_array.data(), exp_reg_array.size(), CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG, camera.sensor->data_word);
+}
+
+void CameraState::sendState() {
+  camera.buf.sendFrameToVipc();
+
+  MessageBuilder msg;
+  auto framed = (msg.initEvent().*camera.cc.init_camera_state)();
+  const FrameMetadata &meta = camera.buf.cur_frame_data;
+  framed.setFrameId(meta.frame_id);
+  framed.setRequestId(meta.request_id);
+  framed.setTimestampEof(meta.timestamp_eof);
+  framed.setTimestampSof(meta.timestamp_sof);
+  framed.setIntegLines(exposure_time);
+  framed.setGain(analog_gain_frac * get_gain_factor());
+  framed.setHighConversionGain(dc_gain_enabled);
+  framed.setMeasuredGreyFraction(measured_grey_fraction);
+  framed.setTargetGreyFraction(target_grey_fraction);
+  framed.setProcessingTime(meta.processing_time);
+
+  const float ev = cur_ev[meta.frame_id % 3];
+  const float perc = util::map_val(ev, camera.sensor->min_ev, camera.sensor->max_ev, 0.0f, 100.0f);
+  framed.setExposureValPercent(perc);
+  framed.setSensor(camera.sensor->image_sensor);
+
+  // Log raw frames for road camera
+  if (env_log_raw_frames && camera.cc.stream_type == VISION_STREAM_ROAD && meta.frame_id % 100 == 5) {  // no overlap with qlog decimation
+    framed.setImage(get_raw_frame_image(&camera.buf));
+  }
+
+  set_camera_exposure(calculate_exposure_value(&camera.buf, ae_xywh, 2, camera.cc.stream_type != VISION_STREAM_DRIVER ? 2 : 4));
+
+  // Send the message
+  pm->send(camera.cc.publish_name, msg);
+}
+
+void camerad_thread() {
+  // TODO: centralize enabled handling
+
+  cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
+  const cl_context_properties props[] = {CL_CONTEXT_PRIORITY_HINT_QCOM, CL_PRIORITY_HINT_HIGH_QCOM, 0};
+  cl_context ctx = CL_CHECK_ERR(clCreateContext(props, 1, &device_id, NULL, NULL, &err));
+
+  VisionIpcServer v("camerad", device_id, ctx);
+
+  // *** initial ISP init ***
+  SpectraMaster m;
+  m.init();
+
+  // *** per-cam init ***
+  std::vector<std::unique_ptr<CameraState>> cams;
+  for (const auto &config : ALL_CAMERA_CONFIGS) {
+    auto cam = std::make_unique<CameraState>(&m, config);
+    cam->init(&v, device_id, ctx);
+    cams.emplace_back(std::move(cam));
+  }
+
+  v.start_listener();
+
+  // start devices
+  LOG("-- Starting devices");
+  for (auto &cam : cams) cam->camera.sensors_start();
+
+  // poll events
+  LOG("-- Dequeueing Video events");
+  while (!do_exit) {
+    struct pollfd fds[1] = {{.fd = m.video0_fd, .events = POLLPRI}};
+    int ret = poll(fds, std::size(fds), 1000);
+    if (ret < 0) {
+      if (errno == EINTR || errno == EAGAIN) continue;
+      LOGE("poll failed (%d - %d)", ret, errno);
+      break;
+    }
+
+    if (!(fds[0].revents & POLLPRI)) continue;
+
+    struct v4l2_event ev = {0};
+    ret = HANDLE_EINTR(ioctl(fds[0].fd, VIDIOC_DQEVENT, &ev));
+    if (ret == 0) {
+      if (ev.type == V4L_EVENT_CAM_REQ_MGR_EVENT) {
+        struct cam_req_mgr_message *event_data = (struct cam_req_mgr_message *)ev.u.data;
+        if (env_debug_frames) {
+          printf("sess_hdl 0x%6X, link_hdl 0x%6X, frame_id %lu, req_id %lu, timestamp %.2f ms, sof_status %d\n", event_data->session_hdl, event_data->u.frame_msg.link_hdl,
+                 event_data->u.frame_msg.frame_id, event_data->u.frame_msg.request_id, event_data->u.frame_msg.timestamp/1e6, event_data->u.frame_msg.sof_status);
+          do_exit = do_exit || event_data->u.frame_msg.frame_id > (1*20);
+        }
+
+        for (auto &cam : cams) {
+          if (event_data->session_hdl == cam->camera.session_handle) {
+            if (cam->camera.handle_camera_event(event_data)) {
+              cam->sendState();
+            }
+            break;
+          }
+        }
+      } else {
+        LOGE("unhandled event %d\n", ev.type);
+      }
+    } else {
+      LOGE("VIDIOC_DQEVENT failed, errno=%d", errno);
+    }
+  }
+}

system/camerad/cameras/cdm.cc

@@ -0,0 +1,47 @@
+#include "cdm.h"
+#include "stddef.h"
+
+int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel, uint8_t opcode) {
+  struct cdm_dmi_cmd *cmd = (struct cdm_dmi_cmd*)dst;
+  cmd->cmd = opcode;
+  cmd->length = length - 1;
+  cmd->reserved = 0;
+  cmd->addr = 0; // gets patched in
+  cmd->DMIAddr = dmi_addr;
+  cmd->DMISel = sel;
+
+  *addr = (uint64_t)(dst + offsetof(struct cdm_dmi_cmd, addr));
+  return sizeof(struct cdm_dmi_cmd);
+}
+
+int write_cont(uint8_t *dst, uint32_t reg, const 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, const 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

@@ -0,0 +1,79 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <string.h>
+#include <vector>
+#include <memory>
+
+// from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h}
+
+enum cam_cdm_command {
+  CAM_CDM_CMD_UNUSED = 0x0,
+  CAM_CDM_CMD_DMI = 0x1,
+  CAM_CDM_CMD_NOT_DEFINED = 0x2,
+  CAM_CDM_CMD_REG_CONT = 0x3,
+  CAM_CDM_CMD_REG_RANDOM = 0x4,
+  CAM_CDM_CMD_BUFF_INDIRECT = 0x5,
+  CAM_CDM_CMD_GEN_IRQ = 0x6,
+  CAM_CDM_CMD_WAIT_EVENT = 0x7,
+  CAM_CDM_CMD_CHANGE_BASE = 0x8,
+  CAM_CDM_CMD_PERF_CTRL = 0x9,
+  CAM_CDM_CMD_DMI_32 = 0xa,
+  CAM_CDM_CMD_DMI_64 = 0xb,
+  CAM_CDM_CMD_PRIVATE_BASE = 0xc,
+  CAM_CDM_CMD_SWD_DMI_32 = (CAM_CDM_CMD_PRIVATE_BASE + 0x64),
+  CAM_CDM_CMD_SWD_DMI_64 = (CAM_CDM_CMD_PRIVATE_BASE + 0x65),
+  CAM_CDM_CMD_PRIVATE_BASE_MAX = 0x7F
+};
+
+// our helpers
+int write_random(uint8_t *dst, const std::vector<uint32_t> &vals);
+int write_cont(uint8_t *dst, uint32_t reg, const std::vector<uint32_t> &vals);
+int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel, uint8_t opcode = CAM_CDM_CMD_DMI_32);
+
+/**
+ * struct cdm_regrandom_cmd - Definition for CDM random register command.
+ * @count: Number of register writes
+ * @reserved: reserved bits
+ * @cmd: Command ID (CDMCmd)
+ */
+struct cdm_regrandom_cmd {
+  unsigned int count    : 16;
+  unsigned int reserved : 8;
+  unsigned int cmd      : 8;
+} __attribute__((__packed__));
+
+/**
+ * struct cdm_regcontinuous_cmd - Definition for a CDM register range command.
+ * @count: Number of register writes
+ * @reserved0: reserved bits
+ * @cmd: Command ID (CDMCmd)
+ * @offset: Start address of the range of registers
+ * @reserved1: reserved bits
+ */
+struct cdm_regcontinuous_cmd {
+  unsigned int count     : 16;
+  unsigned int reserved0 : 8;
+  unsigned int cmd       : 8;
+  unsigned int offset    : 24;
+  unsigned int reserved1 : 8;
+} __attribute__((__packed__));
+
+/**
+ * struct cdm_dmi_cmd - Definition for a CDM DMI command.
+ * @length: Number of bytes in LUT - 1
+ * @reserved: reserved bits
+ * @cmd: Command ID (CDMCmd)
+ * @addr: Address of the LUT in memory
+ * @DMIAddr: Address of the target DMI config register
+ * @DMISel: DMI identifier
+ */
+struct cdm_dmi_cmd {
+  unsigned int length   : 16;
+  unsigned int reserved : 8;
+  unsigned int cmd      : 8;
+  unsigned int addr;
+  unsigned int DMIAddr  : 24;
+  unsigned int DMISel   : 8;
+} __attribute__((__packed__));

system/camerad/cameras/hw.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "common/util.h"
+#include "cereal/gen/cpp/log.capnp.h"
+#include "msgq/visionipc/visionipc_server.h"
+
+#include "media/cam_isp_ife.h"
+
+
+typedef enum {
+  ISP_RAW_OUTPUT,   // raw frame from sensor
+  ISP_IFE_PROCESSED,  // fully processed image through the IFE
+  ISP_BPS_PROCESSED,  // fully processed image through the BPS
+} SpectraOutputType;
+
+// For the comma 3X three camera platform
+
+struct CameraConfig {
+  int camera_num;
+  VisionStreamType stream_type;
+  float focal_len;  // millimeters
+  const char *publish_name;
+  cereal::FrameData::Builder (cereal::Event::Builder::*init_camera_state)();
+  bool enabled;
+  uint32_t phy;
+  bool vignetting_correction;
+  SpectraOutputType output_type;
+};
+
+// NOTE: to be able to disable road and wide road, we still have to configure the sensor over i2c
+// If you don't do this, the strobe GPIO is an output (even in reset it seems!)
+const CameraConfig WIDE_ROAD_CAMERA_CONFIG = {
+  .camera_num = 0,
+  .stream_type = VISION_STREAM_WIDE_ROAD,
+  .focal_len = 1.71,
+  .publish_name = "wideRoadCameraState",
+  .init_camera_state = &cereal::Event::Builder::initWideRoadCameraState,
+  .enabled = !getenv("DISABLE_WIDE_ROAD"),
+  .phy = CAM_ISP_IFE_IN_RES_PHY_0,
+  .vignetting_correction = false,
+  .output_type = ISP_IFE_PROCESSED,
+};
+
+const CameraConfig ROAD_CAMERA_CONFIG = {
+  .camera_num = 1,
+  .stream_type = VISION_STREAM_ROAD,
+  .focal_len = 8.0,
+  .publish_name = "roadCameraState",
+  .init_camera_state = &cereal::Event::Builder::initRoadCameraState,
+  .enabled = !getenv("DISABLE_ROAD"),
+  .phy = CAM_ISP_IFE_IN_RES_PHY_1,
+  .vignetting_correction = true,
+  .output_type = ISP_IFE_PROCESSED,
+};
+
+const CameraConfig DRIVER_CAMERA_CONFIG = {
+  .camera_num = 2,
+  .stream_type = VISION_STREAM_DRIVER,
+  .focal_len = 1.71,
+  .publish_name = "driverCameraState",
+  .init_camera_state = &cereal::Event::Builder::initDriverCameraState,
+  .enabled = !getenv("DISABLE_DRIVER"),
+  .phy = CAM_ISP_IFE_IN_RES_PHY_2,
+  .vignetting_correction = false,
+  .output_type = ISP_BPS_PROCESSED,
+};
+
+const CameraConfig ALL_CAMERA_CONFIGS[] = {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG};

system/camerad/cameras/ife.h

@@ -0,0 +1,236 @@
+#pragma once
+
+#include "cdm.h"
+
+#include "system/camerad/cameras/hw.h"
+#include "system/camerad/sensors/sensor.h"
+
+int build_common_ife_bps(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches, bool ife) {
+  uint8_t *start = dst;
+
+  /*
+    Common between IFE and BPS.
+  */
+
+  // IFE -> BPS addresses
+  /*
+  std::map<uint32_t, uint32_t> addrs = {
+    {0xf30, 0x3468},
+  };
+  */
+
+  // YUV
+  dst += write_cont(dst, ife ? 0xf30 : 0x3468, {
+    0x00680208,
+    0x00000108,
+    0x00400000,
+    0x03ff0000,
+    0x01c01ed8,
+    0x00001f68,
+    0x02000000,
+    0x03ff0000,
+    0x1fb81e88,
+    0x000001c0,
+    0x02000000,
+    0x03ff0000,
+  });
+
+  return dst - start;
+}
+
+int build_update(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches) {
+  uint8_t *start = dst;
+
+  // init sequence
+  dst += write_random(dst, {
+    0x2c, 0xffffffff,
+    0x30, 0xffffffff,
+    0x34, 0xffffffff,
+    0x38, 0xffffffff,
+    0x3c, 0xffffffff,
+  });
+
+  // demux cfg
+  dst += write_cont(dst, 0x560, {
+    0x00000001,
+    0x04440444,
+    0x04450445,
+    0x04440444,
+    0x04450445,
+    0x000000ca,
+    0x0000009c,
+  });
+
+  // white balance
+  dst += write_cont(dst, 0x6fc, {
+    0x00800080,
+    0x00000080,
+    0x00000000,
+    0x00000000,
+  });
+
+  // module config/enables (e.g. enable debayer, white balance, etc.)
+  dst += write_cont(dst, 0x40, {
+    0x00000c06 | ((uint32_t)(cam.vignetting_correction) << 8),
+  });
+  dst += write_cont(dst, 0x44, {
+    0x00000000,
+  });
+  dst += write_cont(dst, 0x48, {
+    (1 << 3) | (1 << 1),
+  });
+  dst += write_cont(dst, 0x4c, {
+    0x00000019,
+  });
+  dst += write_cont(dst, 0xf00, {
+    0x00000000,
+  });
+
+  // cropping
+  dst += write_cont(dst, 0xe0c, {
+    0x00000e00,
+  });
+  dst += write_cont(dst, 0xe2c, {
+    0x00000e00,
+  });
+
+  // black level scale + offset
+  dst += write_cont(dst, 0x6b0, {
+    ((uint32_t)(1 << 11) << 0xf) | (s->black_level << (14 - s->bits_per_pixel)),
+    0x0,
+    0x0,
+  });
+
+  return dst - start;
+}
+
+
+int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo *s, std::vector<uint32_t> &patches, uint32_t out_width, uint32_t out_height) {
+  uint8_t *start = dst;
+
+  // start with the every frame config
+  dst += build_update(dst, cam, s, patches);
+
+  uint64_t addr;
+
+  // setup
+  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,
+  });
+
+  // linearization
+  dst += write_cont(dst, 0x4dc, {
+    0x00000000,
+  });
+  dst += write_cont(dst, 0x4e0, s->linearization_pts);
+  dst += write_cont(dst, 0x4f0, s->linearization_pts);
+  dst += write_cont(dst, 0x500, s->linearization_pts);
+  dst += write_cont(dst, 0x510, s->linearization_pts);
+  // TODO: this is DMI64 in the dump, does that matter?
+  dst += write_dmi(dst, &addr, s->linearization_lut.size()*sizeof(uint32_t), 0xc24, 9);
+  patches.push_back(addr - (uint64_t)start);
+
+  // vignetting correction
+  dst += write_cont(dst, 0x6bc, {
+    0x0b3c0000,
+    0x00670067,
+    0xd3b1300c,
+    0x13b1300c,
+  });
+  dst += write_cont(dst, 0x6d8, {
+    0xec4e4000,
+    0x0100c003,
+  });
+  dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 14); // GRR
+  patches.push_back(addr - (uint64_t)start);
+  dst += write_dmi(dst, &addr, s->vignetting_lut.size()*sizeof(uint32_t), 0xc24, 15); // GBB
+  patches.push_back(addr - (uint64_t)start);
+
+  // debayer
+  dst += write_cont(dst, 0x6f8, {
+    0x00000100,
+  });
+  dst += write_cont(dst, 0x71c, {
+    0x00008000,
+    0x08000066,
+  });
+
+  // color correction
+  dst += write_cont(dst, 0x760, s->color_correct_matrix);
+
+  // gamma
+  dst += write_cont(dst, 0x798, {
+    0x00000000,
+  });
+  dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 26);  // G
+  patches.push_back(addr - (uint64_t)start);
+  dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 28);  // B
+  patches.push_back(addr - (uint64_t)start);
+  dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 30);  // R
+  patches.push_back(addr - (uint64_t)start);
+
+  // output size/scaling
+  dst += write_cont(dst, 0xa3c, {
+    0x00000003,
+    ((out_width - 1) << 16) | (s->frame_width - 1),
+    0x30036666,
+    0x00000000,
+    0x00000000,
+    s->frame_width - 1,
+    ((out_height - 1) << 16) | (s->frame_height - 1),
+    0x30036666,
+    0x00000000,
+    0x00000000,
+    s->frame_height - 1,
+  });
+  dst += write_cont(dst, 0xa68, {
+    0x00000003,
+    ((out_width / 2 - 1) << 16) | (s->frame_width - 1),
+    0x3006cccc,
+    0x00000000,
+    0x00000000,
+    s->frame_width - 1,
+    ((out_height / 2 - 1) << 16) | (s->frame_height - 1),
+    0x3006cccc,
+    0x00000000,
+    0x00000000,
+    s->frame_height - 1,
+  });
+
+  // cropping
+  dst += write_cont(dst, 0xe10, {
+    out_height - 1,
+    out_width - 1,
+  });
+  dst += write_cont(dst, 0xe30, {
+    out_height / 2 - 1,
+    out_width - 1,
+  });
+  dst += write_cont(dst, 0xe18, {
+    0x0ff00000,
+    0x00000016,
+  });
+  dst += write_cont(dst, 0xe38, {
+    0x0ff00000,
+    0x00000017,
+  });
+
+  dst += build_common_ife_bps(dst, cam, s, patches, true);
+
+  return dst - start;
+}
+
+

system/camerad/cameras/nv12_info.h

@@ -0,0 +1,22 @@
+#pragma once
+
+#include <cassert>
+#include <cstdint>
+#include <tuple>
+
+#include "third_party/linux/include/msm_media_info.h"
+
+// Returns NV12 aligned (stride, y_height, uv_height, buffer_size) for the given frame dimensions.
+inline std::tuple<uint32_t, uint32_t, uint32_t, uint32_t> get_nv12_info(int width, int height) {
+  const uint32_t stride = VENUS_Y_STRIDE(COLOR_FMT_NV12, width);
+  const uint32_t y_height = VENUS_Y_SCANLINES(COLOR_FMT_NV12, height);
+  const uint32_t uv_height = VENUS_UV_SCANLINES(COLOR_FMT_NV12, height);
+  const uint32_t size = VENUS_BUFFER_SIZE(COLOR_FMT_NV12, width, height);
+
+  // Sanity checks for NV12 format assumptions
+  assert(stride == VENUS_UV_STRIDE(COLOR_FMT_NV12, width));
+  assert(y_height / 2 == uv_height);
+  assert((stride * y_height) % 0x1000 == 0);  // uv_offset must be page-aligned
+
+  return {stride, y_height, uv_height, size};
+}

system/camerad/cameras/nv12_info.py

@@ -0,0 +1,21 @@
+# Python version of system/camerad/cameras/nv12_info.h
+# Calculations from third_party/linux/include/msm_media_info.h (VENUS_BUFFER_SIZE)
+
+def align(val: int, alignment: int) -> int:
+  return ((val + alignment - 1) // alignment) * alignment
+
+def get_nv12_info(width: int, height: int) -> tuple[int, int, int, int]:
+  """Returns (stride, y_height, uv_height, buffer_size) for NV12 frame dimensions."""
+  stride = align(width, 128)
+  y_height = align(height, 32)
+  uv_height = align(height // 2, 16)
+
+  # VENUS_BUFFER_SIZE for NV12
+  y_plane = stride * y_height
+  uv_plane = stride * uv_height + 4096
+  size = y_plane + uv_plane + max(16 * 1024, 8 * stride)
+  size = align(size, 4096)
+  size += align(width, 512) * 512  # kernel padding for non-aligned frames
+  size = align(size, 4096)
+
+  return stride, y_height, uv_height, size

system/camerad/cameras/spectra.h

@@ -0,0 +1,222 @@
+#pragma once
+
+#include <sys/mman.h>
+#include <functional>
+#include <memory>
+#include <queue>
+#include <optional>
+#include <utility>
+
+#include "media/cam_req_mgr.h"
+
+#include "common/util.h"
+#include "common/swaglog.h"
+#include "system/camerad/cameras/hw.h"
+#include "system/camerad/cameras/camera_common.h"
+#include "system/camerad/sensors/sensor.h"
+
+#define MAX_IFE_BUFS 20
+
+const int MIPI_SETTLE_CNT = 33;  // Calculated by camera_freqs.py
+
+// For use with the Titan 170 ISP in the SDM845
+// https://github.com/commaai/agnos-kernel-sdm845
+
+// CSLDeviceType/CSLPacketOpcodesIFE from camx
+// cam_packet_header.op_code = (device << 24) | (opcode);
+#define CSLDeviceTypeImageSensor (0x01 << 24)
+#define CSLDeviceTypeIFE         (0x0F << 24)
+#define CSLDeviceTypeBPS         (0x10 << 24)
+#define OpcodesIFEInitialConfig  0x0
+#define OpcodesIFEUpdate         0x1
+
+std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data, uint32_t num_resources=1);
+int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle);
+int device_control(int fd, int op_code, int session_handle, int dev_handle);
+int do_cam_control(int fd, int op_code, void *handle, int size);
+void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
+                      int mmu_hdl = 0, int mmu_hdl2 = 0);
+void release(int video0_fd, uint32_t handle);
+
+class MemoryManager {
+public:
+  void init(int _video0_fd) { video0_fd = _video0_fd; }
+  ~MemoryManager();
+
+  template <class T>
+  auto alloc(int len, uint32_t *handle) {
+    return std::unique_ptr<T, std::function<void(void *)>>((T*)alloc_buf(len, handle), [this](void *ptr) { this->free(ptr); });
+  }
+
+private:
+  void *alloc_buf(int len, uint32_t *handle);
+  void free(void *ptr);
+
+  std::map<void *, uint32_t> handle_lookup;
+  std::map<void *, int> size_lookup;
+  std::map<int, std::queue<void *> > cached_allocations;
+  int video0_fd;
+};
+
+class SpectraMaster {
+public:
+  void init();
+
+  unique_fd video0_fd;
+  unique_fd cam_sync_fd;
+  unique_fd isp_fd;
+  unique_fd icp_fd;
+  int device_iommu = -1;
+  int cdm_iommu = -1;
+  int icp_device_iommu = -1;
+  MemoryManager mem_mgr;
+};
+
+class SpectraBuf {
+public:
+  SpectraBuf() = default;
+
+  ~SpectraBuf() {
+    if (video_fd >= 0 && ptr) {
+      munmap(ptr, mmap_size);
+      release(video_fd, handle);
+    }
+  }
+
+  void init(SpectraMaster *m, int s, int a, bool shared_access, int mmu_hdl = 0, int mmu_hdl2 = 0, int count = 1) {
+    video_fd = m->video0_fd;
+    size = s;
+    alignment = a;
+    mmap_size = aligned_size() * count;
+
+    uint32_t flags = CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE;
+    if (shared_access) {
+      flags |= CAM_MEM_FLAG_HW_SHARED_ACCESS;
+    }
+
+    void *p = alloc_w_mmu_hdl(video_fd, mmap_size, (uint32_t*)&handle, alignment, flags, mmu_hdl, mmu_hdl2);
+    ptr = (unsigned char*)p;
+    assert(ptr != NULL);
+  };
+
+  uint32_t aligned_size() {
+    return ALIGNED_SIZE(size, alignment);
+  };
+
+  int video_fd = -1;
+  unsigned char *ptr = nullptr;
+  int size = 0, alignment = 0, handle = 0, mmap_size = 0;
+};
+
+class SpectraCamera {
+public:
+  SpectraCamera(SpectraMaster *master, const CameraConfig &config);
+  ~SpectraCamera();
+
+  void camera_open(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
+  bool handle_camera_event(const cam_req_mgr_message *event_data);
+  void camera_close();
+  void camera_map_bufs();
+  void config_bps(int idx, int request_id);
+  void config_bps_downscale(int idx, int request_id);  // mici driver cam: full-res + 2x BPS downscale (#37876)
+  void config_ife(int idx, int request_id, bool init=false);
+
+  int clear_req_queue();
+  void enqueue_frame(uint64_t request_id);
+
+  int sensors_init();
+  void sensors_start();
+  void sensors_poke(int request_id);
+  void sensors_i2c(const struct i2c_random_wr_payload* dat, int len, int op_code, bool data_word);
+
+  bool openSensor();
+  void configISP();
+  void configICP();
+  void configCSIPHY();
+  void linkDevices();
+  void destroySyncObjectAt(int index);
+
+  // *** state ***
+
+  int ife_buf_depth = -1;
+  bool open = false;
+  bool enabled = true;
+  CameraConfig cc;
+  std::unique_ptr<const SensorInfo> sensor;
+
+  // YUV image size
+  uint32_t stride;
+  uint32_t y_height;
+  uint32_t uv_height;
+  uint32_t uv_offset;
+  uint32_t yuv_size;
+
+  unique_fd sensor_fd;
+  unique_fd csiphy_fd;
+
+  int32_t session_handle = -1;
+  int32_t sensor_dev_handle = -1;
+  int32_t isp_dev_handle = -1;
+  int32_t icp_dev_handle = -1;
+  int32_t csiphy_dev_handle = -1;
+
+  int32_t link_handle = -1;
+
+  SpectraBuf ife_cmd;
+  SpectraBuf ife_gamma_lut;
+  SpectraBuf ife_linearization_lut;
+  SpectraBuf ife_vignetting_lut;
+
+  SpectraBuf bps_cmd;
+  SpectraBuf bps_cdm_buffer;
+  SpectraBuf bps_cdm_program_array;
+  SpectraBuf bps_cdm_striping_bl;
+  SpectraBuf bps_iq;
+  SpectraBuf bps_striping;
+  SpectraBuf bps_linearization_lut;
+  SpectraBuf bps_gamma_lut;       // only allocated for out_scale>1 (mici driver cam downscale)
+  SpectraBuf bps_fullres_dummy;   // only allocated for out_scale>1 (mici driver cam downscale)
+  std::vector<uint32_t> bps_lin_reg;
+  std::vector<uint32_t> bps_ccm_reg;
+
+  int buf_handle_yuv[MAX_IFE_BUFS] = {};
+  int buf_handle_raw[MAX_IFE_BUFS] = {};
+  int sync_objs_ife[MAX_IFE_BUFS] = {};
+  int sync_objs_bps[MAX_IFE_BUFS] = {};
+  uint64_t request_id_last = 0;
+  uint64_t last_requeue_ts = 0;
+  uint64_t frame_id_raw_last = 0;
+  int invalid_request_count = 0;
+  bool skip_expected = true;
+
+  CameraBuf buf;
+  SpectraMaster *m;
+
+private:
+  void clearAndRequeue(uint64_t from_request_id);
+  bool validateEvent(uint64_t request_id, uint64_t frame_id_raw);
+  bool waitForFrameReady(uint64_t request_id);
+  bool processFrame(int buf_idx, uint64_t request_id, uint64_t frame_id_raw, uint64_t timestamp);
+  static bool syncFirstFrame(int camera_id, uint64_t request_id, uint64_t raw_id, uint64_t timestamp);
+  struct SyncData {
+    uint64_t timestamp;
+    uint64_t frame_id_offset = 0;
+  };
+  inline static std::map<int, SyncData> camera_sync_data;
+  inline static bool first_frame_synced = false;
+
+  // a mode for stressing edge cases: realignment, sync failures, etc.
+  inline bool stress_test(std::string log) {
+    static double last_trigger = 0;
+    static double prob = std::stod(util::getenv("SPECTRA_ERROR_PROB", "-1"));
+    static double dt = std::stod(util::getenv("SPECTRA_ERROR_DT", "1"));
+    bool triggered = (prob > 0) && \
+                     ((static_cast<double>(rand()) / RAND_MAX) < prob) && \
+                     (millis_since_boot() - last_trigger) > dt;
+    if (triggered) {
+      last_trigger = millis_since_boot();
+      LOGE("stress test (cam %d): %s", cc.camera_num, log.c_str());
+    }
+    return triggered;
+  }
+};