From dd54c18bb7b8de1b90ef556b87df66a479b5a7ce Mon Sep 17 00:00:00 2001 From: Dean Lee Date: Sat, 3 Apr 2021 11:52:08 +0800 Subject: [PATCH] camera_qcom: added new function cam_ioctl (#20184) * added new function cam_ioctl * static --- selfdrive/camerad/cameras/camera_qcom.cc | 88 +++++++++--------------- 1 file changed, 34 insertions(+), 54 deletions(-) diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index 40628e52fe..12c24d122c 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -34,6 +34,13 @@ const uint16_t INFINITY_DAC = 364; extern ExitHandler do_exit; +static int cam_ioctl(int fd, unsigned long int request, void *arg, const char *log_msg = nullptr) { + int err = ioctl(fd, request, arg); + if (err != 0 && log_msg) { + LOG(util::string_format("%s: %d", log_msg, err).c_str()); + } + return err; +} // global var for AE/AF ops std::atomic road_cam_exp{{0}}; std::atomic driver_cam_exp{{0}}; @@ -371,8 +378,7 @@ static void sensors_init(MultiCameraState *s) { slave_info.power_setting_array.power_setting = &slave_info.power_setting_array.power_setting_a[0]; slave_info.power_setting_array.power_down_setting = &slave_info.power_setting_array.power_down_setting_a[0]; sensor_init_cfg_data sensor_init_cfg = {.cfgtype = CFG_SINIT_PROBE, .cfg.setting = &slave_info}; - int err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg); - LOG("sensor init cfg (road camera): %d", err); + int err = cam_ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg, "sensor init cfg (road camera)"); assert(err >= 0); struct msm_camera_sensor_slave_info slave_info2 = {0}; @@ -414,8 +420,7 @@ static void sensors_init(MultiCameraState *s) { slave_info2.power_setting_array.power_down_setting = &slave_info2.power_setting_array.power_down_setting_a[0]; sensor_init_cfg.cfgtype = CFG_SINIT_PROBE; sensor_init_cfg.cfg.setting = &slave_info2; - err = ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg); - LOG("sensor init cfg (driver camera): %d", err); + err = cam_ioctl(sensorinit_fd, VIDIOC_MSM_SENSOR_INIT_CFG, &sensor_init_cfg, "sensor init cfg (driver camera)"); assert(err >= 0); } @@ -463,23 +468,19 @@ static void camera_open(CameraState *s, bool is_road_cam) { struct msm_camera_csi_lane_params csi_lane_params = {0}; csi_lane_params.csi_lane_mask = 0x1f; csiphy_cfg_data csiphy_cfg_data = { .cfg.csi_lane_params = &csi_lane_params, .cfgtype = CSIPHY_RELEASE}; - int err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); - LOG("release csiphy: %d", err); + int err = cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "release csiphy"); // CSID: release csid csid_cfg_data.cfgtype = CSID_RELEASE; - err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); - LOG("release csid: %d", err); + cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "release csid"); // SENSOR: send power down struct sensorb_cfg_data sensorb_cfg_data = {.cfgtype = CFG_POWER_DOWN}; - err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); - LOG("sensor power down: %d", err); + cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "sensor power down"); // actuator powerdown actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERDOWN; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator powerdown: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator powerdown"); // reset isp // struct msm_vfe_axi_halt_cmd halt_cmd = { @@ -508,13 +509,11 @@ static void camera_open(CameraState *s, bool is_road_cam) { // CSID: init csid csid_cfg_data.cfgtype = CSID_INIT; - err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); - LOG("init csid: %d", err); + cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "init csid"); // CSIPHY: init csiphy csiphy_cfg_data = {.cfgtype = CSIPHY_INIT}; - err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); - LOG("init csiphy: %d", err); + cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "init csiphy"); // SENSOR: stop stream struct msm_camera_i2c_reg_setting stop_settings = { @@ -526,13 +525,11 @@ static void camera_open(CameraState *s, bool is_road_cam) { }; sensorb_cfg_data.cfgtype = CFG_SET_STOP_STREAM_SETTING; sensorb_cfg_data.cfg.setting = &stop_settings; - err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); - LOG("stop stream: %d", err); + cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "stop stream"); // SENSOR: send power up sensorb_cfg_data = {.cfgtype = CFG_POWER_UP}; - err = ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data); - LOG("sensor power up: %d", err); + cam_ioctl(s->sensor_fd, VIDIOC_MSM_SENSOR_CFG, &sensorb_cfg_data, "sensor power up"); // **** configure the sensor **** @@ -549,12 +546,10 @@ static void camera_open(CameraState *s, bool is_road_cam) { if (is_road_cam) { // init the actuator actuator_cfg_data.cfgtype = CFG_ACTUATOR_POWERUP; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator powerup: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator powerup"); actuator_cfg_data.cfgtype = CFG_ACTUATOR_INIT; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator init: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator init"); struct msm_actuator_reg_params_t actuator_reg_params[] = { { @@ -605,8 +600,7 @@ static void camera_open(CameraState *s, bool is_road_cam) { }, }; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator set info: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator set info"); } if (s->camera_id == CAMERA_ID_IMX298) { @@ -624,8 +618,7 @@ static void camera_open(CameraState *s, bool is_road_cam) { } csiphy_cfg_data.cfgtype = CSIPHY_CFG; csiphy_cfg_data.cfg.csiphy_params = &csiphy_params; - err = ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data); - LOG("csiphy configure: %d", err); + cam_ioctl(s->csiphy_fd, VIDIOC_MSM_CSIPHY_IO_CFG, &csiphy_cfg_data, "csiphy configure"); // CSID: configure csid #define CSI_STATS 0x35 @@ -648,13 +641,11 @@ static void camera_open(CameraState *s, bool is_road_cam) { csid_cfg_data.cfgtype = CSID_CFG; csid_cfg_data.cfg.csid_params = &csid_params; - err = ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data); - LOG("csid configure: %d", err); + cam_ioctl(s->csid_fd, VIDIOC_MSM_CSID_IO_CFG, &csid_cfg_data, "csid configure"); // ISP: SMMU_ATTACH msm_vfe_smmu_attach_cmd smmu_attach_cmd = {.security_mode = 0, .iommu_attach_mode = IOMMU_ATTACH}; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd); - LOG("isp smmu attach: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_SMMU_ATTACH, &smmu_attach_cmd, "isp smmu attach"); // ******************* STREAM RAW ***************************** @@ -714,8 +705,7 @@ static void camera_open(CameraState *s, bool is_road_cam) { ss->buf_request.num_buf = FRAME_BUF_COUNT; ss->buf_request.buf_type = ISP_PRIVATE_BUF; ss->buf_request.handle = 0; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_BUF, &ss->buf_request); - LOG("isp request buf: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_REQUEST_BUF, &ss->buf_request, "isp request buf"); LOG("got buf handle: 0x%x", ss->buf_request.handle); // ENQUEUE all buffers @@ -734,16 +724,14 @@ static void camera_open(CameraState *s, bool is_road_cam) { update_cmd.update_info[0].user_stream_id = ss->stream_req.stream_id; update_cmd.update_info[0].stream_handle = ss->stream_req.axi_stream_handle; update_cmd.update_type = UPDATE_STREAM_ADD_BUFQ; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_UPDATE_STREAM, &update_cmd); - LOG("isp update stream: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_UPDATE_STREAM, &update_cmd, "isp update stream"); } LOG("******** START STREAMS ********"); sub.id = 0; sub.type = 0x1ff; - err = ioctl(s->isp_fd, VIDIOC_SUBSCRIBE_EVENT, &sub); - LOG("isp subscribe: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_SUBSCRIBE_EVENT, &sub, "isp subscribe"); // ISP: START_STREAM s->stream_cfg.cmd = START_STREAM; @@ -751,8 +739,7 @@ static void camera_open(CameraState *s, bool is_road_cam) { for (int i = 0; i < s->stream_cfg.num_streams; i++) { s->stream_cfg.stream_handle[i] = s->ss[i].stream_req.axi_stream_handle; } - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg); - LOG("isp start stream: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg, "isp start stream"); } static void road_camera_start(CameraState *s) { @@ -775,8 +762,7 @@ static void road_camera_start(CameraState *s) { .pos = {INFINITY_DAC, 0}, .delay = {0,} }; - err = ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data); - LOG("actuator set pos: %d", err); + cam_ioctl(s->actuator_fd, VIDIOC_MSM_ACTUATOR_CFG, &actuator_cfg_data, "actuator set pos"); // TODO: confirm this isn't needed /*memset(&actuator_cfg_data, 0, sizeof(actuator_cfg_data)); @@ -994,16 +980,13 @@ void cameras_open(MultiCameraState *s) { // ISPIF: setup ispif_cfg_data = {.cfg_type = ISPIF_INIT, .csid_version = 0x30050000 /* CSID_VERSION_V35*/}; - err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); - LOG("ispif setup: %d", err); + cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif setup"); ispif_cfg_data = {.cfg_type = ISPIF_CFG, .params = ispif_params}; - err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); - LOG("ispif cfg: %d", err); + cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif cfg"); ispif_cfg_data.cfg_type = ISPIF_START_FRAME_BOUNDARY; - err = ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data); - LOG("ispif start_frame_boundary: %d", err); + cam_ioctl(s->ispif_fd, VIDIOC_MSM_ISPIF_CFG, &ispif_cfg_data, "ispif start_frame_boundary"); driver_camera_start(&s->driver_cam); road_camera_start(&s->road_cam); @@ -1013,20 +996,17 @@ void cameras_open(MultiCameraState *s) { static void camera_close(CameraState *s) { // ISP: STOP_STREAM s->stream_cfg.cmd = STOP_STREAM; - int err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg); - LOG("isp stop stream: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_CFG_STREAM, &s->stream_cfg, "isp stop stream"); for (int i = 0; i < 3; i++) { StreamState *ss = &s->ss[i]; if (ss->stream_req.axi_stream_handle != 0) { - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_BUF, &ss->buf_request); - LOG("isp release buf: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_BUF, &ss->buf_request, "isp release buf"); struct msm_vfe_axi_stream_release_cmd stream_release = { .stream_handle = ss->stream_req.axi_stream_handle, }; - err = ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_STREAM, &stream_release); - LOG("isp release stream: %d", err); + cam_ioctl(s->isp_fd, VIDIOC_MSM_ISP_RELEASE_STREAM, &stream_release, "isp release stream"); } } }