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
3 years ago · abfc6de831
parent c6e3e83a01
commit abfc6de831
2 changed files with 117 additions and 58 deletions
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/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 {
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ b/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
-  uint32_t tmp[] = {
-    // size is 0x20, type is 0(CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG)
-    0x2000,
-    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
-    0x3801,
-    0x1, 0x4, // Dual mode, 4 RDI wires
-    0x18148d00, 0x0, 0x18148d00, 0x0, 0x18148d00, 0x0, // rdi clock
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0,  // junk?
-    // offset 0x60
-    // size is 0xe0, type is 2(CAM_ISP_GENERIC_BLOB_TYPE_BW_CONFIG), bandwidth
-    0xe002,
-    0x1, 0x4, // 4 RDI
-    0x0, 0x0, 0x1ad27480, 0x0, 0x1ad27480, 0x0, // left_pix_vote
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, // right_pix_vote
-    0x0, 0x0, 0x6ee11c0, 0x2, 0x6ee11c0, 0x2,  // rdi_vote
-    0x0, 0x0, 0x0, 0x0,
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0,
-    0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0};
+  // parsed by cam_isp_packet_generic_blob_handler
+  struct isp_packet {
+    uint32_t type_0;
+    cam_isp_resource_hfr_config resource_hfr;
+
+    uint32_t type_1;
+    cam_isp_clock_config clock;
+    uint64_t extra_rdi_hz[3];
+
+    uint32_t type_2;
+    cam_isp_bw_config bw;
+    struct cam_isp_bw_vote extra_rdi_vote[6];
+  } __attribute__((packed)) tmp;
+  memset(&tmp, 0, sizeof(tmp));
+
+  tmp.type_0 = CAM_ISP_GENERIC_BLOB_TYPE_HFR_CONFIG;
+  tmp.type_0 |= sizeof(cam_isp_resource_hfr_config) << 8;
+  static_assert(sizeof(cam_isp_resource_hfr_config) == 0x20);
+  tmp.resource_hfr = {
+    .num_ports = 1,  // 10 for YUV (but I don't think we need them)
+    .port_hfr_config[0] = {
+      .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,
-		 .mode_config = 0x0,
+		 .packer_config = 0x0,  // 0xb for YUV
+		 .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].color_pattern = 0x5;
-    io_cfg[0].bpp = 0xc;
-    io_cfg[0].resource_type = CAM_ISP_IFE_OUT_RES_RDI_0;
+    io_cfg[0].format = CAM_FORMAT_MIPI_RAW_10;             // CAM_FORMAT_UBWC_TP10 for YUV
+    io_cfg[0].color_space = CAM_COLOR_SPACE_BASE;          // CAM_COLOR_SPACE_BT601_FULL for YUV
+    io_cfg[0].color_pattern = 0x5;                         // 0x0 for YUV
+    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");
-
-  s->csiphy_fd = open_v4l_by_name_and_index("cam-csiphy-driver", s->camera_num);
-  assert(s->csiphy_fd >= 0);
-  LOGD("opened csiphy");
+  LOGD("opened sensor for %d", s->camera_num);

  // 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);
-        // 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);
+        // 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);
+        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);