msm: camera: isp: Maintain clock rate relation between CSIPHY, CSID and TFE

Adds checks to confirm CSID frequency greater than
CSIPHY frequency, TFE frequency greater than CSID
frequency. Also TFE frequency should be such that
it can accommodate PHY data rate .

CRs-Fixed: 2847155
Change-Id: I5d3dcc53a6f27129f4b82cba1254445f58f6d0cf
Signed-off-by: Shravya Samala <[email protected]>
Signed-off-by: Shardul Bankar <[email protected]>
Signed-off-by: Gaurav Jindal <[email protected]>
Signed-off-by: Alok Chauhan <[email protected]>

drivers/cam_core/cam_context.c

@@ -1,7 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0-only
 /*
  * Copyright (c) 2017-2021, The Linux Foundation. All rights reserved.
- * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
+ * Copyright (c) 2022-2023 Qualcomm Innovation Center, Inc. All rights reserved.
  */
 
 #include <linux/slab.h>
@@ -355,6 +355,31 @@ int cam_context_dump_pf_info(void *data, void *args)
 	return rc;
 }
 
+int cam_context_handle_message(struct cam_context *ctx,
+	uint32_t msg_type, void *data)
+{
+	int rc = 0;
+
+	if (!ctx->state_machine) {
+		CAM_ERR(CAM_CORE, "Context is not ready");
+		return -EINVAL;
+	}
+
+	if ((ctx->state > CAM_CTX_AVAILABLE) &&
+		(ctx->state < CAM_CTX_STATE_MAX)) {
+		if (ctx->state_machine[ctx->state].msg_cb_ops) {
+			rc = ctx->state_machine[ctx->state].msg_cb_ops(
+				ctx, msg_type, data);
+		} else {
+			CAM_WARN(CAM_CORE,
+				"No message handler for ctx %d, state %d msg_type :%d",
+				ctx->dev_hdl, ctx->state, msg_type);
+		}
+	}
+
+	return rc;
+}
+
 int cam_context_handle_acquire_dev(struct cam_context *ctx,
 	struct cam_acquire_dev_cmd *cmd)
 {

drivers/cam_core/cam_context.h

@@ -168,7 +168,9 @@ struct cam_ctx_crm_ops {
  *                         context info
  * @recovery_ops:          Function to be invoked to try hardware recovery
  * @mini_dump_ops:         Function for mini dump
- * @evt_inject_ops:        Function for event injection
+ * @err_inject_ops:        Function for error injection
+ * @msg_cb_ops:            Function to be called on any message from
+ *                         other subdev notifications
  *
  */
 struct cam_ctx_ops {
@@ -180,6 +182,7 @@ struct cam_ctx_ops {
 	cam_ctx_recovery_cb_func     recovery_ops;
 	cam_ctx_mini_dump_cb_func    mini_dump_ops;
 	cam_ctx_err_inject_cb_func   evt_inject_ops;
+	cam_ctx_message_cb_func      msg_cb_ops;
 };
 
 
@@ -416,6 +419,19 @@ int cam_context_mini_dump_from_hw(struct cam_context *ctx,
 int cam_context_dump_pf_info(void *ctx,
 	void *pf_args);
 
+/**
+ * cam_context_handle_message()
+ *
+ * @brief:        Handle message callback command
+ *
+ * @ctx:          Object pointer for cam_context
+ * @msg_type:     message type sent from other subdev
+ * @data:         data from other subdev
+ *
+ */
+int cam_context_handle_message(struct cam_context *ctx,
+	uint32_t msg_type, void *data);
+
 /**
  * cam_context_handle_acquire_dev()
  *

drivers/cam_core/cam_hw_mgr_intf.h

@@ -80,6 +80,10 @@ typedef int (*cam_ctx_mini_dump_cb_func)(void *context,
 typedef int (*cam_ctx_err_inject_cb_func)(void *context,
 	void *args);
 
+/* message callback function type */
+typedef int (*cam_ctx_message_cb_func)(void *context,
+	uint32_t message_type, void *data);
+
 /**
  * struct cam_hw_update_entry - Entry for hardware config
  *

drivers/cam_isp/cam_isp_context.c

@@ -23,6 +23,7 @@
 #include "cam_req_mgr_debug.h"
 #include "cam_cpas_api.h"
 #include "cam_ife_hw_mgr.h"
+#include "cam_subdev.h"
 
 static const char isp_dev_name[] = "cam-isp";
 
@@ -36,6 +37,8 @@ static int cam_isp_context_dump_requests(void *data,
 	void *pf_args);
 
 static int cam_isp_context_hw_recovery(void *priv, void *data);
+static int cam_isp_context_handle_message(void *context,
+	uint32_t msg_type, void *data);
 
 static int __cam_isp_ctx_start_dev_in_ready(struct cam_context *ctx,
 	struct cam_start_stop_dev_cmd *cmd);
@@ -8858,6 +8861,7 @@ static struct cam_ctx_ops
 		.pagefault_ops = cam_isp_context_dump_requests,
 		.dumpinfo_ops = cam_isp_context_info_dump,
 		.evt_inject_ops = cam_isp_context_inject_evt,
+		.msg_cb_ops = cam_isp_context_handle_message,
 	},
 	/* Activated */
 	{
@@ -8882,6 +8886,7 @@ static struct cam_ctx_ops
 		.dumpinfo_ops = cam_isp_context_info_dump,
 		.recovery_ops = cam_isp_context_hw_recovery,
 		.evt_inject_ops = cam_isp_context_inject_evt,
+		.msg_cb_ops = cam_isp_context_handle_message,
 	},
 };
 
@@ -9009,6 +9014,34 @@ end:
 	return rc;
 }
 
+static int cam_isp_context_handle_message(void *context,
+	uint32_t msg_type, void *data)
+{
+	int                            rc = -EINVAL;
+	struct cam_hw_cmd_args         hw_cmd_args = {0};
+	struct cam_isp_hw_cmd_args     isp_hw_cmd_args = {0};
+	struct cam_context            *ctx = (struct cam_context *)context;
+
+	hw_cmd_args.ctxt_to_hw_map = ctx->ctxt_to_hw_map;
+
+	switch (msg_type) {
+	case CAM_SUBDEV_MESSAGE_CLOCK_UPDATE:
+		hw_cmd_args.cmd_type = CAM_HW_MGR_CMD_INTERNAL;
+		isp_hw_cmd_args.cmd_type = CAM_ISP_HW_MGR_CMD_UPDATE_CLOCK;
+		isp_hw_cmd_args.cmd_data = data;
+		hw_cmd_args.u.internal_args = (void *)&isp_hw_cmd_args;
+		rc = ctx->hw_mgr_intf->hw_cmd(ctx->hw_mgr_intf->hw_mgr_priv,
+			&hw_cmd_args);
+
+		if (rc)
+			CAM_ERR(CAM_ISP, "Update clock rate failed rc: %d", rc);
+		break;
+	default:
+		CAM_ERR(CAM_ISP, "Invalid message type %d", msg_type);
+	}
+	return rc;
+}
+
 static int cam_isp_context_debug_register(void)
 {
 	int rc = 0;

drivers/cam_isp/cam_isp_dev.c

@@ -73,6 +73,22 @@ static void cam_isp_dev_iommu_fault_handler(struct cam_smmu_pf_info *pf_smmu_inf
 	}
 }
 
+static void cam_isp_subdev_handle_message(
+		struct v4l2_subdev *sd,
+		enum cam_subdev_message_type_t message_type,
+		void *data)
+{
+	int i, rc = 0;
+	struct cam_node  *node = v4l2_get_subdevdata(sd);
+
+	CAM_DBG(CAM_ISP, "node name %s", node->name);
+	for (i = 0; i < node->ctx_size; i++) {
+		rc = cam_context_handle_message(&(node->ctx_list[i]), message_type, data);
+		if (rc)
+			CAM_ERR(CAM_ISP, "Failed to handle message for %s", node->name);
+	}
+}
+
 static const struct of_device_id cam_isp_dt_match[] = {
 	{
 		.compatible = "qcom,cam-isp"
@@ -170,6 +186,7 @@ static int cam_isp_dev_component_bind(struct device *dev,
 	} else if (strnstr(compat_str, "tfe", strlen(compat_str))) {
 		rc = cam_subdev_probe(&g_isp_dev.sd, pdev, CAM_ISP_DEV_NAME,
 		CAM_TFE_DEVICE_TYPE);
+		g_isp_dev.sd.msg_cb = cam_isp_subdev_handle_message;
 		g_isp_dev.isp_device_type = CAM_TFE_DEVICE_TYPE;
 		g_isp_dev.max_context = CAM_TFE_CTX_MAX;
 	} else  {

drivers/cam_isp/isp_hw_mgr/cam_tfe_hw_mgr.c

@@ -1993,6 +1993,63 @@ static int cam_tfe_mgr_acquire_get_unified_structure(
 		CAM_ERR(CAM_ISP, "Invalid ver of i/p port info from user");
 		return -EINVAL;
 	}
+	return 0;
+}
+
+int cam_tfe_hw_mgr_csiphy_clk_sync(
+	struct cam_tfe_hw_mgr_ctx *ctx, void *cmd_args)
+{
+	int                          rc = -EINVAL;
+	unsigned long                phy_clock_rate = 0;
+	unsigned long                csid_clk_rate = 0, tfe_clk_rate = 0, temp_clk_rate = 0;
+	struct cam_hw_intf           *hw_intf;
+	int                          i;
+
+	if (!ctx || !cmd_args) {
+		CAM_ERR(CAM_ISP, "Invalid arguments");
+		return -EINVAL;
+	}
+
+	phy_clock_rate = (*((unsigned long *)cmd_args));
+	for (i = 0; i < ctx->num_base; i++) {
+		if (!ctx->hw_mgr->tfe_csid_dev_caps[ctx->base[i].idx].sync_clk)
+			continue;
+
+		hw_intf = g_tfe_hw_mgr.csid_devices[ctx->base[i].idx];
+
+		temp_clk_rate = phy_clock_rate;
+		rc = hw_intf->hw_ops.process_cmd(hw_intf->hw_priv,
+			CAM_ISP_HW_CMD_DYNAMIC_CLOCK_UPDATE,
+			&temp_clk_rate, sizeof(unsigned long));
+		if (rc) {
+			CAM_ERR(CAM_ISP, "Failed to set CSID Clock rate");
+			return rc;
+		}
+		csid_clk_rate = temp_clk_rate;
+
+		hw_intf = g_tfe_hw_mgr.tfe_devices[hw_intf->hw_idx]->hw_intf;
+		rc = hw_intf->hw_ops.process_cmd(
+			hw_intf->hw_priv,
+			CAM_ISP_HW_CMD_DYNAMIC_CLOCK_UPDATE,
+			&temp_clk_rate, sizeof(unsigned long));
+		if (rc) {
+			CAM_ERR(CAM_ISP, "Failed to set TFE Clock rate");
+			return rc;
+		}
+		tfe_clk_rate = temp_clk_rate;
+	}
+
+	CAM_DBG(CAM_ISP, "Clock rates: phy:%llu csid:%llu tfe:%llu",
+		phy_clock_rate, csid_clk_rate, tfe_clk_rate);
+
+	if ((phy_clock_rate > csid_clk_rate) || (csid_clk_rate > tfe_clk_rate) ||
+		(phy_clock_rate > tfe_clk_rate)) {
+		CAM_ERR(CAM_ISP,
+			"Invalid clock rates, phy:%llu csid:%llu tfe:%llu",
+			phy_clock_rate, csid_clk_rate, tfe_clk_rate);
+
+		return -EINVAL;
+	}
 
 	return 0;
 }
@@ -5105,6 +5162,9 @@ static int cam_tfe_mgr_cmd(void *hw_mgr_priv, void *cmd_args)
 				&isp_hw_cmd_args->u.sof_ts.boot,
 				&isp_hw_cmd_args->u.sof_ts.prev);
 			break;
+		case CAM_ISP_HW_MGR_CMD_UPDATE_CLOCK:
+			rc = cam_tfe_hw_mgr_csiphy_clk_sync(ctx, isp_hw_cmd_args->cmd_data);
+			break;
 		default:
 			CAM_ERR(CAM_ISP, "Invalid HW mgr command:0x%x, ISP HW mgr cmd:0x%x",
 				hw_cmd_args->cmd_type, isp_hw_cmd_args->cmd_type);

drivers/cam_isp/isp_hw_mgr/include/cam_isp_hw_mgr_intf.h

@@ -476,6 +476,7 @@ enum cam_isp_hw_mgr_command {
 	CAM_ISP_HW_MGR_GET_SOF_TS,
 	CAM_ISP_HW_MGR_DUMP_STREAM_INFO,
 	CAM_ISP_HW_MGR_GET_BUS_COMP_GROUP,
+	CAM_ISP_HW_MGR_CMD_UPDATE_CLOCK,
 	CAM_ISP_HW_MGR_CMD_MAX,
 };
 

drivers/cam_isp/isp_hw_mgr/isp_hw/include/cam_isp_hw.h

@@ -255,6 +255,7 @@ enum cam_isp_hw_cmd_type {
 	CAM_ISP_HW_CMD_IRQ_INJECTION,
 	CAM_ISP_HW_CMD_DUMP_IRQ_DESCRIPTION,
 	CAM_ISP_HW_CMD_GET_SET_PRIM_SOF_TS_ADDR,
+	CAM_ISP_HW_CMD_DYNAMIC_CLOCK_UPDATE,
 	CAM_ISP_HW_CMD_MAX,
 };
 

drivers/cam_isp/isp_hw_mgr/isp_hw/include/cam_tfe_csid_hw_intf.h

@@ -99,6 +99,7 @@ struct cam_isp_tfe_in_port_generic_info {
  * @major_version : major version
  * @minor_version:  minor version
  * @version_incr:   version increment
+ * @sync_clk:       sync clocks such that freq(TFE)>freq(CSID)>freq(CSIPHY)
  *
  */
 struct cam_tfe_csid_hw_caps {
@@ -107,6 +108,7 @@ struct cam_tfe_csid_hw_caps {
 	uint32_t      major_version;
 	uint32_t      minor_version;
 	uint32_t      version_incr;
+	bool          sync_clk;
 };
 
 /**

drivers/cam_isp/isp_hw_mgr/isp_hw/tfe_csid_hw/cam_tfe_csid640.h

@@ -1,6 +1,7 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 /*
  * Copyright (c) 2019-2021, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2023, Qualcomm Innovation Center, Inc. All rights reserved.
  */
 
 #ifndef _CAM_TFE_CSID_640_H_
@@ -264,6 +265,7 @@ static struct cam_tfe_csid_common_reg_offset
 	.format_measure_height_shift_val              = 16,
 	.format_measure_height_mask_val               = 0xe,
 	.format_measure_width_mask_val                = 0x10,
+	.sync_clk                                     = true,
 };
 
 static struct cam_tfe_csid_reg_offset cam_tfe_csid_640_reg_offset = {

drivers/cam_isp/isp_hw_mgr/isp_hw/tfe_csid_hw/cam_tfe_csid_core.c

@@ -2279,6 +2279,7 @@ static int cam_tfe_csid_get_hw_caps(void *hw_priv,
 	hw_caps->major_version = csid_reg->cmn_reg->major_version;
 	hw_caps->minor_version = csid_reg->cmn_reg->minor_version;
 	hw_caps->version_incr = csid_reg->cmn_reg->version_incr;
+	hw_caps->sync_clk = csid_reg->cmn_reg->sync_clk;
 
 	CAM_DBG(CAM_ISP,
 		"CSID:%d No rdis:%d, no pix:%d, major:%d minor:%d ver :%d",
@@ -2888,6 +2889,36 @@ static int cam_tfe_csid_dump_csid_clock(
 	return 0;
 }
 
+static int cam_tfe_csid_set_csid_clock_dynamically(
+	struct cam_tfe_csid_hw *csid_hw, void *cmd_args)
+{
+	struct cam_hw_soc_info   *soc_info;
+	unsigned long            *clk_rate;
+	int rc = 0;
+
+	soc_info = &csid_hw->hw_info->soc_info;
+	clk_rate = (unsigned long *)cmd_args;
+
+	CAM_DBG(CAM_ISP, "CSID[%u] clock rate requested: %llu curr: %llu",
+		csid_hw->hw_intf->hw_idx, *clk_rate, soc_info->applied_src_clk_rates.sw_client);
+
+	if (*clk_rate <= soc_info->applied_src_clk_rates.sw_client)
+		goto end;
+
+	rc = cam_soc_util_set_src_clk_rate(soc_info, CAM_CLK_SW_CLIENT_IDX, *clk_rate, 0);
+	if (rc) {
+		CAM_ERR(CAM_ISP,
+			"unable to set clock dynamically rate:%llu", *clk_rate);
+		return rc;
+	}
+end:
+	*clk_rate = soc_info->applied_src_clk_rates.sw_client;
+	CAM_DBG(CAM_ISP, "CSID[%u] new clock rate %llu",
+		csid_hw->hw_intf->hw_idx, soc_info->applied_src_clk_rates.sw_client);
+
+	return rc;
+}
+
 static int cam_tfe_csid_get_regdump(struct cam_tfe_csid_hw *csid_hw,
 	void *cmd_args)
 {
@@ -3173,6 +3204,9 @@ static int cam_tfe_csid_process_cmd(void *hw_priv,
 	case CAM_TFE_CSID_LOG_ACQUIRE_DATA:
 		rc = cam_tfe_csid_log_acquire_data(csid_hw, cmd_args);
 		break;
+	case CAM_ISP_HW_CMD_DYNAMIC_CLOCK_UPDATE:
+		rc = cam_tfe_csid_set_csid_clock_dynamically(csid_hw, cmd_args);
+		break;
 	default:
 		CAM_ERR(CAM_ISP, "CSID:%d unsupported cmd:%d",
 			csid_hw->hw_intf->hw_idx, cmd_type);

drivers/cam_isp/isp_hw_mgr/isp_hw/tfe_csid_hw/cam_tfe_csid_core.h

@@ -1,6 +1,7 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 /*
  * Copyright (c) 2019-2021, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2023, Qualcomm Innovation Center, Inc. All rights reserved.
  */
 
 #ifndef _CAM_TFE_CSID_HW_H_
@@ -283,6 +284,7 @@ struct cam_tfe_csid_common_reg_offset {
 	uint32_t format_measure_height_mask_val;
 	uint32_t format_measure_width_mask_val;
 	bool     format_measure_support;
+	bool     sync_clk;
 };
 
 /**
@@ -477,6 +479,7 @@ struct cam_csid_evt_payload {
  *                            or not
  * @prev_boot_timestamp       previous frame bootime stamp
  * @prev_qtimer_ts            previous frame qtimer csid timestamp
+ * @sync_clk                  sync clocks such that freq(TFE)>freq(CSID)>freq(CSIPHY)
  *
  */
 struct cam_tfe_csid_hw {
@@ -511,6 +514,7 @@ struct cam_tfe_csid_hw {
 	bool                                ppi_enable;
 	uint64_t                            prev_boot_timestamp;
 	uint64_t                            prev_qtimer_ts;
+	bool                                sync_clk;
 };
 
 int cam_tfe_csid_hw_probe_init(struct cam_hw_intf  *csid_hw_intf,

drivers/cam_isp/isp_hw_mgr/isp_hw/tfe_hw/cam_tfe_core.c

@@ -971,6 +971,41 @@ static int cam_tfe_top_set_hw_clk_rate(
 	return rc;
 }
 
+static int cam_tfe_top_dynamic_clock_update(
+	struct cam_tfe_top_priv  *top_priv,
+	void                     *cmd_args,
+	uint32_t                 arg_size)
+{
+	struct cam_hw_soc_info   *soc_info;
+	unsigned long            *clk_rate;
+	int rc = 0;
+
+	soc_info = top_priv->common_data.soc_info;
+	clk_rate = (unsigned long *)cmd_args;
+	CAM_DBG(CAM_ISP, "TFE[%u] clock rate requested: %llu curr: %llu",
+		top_priv->common_data.hw_intf->hw_idx, *clk_rate,
+		soc_info->applied_src_clk_rates.sw_client);
+
+	if (*clk_rate <= top_priv->hw_clk_rate)
+		goto end;
+
+	rc = cam_soc_util_set_src_clk_rate(soc_info, CAM_CLK_SW_CLIENT_IDX, *clk_rate, 0);
+	if (!rc) {
+		top_priv->hw_clk_rate = *clk_rate;
+	} else {
+		CAM_ERR(CAM_ISP,
+			"unable to set clock dynamically rate: %llu",
+			*clk_rate);
+		return rc;
+	}
+end:
+	*clk_rate = soc_info->applied_src_clk_rates.sw_client;
+	CAM_DBG(CAM_ISP, "TFE[%u] new clock rate %llu",
+		top_priv->common_data.hw_intf->hw_idx, soc_info->applied_src_clk_rates.sw_client);
+
+	return rc;
+}
+
 static struct cam_axi_vote *cam_tfe_top_delay_bw_reduction(
 	struct cam_tfe_top_priv *top_priv,
 	uint64_t *to_be_applied_bw)
@@ -2920,6 +2955,10 @@ int cam_tfe_process_cmd(void *hw_priv, uint32_t cmd_type,
 		rc = cam_tfe_set_top_debug(core_info, cmd_args,
 			arg_size);
 		break;
+	case CAM_ISP_HW_CMD_DYNAMIC_CLOCK_UPDATE:
+		rc = cam_tfe_top_dynamic_clock_update(core_info->top_priv, cmd_args,
+			arg_size);
+		break;
 	case CAM_ISP_HW_CMD_GET_BUF_UPDATE:
 	case CAM_ISP_HW_CMD_GET_HFR_UPDATE:
 	case CAM_ISP_HW_CMD_STRIPE_UPDATE:

drivers/cam_req_mgr/cam_subdev.h

@@ -1,7 +1,7 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 /*
  * Copyright (c) 2017-2021, The Linux Foundation. All rights reserved.
- * Copyright (c) 2022 Qualcomm Innovation Center, Inc. All rights reserved.
+ * Copyright (c) 2022-2023 Qualcomm Innovation Center, Inc. All rights reserved.
  */
 
 #ifndef _CAM_SUBDEV_H_
@@ -82,6 +82,7 @@ enum cam_subdev_message_type_t {
 	CAM_SUBDEV_MESSAGE_CONN_CSID_INFO,
 	CAM_SUBDEV_MESSAGE_DRV_INFO,
 	CAM_SUBDEV_MESSAGE_NOTIFY_HALT_RESUME,
+	CAM_SUBDEV_MESSAGE_CLOCK_UPDATE
 };
 
 /* Enum for close sequence priority */