From 6e471ff113d1fef61ed46a45bdf9b1f3175febaa Mon Sep 17 00:00:00 2001
From: Finley Xiao <finley.xiao@rock-chips.com>
Date: Tue, 10 May 2022 10:25:35 +0800
Subject: [PATCH] cpufreq: rockchip: Add support to set soc info for rk3588

1. Change length according to speed grade.
2. Set supported platforms according to SoC version and speed grade.

Signed-off-by: Finley Xiao <finley.xiao@rock-chips.com>
Change-Id: I448d3f784216bc034ea2900e5ab837920721cdd0
---
 drivers/cpufreq/rockchip-cpufreq.c         | 75 ++++++++++++++++++++++
 include/soc/rockchip/rockchip_opp_select.h |  7 +-
 2 files changed, 80 insertions(+), 2 deletions(-)

diff --git a/drivers/cpufreq/rockchip-cpufreq.c b/drivers/cpufreq/rockchip-cpufreq.c
index c7cd4f243f7d..a9f381965697 100644
--- a/drivers/cpufreq/rockchip-cpufreq.c
+++ b/drivers/cpufreq/rockchip-cpufreq.c
@@ -182,6 +182,78 @@ out:
 	return ret;
 }
 
+static int rk3588_change_length(struct device *dev, struct device_node *np,
+				int bin, int process, int volt_sel)
+{
+	struct clk *clk;
+	unsigned long old_rate;
+	unsigned int low_len_sel;
+	u32 opp_flag = 0;
+	int ret = 0;
+
+	clk = clk_get(dev, NULL);
+	if (IS_ERR(clk)) {
+		dev_warn(dev, "failed to get cpu clk\n");
+		return PTR_ERR(clk);
+	}
+
+	/* RK3588 low speed grade should change to low length */
+	if (of_property_read_u32(np, "rockchip,pvtm-low-len-sel",
+				 &low_len_sel))
+		goto out;
+	if (volt_sel > low_len_sel)
+		goto out;
+	opp_flag = OPP_LENGTH_LOW;
+
+	old_rate = clk_get_rate(clk);
+	ret = clk_set_rate(clk, old_rate | opp_flag);
+	if (ret) {
+		dev_err(dev, "failed to change length\n");
+		goto out;
+	}
+	clk_set_rate(clk, old_rate);
+out:
+	clk_put(clk);
+
+	return ret;
+}
+
+static int rk3588_set_supported_hw(struct device *dev, struct device_node *np,
+				   int bin, int process, int volt_sel)
+{
+	struct opp_table *opp_table;
+	u32 supported_hw[2];
+
+	if (!of_property_read_bool(np, "rockchip,supported-hw"))
+		return 0;
+
+	/* SoC Version */
+	supported_hw[0] = BIT(bin);
+	/* Speed Grade */
+	supported_hw[1] = BIT(volt_sel);
+	opp_table = dev_pm_opp_set_supported_hw(dev, supported_hw, 2);
+	if (IS_ERR(opp_table)) {
+		dev_err(dev, "failed to set supported opp\n");
+		return PTR_ERR(opp_table);
+	}
+
+	return 0;
+}
+
+static int rk3588_set_soc_info(struct device *dev, struct device_node *np,
+			       int bin, int process, int volt_sel)
+{
+	if (volt_sel < 0)
+		return 0;
+	if (bin < 0)
+		bin = 0;
+
+	rk3588_change_length(dev, np, bin, process, volt_sel);
+	rk3588_set_supported_hw(dev, np, bin, process, volt_sel);
+
+	return 0;
+}
+
 static int rk3588_cpu_set_read_margin(struct device *dev,
 				      struct rockchip_opp_info *opp_info,
 				      u32 rm)
@@ -252,6 +324,7 @@ static const struct rockchip_opp_data rk3399_cpu_opp_data = {
 };
 
 static const struct rockchip_opp_data rk3588_cpu_opp_data = {
+	.set_soc_info = rk3588_set_soc_info,
 	.set_read_margin = rk3588_cpu_set_read_margin,
 };
 
@@ -492,6 +565,8 @@ static int rockchip_cpufreq_cluster_init(int cpu, struct cluster_info *cluster)
 		opp_info->data->get_soc_info(dev, np, &bin, &process);
 	rockchip_get_scale_volt_sel(dev, "cpu_leakage", reg_name, bin, process,
 				    &cluster->scale, &volt_sel);
+	if (opp_info->data && opp_info->data->set_soc_info)
+		opp_info->data->set_soc_info(dev, np, bin, process, volt_sel);
 	pname_table = rockchip_set_opp_prop_name(dev, process, volt_sel);
 
 	if (of_find_property(dev->of_node, "cpu-supply", NULL) &&
diff --git a/include/soc/rockchip/rockchip_opp_select.h b/include/soc/rockchip/rockchip_opp_select.h
index c7dcad35043d..473485ccd0ba 100644
--- a/include/soc/rockchip/rockchip_opp_select.h
+++ b/include/soc/rockchip/rockchip_opp_select.h
@@ -9,10 +9,11 @@
 #define VOLT_RM_TABLE_END	~1
 
 #define OPP_INTERMEDIATE_MASK	0x3f
-#define OPP_INTERMEDIATE_RATE	0x01
-#define OPP_SCALING_UP_RATE	0x02
+#define OPP_INTERMEDIATE_RATE	BIT(0)
+#define OPP_SCALING_UP_RATE	BIT(1)
 #define OPP_SCALING_UP_INTER	(OPP_INTERMEDIATE_RATE | OPP_SCALING_UP_RATE)
 #define OPP_SCALING_DOWN_INTER	OPP_INTERMEDIATE_RATE
+#define OPP_LENGTH_LOW		BIT(2)
 
 struct rockchip_opp_info;
 
@@ -24,6 +25,8 @@ struct volt_rm_table {
 struct rockchip_opp_data {
 	int (*get_soc_info)(struct device *dev, struct device_node *np,
 			    int *bin, int *process);
+	int (*set_soc_info)(struct device *dev, struct device_node *np,
+			    int bin, int process, int volt_sel);
 	int (*set_read_margin)(struct device *dev,
 			       struct rockchip_opp_info *opp_info,
 			       u32 rm);