Prepare for using the ISP (#23621)

* parse out isp packet with structs

* dsp mode

* support only driver / comments for yuv

* minor touchups

* DEBUG_FRAMES

Co-authored-by: Comma Device <device@comma.ai>
old-commit-hash: cb6a68373b

selfdrive/camerad/cameras/camera_common.h

@@ -41,6 +41,11 @@ const bool env_send_driver = getenv("SEND_DRIVER") != NULL;
 const bool env_send_road = getenv("SEND_ROAD") != NULL;
 const bool env_send_wide_road = getenv("SEND_WIDE_ROAD") != NULL;
 
+// for debugging
+// note: ONLY_ROAD doesn't work, likely due to a mixup with wideRoad cam in the kernel
+const bool env_only_driver = getenv("ONLY_DRIVER") != NULL;
+const bool env_debug_frames = getenv("DEBUG_FRAMES") != NULL;
+
 typedef void (*release_cb)(void *cookie, int buf_idx);
 
 typedef struct CameraInfo {

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -60,7 +60,7 @@ int cam_control(int fd, int op_code, void *handle, int size) {
   struct cam_control camcontrol = {0};
   camcontrol.op_code = op_code;
   camcontrol.handle = (uint64_t)handle;
-  if (size == 0) { 
+  if (size == 0) {
     camcontrol.size = 8;
     camcontrol.handle_type = CAM_HANDLE_MEM_HANDLE;
   } else {
@@ -353,6 +353,9 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
   struct cam_packet *pkt = (struct cam_packet *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, size, &cam_packet_handle);
   pkt->num_cmd_buf = 2;
   pkt->kmd_cmd_buf_index = 0;
+  // YUV has kmd_cmd_buf_offset = 1780
+  // I guess this is the ISP command
+  // YUV also has patch_offset = 0x1030 and num_patches = 10
 
   if (io_mem_handle != 0) {
     pkt->io_configs_offset = sizeof(struct cam_cmd_buf_desc)*2;
@@ -377,28 +380,65 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
   buf_desc[0].mem_handle = buf0_mem_handle;
   buf_desc[0].offset = buf0_offset;
 
-   // cam_isp_packet_generic_blob_handler
+  // parsed by cam_isp_packet_generic_blob_handler
-  uint32_t tmp[] = {
+  struct isp_packet {
-    // size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG)
+    uint32_t type_0;
-    0x2000,
+    cam_isp_resource_hfr_config resource_hfr;
-    0x1, 0x0, CAM_ISP_IFE_OUT_RES_RDI_0, 0x1, 0x0, 0x1, 0x0, 0x0, // 1 port, CAM_ISP_IFE_OUT_RES_RDI_0
+
-    // size is 0x38, type is 1(CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG), clocks
+    uint32_t type_1;
-    0x3801,
+    cam_isp_clock_config clock;
-    0x1, 0x4, // Dual mode, 4 RDI wires
+    uint64_t extra_rdi_hz[3];
-    0x18148d00, 0x0, 0x18148d00, 0x0, 0x18148d00, 0x0, // rdi clock
+
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0,  // junk?
+    uint32_t type_2;
-    // offset 0x60
+    cam_isp_bw_config bw;
-    // size is 0xe0, type is 2(CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG), bandwidth
+    struct cam_isp_bw_vote extra_rdi_vote[6];
-    0xe002,
+  } __attribute__((packed)) tmp;
-    0x1, 0x4, // 4 RDI
+  memset(&tmp, 0, sizeof(tmp));
-    0x0, 0x0, 0x1ad27480, 0x0, 0x1ad27480, 0x0, // left_pix_vote
+
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // right_pix_vote
+  tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG;
-    0x0, 0x0, 0x6ee11c0, 0x2, 0x6ee11c0, 0x2,  // rdi_vote
+  tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8;
-    0x0, 0x0, 0x0, 0x0,
+  static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20);
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+  tmp.resource_hfr = {
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+    .num_ports = 1,  // 10 for YUV (but I don't think we need them)
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
+    .port_hfr_config[0] = {
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+      .resource_type = CAM_ISP_IFE_OUT_RES_RDI_0, // CAM_ISP_IFE_OUT_RES_FULL for YUV
+      .subsample_pattern = 1,
+      .subsample_period = 0,
+      .framedrop_pattern = 1,
+      .framedrop_period = 0,
+    }};
+
+  tmp.type_1 = CAM_ISP_GENERIC_BLOB_TYPE_CLOCK_CONFIG;
+  tmp.type_1 |= (sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) << 8;
+  static_assert((sizeof(cam_isp_clock_config) + sizeof(tmp.extra_rdi_hz)) == 0x38);
+  tmp.clock = {
+    .usage_type = 1, // dual mode
+    .num_rdi = 4,
+    .left_pix_hz = 404000000,
+    .right_pix_hz = 404000000,
+    .rdi_hz[0] = 404000000,
+  };
+
+
+  tmp.type_2 = CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG;
+  tmp.type_2 |= (sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) << 8;
+  static_assert((sizeof(cam_isp_bw_config) + sizeof(tmp.extra_rdi_vote)) == 0xe0);
+  tmp.bw = {
+    .usage_type = 1, // dual mode
+    .num_rdi = 4,
+    .left_pix_vote = {
+      .resource_id = 0,
+      .cam_bw_bps = 450000000,
+      .ext_bw_bps = 450000000,
+    },
+    .rdi_vote[0] = {
+      .resource_id = 0,
+      .cam_bw_bps = 8706200000,
+      .ext_bw_bps = 8706200000,
+    },
+  };
+
+  static_assert(offsetof(struct isp_packet, type_2) == 0x60);
 
   buf_desc[1].size = sizeof(tmp);
   buf_desc[1].offset = io_mem_handle != 0 ? 0x60 : 0;
@@ -406,7 +446,7 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
   buf_desc[1].type = CAM_CMD_BUF_GENERIC;
   buf_desc[1].meta_data = CAM_ISP_PACKET_META_GENERIC_BLOB_COMMON;
   uint32_t *buf2 = (uint32_t *)alloc_w_mmu_hdl(s->multi_cam_state->video0_fd, buf_desc[1].size, (uint32_t*)&buf_desc[1].mem_handle, 0x20);
-  memcpy(buf2, tmp, sizeof(tmp));
+  memcpy(buf2, &tmp, sizeof(tmp));
 
   if (io_mem_handle != 0) {
     io_cfg[0].mem_handle[0] = io_mem_handle;
@@ -415,19 +455,20 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
 		 .height = FRAME_HEIGHT,
 		 .plane_stride = FRAME_STRIDE,
 		 .slice_height = FRAME_HEIGHT,
-		 .meta_stride = 0x0,
+		 .meta_stride = 0x0,    // YUV has meta(stride=0x400, size=0x5000)
 		 .meta_size = 0x0,
 		 .meta_offset = 0x0,
-		 .packer_config = 0x0,
+		 .packer_config = 0x0,  // 0xb for YUV
-		 .mode_config = 0x0,
+		 .mode_config = 0x0,    // 0x9ef for YUV
 		 .tile_config = 0x0,
 		 .h_init = 0x0,
 		 .v_init = 0x0,
 		};
-    io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;
+    io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;             // CAM_FORMAT_UBWC_TP10 for YUV
-    io_cfg[0].color_pattern = 0x5;
+    io_cfg[0].color_space = CAM_COLOR_SPACE_BASE;          // CAM_COLOR_SPACE_BT601_FULL for YUV
-    io_cfg[0].bpp = 0xc;
+    io_cfg[0].color_pattern = 0x5;                         // 0x0 for YUV
-    io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;
+    io_cfg[0].bpp = 0xa;
+    io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;   // CAM_ISP_IFE_OUT_RES_FULL for YUV
     io_cfg[0].fence = fence;
     io_cfg[0].direction = CAM_BUF_OUTPUT;
     io_cfg[0].subsample_pattern = 0x1;
@@ -553,18 +594,14 @@ int open_v4l_by_name_and_index(const char name[], int index, int flags = O_RDWR
 static void camera_open(CameraState *s) {
   s->sensor_fd = open_v4l_by_name_and_index("cam-sensor-driver", s->camera_num);
   assert(s->sensor_fd >= 0);
-  LOGD("opened sensor");
+  LOGD("opened sensor for %d", s->camera_num);
-
-  s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
-  assert(s->csiphy_fd >= 0);
-  LOGD("opened csiphy");
 
   // probe the sensor
   LOGD("-- Probing sensor %d", s->camera_num);
   sensors_init(s->multi_cam_state->video0_fd, s->sensor_fd, s->camera_num);
 
   // create session
-  struct cam_req_mgr_session_info session_info = {}; 
+  struct cam_req_mgr_session_info session_info = {};
   int ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_CREATE_SESSION, &session_info, sizeof(session_info));
   LOGD("get session: %d 0x%X", ret, session_info.session_hdl);
   s->session_handle = session_info.session_hdl;
@@ -605,7 +642,7 @@ static void camera_open(CameraState *s) {
 
       .pixel_clk = 0x0,
       .batch_size = 0x0,
-      .dsp_mode = 0x0,
+      .dsp_mode = CAM_ISP_DSP_MODE_NONE,
       .hbi_cnt = 0x0,
       .custom_csid = 0x0,
 
@@ -627,9 +664,13 @@ static void camera_open(CameraState *s) {
 
   auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource);
   assert(isp_dev_handle);
-  s->isp_dev_handle = *isp_dev_handle; 
+  s->isp_dev_handle = *isp_dev_handle;
   LOGD("acquire isp dev");
 
+  s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
+  assert(s->csiphy_fd >= 0);
+  LOGD("opened csiphy for %d", s->camera_num);
+
   struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
   auto csiphy_dev_handle = device_acquire(s->csiphy_fd, s->session_handle, &csiphy_acquire_dev_info);
   assert(csiphy_dev_handle);
@@ -645,6 +686,7 @@ static void camera_open(CameraState *s) {
   //sensors_i2c(s, start_reg_array, std::size(start_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMON);
   //sensors_i2c(s, stop_reg_array, std::size(stop_reg_array), CAM_SENSOR_PACKET_OPCODE_SENSOR_STREAMOFF);
 
+
   // config csiphy
   LOG("-- Config CSI PHY");
   {
@@ -686,8 +728,8 @@ static void camera_open(CameraState *s) {
   req_mgr_link_info.dev_hdls[0] = s->isp_dev_handle;
   req_mgr_link_info.dev_hdls[1] = s->sensor_dev_handle;
   ret = cam_control(s->multi_cam_state->video0_fd, CAM_REQ_MGR_LINK, &req_mgr_link_info, sizeof(req_mgr_link_info));
-  LOGD("link: %d", ret);
   s->link_handle = req_mgr_link_info.link_hdl;
+  LOGD("link: %d hdl: 0x%X", ret, s->link_handle);
 
   struct cam_req_mgr_link_control req_mgr_link_control = {0};
   req_mgr_link_control.ops = CAM_REQ_MGR_LINK_ACTIVATE;
@@ -708,15 +750,17 @@ static void camera_open(CameraState *s) {
 }
 
 void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
-  camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
-              VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
-  printf("road camera initted \n");
-  camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
-              VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
-  printf("wide road camera initted \n");
   camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
               VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
   printf("driver camera initted \n");
+  if (!env_only_driver) {
+    camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
+                VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
+    printf("road camera initted \n");
+    camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
+                VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
+    printf("wide road camera initted \n");
+  }
 
   s->sm = new SubMaster({"driverState"});
   s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"});
@@ -763,12 +807,14 @@ void cameras_open(MultiCameraState *s) {
   ret = HANDLE_EINTR(ioctl(s->video0_fd, VIDIOC_SUBSCRIBE_EVENT, &sub));
   printf("req mgr subscribe: %d\n", ret);
 
-  camera_open(&s->road_cam);
-  printf("road camera opened \n");
-  camera_open(&s->wide_road_cam);
-  printf("wide road camera opened \n");
   camera_open(&s->driver_cam);
   printf("driver camera opened \n");
+  if (!env_only_driver) {
+    camera_open(&s->road_cam);
+    printf("road camera opened \n");
+    camera_open(&s->wide_road_cam);
+    printf("wide road camera opened \n");
+  }
 }
 
 static void camera_close(CameraState *s) {
@@ -816,9 +862,11 @@ static void camera_close(CameraState *s) {
 }
 
 void cameras_close(MultiCameraState *s) {
-  camera_close(&s->road_cam);
-  camera_close(&s->wide_road_cam);
   camera_close(&s->driver_cam);
+  if (!env_only_driver) {
+    camera_close(&s->road_cam);
+    camera_close(&s->wide_road_cam);
+  }
 
   delete s->sm;
   delete s->pm;
@@ -1010,16 +1058,20 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
 void cameras_run(MultiCameraState *s) {
   LOG("-- Starting threads");
   std::vector<std::thread> threads;
-  threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
   threads.push_back(start_process_thread(s, &s->driver_cam, common_process_driver_camera));
-  threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
+  if (!env_only_driver) {
+    threads.push_back(start_process_thread(s, &s->road_cam, process_road_camera));
+    threads.push_back(start_process_thread(s, &s->wide_road_cam, process_road_camera));
+  }
 
   // start devices
   LOG("-- Starting devices");
   int start_reg_len = sizeof(start_reg_array) / sizeof(struct i2c_random_wr_payload);
-  sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
-  sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
   sensors_i2c(&s->driver_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+  if (!env_only_driver) {
+    sensors_i2c(&s->road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+    sensors_i2c(&s->wide_road_cam, start_reg_array, start_reg_len, CAM_SENSOR_PACKET_OPCODE_SENSOR_CONFIG);
+  }
 
   // poll events
   LOG("-- Dequeueing Video events");
@@ -1043,8 +1095,10 @@ void cameras_run(MultiCameraState *s) {
     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;
-        // LOGD("v4l2 event: sess_hdl %d, link_hdl %d, frame_id %d, req_id %lld, timestamp 0x%llx, 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, event_data->u.frame_msg.sof_status);
+        // LOGD("v4l2 event: sess_hdl 0x%X, link_hdl 0x%X, frame_id %d, req_id %lld, timestamp 0x%llx, 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, event_data->u.frame_msg.sof_status);
-        // printf("sess_hdl %d, link_hdl %d, frame_id %lu, req_id %lu, timestamp 0x%lx, 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, event_data->u.frame_msg.sof_status);
+        if (env_debug_frames) {
+          printf("sess_hdl 0x%X, link_hdl 0x%X, frame_id %lu, req_id %lu, timestamp 0x%lx, 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, event_data->u.frame_msg.sof_status);
+        }
 
         if (event_data->session_hdl == s->road_cam.session_handle) {
           handle_camera_event(&s->road_cam, event_data);