diff --git a/drivers/cam_sensor_module/cam_csiphy/cam_csiphy_core.c b/drivers/cam_sensor_module/cam_csiphy/cam_csiphy_core.c index c5c7b70edba6..ece371256868 100644 --- a/drivers/cam_sensor_module/cam_csiphy/cam_csiphy_core.c +++ b/drivers/cam_sensor_module/cam_csiphy/cam_csiphy_core.c @@ -1,6 +1,6 @@ // SPDX-License-Identifier: GPL-2.0-only /* - * Copyright (c) 2017-2020, The Linux Foundation. All rights reserved. + * Copyright (c) 2017-2021, The Linux Foundation. All rights reserved. */ #include @@ -16,6 +16,7 @@ #include "cam_mem_mgr.h" #include "cam_cpas_api.h" #include "cam_compat.h" +#include "cam_subdev.h" #define SCM_SVC_CAMERASS 0x18 #define SECURE_SYSCALL_ID 0x6 @@ -1379,8 +1380,9 @@ int32_t cam_csiphy_core_cfg(void *phy_dev, struct cam_ahb_vote ahb_vote; struct cam_axi_vote axi_vote = {0}; struct cam_start_stop_dev_cmd config; - int32_t offset; + int32_t i, offset; int clk_vote_level = -1; + unsigned long clk_rate = 0; rc = copy_from_user(&config, (void __user *)cmd->handle, sizeof(config)); @@ -1421,6 +1423,21 @@ int32_t cam_csiphy_core_cfg(void *phy_dev, rc = 0; } + for (i = 0; i < csiphy_dev->soc_info.num_clk; i++) { + if (i == csiphy_dev->soc_info.src_clk_idx) { + CAM_DBG(CAM_CSIPHY, "Skipping call back for src clk %s", + csiphy_dev->soc_info.clk_name[i]); + continue; + } + clk_rate = cam_soc_util_get_clk_rate_applied( + &csiphy_dev->soc_info, i, false, clk_vote_level); + if(clk_rate > 0) { + cam_subdev_notify_message(CAM_TFE_DEVICE_TYPE, + CAM_SUBDEV_MESSAGE_CLOCK_UPDATE, + clk_rate); + } + } + if (csiphy_dev->csiphy_info[offset].secure_mode == 1) { if (!cam_cpas_is_feature_supported( CAM_CPAS_SECURE_CAMERA_ENABLE, diff --git a/drivers/cam_sensor_module/cam_csiphy/cam_csiphy_soc.c b/drivers/cam_sensor_module/cam_csiphy/cam_csiphy_soc.c index 7dc0128940b7..dde463e37ce6 100644 --- a/drivers/cam_sensor_module/cam_csiphy/cam_csiphy_soc.c +++ b/drivers/cam_sensor_module/cam_csiphy/cam_csiphy_soc.c @@ -14,6 +14,7 @@ #include "include/cam_csiphy_1_2_5_hwreg.h" #include "include/cam_csiphy_2_0_hwreg.h" #include "include/cam_csiphy_2_1_0_hwreg.h" +#include "cam_subdev.h" /* Clock divide factor for CPHY spec v1.0 */ #define CSIPHY_DIVISOR_16 16 @@ -187,6 +188,7 @@ int32_t cam_csiphy_enable_hw(struct csiphy_device *csiphy_dev, int32_t index) int32_t rc = 0; struct cam_hw_soc_info *soc_info; enum cam_vote_level vote_level = CAM_SVS_VOTE; + unsigned long clk_rate = 0; soc_info = &csiphy_dev->soc_info; @@ -207,6 +209,11 @@ int32_t cam_csiphy_enable_hw(struct csiphy_device *csiphy_dev, int32_t index) rc = cam_soc_util_set_src_clk_rate(soc_info, soc_info->clk_rate[0][soc_info->src_clk_idx]); + clk_rate = cam_soc_util_get_clk_rate_applied(soc_info, + soc_info->src_clk_idx, true, vote_level); + cam_subdev_notify_message(CAM_TFE_DEVICE_TYPE, + CAM_SUBDEV_MESSAGE_CLOCK_UPDATE, + clk_rate); if (rc < 0) { CAM_ERR(CAM_CSIPHY, "csiphy_clk_set_rate failed rc: %d", rc); diff --git a/drivers/cam_utils/cam_soc_util.c b/drivers/cam_utils/cam_soc_util.c index 8b648c4bf446..93ff740d22aa 100644 --- a/drivers/cam_utils/cam_soc_util.c +++ b/drivers/cam_utils/cam_soc_util.c @@ -312,6 +312,32 @@ static int cam_soc_util_get_clk_level_to_apply( return 0; } +unsigned long cam_soc_util_get_clk_rate_applied( + struct cam_hw_soc_info *soc_info, int32_t index, bool is_src, + enum cam_vote_level clk_level) +{ + unsigned long clk_rate = 0; + struct clk *clk = NULL; + int rc = 0; + enum cam_vote_level apply_level; + + if (is_src) { + clk = soc_info->clk[index]; + clk_rate = clk_get_rate(clk); + } + else { + rc = cam_soc_util_get_clk_level_to_apply(soc_info, clk_level, + &apply_level); + if (rc) + return rc; + if(soc_info->clk_rate[apply_level][index] > 0) { + clk = soc_info->clk[index]; + clk_rate = clk_get_rate(clk); + } + } + return clk_rate; +} + int cam_soc_util_irq_enable(struct cam_hw_soc_info *soc_info) { if (!soc_info) { diff --git a/drivers/cam_utils/cam_soc_util.h b/drivers/cam_utils/cam_soc_util.h index 8db6f1869cf8..5f0a0d8e88cb 100644 --- a/drivers/cam_utils/cam_soc_util.h +++ b/drivers/cam_utils/cam_soc_util.h @@ -677,6 +677,10 @@ int cam_soc_util_clk_enable_default(struct cam_hw_soc_info *soc_info, int cam_soc_util_get_clk_level(struct cam_hw_soc_info *soc_info, int64_t clk_rate, int clk_idx, int32_t *clk_lvl); +unsigned long cam_soc_util_get_clk_rate_applied( + struct cam_hw_soc_info *soc_info, int32_t index, bool is_src, + enum cam_vote_level clk_level); + /* Callback to get reg space data for specific HW */ typedef int (*cam_soc_util_regspace_data_cb)(uint32_t reg_base_type, void *ctx, struct cam_hw_soc_info **soc_info_ptr,