soc: rockchip: opp_select: Implement rockchip_init_pvtpll_table()

This patch adds support to parse pvtpll config from devicetree, and
change config through sip smc interface.

Signed-off-by: Finley Xiao <finley.xiao@rock-chips.com>
Change-Id: I5f23b9eb1d358a95b690ca214e35927e74451af1

drivers/soc/rockchip/rockchip_opp_select.c

@@ -12,6 +12,7 @@
 #include <linux/nvmem-consumer.h>
 #include <linux/regmap.h>
 #include <linux/regulator/consumer.h>
+#include <linux/rockchip/rockchip_sip.h>
 #include <linux/slab.h>
 #include <linux/soc/rockchip/pvtm.h>
 #include <linux/thermal.h>
@@ -1038,6 +1039,93 @@ out:
 }
 EXPORT_SYMBOL(rockchip_pvtpll_add_length);
 
+void rockchip_init_pvtpll_table(struct rockchip_opp_info *info, int bin)
+{
+	struct device_node *np = NULL;
+	struct property *prop = NULL;
+	struct of_phandle_args clkspec = { 0 };
+	struct arm_smccc_res res;
+	char prop_name[NAME_MAX];
+	u32 *value;
+	int count;
+	int ret, i;
+
+	if (!info)
+		return;
+
+	np = of_parse_phandle(info->dev->of_node, "operating-points-v2", 0);
+	if (!np) {
+		dev_warn(info->dev, "OPP-v2 not supported\n");
+		return;
+	}
+
+	ret = of_parse_phandle_with_args(info->dev->of_node, "clocks",
+					"#clock-cells", 0, &clkspec);
+	if (ret)
+		goto out;
+	info->pvtpll_clk_id = clkspec.args[0];
+	of_node_put(clkspec.np);
+
+	res = sip_smc_get_pvtpll_info(PVTPLL_GET_INFO, info->pvtpll_clk_id);
+	if (res.a0)
+		goto out;
+	if (!res.a1)
+		info->pvtpll_low_temp = true;
+
+	if (bin > 0) {
+		snprintf(prop_name, sizeof(prop_name),
+			 "rockchip,pvtpll-table-B%d", bin);
+		prop = of_find_property(np, prop_name, NULL);
+	}
+	if (!prop)
+		sprintf(prop_name, "rockchip,pvtpll-table");
+
+	prop = of_find_property(np, prop_name, NULL);
+	if (!prop)
+		goto out;
+
+	count = of_property_count_u32_elems(np, prop_name);
+	if (count < 0) {
+		dev_err(info->dev, "%s: Invalid %s property (%d)\n",
+			__func__, prop_name, count);
+		goto out;
+	} else if (count % 5) {
+		dev_err(info->dev, "Invalid count of %s\n", prop_name);
+		goto out;
+	}
+
+	value = kmalloc_array(count, sizeof(*value), GFP_KERNEL);
+	if (!value)
+		goto out;
+	ret = of_property_read_u32_array(np, prop_name, value, count);
+	if (ret) {
+		dev_err(info->dev, "%s: error parsing %s: %d\n",
+			__func__, prop_name, ret);
+		goto free_value;
+	}
+
+	for (i = 0; i < count; i += 5) {
+		res = sip_smc_pvtpll_config(PVTPLL_ADJUST_TABLE,
+					    info->pvtpll_clk_id, value[i],
+					    value[i + 1], value[i + 2],
+					    value[i + 3], value[i + 4]);
+		if (res.a0) {
+			dev_err(info->dev,
+				"%s: error cfg clk_id=%u %u %u %u %u %u (%d)\n",
+				__func__, info->pvtpll_clk_id, value[i],
+				value[i + 1], value[i + 2], value[i + 3],
+				value[i + 4], (int)res.a0);
+			goto free_value;
+		}
+	}
+
+free_value:
+	kfree(value);
+out:
+	of_node_put(np);
+}
+EXPORT_SYMBOL(rockchip_init_pvtpll_table);
+
 static int rockchip_get_pvtm_pvtpll(struct device *dev, struct device_node *np,
 				    char *reg_name)
 {
@@ -1957,6 +2045,7 @@ int rockchip_init_opp_table(struct device *dev, struct rockchip_opp_info *info,
 
 next:
 	rockchip_get_soc_info(dev, np, &bin, &process);
+	rockchip_init_pvtpll_table(info, bin);
 	rockchip_get_scale_volt_sel(dev, lkg_name, reg_name, bin, process,
 				    &scale, &volt_sel);
 	if (info && info->data && info->data->set_soc_info)

include/soc/rockchip/rockchip_opp_select.h

@@ -78,6 +78,8 @@ struct rockchip_opp_info {
 	u32 low_rm;
 	u32 current_rm;
 	u32 target_rm;
+	u32 pvtpll_clk_id;
+	bool pvtpll_low_temp;
 };
 
 #if IS_ENABLED(CONFIG_ROCKCHIP_OPP)
@@ -87,6 +89,7 @@ void rockchip_of_get_lkg_sel(struct device *dev, struct device_node *np,
 			     int *volt_sel, int *scale_sel);
 void rockchip_pvtpll_calibrate_opp(struct rockchip_opp_info *info);
 void rockchip_pvtpll_add_length(struct rockchip_opp_info *info);
+void rockchip_init_pvtpll_table(struct rockchip_opp_info *info, int bin);
 void rockchip_of_get_pvtm_sel(struct device *dev, struct device_node *np,
 			      char *reg_name, int bin, int process,
 			      int *volt_sel, int *scale_sel);
@@ -154,6 +157,11 @@ static inline void rockchip_pvtpll_add_length(struct rockchip_opp_info *info)
 {
 }
 
+static inline void rockchip_init_pvtpll_table(struct rockchip_opp_info *info,
+					      int bin)
+{
+}
+
 static inline void rockchip_of_get_pvtm_sel(struct device *dev,
 					    struct device_node *np,
 					    char *reg_name, int bin, int process,