camera_qcom2: new functions device_acquire & device_config (#22077)

* new function device_acquire & device_config

* add space

* apply reviews

* use the specified struct for each command

* fix typo

* apply review

* remove static

* revert device_control
old-commit-hash: 0d3eec385c

selfdrive/camerad/cameras/camera_qcom2.cc

@@ -76,12 +76,30 @@ int cam_control(int fd, int op_code, void *handle, int size) {
   return ret;
 }
 
+std::optional<int32_t> device_acquire(int fd, int32_t session_handle, void *data) {
+  struct cam_acquire_dev_cmd cmd = {
+      .session_handle = session_handle,
+      .handle_type = CAM_HANDLE_USER_POINTER,
+      .num_resources = (uint32_t)(data ? 1 : 0),
+      .resource_hdl = (uint64_t)data,
+  };
+  int err = cam_control(fd, CAM_ACQUIRE_DEV, &cmd, sizeof(cmd));
+  return err == 0 ? std::make_optional(cmd.dev_handle) : std::nullopt;
+};
+
+int device_config(int fd, int32_t session_handle, int32_t dev_handle, uint64_t packet_handle) {
+  struct cam_config_dev_cmd cmd = {
+      .session_handle = session_handle,
+      .dev_handle = dev_handle,
+      .packet_handle = packet_handle,
+  };
+  return cam_control(fd, CAM_CONFIG_DEV, &cmd, sizeof(cmd));
+}
+
 int device_control(int fd, int op_code, int session_handle, int dev_handle) {
   // start stop and release are all the same
-  static struct cam_release_dev_cmd release_dev_cmd;
+  struct cam_start_stop_dev_cmd cmd { .session_handle = session_handle, .dev_handle = dev_handle };
-  release_dev_cmd.session_handle = session_handle;
+  return cam_control(fd, op_code, &cmd, sizeof(cmd));
-  release_dev_cmd.dev_handle = dev_handle;
-  return cam_control(fd, op_code, &release_dev_cmd, sizeof(release_dev_cmd));
 }
 
 void *alloc_w_mmu_hdl(int video0_fd, int len, uint32_t *handle, int align = 8, int flags = CAM_MEM_FLAG_KMD_ACCESS | CAM_MEM_FLAG_UMD_ACCESS | CAM_MEM_FLAG_CMD_BUF_TYPE,
@@ -151,13 +169,7 @@ void sensors_poke(struct CameraState *s, int request_id) {
   pkt->header.op_code = 0x7f;
   pkt->header.request_id = request_id;
 
-  struct cam_config_dev_cmd config_dev_cmd = {};
+  int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
-  config_dev_cmd.session_handle = s->session_handle;
-  config_dev_cmd.dev_handle = s->sensor_dev_handle;
-  config_dev_cmd.offset = 0;
-  config_dev_cmd.packet_handle = cam_packet_handle;
-
-  int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
   assert(ret == 0);
 
   munmap(pkt, size);
@@ -186,13 +198,7 @@ void sensors_i2c(struct CameraState *s, struct i2c_random_wr_payload* dat, int l
   i2c_random_wr->header.addr_type = CAMERA_SENSOR_I2C_TYPE_WORD;
   memcpy(i2c_random_wr->random_wr_payload, dat, len*sizeof(struct i2c_random_wr_payload));
 
-  struct cam_config_dev_cmd config_dev_cmd = {};
+  int ret = device_config(s->sensor_fd, s->session_handle, s->sensor_dev_handle, cam_packet_handle);
-  config_dev_cmd.session_handle = s->session_handle;
-  config_dev_cmd.dev_handle = s->sensor_dev_handle;
-  config_dev_cmd.offset = 0;
-  config_dev_cmd.packet_handle = cam_packet_handle;
-
-  int ret = cam_control(s->sensor_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
   assert(ret == 0);
 
   munmap(i2c_random_wr, buf_desc[0].size);
@@ -432,13 +438,8 @@ void config_isp(struct CameraState *s, int io_mem_handle, int fence, int request
     io_cfg[0].framedrop_pattern = 0x1;
   }
 
-  struct cam_config_dev_cmd config_dev_cmd = {};
+  int ret = device_config(s->multi_cam_state->isp_fd, s->session_handle, s->isp_dev_handle, cam_packet_handle);
-  config_dev_cmd.session_handle = s->session_handle;
+  assert(ret == 0);
-  config_dev_cmd.dev_handle = s->isp_dev_handle;
-  config_dev_cmd.offset = 0;
-  config_dev_cmd.packet_handle = cam_packet_handle;
-
-  int ret = cam_control(s->multi_cam_state->isp_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
   if (ret != 0) {
     printf("ISP CONFIG FAILED\n");
   }
@@ -575,20 +576,12 @@ static void camera_open(CameraState *s) {
 
   // access the sensor
   LOGD("-- Accessing sensor");
-  static struct cam_acquire_dev_cmd acquire_dev_cmd = {0};
+  auto sensor_dev_handle = device_acquire(s->sensor_fd, s->session_handle, nullptr);
-  acquire_dev_cmd.session_handle = s->session_handle;
+  assert(sensor_dev_handle);
-  acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER;
+  s->sensor_dev_handle = *sensor_dev_handle;
-  ret = cam_control(s->sensor_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
+  LOGD("acquire sensor dev");
-  LOGD("acquire sensor dev: %d", ret);
-  s->sensor_dev_handle = acquire_dev_cmd.dev_handle;
 
   static struct cam_isp_resource isp_resource = {0};
-
-  acquire_dev_cmd.session_handle = s->session_handle;
-  acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER;
-  acquire_dev_cmd.num_resources = 1;
-  acquire_dev_cmd.resource_hdl = (uint64_t)&isp_resource;
-
   isp_resource.resource_id = CAM_ISP_RES_ID_PORT;
   isp_resource.length = sizeof(struct cam_isp_in_port_info) + sizeof(struct cam_isp_out_port_info)*(1-1);
   isp_resource.handle_type = CAM_HANDLE_USER_POINTER;
@@ -650,23 +643,17 @@ static void camera_open(CameraState *s) {
     .comp_grp_id = 0x0, .split_point = 0x0, .secure_mode = 0x0,
   };
 
-  ret = cam_control(s->multi_cam_state->isp_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
+  auto isp_dev_handle = device_acquire(s->multi_cam_state->isp_fd, s->session_handle, &isp_resource);
-  LOGD("acquire isp dev: %d", ret);
+  assert(isp_dev_handle);
+  s->isp_dev_handle = *isp_dev_handle; 
+  LOGD("acquire isp dev");
   free(in_port_info);
-  s->isp_dev_handle = acquire_dev_cmd.dev_handle;
 
-  static struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {0};
+  struct cam_csiphy_acquire_dev_info csiphy_acquire_dev_info = {.combo_mode = 0};
-  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);
-  acquire_dev_cmd.session_handle = s->session_handle;
+  s->csiphy_dev_handle = *csiphy_dev_handle;
-  acquire_dev_cmd.handle_type = CAM_HANDLE_USER_POINTER;
+  LOGD("acquire csiphy dev");
-  acquire_dev_cmd.num_resources = 1;
-  acquire_dev_cmd.resource_hdl = (uint64_t)&csiphy_acquire_dev_info;
-
-  ret = cam_control(s->csiphy_fd, CAM_ACQUIRE_DEV, &acquire_dev_cmd, sizeof(acquire_dev_cmd));
-
-  LOGD("acquire csiphy dev: %d", ret);
-  s->csiphy_dev_handle = acquire_dev_cmd.dev_handle;
 
   // acquires done
 
@@ -706,13 +693,7 @@ static void camera_open(CameraState *s) {
     csiphy_info->settle_time = MIPI_SETTLE_CNT * 200000000ULL;
     csiphy_info->data_rate = 48000000;  // Calculated by camera_freqs.py
 
-    static struct cam_config_dev_cmd config_dev_cmd = {};
+    int ret = device_config(s->csiphy_fd, s->session_handle, s->csiphy_dev_handle, cam_packet_handle);
-    config_dev_cmd.session_handle = s->session_handle;
-    config_dev_cmd.dev_handle = s->csiphy_dev_handle;
-    config_dev_cmd.offset = 0;
-    config_dev_cmd.packet_handle = cam_packet_handle;
-
-    int ret = cam_control(s->csiphy_fd, CAM_CONFIG_DEV, &config_dev_cmd, sizeof(config_dev_cmd));
     assert(ret == 0);
 
     munmap(csiphy_info, buf_desc[0].size);

selfdrive/camerad/cameras/camera_qcom2.h

@@ -32,10 +32,9 @@ typedef struct CameraState {
   int camera_num;
 
   int32_t session_handle;
-
+  int32_t sensor_dev_handle;
-  uint32_t sensor_dev_handle;
+  int32_t isp_dev_handle;
-  uint32_t isp_dev_handle;
+  int32_t csiphy_dev_handle;
-  uint32_t csiphy_dev_handle;
 
   int32_t link_handle;