[PATCH 08/13] clk: qcom: clk-rcg2: Add support for CRM based RCG ops

[Jagadeesh Kona]

Add clock ops to control RCGs via the CESTA Resource Manager(CRM).
This adds support for clk_rcg2_crmb_ops where the RCG's frequency
table is dynamically populated by reading the LUT entries provided
by CRM. For scaling the RCG, the RCG frequency is converted to BCM
TCS bandwidth vote and sent to CESTA HW using CRM APIs. The CESTA
HW would then scale the RCG to the desired frequency requested via
CRM.

Co-developed-by: Taniya Das <taniya.das@xxxxxxxxxxxxxxxx>
Signed-off-by: Taniya Das <taniya.das@xxxxxxxxxxxxxxxx>
Signed-off-by: Jagadeesh Kona <jagadeesh.kona@xxxxxxxxxxxxxxxx>
---
 drivers/clk/qcom/clk-rcg.h  |  23 ++++-
 drivers/clk/qcom/clk-rcg2.c | 242 ++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 264 insertions(+), 1 deletion(-)

diff --git a/drivers/clk/qcom/clk-rcg.h b/drivers/clk/qcom/clk-rcg.h
index 4fbdf4880d0308714a2d1dc8c510ec3819206b9e..4872e7f9caa1096bdebca36dfbb439dc790638ef 100644
--- a/drivers/clk/qcom/clk-rcg.h
+++ b/drivers/clk/qcom/clk-rcg.h
@@ -1,5 +1,8 @@
 /* SPDX-License-Identifier: GPL-2.0 */
-/* Copyright (c) 2013, 2018, The Linux Foundation. All rights reserved. */
+/*
+ * Copyright (c) 2013, 2018, The Linux Foundation. All rights reserved.
+ * Copyright (c) Qualcomm Technologies, Inc. and/or its subsidiaries.
+ */
 
 #ifndef __QCOM_CLK_RCG_H__
 #define __QCOM_CLK_RCG_H__
@@ -175,9 +178,26 @@ struct clk_rcg2 {
 	u8			cfg_off;
 	u32			parked_cfg;
 	bool			hw_clk_ctrl;
+
+};
+
+/**
+ * struct clk_rcg2_crm - root clock generator controlled via CRM
+ * (CESTA Resource Manager)
+ *
+ * @rcg: root clock generator
+ * @crm_vcd: Virtual Clock Domain(VCD) index for a CRM controlled RCG
+ * @crm: pointer to CLK CRM instance used for reading frequency LUTs
+ *	 and to communicate with CESTA HW
+ */
+struct clk_rcg2_crm {
+	struct clk_rcg2		rcg;
+	u8			crm_vcd;
+	struct clk_crm		*crm;
 };
 
 #define to_clk_rcg2(_hw) container_of(to_clk_regmap(_hw), struct clk_rcg2, clkr)
+#define to_clk_rcg2_crm(_hw) container_of(to_clk_rcg2(_hw), struct clk_rcg2_crm, rcg)
 
 struct clk_rcg2_gfx3d {
 	u8 div;
@@ -202,6 +222,7 @@ extern const struct clk_ops clk_rcg2_shared_ops;
 extern const struct clk_ops clk_rcg2_shared_floor_ops;
 extern const struct clk_ops clk_rcg2_shared_no_init_park_ops;
 extern const struct clk_ops clk_dp_ops;
+extern const struct clk_ops clk_rcg2_crmb_ops;
 
 struct clk_rcg_dfs_data {
 	struct clk_rcg2 *rcg;
diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index 6064a0e17d5190e9d228688c04c0d4947876b4e6..668115b94a0f202cf3ac4f68efe3cef1684055be 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -1,6 +1,7 @@
 // SPDX-License-Identifier: GPL-2.0
 /*
  * Copyright (c) 2013, 2018, The Linux Foundation. All rights reserved.
+ * Copyright (c) Qualcomm Technologies, Inc. and/or its subsidiaries.
  */
 
 #include <linux/kernel.h>
@@ -16,9 +17,12 @@
 #include <linux/math64.h>
 #include <linux/gcd.h>
 #include <linux/minmax.h>
+#include <linux/pm_runtime.h>
 #include <linux/slab.h>
 
 #include <asm/div64.h>
+#include <soc/qcom/crm.h>
+#include <soc/qcom/tcs.h>
 
 #include "clk-rcg.h"
 #include "common.h"
@@ -59,6 +63,33 @@
 #define SE_PERF_M_DFSR(level)	(0x5c + 0x4 * (level))
 #define SE_PERF_N_DFSR(level)	(0x9c + 0x4 * (level))
 
+/* Cesta configuration*/
+#define MAX_CRM_SW_DRV_STATE	3
+
+/* Address offset for specific VCD */
+#define CRMC_OFFS_VCD(crm, _vcd) \
+	((crm)->regs.vcd_offset * (_vcd))
+
+/* LUT registers address offset for specific vcd and lut entry level */
+#define CRMC_OFFS_LUT(crm, vcd, _level) \
+	(CRMC_OFFS_VCD(crm, vcd) + ((crm)->regs.lut_level_offset * (_level)))
+
+/* CFG_RCGR LUT register address offset for specific vcd and lut entry level */
+#define CRMC_OFFS_CFG_RCGR(crm, vcd, level) \
+	(CRMC_OFFS_LUT(crm, vcd, level) + (crm)->regs.reg_cfg_rcgr_lut_base)
+
+/* L_VAL LUT register address offset for specific vcd and lut entry level */
+#define CRMC_OFFS_L_VAL(crm, vcd, level) \
+	(CRMC_OFFS_LUT(crm, vcd, level) + (crm)->regs.reg_l_val_lut_base)
+
+#define PLL_L_VAL_MASK		GENMASK(7, 0)
+#define PLL_ALPHA_VAL_MASK	GENMASK(31, 16)
+#define PLL_ALPHA_VAL_SHIFT	16
+#define PERF_OL_OFF		0
+#define XO_FREQ			19200000ULL
+#define CALC_PLL_RATE(l, a)	((XO_FREQ * (l)) + ((XO_FREQ * (a)) >> 16))
+#define HZ_TO_MHZ(rate)		(rate / 1000000ULL)
+
 enum freq_policy {
 	FLOOR,
 	CEIL,
@@ -1859,3 +1890,214 @@ const struct clk_ops clk_dp_ops = {
 	.determine_rate = clk_rcg2_dp_determine_rate,
 };
 EXPORT_SYMBOL_GPL(clk_dp_ops);
+
+static int clk_rcg2_crm_populate_freq(struct clk_hw *hw, unsigned int lut_index,
+				      struct freq_tbl *f)
+{
+	u32 mask, cfg_rcgr, src, pll_lval, lval, alpha_val, num_parents, pindex;
+	struct clk_rcg2_crm *rcg_crm = to_clk_rcg2_crm(hw);
+	struct clk_rcg2 *rcg = &rcg_crm->rcg;
+	struct clk_crm *crm = rcg_crm->crm;
+	u32 vcd = rcg_crm->crm_vcd;
+	unsigned long prate = 0;
+	struct clk_hw *parent;
+
+	/* Read CFG_RCGR and PLL LUT entries */
+	regmap_read(crm->regmap_crmc, CRMC_OFFS_CFG_RCGR(crm, vcd, lut_index), &cfg_rcgr);
+	regmap_read(crm->regmap_crmc, CRMC_OFFS_L_VAL(crm, vcd, lut_index), &pll_lval);
+
+	/* Calculate the pre_div and parent source from LUT entry */
+	mask = BIT(rcg->hid_width) - 1;
+	f->pre_div = 1;
+	if (cfg_rcgr & mask)
+		f->pre_div = cfg_rcgr & mask;
+
+	src = cfg_rcgr & CFG_SRC_SEL_MASK;
+	src >>= CFG_SRC_SEL_SHIFT;
+
+	lval = pll_lval & PLL_L_VAL_MASK;
+	alpha_val = (pll_lval & PLL_ALPHA_VAL_MASK) >> PLL_ALPHA_VAL_SHIFT;
+
+	/* Find the matching parent and calculate it's rate for LUT entry */
+	num_parents = clk_hw_get_num_parents(hw);
+	for (pindex = 0; pindex < num_parents; pindex++) {
+		if (src == rcg->parent_map[pindex].cfg) {
+			f->src = rcg->parent_map[pindex].src;
+
+			parent = clk_hw_get_parent_by_index(hw, pindex);
+			if (!parent)
+				return -EINVAL;
+
+			if (!lval)
+				prate = clk_hw_get_rate(parent);
+			else
+				prate = CALC_PLL_RATE(lval, alpha_val);
+
+			break;
+		}
+	}
+
+	if (!prate)
+		return -EINVAL;
+
+	/* Calculate the RCG's rate from parent rate */
+	f->freq = calc_rate(prate, 0, 0, 0, f->pre_div);
+
+	return 0;
+}
+
+static int clk_rcg2_crm_populate_freq_table(struct clk_hw *hw)
+{
+	struct freq_tbl *freq_tbl, *curr_freq_tbl;
+	struct clk_rcg2_crm *rcg_crm = to_clk_rcg2_crm(hw);
+	struct clk_rcg2 *rcg = &rcg_crm->rcg;
+	struct clk_crm *crm = rcg_crm->crm;
+	u32 prev_freq = 0;
+	int i, ret;
+
+	/* Allocate space for 1 extra since table is NULL terminated */
+	freq_tbl = kcalloc(crm->max_perf_ol + 1, sizeof(*freq_tbl), GFP_KERNEL);
+	if (!freq_tbl)
+		return -ENOMEM;
+
+	for (i = 0; i < crm->max_perf_ol; i++) {
+		/* Skip first LUT entry as first entry is used to disable RCG */
+		ret = clk_rcg2_crm_populate_freq(hw, i + 1, freq_tbl + i);
+		if (ret) {
+			kfree(freq_tbl);
+			return ret;
+		}
+
+		curr_freq_tbl = freq_tbl + i;
+
+		/* Two of the same/decreasing frequencies means end of LUT */
+		if (prev_freq >= curr_freq_tbl->freq) {
+			curr_freq_tbl->freq = 0;
+			break;
+		}
+
+		prev_freq = curr_freq_tbl->freq;
+	}
+
+	rcg->freq_tbl = freq_tbl;
+
+	return 0;
+}
+
+static int clk_rcg2_crm_determine_rate(struct clk_hw *hw,
+				       struct clk_rate_request *req)
+{
+	struct device *dev = clk_hw_get_dev(hw);
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	int ret;
+
+	if (!rcg->freq_tbl) {
+		ret = pm_runtime_get_sync(dev);
+		if (ret < 0)
+			return ret;
+
+		ret = clk_rcg2_crm_populate_freq_table(hw);
+		if (ret) {
+			pm_runtime_put(dev);
+			return ret;
+		}
+
+		pm_runtime_put(dev);
+	}
+
+	return clk_rcg2_determine_rate(hw, req);
+}
+
+static unsigned long
+clk_rcg2_crm_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+
+	if (!rcg->freq_tbl)
+		return clk_rcg2_recalc_rate(hw, parent_rate);
+
+	return rcg->freq_tbl[rcg->parked_cfg].freq;
+}
+
+static int clk_rcg2_crm_init(struct clk_hw *hw)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+
+	rcg->freq_tbl = NULL;
+
+	return 0;
+}
+
+static int clk_rcg2_vote_bw(struct clk_hw *hw, unsigned long rate)
+{
+	struct clk_rcg2_crm *rcg_crm = to_clk_rcg2_crm(hw);
+	struct clk_crm *crm = rcg_crm->crm;
+	struct crm_cmd cmd;
+	int ret, i;
+
+	cmd.resource_idx = 0;
+	cmd.wait = true;
+	cmd.data = BCM_TCS_CMD(1, 1, 0, HZ_TO_MHZ(rate));
+
+	for (i = 0; i < MAX_CRM_SW_DRV_STATE; i++) {
+		cmd.pwr_state = i;
+		ret = crm_write_bw_vote(crm->crm_dev, crm->client_idx, &cmd);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
+}
+
+static int clk_rcg2_crmb_prepare(struct clk_hw *hw)
+{
+	unsigned long rate = clk_hw_get_rate(hw);
+
+	if (!rate)
+		return -EINVAL;
+
+	return clk_rcg2_vote_bw(hw, rate);
+}
+
+static void clk_rcg2_crmb_unprepare(struct clk_hw *hw)
+{
+	clk_rcg2_vote_bw(hw, PERF_OL_OFF);
+}
+
+static int clk_rcg2_crmb_set_rate(struct clk_hw *hw, unsigned long rate,
+				  unsigned long parent_rate)
+{
+	struct clk_rcg2_crm *rcg_crm = to_clk_rcg2_crm(hw);
+	struct clk_rcg2 *rcg = &rcg_crm->rcg;
+	struct clk_crm *crm = rcg_crm->crm;
+	int perf_index;
+
+	perf_index = qcom_find_crm_freq_index(rcg->freq_tbl, rate);
+	if (perf_index <= PERF_OL_OFF || perf_index > crm->max_perf_ol)
+		return -EINVAL;
+
+	/*
+	 * perf_index returned from qcom_find_crm_freq_index() starts from
+	 * index 1 since 0 is treated as OFF by CRM, but RCG's frequency
+	 * table starts from index 0, store parked_cfg aligning to RCG's
+	 * frequency table index.
+	 */
+	rcg->parked_cfg = perf_index - 1;
+
+	if (!clk_hw_is_prepared(hw))
+		return 0;
+
+	return clk_rcg2_vote_bw(hw, rate);
+}
+
+const struct clk_ops clk_rcg2_crmb_ops = {
+	.prepare = clk_rcg2_crmb_prepare,
+	.unprepare = clk_rcg2_crmb_unprepare,
+	.is_enabled = clk_rcg2_is_enabled,
+	.get_parent = clk_rcg2_get_parent,
+	.set_rate = clk_rcg2_crmb_set_rate,
+	.determine_rate = clk_rcg2_crm_determine_rate,
+	.recalc_rate = clk_rcg2_crm_recalc_rate,
+	.init = clk_rcg2_crm_init,
+};
+EXPORT_SYMBOL_GPL(clk_rcg2_crmb_ops);

-- 
2.34.1