camerad: match some BPS settings (commaai#34548)

* start common * fix bayer pattern * lil more * all 1q10 * cc en * same pts? * this is weird * some cleanup * less * off * clean up --------- Co-authored-by: Comma Device <[email protected]>
Edison-CBS · Feb 15, 2025 · d5dde8d · d5dde8d
1 parent 70ec6c2
commit d5dde8d
Show file tree

Hide file tree

Showing 5 changed files with 94 additions and 46 deletions.
diff --git a/system/camerad/cameras/bps_blobs.h b/system/camerad/cameras/bps_blobs.h
diff --git a/system/camerad/cameras/camera_qcom2.cc b/system/camerad/cameras/camera_qcom2.cc
@@ -55,7 +55,7 @@ class CameraState {
 
   float fl_pix = 0;
 
-  CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config, config.stream_type == VISION_STREAM_ROAD ? ISP_RAW_OUTPUT : ISP_IFE_PROCESSED) {};
+  CameraState(SpectraMaster *master, const CameraConfig &config) : camera(master, config, config.stream_type == VISION_STREAM_DRIVER ? ISP_BPS_PROCESSED : ISP_IFE_PROCESSED) {};
   ~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);
@@ -283,7 +283,7 @@ void camerad_thread() {
 
   // *** per-cam init ***
   std::vector<std::unique_ptr<CameraState>> cams;
-  for (const auto &config : {WIDE_ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG, ROAD_CAMERA_CONFIG}) {
+  for (const auto &config : {WIDE_ROAD_CAMERA_CONFIG, ROAD_CAMERA_CONFIG, DRIVER_CAMERA_CONFIG}) {
     auto cam = std::make_unique<CameraState>(&m, config);
     cam->init(&v, device_id, ctx);
     cams.emplace_back(std::move(cam));

diff --git a/system/camerad/cameras/ife.h b/system/camerad/cameras/ife.h
@@ -5,6 +5,38 @@
 #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;
@@ -150,22 +182,6 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo
   dst += write_dmi(dst, &addr, s->gamma_lut_rgb.size()*sizeof(uint32_t), 0xc24, 30);  // R
   patches.push_back(addr - (uint64_t)start);
 
-  // YUV
-  dst += write_cont(dst, 0xf30, {
-    0x00680208,
-    0x00000108,
-    0x00400000,
-    0x03ff0000,
-    0x01c01ed8,
-    0x00001f68,
-    0x02000000,
-    0x03ff0000,
-    0x1fb81e88,
-    0x000001c0,
-    0x02000000,
-    0x03ff0000,
-  });
-
   // output size/scaling
   dst += write_cont(dst, 0xa3c, {
     0x00000003,
@@ -212,6 +228,8 @@ int build_initial_config(uint8_t *dst, const CameraConfig cam, const SensorInfo
     0x00000017,
   });
 
+  dst += build_common_ife_bps(dst, cam, s, patches, true);
+
   return dst - start;
 }
 

diff --git a/system/camerad/cameras/spectra.cc b/system/camerad/cameras/spectra.cc
@@ -482,7 +482,7 @@ void SpectraCamera::config_bps(int idx, int request_id) {
   */
 
   int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2 + sizeof(struct cam_buf_io_cfg)*2;
-  size += sizeof(struct cam_patch_desc)*8;
+  size += sizeof(struct cam_patch_desc)*9;
 
   uint32_t cam_packet_handle = 0;
   auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
@@ -525,6 +525,7 @@ void SpectraCamera::config_bps(int idx, int request_id) {
   } cdm_tmp;
 
   // *** cmd buf ***
+  std::vector<uint32_t> patches;
   struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
   {
     pkt->num_cmd_buf = 2;
@@ -558,33 +559,46 @@ void SpectraCamera::config_bps(int idx, int request_id) {
 
     int cdm_len = 0;
 
-    // debayer params
+    if (bps_lin_reg.size() == 0) {
+      for (int i = 0; i < 4; i++) {
+        bps_lin_reg.push_back(((sensor->linearization_pts[i] & 0xffff) << 0x10) | (sensor->linearization_pts[i] >> 0x10));
+      }
+    }
+
+    if (bps_ccm_reg.size() == 0) {
+      for (int i = 0; i < 3; i++) {
+        bps_ccm_reg.push_back(sensor->color_correct_matrix[i] | (sensor->color_correct_matrix[i+3] << 0x10));
+        bps_ccm_reg.push_back(sensor->color_correct_matrix[i+6]);
+      }
+    }
+
+    // white balance
     cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2868, {
-      0x06900400,
-      0x000006a6,
+      0x04000400,
+      0x00000400,
       0x00000000,
       0x00000000,
     });
+    // debayer
     cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2878, {
       0x00000080,
       0x00800066,
     });
-
-    // YUV color xform
-    cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x3468, {
-      0x00680208,
-      0x00000108,
-      0x00400000,
-      0x03ff0000,
-      0x01c01ed8,
-      0x00001f68,
-      0x02000000,
-      0x03ff0000,
-      0x1fb81e88,
-      0x000001c0,
-      0x02000000,
-      0x03ff0000,
-    });
+    // linearization, EN=0
+    cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1868, bps_lin_reg);
+    cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1878, bps_lin_reg);
+    cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1888, bps_lin_reg);
+    cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x1898, bps_lin_reg);
+    /*
+    uint8_t *start = (unsigned char *)bps_cdm_program_array.ptr + cdm_len;
+    uint64_t addr;
+    cdm_len += write_dmi((unsigned char *)bps_cdm_program_array.ptr + cdm_len, &addr, sensor->linearization_lut.size()*sizeof(uint32_t), 0x1808, 1);
+    patches.push_back(addr - (uint64_t)start);
+    */
+    // color correction
+    cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2e68, bps_ccm_reg);
+
+    cdm_len += build_common_ife_bps((unsigned char *)bps_cdm_program_array.ptr + cdm_len, cc, sensor.get(), patches, false);
 
     pa->length = cdm_len - 1;
 
@@ -665,8 +679,13 @@ void SpectraCamera::config_bps(int idx, int request_id) {
 
   // *** patches ***
   {
+    assert(patches.size() == 0 | patches.size() == 1);
     pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs;
 
+    if (patches.size() > 0) {
+      add_patch(pkt.get(), bps_cmd.handle, patches[0], bps_linearization_lut.handle, 0);
+    }
+
     // input frame
     add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[0].ptr[0]), buf_handle_raw[idx], 0);
 
@@ -1221,6 +1240,14 @@ void SpectraCamera::configICP() {
   bps_cdm_striping_bl.init(m, 0xa100, 0x20,
                            CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE | CAM_MEM_FLAG_HW_SHARED_ACCESS,
                            m->icp_device_iommu);
+
+  // LUTs
+  /*
+  bps_linearization_lut.init(m, sensor->linearization_lut.size()*sizeof(uint32_t), 0x20,
+              CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE | CAM_MEM_FLAG_HW_SHARED_ACCESS,
+              m->icp_device_iommu);
+  memcpy(bps_linearization_lut.ptr, sensor->linearization_lut.data(), bps_linearization_lut.size);
+  */
 }
 
 void SpectraCamera::configCSIPHY() {

diff --git a/system/camerad/cameras/spectra.h b/system/camerad/cameras/spectra.h
@@ -172,6 +172,9 @@ class SpectraCamera {
   SpectraBuf bps_cdm_striping_bl;
   SpectraBuf bps_iq;
   SpectraBuf bps_striping;
+  SpectraBuf bps_linearization_lut;
+  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] = {};