camerad: final debayer prep (#33598)

* prep part 4?

* cleanup

* little less magic

* reorg

* final touches

---------

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

common/util.h

@@ -37,6 +37,8 @@ const double MS_TO_MPH = MS_TO_KPH * KM_TO_MILE;
 const double METER_TO_MILE = KM_TO_MILE / 1000.0;
 const double METER_TO_FOOT = 3.28084;
 
+#define ALIGNED_SIZE(x, align) (((x) + (align)-1) & ~((align)-1))
+
 namespace util {
 
 void set_thread_name(const char* name);

system/camerad/cameras/spectra.cc

@@ -395,20 +395,15 @@ int SpectraCamera::sensors_init() {
   return ret;
 }
 
-void SpectraCamera::config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset) {
-  uint32_t cam_packet_handle = 0;
-  int size = sizeof(struct cam_packet)+sizeof(struct cam_cmd_buf_desc)*2;
+void SpectraCamera::config_isp(int io_mem_handle, int fence, int request_id, int buf0_idx) {
+  printf("cam cmd buf %lu %lu\n", sizeof(struct cam_cmd_buf_desc), sizeof(struct cam_buf_io_cfg));
+  int size = sizeof(struct cam_packet) + sizeof(struct cam_cmd_buf_desc)*2;
   if (io_mem_handle != 0) {
     size += sizeof(struct cam_buf_io_cfg);
   }
-  auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
-  pkt->num_cmd_buf = 2;
-  pkt->kmd_cmd_buf_index = 0;
 
-  if (io_mem_handle != 0) {
-    pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*pkt->num_cmd_buf;
-    pkt->num_io_configs = 1;
-  }
+  uint32_t cam_packet_handle = 0;
+  auto pkt = mm.alloc<struct cam_packet>(size, &cam_packet_handle);
 
   if (io_mem_handle != 0) {
     pkt->header.op_code = 0xf000001;
@@ -418,17 +413,34 @@ void SpectraCamera::config_isp(int io_mem_handle, int fence, int request_id, int
     pkt->header.request_id = 1;
   }
   pkt->header.size = size;
+
+  // *** kmd cmd buf ***
+  {
+    pkt->kmd_cmd_buf_index = 0;
+    pkt->kmd_cmd_buf_offset = 0;
+  }
+
+  // *** patches ***
+  {
+    pkt->num_patches = 0;
+    pkt->patch_offset = 0x0;
+  }
+
+  // *** cmd buf ***
+  {
     struct cam_cmd_buf_desc *buf_desc = (struct cam_cmd_buf_desc *)&pkt->payload;
-  struct cam_buf_io_cfg *io_cfg = (struct cam_buf_io_cfg *)((char*)&pkt->payload + pkt->io_configs_offset);
+    pkt->num_cmd_buf = 2;
 
+    // *** first command ***
     // TODO: support MMU
-  buf_desc[0].size = 65624;
+    buf_desc[0].size = buf0_size;
     buf_desc[0].length = 0;
     buf_desc[0].type = CAM_CMD_BUF_DIRECT;
     buf_desc[0].meta_data = 3;
-  buf_desc[0].mem_handle = buf0_mem_handle;
-  buf_desc[0].offset = buf0_offset;
+    buf_desc[0].mem_handle = buf0_handle;
+    buf_desc[0].offset = ALIGNED_SIZE(buf0_size, buf0_alignment)*buf0_idx;
 
+    // *** second command ***
     // parsed by cam_isp_packet_generic_blob_handler
     struct isp_packet {
       uint32_t type_0;
@@ -495,19 +507,30 @@ void SpectraCamera::config_isp(int io_mem_handle, int fence, int request_id, int
     buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
     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 ***
   if (io_mem_handle != 0) {
+    // configure output frame
+    pkt->num_io_configs = 1;
+    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] = io_mem_handle;
+
     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,
-      .meta_stride = 0x0,  // YUV has meta(stride=0x400, size=0x5000)
+
+      // these are for UBWC, we'll want that eventually
+      .meta_stride = 0x0,
       .meta_size = 0x0,
       .meta_offset = 0x0,
-      .packer_config = 0x0,  // 0xb for YUV
-      .mode_config = 0x0,    // 0x9ef for YUV
+      .packer_config = 0x0,
+      .mode_config = 0x0,
       .tile_config = 0x0,
       .h_init = 0x0,
       .v_init = 0x0,
@@ -525,9 +548,6 @@ void SpectraCamera::config_isp(int io_mem_handle, int fence, int request_id, int
 
   int ret = device_config(m->isp_fd, session_handle, isp_dev_handle, cam_packet_handle);
   assert(ret == 0);
-  if (ret != 0) {
-    LOGE("isp config failed");
-  }
 }
 
 void SpectraCamera::enqueue_buffer(int i, bool dp) {
@@ -580,7 +600,7 @@ void SpectraCamera::enqueue_buffer(int i, bool dp) {
   sensors_poke(request_id);
 
   // submit request to the ife
-  config_isp(buf_handle[i], sync_objs[i], request_id, buf0_handle, 65632*(i+1));
+  config_isp(buf_handle[i], sync_objs[i], request_id, i);
 }
 
 void SpectraCamera::camera_map_bufs() {
@@ -639,11 +659,10 @@ bool SpectraCamera::openSensor() {
 }
 
 void SpectraCamera::configISP() {
-  // 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!)
   if (!enabled) return;
 
   struct cam_isp_in_port_info in_port_info = {
+    // ISP input to the CSID
     .res_type = cc.phy,
     .lane_type = CAM_ISP_LANE_TYPE_DPHY,
     .lane_num = 4,
@@ -674,6 +693,7 @@ void SpectraCamera::configISP() {
     .hbi_cnt = 0x0,
     .custom_csid = 0x0,
 
+    // ISP outputs
     .num_out_res = 0x1,
     .data[0] = (struct cam_isp_out_port_info){
       .res_type = CAM_ISP_IFE_OUT_RES_RDI_0,
@@ -696,9 +716,11 @@ void SpectraCamera::configISP() {
   LOGD("acquire isp dev");
 
   // config ISP
-  alloc_w_mmu_hdl(m->video0_fd, 984480, (uint32_t*)&buf0_handle, 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);
-  config_isp(0, 0, 1, buf0_handle, 0);
+  // TODO: unclear where this 15 comes from
+  alloc_w_mmu_hdl(m->video0_fd, 15*ALIGNED_SIZE(buf0_size, buf0_alignment), (uint32_t*)&buf0_handle, buf0_alignment,
+                  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);
+  config_isp(0, 0, 1, 0);
 }
 
 void SpectraCamera::configCSIPHY() {

system/camerad/cameras/spectra.h

@@ -68,7 +68,7 @@ public:
   void camera_close();
   void camera_map_bufs();
   void camera_init(VisionIpcServer *v, cl_device_id device_id, cl_context ctx);
-  void config_isp(int io_mem_handle, int fence, int request_id, int buf0_mem_handle, int buf0_offset);
+  void config_isp(int io_mem_handle, int fence, int request_id, int buf0_idx);
 
   int clear_req_queue();
   void enqueue_buffer(int i, bool dp);
@@ -101,6 +101,9 @@ public:
 
   int32_t link_handle = -1;
 
+  const int buf0_size = 65624; // unclear what this is and how it's determined, for internal ISP use? it's just copied from an ioctl dump
+  const int buf0_alignment = 0x20;
+
   int buf0_handle = 0;
   int buf_handle[FRAME_BUF_COUNT] = {};
   int sync_objs[FRAME_BUF_COUNT] = {};

system/camerad/cameras/tici.h

@@ -18,6 +18,8 @@ struct CameraConfig {
   uint32_t phy;
 };
 
+// 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,