camerad: setup IFE gamma correction (#33837)

* setup gamma

* ife happy

* config for sensor

* fill and clean up

* cleanup

---------

Co-authored-by: Comma Device <device@comma.ai>
Co-authored-by: ZwX1616 <zwx1616@gmail.com>

system/camerad/cameras/cdm.cc

@@ -1,4 +1,18 @@
 #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) {
+  struct cdm_dmi_cmd *cmd = (struct cdm_dmi_cmd*)dst;
+  cmd->cmd = CAM_CDM_CMD_DMI_32;
+  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, std::vector<uint32_t> vals) {
   struct cdm_regcontinuous_cmd *cmd = (struct cdm_regcontinuous_cmd*)dst;

system/camerad/cameras/cdm.h

@@ -9,6 +9,7 @@
 // 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);
+int write_dmi(uint8_t *dst, uint64_t *addr, uint32_t length, uint32_t dmi_addr, uint8_t sel);
 
 // from drivers/media/platform/msm/camera/cam_cdm/cam_cdm_util.{c,h}
 

system/camerad/cameras/ife.h

@@ -4,7 +4,7 @@
 #include "system/camerad/sensors/sensor.h"
 
 
-int build_initial_config(uint8_t *dst, const SensorInfo *s) {
+int build_initial_config(uint8_t *dst, const SensorInfo *s, std::vector<uint32_t> &patches) {
   uint8_t *start = dst;
 
   dst += write_random(dst, {
@@ -178,49 +178,17 @@ int build_initial_config(uint8_t *dst, const SensorInfo *s) {
 
   dst += write_cont(dst, 0x760, s->color_correct_matrix);
 
-  dst += write_cont(dst, 0x794, {
+  // gamma
-    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
+  uint64_t addr;
-  cdm_dmi_cmd_t 580
+  dst += write_dmi(dst, &addr, 256, 0xc24, 26);  // G
-    .length = 255
+  patches.push_back(addr - (uint64_t)start);
-    .reserved = 33
+  dst += write_dmi(dst, &addr, 256, 0xc24, 28);  // B
-    .cmd = 10
+  patches.push_back(addr - (uint64_t)start);
-    .addr = 0
+  dst += write_dmi(dst, &addr, 256, 0xc24, 30);  // R
-    .DMIAddr = 3108
+  patches.push_back(addr - (uint64_t)start);
-    .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,
@@ -517,50 +485,6 @@ int build_first_update(uint8_t *dst) {
     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,
@@ -644,7 +568,7 @@ int build_first_update(uint8_t *dst) {
   return dst - start;
 }
 
-int build_update(uint8_t *dst, const CameraConfig &cc, const SensorInfo *s) {
+int build_update(uint8_t *dst, const CameraConfig &cc, const SensorInfo *s, std::vector<uint32_t> &patches) {
   uint8_t *start = dst;
 
   dst += write_random(dst, {
@@ -701,7 +625,7 @@ int build_update(uint8_t *dst, const CameraConfig &cc, const SensorInfo *s) {
   });
 
   dst += write_cont(dst, 0x48, {
-    0b10,
+    (1 << 3) | (1 << 1),
   });
 
   dst += write_cont(dst, 0x4c, {

system/camerad/cameras/spectra.cc

@@ -463,6 +463,7 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     * IFE = Image Front End
   */
   int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2;
+  size += sizeof(struct cam_patch_desc)*3;
   if (!init) {
     size += sizeof(struct cam_buf_io_cfg);
   }
@@ -480,6 +481,7 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
   pkt->header.size = size;
 
   // *** 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;
@@ -495,11 +497,11 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     // 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, sensor.get());
+        buf_desc[0].length = build_initial_config((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, sensor.get(), patches);
       } 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, cc, sensor.get());
+        buf_desc[0].length = build_update((unsigned char*)ife_cmd.ptr + buf_desc[0].offset, cc, sensor.get(), patches);
       }
     }
 
@@ -632,8 +634,18 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
   // *** patches ***
   // sets up the kernel driver to do address translation for the IFE
   {
-    pkt->num_patches = 0;
+    pkt->num_patches = patches.size();
     pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs;
+    if (pkt->num_patches > 0) {
+      // gamma LUT
+      for (int i = 0; i < 3; i++) {
+        struct cam_patch_desc *patch = (struct cam_patch_desc *)((char*)&pkt->payload + pkt->patch_offset + sizeof(cam_patch_desc)*i);
+        patch->dst_buf_hdl = ife_cmd.handle;
+        patch->src_buf_hdl = ife_gamma_lut.handle;
+        patch->dst_offset = patches[i];
+        patch->src_offset = ife_gamma_lut.size*i;
+      }
+    }
   }
 
   int ret = device_config(m->isp_fd, session_handle, isp_dev_handle, cam_packet_handle);
@@ -841,10 +853,17 @@ void SpectraCamera::configISP() {
   isp_dev_handle = *isp_dev_handle_;
   LOGD("acquire isp dev");
 
-  // config IFE
+  // allocate IFE memory, then configure it
   ife_cmd.init(m, 67984, 0x20,
                CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
                m->device_iommu, m->cdm_iommu, FRAME_BUF_COUNT);
+  ife_gamma_lut.init(m, 64*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,
+                     m->device_iommu, m->cdm_iommu, 3); // 3 for RGB
+  for (int i = 0; i < 3; i++) {
+    memcpy(ife_gamma_lut.ptr + ife_gamma_lut.size*i, sensor->gamma_lut_rgb.data(), ife_gamma_lut.size);
+  }
+
   config_ife(0, 1, true);
 }
 

system/camerad/cameras/spectra.h

@@ -74,7 +74,8 @@ public:
   void init(SpectraMaster *m, int s, int a, int flags, int mmu_hdl = 0, int mmu_hdl2 = 0, int count=1) {
     size = s;
     alignment = a;
-    ptr = alloc_w_mmu_hdl(m->video0_fd, ALIGNED_SIZE(size, alignment)*count, (uint32_t*)&handle, alignment, flags, mmu_hdl, mmu_hdl2);
+    void *p = alloc_w_mmu_hdl(m->video0_fd, ALIGNED_SIZE(size, alignment)*count, (uint32_t*)&handle, alignment, flags, mmu_hdl, mmu_hdl2);
+    ptr = (unsigned char*)p;
     assert(ptr != NULL);
   };
 
@@ -82,7 +83,7 @@ public:
     return ALIGNED_SIZE(size, alignment);
   };
 
-  void *ptr;
+  unsigned char *ptr;
   int size, alignment, handle;
 };
 
@@ -139,6 +140,7 @@ public:
   int32_t link_handle = -1;
 
   SpectraBuf ife_cmd;
+  SpectraBuf ife_gamma_lut;
 
   SpectraBuf bps_cmd;
   SpectraBuf bps_cdm_buffer;

system/camerad/sensors/ar0231.cc

@@ -124,6 +124,18 @@ AR0231::AR0231() {
     0x00000fbc, 0x000000bb, 0x00000009,
     0x00000fb6, 0x00000fe0, 0x000000ea,
   };
+  for (int i = 0; i < 64; i++) {
+    float fx = i / 63.0;
+    const float gamma_k = 0.75;
+    const float gamma_b = 0.125;
+    const float mp = 0.01; // ideally midpoint should be adaptive
+    const float rk = 9 - 100*mp;
+    // poly approximation for s curve
+    fx = (fx > mp) ?
+      ((rk * (fx-mp) * (1-(gamma_k*mp+gamma_b)) * (1+1/(rk*(1-mp))) / (1+rk*(fx-mp))) + gamma_k*mp + gamma_b) :
+      ((rk * (fx-mp) * (gamma_k*mp+gamma_b) * (1+1/(rk*mp)) / (1-rk*(fx-mp))) + gamma_k*mp + gamma_b);
+    gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5));
+  }
 }
 
 void AR0231::processRegisters(uint8_t *cur_buf, cereal::FrameData::Builder &framed) const {

system/camerad/sensors/os04c10.cc

@@ -68,6 +68,10 @@ OS04C10::OS04C10() {
     0x00000fa7, 0x000000d9, 0x00001000,
     0x00000fca, 0x00000fef, 0x000000c7,
   };
+  for (int i = 0; i < 64; i++) {
+    float fx = i / 63.0;
+    gamma_lut_rgb.push_back((uint32_t)(pow(fx, 0.7)*1023.0 + 0.5));
+  }
 }
 
 std::vector<i2c_random_wr_payload> OS04C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const {

system/camerad/sensors/ox03c10.cc

@@ -68,6 +68,11 @@ OX03C10::OX03C10() {
     0x00000fcc, 0x000000b9, 0x00000ffb,
     0x00000fc2, 0x00000ff6, 0x000000c9,
   };
+  for (int i = 0; i < 64; i++) {
+    float fx = i / 63.0;
+    fx = -0.507089*exp(-12.54124638*fx) + 0.9655*pow(fx, 0.5) - 0.472597*fx + 0.507089;
+    gamma_lut_rgb.push_back((uint32_t)(fx*1023.0 + 0.5));
+  }
 }
 
 std::vector<i2c_random_wr_payload> OX03C10::getExposureRegisters(int exposure_time, int new_exp_g, bool dc_gain_enabled) const {

system/camerad/sensors/sensor.h

@@ -68,7 +68,8 @@ public:
 
   // ISP image processing params
   uint32_t black_level;
-  std::vector<uint32_t> color_correct_matrix;
+  std::vector<uint32_t> color_correct_matrix;  // 3x3
+  std::vector<uint32_t> gamma_lut_rgb;  // gamma LUTs are length 64 * sizeof(uint32_t); same for r/g/b here
 };
 
 class AR0231 : public SensorInfo {