camerad: debayer in the BPS (#33763)

* bps support

* cleanup with offsetof

* cleanup

---------

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

system/camerad/cameras/spectra.cc

@@ -17,10 +17,9 @@
 #include "common/swaglog.h"
 #include "system/camerad/cameras/ife.h"
 #include "system/camerad/cameras/spectra.h"
+#include "system/camerad/cameras/bps_blobs.h"
 #include "third_party/linux/include/msm_media_info.h"
 
-// For debugging:
-// echo "4294967295" > /sys/module/cam_debug_util/parameters/debug_mdl
 
 // ************** low level camera helpers ****************
 
@@ -454,12 +453,14 @@ int SpectraCamera::sensors_init() {
   return ret;
 }
 
-void add_patch(void *ptr, int n, int32_t dst_hdl, uint32_t dst_offset, int32_t src_hdl, uint32_t src_offset) {
-  struct cam_patch_desc *p = (struct cam_patch_desc *)((unsigned char*)ptr + sizeof(struct cam_patch_desc)*n);
+void add_patch(struct cam_packet *pkt, int32_t dst_hdl, uint32_t dst_offset, int32_t src_hdl, uint32_t src_offset) {
+  void *ptr = (char*)&pkt->payload + pkt->patch_offset;
+  struct cam_patch_desc *p = (struct cam_patch_desc *)((unsigned char*)ptr + sizeof(struct cam_patch_desc)*pkt->num_patches);
   p->dst_buf_hdl = dst_hdl;
   p->src_buf_hdl = src_hdl;
   p->dst_offset = dst_offset;
   p->src_offset = src_offset;
+  pkt->num_patches++;
 };
 
 void SpectraCamera::config_bps(int idx, int request_id) {
@@ -468,8 +469,209 @@ void SpectraCamera::config_bps(int idx, int request_id) {
     * BPS = Bayer Processing Segment
   */
 
-  (void)idx;
-  (void)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;
+
+  uint32_t cam_packet_handle = 0;
+  auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
+
+  pkt->header.op_code = CSLDeviceTypeBPS | CAM_ICP_OPCODE_BPS_UPDATE;
+  pkt->header.request_id = request_id;
+  pkt->header.size = size;
+
+  typedef struct {
+    struct {
+      uint32_t ptr[2];
+      uint32_t unknown[2];
+    } frames[9];
+
+    uint32_t unknown1;
+    uint32_t unknown2;
+    uint32_t unknown3;
+    uint32_t unknown4;
+
+    uint32_t cdm_addr;
+    uint32_t cdm_size;
+    uint32_t settings_addr;
+    uint32_t striping_addr;
+    uint32_t cdm_addr2;
+
+    uint32_t req_id;
+    uint64_t handle;
+  } bps_tmp;
+
+  typedef struct {
+    uint32_t a;
+    uint32_t n;
+    unsigned base : 32;
+    unsigned unused : 12;
+    unsigned length : 20;
+    uint32_t p;
+    uint32_t u;
+    uint32_t h;
+    uint32_t b;
+  } cdm_tmp;
+
+  // *** cmd buf ***
+  struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
+  {
+    pkt->num_cmd_buf = 2;
+    pkt->kmd_cmd_buf_index = -1;
+    pkt->kmd_cmd_buf_offset = 0;
+
+    buf_desc[0].meta_data = 0;
+    buf_desc[0].mem_handle = bps_cmd.handle;
+    buf_desc[0].type = CAM_CMD_BUF_FW;
+    buf_desc[0].offset = bps_cmd.aligned_size()*idx;
+
+    buf_desc[0].length = sizeof(bps_tmp) + sizeof(cdm_tmp);
+    buf_desc[0].size = buf_desc[0].length;
+
+    // rest gets patched in
+    bps_tmp *fp = (bps_tmp *)((unsigned char *)bps_cmd.ptr + buf_desc[0].offset);
+    memset(fp, 0, buf_desc[0].length);
+    fp->handle = (uint64_t)icp_dev_handle;
+    fp->cdm_size = bps_cdm_striping_bl.size;   // this comes from the striping lib create call
+    fp->req_id = 0; // why always 0?
+
+    cdm_tmp *pa = (cdm_tmp *)((unsigned char *)fp + sizeof(bps_tmp));
+    pa->a = 0;
+    pa->n = 1;
+    pa->p = 20;  // GENERIC
+    pa->u = 0;
+    pa->h = 0;
+    pa->b = 0;
+    pa->unused = 0;
+    pa->base = 0; // this gets patched
+
+    int cdm_len = 0;
+
+    // debayer params
+    cdm_len += write_cont((unsigned char *)bps_cdm_program_array.ptr + cdm_len, 0x2868, {
+      0x06900400,
+      0x000006a6,
+      0x00000000,
+      0x00000000,
+    });
+    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,
+    });
+
+    pa->length = cdm_len - 1;
+
+    // *** second command ***
+    // parsed by cam_icp_packet_generic_blob_handler
+    struct isp_packet {
+      uint32_t header;
+      struct cam_icp_clk_bw_request clk;
+    } __attribute__((packed)) tmp;
+    tmp.header = CAM_ICP_CMD_GENERIC_BLOB_CLK;
+    tmp.header |= (sizeof(cam_icp_clk_bw_request)) << 8;
+    tmp.clk.budget_ns = 0x1fca058;
+    tmp.clk.frame_cycles = 2329024; // comes from the striping lib
+    tmp.clk.rt_flag = 0x0;
+    tmp.clk.uncompressed_bw = 0x38512180;
+    tmp.clk.compressed_bw = 0x38512180;
+
+    buf_desc[1].size = sizeof(tmp);
+    buf_desc[1].offset = 0;
+    buf_desc[1].length = buf_desc[1].size - buf_desc[1].offset;
+    buf_desc[1].type = CAM_CMD_BUF_GENERIC;
+    buf_desc[1].meta_data = CAM_ICP_CMD_META_GENERIC_BLOB;
+    auto buf2 = mm.alloc<uint32_t>(buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle);
+    memcpy(buf2.get(), &tmp, sizeof(tmp));
+  }
+
+  // *** io config ***
+  pkt->num_io_configs = 2;
+  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);
+  {
+    // input frame
+    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){
+      .width = sensor->frame_width,
+      .height = sensor->frame_height + sensor->extra_height,
+      .plane_stride = sensor->frame_stride,
+      .slice_height = sensor->frame_height + sensor->extra_height,
+    };
+    io_cfg[0].format = sensor->mipi_format;
+    io_cfg[0].color_space = CAM_COLOR_SPACE_BASE;
+    io_cfg[0].color_pattern = 0x5;
+    io_cfg[0].bpp = (sensor->mipi_format == CAM_FORMAT_MIPI_RAW_10 ? 0xa : 0xc);
+    io_cfg[0].resource_type = CAM_ICP_BPS_INPUT_IMAGE;
+    io_cfg[0].fence = sync_objs[idx];
+    io_cfg[0].direction = CAM_BUF_INPUT;
+    io_cfg[0].subsample_pattern = 0x1;
+    io_cfg[0].framedrop_pattern = 0x1;
+
+    // output frame
+    io_cfg[1].mem_handle[0] = buf_handle_yuv[idx];
+    io_cfg[1].mem_handle[1] = buf_handle_yuv[idx];
+    io_cfg[1].planes[0] = (struct cam_plane_cfg){
+      .width = sensor->frame_width,
+      .height = sensor->frame_height,
+      .plane_stride = stride,
+      .slice_height = y_height,
+    };
+    io_cfg[1].planes[1] = (struct cam_plane_cfg){
+      .width = sensor->frame_width,
+      .height = sensor->frame_height/2,
+      .plane_stride = stride,
+      .slice_height = uv_height,
+    };
+    io_cfg[1].offsets[1] = ALIGNED_SIZE(io_cfg[1].planes[0].plane_stride*io_cfg[1].planes[0].slice_height, 0x1000);
+    assert(io_cfg[1].offsets[1] == uv_offset);
+
+    io_cfg[1].format = CAM_FORMAT_NV12;  // TODO: why is this 21 in the dump? should be 12
+    io_cfg[1].color_space = CAM_COLOR_SPACE_BT601_FULL;
+    io_cfg[1].resource_type = CAM_ICP_BPS_OUTPUT_IMAGE_FULL;
+    io_cfg[1].fence = sync_objs_bps_out[idx];
+    io_cfg[1].direction = CAM_BUF_OUTPUT;
+    io_cfg[1].subsample_pattern = 0x1;
+    io_cfg[1].framedrop_pattern = 0x1;
+  }
+
+  // *** patches ***
+  {
+    pkt->patch_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf + sizeof(struct cam_buf_io_cfg)*pkt->num_io_configs;
+
+    // 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);
+
+    // output frame
+    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[0]), buf_handle_yuv[idx], 0);
+    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, frames[1].ptr[1]), buf_handle_yuv[idx], io_cfg[1].offsets[1]);
+
+    // rest of buffers
+    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, settings_addr), bps_iq.handle, 0);
+    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, cdm_addr2), bps_cmd.handle, sizeof(bps_tmp));
+    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + 0xc8, bps_cdm_program_array.handle, 0);
+    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, striping_addr), bps_striping.handle, 0);
+    add_patch(pkt.get(), bps_cmd.handle, buf_desc[0].offset + offsetof(bps_tmp, cdm_addr), bps_cdm_striping_bl.handle, 0);
+  }
+
+  int ret = device_config(m->icp_fd, session_handle, icp_dev_handle, cam_packet_handle);
+  assert(ret == 0);
 }
 
 void SpectraCamera::config_ife(int idx, int request_id, bool init) {
@@ -650,21 +852,18 @@ void SpectraCamera::config_ife(int idx, int request_id, bool init) {
     // order here corresponds to the one in build_initial_config
     assert(patches.size() == 6 || patches.size() == 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) {
-      void *p = (char*)&pkt->payload + pkt->patch_offset;
-
+    if (patches.size() > 0) {
       // linearization LUT
-      add_patch(p, 0, ife_cmd.handle, patches[0], ife_linearization_lut.handle, 0);
+      add_patch(pkt.get(), ife_cmd.handle, patches[0], ife_linearization_lut.handle, 0);
 
       // vignetting correction LUTs
-      add_patch(p, 1, ife_cmd.handle, patches[1], ife_vignetting_lut.handle, 0);
-      add_patch(p, 2, ife_cmd.handle, patches[2], ife_vignetting_lut.handle, ife_vignetting_lut.size);
+      add_patch(pkt.get(), ife_cmd.handle, patches[1], ife_vignetting_lut.handle, 0);
+      add_patch(pkt.get(), ife_cmd.handle, patches[2], ife_vignetting_lut.handle, ife_vignetting_lut.size);
 
       // gamma LUTs
       for (int i = 0; i < 3; i++) {
-        add_patch(p, i+3, ife_cmd.handle, patches[i+3], ife_gamma_lut.handle, ife_gamma_lut.size*i);
+        add_patch(pkt.get(), ife_cmd.handle, patches[i+3], ife_gamma_lut.handle, ife_gamma_lut.size*i);
       }
     }
   }
@@ -937,11 +1136,19 @@ void SpectraCamera::configICP() {
     Configures both the ICP and BPS.
   */
 
+  int cfg_handle;
+
+  uint32_t cfg_size = sizeof(bps_cfg[0]) / sizeof(bps_cfg[0][0]);
+  void *cfg = alloc_w_mmu_hdl(m->video0_fd, cfg_size, (uint32_t*)&cfg_handle, 0x1,
+                              CAM_MEM_FLAG_HW_READ_WRITE | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_HW_SHARED_ACCESS,
+                              m->icp_device_iommu);
+  memcpy(cfg, bps_cfg[sensor->num()], cfg_size);
+
   struct cam_icp_acquire_dev_info icp_info = {
     .scratch_mem_size = 0x0,
-    .dev_type = 0x1,  // BPS
-    .io_config_cmd_size = 0,
-    .io_config_cmd_handle = 0,
+    .dev_type = CAM_ICP_RES_TYPE_BPS,
+    .io_config_cmd_size = cfg_size,
+    .io_config_cmd_handle = cfg_handle,
     .secure_mode = 0,
     .num_out_res = 1,
     .in_res = (struct cam_icp_res_info){
@@ -962,24 +1169,35 @@ void SpectraCamera::configICP() {
   icp_dev_handle = *h;
   LOGD("acquire icp dev");
 
-  // BPS CMD buffer
-  unsigned char striping_out[] = "\x00";
-  bps_cmd.init(m, ife_buf_depth*ALIGNED_SIZE(464, 0x20), 0x20,
+  release(m->video0_fd, cfg_handle);
+
+  // BPS has a lot of buffers to init
+  bps_cmd.init(m, 464, 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);
+               m->icp_device_iommu, 0, ife_buf_depth);
 
-  bps_iq.init(m, 560, 0x20,
+  // BPSIQSettings struct
+  uint32_t settings_size = sizeof(bps_settings[0]) / sizeof(bps_settings[0][0]);
+  bps_iq.init(m, settings_size, 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);
-  bps_cdm_program_array.init(m, 0x40, 0x20,
+  memcpy(bps_iq.ptr, bps_settings[sensor->num()], settings_size);
+
+  // for cdm register writes, just make it bigger than you need
+  bps_cdm_program_array.init(m, 0x1000, 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);
-  bps_striping.init(m, sizeof(striping_out), 0x20,
+
+  // striping lib output
+  uint32_t striping_size = sizeof(bps_striping_output[0]) / sizeof(bps_striping_output[0][0]);
+  bps_striping.init(m, striping_size, 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_striping.ptr, striping_out, sizeof(striping_out));
+  memcpy(bps_striping.ptr, bps_striping_output[sensor->num()], striping_size);
 
-  bps_cdm_striping_bl.init(m,  65216, 0x20,
+  // used internally by the BPS, we just allocate it.
+  // size comes from the BPSStripingLib
+  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);
 }

system/camerad/sensors/ar0231.cc

@@ -1,7 +1,6 @@
 #include <cassert>
 #include <cmath>
 
-#include "common/swaglog.h"
 #include "system/camerad/sensors/sensor.h"
 
 namespace {

system/camerad/sensors/sensor.h

@@ -81,6 +81,10 @@ public:
   std::vector<uint32_t> linearization_lut;     // length 36
   std::vector<uint32_t> linearization_pts;     // length 4
   std::vector<uint32_t> vignetting_lut;        // length 221
+
+  const int num() const {
+    return static_cast<int>(image_sensor);
+  };
 };
 
 class AR0231 : public SensorInfo {