From b5de5be5e01330e5f8255219de9d9ee6433f9c66 Mon Sep 17 00:00:00 2001
From: Xubin Bai <quic_xubibai@quicinc.com>
Date: Mon, 15 Aug 2022 23:49:46 -0700
Subject: [PATCH] interconnect: qcom: icc-rpmh: Add support to scale local
 clocks

Add support to scale local clocks along with BW votes using
the clk framework, as opposed to only voting shared clocks
through BCM and then voting to DFS to scale clocks.

Change-Id: I62190e08a382551b2759cee22ce4bd50e760604f
Signed-off-by: Xubin Bai <quic_xubibai@quicinc.com>
---
 drivers/interconnect/qcom/icc-rpmh.c | 54 +++++++++++++++++++++++++++-
 drivers/interconnect/qcom/icc-rpmh.h | 12 +++++++
 2 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/drivers/interconnect/qcom/icc-rpmh.c b/drivers/interconnect/qcom/icc-rpmh.c
index 181c0b5c1edb..b80fb2d5fc38 100644
--- a/drivers/interconnect/qcom/icc-rpmh.c
+++ b/drivers/interconnect/qcom/icc-rpmh.c
@@ -108,7 +108,9 @@ int qcom_icc_set(struct icc_node *src, struct icc_node *dst)
 {
 	struct qcom_icc_provider *qp;
 	struct icc_node *node;
-	int i;
+	struct qcom_icc_node *qn;
+	u64 clk_rate;
+	int i, ret;
 
 	if (!src)
 		node = dst;
@@ -116,6 +118,45 @@ int qcom_icc_set(struct icc_node *src, struct icc_node *dst)
 		node = src;
 
 	qp = to_qcom_provider(node->provider);
+	qn = node->data;
+
+	if (qn->bw_scale_numerator && qn->bw_scale_denominator) {
+		node->avg_bw *= qn->bw_scale_numerator;
+		do_div(node->avg_bw, qn->bw_scale_denominator);
+
+		node->peak_bw *= qn->bw_scale_numerator;
+		do_div(node->peak_bw, qn->bw_scale_denominator);
+	}
+
+	if (qn->clk) {
+		/*
+		 * Multiply by 1000 to convert the unit of bandwidth from KBps
+		 * to Bps, then divide by the bandwidth to get the clk rate in Hz.
+		 */
+		clk_rate = (u64)max(node->avg_bw, node->peak_bw) * 1000 / qn->buswidth;
+		clk_rate = clk_rate > U32_MAX ? U32_MAX : clk_rate;
+
+		if (clk_rate > 0) {
+			ret = clk_set_rate(qn->clk, clk_rate);
+			if (ret)
+				dev_warn(qp->dev, "Failed to set %s rate to %d for %s\n",
+					 qn->clk_name, clk_rate, qn->name);
+
+			if (qn->toggle_clk && !qn->clk_enabled) {
+				ret = clk_prepare_enable(qn->clk);
+				if (ret) {
+					dev_err(qp->dev, "Failed to enable %s for %s\n",
+						qn->clk_name, qn->name);
+					return ret;
+				}
+
+				qn->clk_enabled = true;
+			}
+		} else if (qn->toggle_clk && qn->clk_enabled) {
+			clk_disable_unprepare(qn->clk);
+			qn->clk_enabled = false;
+		}
+	}
 
 	for (i = 0; i < qp->num_voters; i++)
 		qcom_icc_bcm_voter_commit(qp->voters[i]);
@@ -418,6 +459,17 @@ int qcom_icc_rpmh_probe(struct platform_device *pdev)
 			goto err;
 		}
 
+		if (qn->clk_name) {
+			qn->clk = devm_clk_get(qp->dev, qn->clk_name);
+			if (IS_ERR(qn->clk)) {
+				ret = PTR_ERR(qn->clk);
+				if (ret != -EPROBE_DEFER)
+					dev_err(qp->dev, "failed to get %s, err:(%d)\n",
+						qn->clk_name, ret);
+				goto err;
+			}
+		}
+
 		if (qn->qosbox && !qp->skip_qos)
 			qn->noc_ops->set_qos(qn);
 
diff --git a/drivers/interconnect/qcom/icc-rpmh.h b/drivers/interconnect/qcom/icc-rpmh.h
index 295c3d157562..38bee8a5005a 100644
--- a/drivers/interconnect/qcom/icc-rpmh.h
+++ b/drivers/interconnect/qcom/icc-rpmh.h
@@ -68,6 +68,12 @@ struct bcm_db {
  * @perf_mode: current OR aggregate value of all QCOM_ICC_TAG_PERF_MODE votes
  * @bcms: list of bcms associated with this logical node
  * @num_bcms: num of @bcms
+ * @clk: the local clock at this node
+ * @clk_name: the local clock name at this node
+ * @toggle_clk: flag used to indicate whether local clock can be enabled/disabled
+ * @clk_enabled: flag used to indicate whether local clock have been enabled
+ * @bw_scale_numerator: the numerator of the bandwidth scale factor
+ * @bw_scale_denominator: the denominator of the bandwidth scale factor
  */
 struct qcom_icc_node {
 	const char *name;
@@ -84,6 +90,12 @@ struct qcom_icc_node {
 	struct regmap *regmap;
 	struct qcom_icc_qosbox *qosbox;
 	const struct qcom_icc_noc_ops *noc_ops;
+	struct clk *clk;
+	const char *clk_name;
+	bool toggle_clk;
+	bool clk_enabled;
+	u16 bw_scale_numerator;
+	u16 bw_scale_denominator;
 };
 
 /**