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>
@@ -1350,6 +1351,93 @@ static int rockchip_get_soc_info(struct device *dev, struct device_node *np,
 	return 0;
 }
 
+static void rockchip_init_pvtpll_table(struct device *dev,
+				       struct rockchip_opp_info *info)
+{
+	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(dev->of_node, "operating-points-v2", 0);
+	if (!np) {
+		dev_warn(dev, "OPP-v2 not supported\n");
+		return;
+	}
+
+	ret = of_parse_phandle_with_args(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 (info->bin > 0) {
+		snprintf(prop_name, sizeof(prop_name),
+			 "rockchip,pvtpll-table-B%d", info->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(dev, "%s: Invalid %s property (%d)\n", __func__,
+			prop_name, count);
+		goto out;
+	} else if (count % 5) {
+		dev_err(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(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(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);
+}
+
 static int rockchip_set_opp_supported_hw(struct device *dev,
 					 struct device_node *np,
 					 struct rockchip_opp_info *info)
@@ -1609,6 +1697,7 @@ int rockchip_init_opp_info(struct device *dev, struct rockchip_opp_info *info,
 		info->data->get_soc_info(dev, np, &info->bin, &info->process);
 	rockchip_get_soc_info(dev, np, &info->bin, &info->process);
 
+	rockchip_init_pvtpll_table(dev, info);
 	rockchip_get_scale_volt_sel(dev, "leakage", reg_name, info);
 
 	if (info && info->data && info->data->set_soc_info)

include/soc/rockchip/rockchip_opp_select.h

@@ -95,6 +95,8 @@ struct pvtpll_opp_table {
  * @regulator_count:	Number of power supply regulators.
  * @init_freq:		Set the initial frequency when init opp table.
  * @is_rate_volt_checked: Marks if device has checked initial rate and voltage.
+ * @pvtpll_clk_id:      Device's clock id.
+ * @pvtpll_low_temp:    Marks if device has low temperature pvtpll config.
  */
 struct rockchip_opp_info {
 	struct device *dev;
@@ -129,6 +131,9 @@ struct rockchip_opp_info {
 	int regulator_count;
 	unsigned int init_freq;
 	bool is_rate_volt_checked;
+
+	u32 pvtpll_clk_id;
+	bool pvtpll_low_temp;
 };
 
 #if IS_ENABLED(CONFIG_ROCKCHIP_OPP)