msm: camera: icp: Update A5 clock per frame

A5 (ICP) clock corners IPE clock source. This change
updates ICP clock based on the rate updated for IPE.

Change-Id: I2c0b711d34ea8ab8ccf458ecb77402ecf6bf967e
Signed-off-by: Karthik Anantha Ram <[email protected]>
Signed-off-by: Mukund Madhusudan Atre <[email protected]>

drivers/cam_icp/icp_hw/a5_hw/a5_core.c

@@ -519,6 +519,25 @@ int cam_a5_process_cmd(void *device_priv, uint32_t cmd_type,
 
 		break;
 	}
+	case CAM_ICP_A5_CMD_CLK_UPDATE: {
+		int32_t clk_level = 0;
+
+		if (!cmd_args) {
+			CAM_ERR(CAM_ICP, "Invalid args");
+			return -EINVAL;
+		}
+
+		clk_level = *((int32_t *)cmd_args);
+		CAM_DBG(CAM_ICP,
+			"Update ICP clock to level [%d]", clk_level);
+		rc = cam_a5_update_clk_rate(soc_info, clk_level);
+		if (rc)
+			CAM_ERR(CAM_ICP,
+				"Failed to update clk to level: %d rc: %d",
+				clk_level, rc);
+
+		break;
+	}
 	default:
 		break;
 	}

drivers/cam_icp/icp_hw/a5_hw/a5_soc.c

@@ -196,3 +196,47 @@ int cam_a5_disable_soc_resources(struct cam_hw_soc_info *soc_info)
 
 	return rc;
 }
+
+int cam_a5_update_clk_rate(struct cam_hw_soc_info *soc_info,
+	int32_t clk_level)
+{
+	int32_t src_clk_idx = 0;
+	int32_t clk_rate = 0;
+
+	if (!soc_info) {
+		CAM_ERR(CAM_ICP, "Invalid args");
+		return -EINVAL;
+	}
+
+	if ((clk_level < 0) || (clk_level >= CAM_MAX_VOTE)) {
+		CAM_ERR(CAM_ICP, "clock level %d is not valid",
+			clk_level);
+		return -EINVAL;
+	}
+
+	if (!soc_info->clk_level_valid[clk_level]) {
+		CAM_ERR(CAM_ICP,
+			"Clock level %d not supported",
+			clk_level);
+		return -EINVAL;
+	}
+
+	src_clk_idx = soc_info->src_clk_idx;
+	if ((src_clk_idx < 0) || (src_clk_idx >= CAM_SOC_MAX_CLK)) {
+		CAM_WARN(CAM_ICP, "src_clk not defined for %s",
+			soc_info->dev_name);
+		return -EINVAL;
+	}
+
+	clk_rate = soc_info->clk_rate[clk_level][src_clk_idx];
+	if ((soc_info->clk_level_valid[CAM_TURBO_VOTE]) &&
+		(soc_info->clk_rate[CAM_TURBO_VOTE][src_clk_idx] != 0) &&
+		(clk_rate > soc_info->clk_rate[CAM_TURBO_VOTE][src_clk_idx])) {
+		CAM_DBG(CAM_ICP, "clk_rate %d greater than max, reset to %d",
+			clk_rate,
+			soc_info->clk_rate[CAM_TURBO_VOTE][src_clk_idx]);
+		clk_rate = soc_info->clk_rate[CAM_TURBO_VOTE][src_clk_idx];
+	}
+
+	return cam_soc_util_set_src_clk_rate(soc_info, clk_rate);
+}

drivers/cam_icp/icp_hw/a5_hw/a5_soc.h

@@ -34,4 +34,6 @@ int cam_a5_enable_soc_resources(struct cam_hw_soc_info *soc_info);
 
 int cam_a5_disable_soc_resources(struct cam_hw_soc_info *soc_info);
 
+int cam_a5_update_clk_rate(struct cam_hw_soc_info *soc_info,
+	int32_t clk_level);
 #endif

drivers/cam_icp/icp_hw/icp_hw_mgr/cam_icp_hw_mgr.c

@@ -1320,12 +1320,14 @@ static int cam_icp_update_clk_rate(struct cam_icp_hw_mgr *hw_mgr,
 	struct cam_hw_intf *ipe0_dev_intf = NULL;
 	struct cam_hw_intf *ipe1_dev_intf = NULL;
 	struct cam_hw_intf *bps_dev_intf = NULL;
+	struct cam_hw_intf *a5_dev_intf = NULL;
 	struct cam_hw_intf *dev_intf = NULL;
 	struct cam_a5_clk_update_cmd clk_upd_cmd;
 
 	ipe0_dev_intf = hw_mgr->ipe0_dev_intf;
 	ipe1_dev_intf = hw_mgr->ipe1_dev_intf;
 	bps_dev_intf = hw_mgr->bps_dev_intf;
+	a5_dev_intf = hw_mgr->a5_dev_intf;
 
 
 	if ((!ipe0_dev_intf) || (!bps_dev_intf)) {
@@ -1347,16 +1349,26 @@ static int cam_icp_update_clk_rate(struct cam_icp_hw_mgr *hw_mgr,
 		ctx_data->icp_dev_acquire_info->dev_type);
 	clk_upd_cmd.curr_clk_rate = curr_clk_rate;
 	clk_upd_cmd.ipe_bps_pc_enable = icp_hw_mgr.ipe_bps_pc_flag;
+	clk_upd_cmd.clk_level = -1;
 
 	dev_intf->hw_ops.process_cmd(dev_intf->hw_priv, id,
 		&clk_upd_cmd, sizeof(struct cam_a5_clk_update_cmd));
 
-	if (ctx_data->icp_dev_acquire_info->dev_type != CAM_ICP_RES_TYPE_BPS)
-		if (ipe1_dev_intf)
+	if (ctx_data->icp_dev_acquire_info->dev_type != CAM_ICP_RES_TYPE_BPS) {
+		if (ipe1_dev_intf) {
 			ipe1_dev_intf->hw_ops.process_cmd(
 				ipe1_dev_intf->hw_priv, id,
 				&clk_upd_cmd,
 				sizeof(struct cam_a5_clk_update_cmd));
+		}
+
+		/* update a5 clock */
+		CAM_DBG(CAM_ICP, "Update ICP clk to level [%d]",
+			clk_upd_cmd.clk_level);
+		a5_dev_intf->hw_ops.process_cmd(a5_dev_intf->hw_priv,
+			CAM_ICP_A5_CMD_CLK_UPDATE, &clk_upd_cmd.clk_level,
+			sizeof(clk_upd_cmd.clk_level));
+	}
 
 	return 0;
 }

drivers/cam_icp/icp_hw/icp_hw_mgr/include/cam_a5_hw_intf.h

@@ -1,6 +1,6 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 /*
- * Copyright (c) 2017-2018, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017-2019, The Linux Foundation. All rights reserved.
  */
 
 #ifndef CAM_A5_HW_INTF_H
@@ -26,6 +26,7 @@ enum cam_icp_a5_cmd_type {
 	CAM_ICP_A5_CMD_CPAS_STOP,
 	CAM_ICP_A5_CMD_UBWC_CFG,
 	CAM_ICP_A5_CMD_PC_PREP,
+	CAM_ICP_A5_CMD_CLK_UPDATE,
 	CAM_ICP_A5_CMD_MAX,
 };
 

drivers/cam_icp/icp_hw/icp_hw_mgr/include/cam_icp_hw_intf.h

@@ -25,9 +25,14 @@ enum cam_a5_hw_type {
  *
  * @curr_clk_rate:        clk rate to HW
  * @ipe_bps_pc_enable     power collpase enable flag
+ * @clk_level:            clk level corresponding to the clk rate
+ *                        populated as output while the clk is being
+ *                        updated to the given rate
  */
 struct cam_a5_clk_update_cmd {
 	uint32_t  curr_clk_rate;
 	bool  ipe_bps_pc_enable;
+	int32_t clk_level;
 };
+
 #endif

drivers/cam_icp/icp_hw/ipe_hw/ipe_core.c

@@ -361,6 +361,7 @@ int cam_ipe_process_cmd(void *device_priv, uint32_t cmd_type,
 		struct cam_a5_clk_update_cmd *clk_upd_cmd =
 			(struct cam_a5_clk_update_cmd *)cmd_args;
 		uint32_t clk_rate = clk_upd_cmd->curr_clk_rate;
+		int32_t clk_level  = 0, err = 0;
 
 		CAM_DBG(CAM_ICP, "ipe_src_clk rate = %d", (int)clk_rate);
 		if (!core_info->clk_enable) {
@@ -386,8 +387,15 @@ int cam_ipe_process_cmd(void *device_priv, uint32_t cmd_type,
 		rc = cam_ipe_update_clk_rate(soc_info, clk_rate);
 		if (rc)
 			CAM_ERR(CAM_ICP, "Failed to update clk");
-		}
+
+		err = cam_soc_util_get_clk_level(soc_info,
+				clk_rate, soc_info->src_clk_idx,
+				&clk_level);
+		if (err == 0)
+			clk_upd_cmd->clk_level = clk_level;
+
 		break;
+	}
 	case CAM_ICP_IPE_CMD_DISABLE_CLK:
 		if (core_info->clk_enable == true)
 			cam_ipe_toggle_clk(soc_info, false);