soc: rockchip: add functions to select opp level form device-tree

Some chips need adjust opp level by different chip-process, add
common functions to select opp level from device-tree, so modules
can select opp level easy.

Change-Id: Ifbd5f720e6a52a68f13697bbb37ac01ff4a87e3e
Signed-off-by: Liang Chen <cl@rock-chips.com>

drivers/cpufreq/rockchip-cpufreq.c

@@ -29,21 +29,14 @@
 #include <linux/slab.h>
 #include <linux/soc/rockchip/pvtm.h>
 #include <linux/thermal.h>
+#include <soc/rockchip/rockchip_opp_select.h>
 
 #include "../clk/rockchip/clk.h"
 
 #define MAX_PROP_NAME_LEN	3
-#define LEAKAGE_TABLE_END	~1
 #define LEAKAGE_INVALID		0xff
-
 #define REBOOT_FREQ		816000 /* kHz */
 
-struct volt_sel_table {
-	int min;
-	int max;
-	int sel;
-};
-
 struct cluster_info {
 	struct list_head list_head;
 	cpumask_t cpus;
@@ -57,20 +50,6 @@ struct cluster_info {
 	bool rebooting;
 	bool freq_limit;
 };
-
-struct pvtm_config {
-	unsigned int freq;
-	unsigned int volt;
-	unsigned int ch[2];
-	unsigned int sample_time;
-	unsigned int num;
-	unsigned int err;
-	unsigned int ref_temp;
-	int temp_prop[2];
-	const char *tz_name;
-	struct thermal_zone_device *tz;
-};
-
 static LIST_HEAD(cluster_info_list);
 
 static struct cluster_info *rockchip_cluster_info_lookup(int cpu)
@@ -138,204 +117,14 @@ static const struct of_device_id rockchip_cpufreq_of_match[] = {
 	{},
 };
 
-static int rockchip_get_volt_sel_table(struct device_node *np, char *porp_name,
-				       struct volt_sel_table **table)
-{
-	struct volt_sel_table *sel_table;
-	const struct property *prop;
-	int count, i;
-
-	prop = of_find_property(np, porp_name, NULL);
-	if (!prop)
-		return -EINVAL;
-
-	if (!prop->value)
-		return -ENODATA;
-
-	count = of_property_count_u32_elems(np, porp_name);
-	if (count < 0)
-		return -EINVAL;
-
-	if (count % 3)
-		return -EINVAL;
-
-	sel_table = kzalloc(sizeof(*sel_table) * (count / 3 + 1), GFP_KERNEL);
-	if (!sel_table)
-		return -ENOMEM;
-
-	for (i = 0; i < count / 3; i++) {
-		of_property_read_u32_index(np, porp_name, 3 * i,
-					   &sel_table[i].min);
-		of_property_read_u32_index(np, porp_name, 3 * i + 1,
-					   &sel_table[i].max);
-		of_property_read_u32_index(np, porp_name, 3 * i + 2,
-					   &sel_table[i].sel);
-	}
-	sel_table[i].min = 0;
-	sel_table[i].max = 0;
-	sel_table[i].sel = LEAKAGE_TABLE_END;
-
-	*table = sel_table;
-
-	return 0;
-}
-
-static int rockchip_get_volt_sel(struct device_node *np, char *name,
-				 int value, int *sel)
-{
-	struct volt_sel_table *table;
-	int i, j = -1, ret;
-
-	ret = rockchip_get_volt_sel_table(np, name, &table);
-	if (ret)
-		return -EINVAL;
-
-	for (i = 0; table[i].sel != LEAKAGE_TABLE_END; i++) {
-		if (value >= table[i].min)
-			j = i;
-	}
-	if (j != -1)
-		*sel = table[j].sel;
-	else
-		ret = -EINVAL;
-
-	kfree(table);
-
-	return ret;
-}
-
-static int rockchip_parse_pvtm_config(struct device_node *np,
-				      struct pvtm_config *pvtm)
-{
-	if (!of_find_property(np, "rockchip,pvtm-voltage-sel", NULL))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-freq", &pvtm->freq))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-volt", &pvtm->volt))
-		return -EINVAL;
-	if (of_property_read_u32_array(np, "rockchip,pvtm-ch", pvtm->ch, 2))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-sample-time",
-				 &pvtm->sample_time))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-number", &pvtm->num))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-error", &pvtm->err))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-ref-temp", &pvtm->ref_temp))
-		return -EINVAL;
-	if (of_property_read_u32_array(np, "rockchip,pvtm-temp-prop",
-				       pvtm->temp_prop, 2))
-		return -EINVAL;
-	if (of_property_read_string(np, "rockchip,pvtm-thermal-zone",
-				    &pvtm->tz_name))
-		return -EINVAL;
-	pvtm->tz = thermal_zone_get_zone_by_name(pvtm->tz_name);
-	if (IS_ERR(pvtm->tz))
-		return -EINVAL;
-	if (!pvtm->tz->ops->get_temp)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int rockchip_get_pvtm_specific_value(struct device *dev,
-					    struct device_node *np,
-					    struct clk *clk,
-					    struct regulator *reg,
-					    int *target_value)
-{
-	struct pvtm_config *pvtm;
-	unsigned long old_freq;
-	unsigned int old_volt;
-	int cur_temp, diff_temp;
-	int cur_value, total_value, avg_value, diff_value;
-	int min_value, max_value;
-	int ret = 0, i = 0, retry = 2;
-
-	pvtm = kzalloc(sizeof(*pvtm), GFP_KERNEL);
-	if (!pvtm)
-		return -ENOMEM;
-
-	ret = rockchip_parse_pvtm_config(np, pvtm);
-	if (ret)
-		goto pvtm_value_out;
-
-	old_freq = clk_get_rate(clk);
-	old_volt = regulator_get_voltage(reg);
-
-	/*
-	 * Set pvtm_freq to the lowest frequency in dts,
-	 * so change frequency first.
-	 */
-	ret = clk_set_rate(clk, pvtm->freq * 1000);
-	if (ret) {
-		dev_err(dev, "Failed to set pvtm freq\n");
-		goto pvtm_value_out;
-	}
-
-	ret = regulator_set_voltage(reg, pvtm->volt, pvtm->volt);
-	if (ret) {
-		dev_err(dev, "Failed to set pvtm_volt\n");
-		goto restore_clk;
-	}
-
-	/* The first few values may be fluctuant, if error is too big, retry*/
-	while (retry--) {
-		total_value = 0;
-		min_value = INT_MAX;
-		max_value = 0;
-
-		for (i = 0; i < pvtm->num; i++) {
-			cur_value = rockchip_get_pvtm_value(pvtm->ch[0],
-							    pvtm->ch[1],
-							    pvtm->sample_time);
-			if (!cur_value)
-				goto resetore_volt;
-			if (cur_value < min_value)
-				min_value = cur_value;
-			if (cur_value > max_value)
-				max_value = cur_value;
-			total_value += cur_value;
-		}
-		if (max_value - min_value < pvtm->err)
-			break;
-	}
-	avg_value = total_value / pvtm->num;
-
-	/*
-	 * As pvtm is influenced by temperature, compute difference between
-	 * current temperature and reference temperature
-	 */
-	pvtm->tz->ops->get_temp(pvtm->tz, &cur_temp);
-	diff_temp = (cur_temp / 1000 - pvtm->ref_temp);
-	diff_value = diff_temp *
-		(diff_temp < 0 ? pvtm->temp_prop[0] : pvtm->temp_prop[1]);
-	*target_value = avg_value + diff_value;
-
-	dev_info(dev, "temp=%d, pvtm=%d (%d + %d)\n",
-		 cur_temp, *target_value, avg_value, diff_value);
-
-resetore_volt:
-	regulator_set_voltage(reg, old_volt, old_volt);
-restore_clk:
-	clk_set_rate(clk, old_freq);
-pvtm_value_out:
-	kfree(pvtm);
-
-	return ret;
-}
-
 static int rockchip_cpufreq_cluster_init(int cpu, struct cluster_info *cluster)
 {
 	int (*get_soc_version)(struct device_node *np, int *soc_version);
 	const struct of_device_id *match;
 	struct device_node *node, *np;
 	struct clk *clk;
-	struct clk *cpu_clk;
-	struct regulator *cpu_reg;
 	struct device *dev;
-	int lkg_volt_sel = -1, pvtm_volt_sel = -1, lkg_scale_sel = -1;
+	int lkg_volt_sel, pvtm_volt_sel, lkg_scale_sel;
 	int ret;
 
 	dev = get_cpu_device(cpu);
@@ -371,63 +160,25 @@ static int rockchip_cpufreq_cluster_init(int cpu, struct cluster_info *cluster)
 			     &cluster->threshold_freq);
 	cluster->freq_limit = of_property_read_bool(np, "rockchip,freq-limit");
 
-	cluster->volt_sel = -1;
-	ret = rockchip_get_efuse_value(np, "cpu_leakage", &cluster->leakage);
-	if (!ret) {
-		dev_info(dev, "leakage=%d\n", cluster->leakage);
-		ret = rockchip_get_volt_sel(np, "rockchip,leakage-voltage-sel",
-					    cluster->leakage, &lkg_volt_sel);
-		if (!ret) {
-			dev_info(dev, "leakage-sel=%d\n", lkg_volt_sel);
-			cluster->volt_sel = lkg_volt_sel;
+	lkg_scale_sel = rockchip_of_get_lkg_scale_sel(dev, "cpu_leakage");
+	if (lkg_scale_sel > 0) {
+		clk = of_clk_get_by_name(np, NULL);
+		if (IS_ERR(clk)) {
+			dev_err(dev, "Failed to get opp clk");
+			return PTR_ERR(clk);
 		}
-
-		ret = rockchip_get_volt_sel(np, "rockchip,leakage-scaling-sel",
-					    cluster->leakage,
-					    &lkg_scale_sel);
-		if (!ret) {
-			dev_info(dev, "scale-sel=%d\n", lkg_scale_sel);
-			clk = of_clk_get_by_name(np, NULL);
-			if (IS_ERR(clk)) {
-				dev_err(dev, "Failed to get opp clk");
-				return PTR_ERR(clk);
-			}
-			ret = rockchip_pll_clk_adaptive_scaling(clk,
-								lkg_scale_sel);
-			if (ret) {
-				dev_err(dev, "Failed to adaptive scaling\n");
-				return ret;
-			}
+		ret = rockchip_pll_clk_adaptive_scaling(clk,
+							lkg_scale_sel);
+		if (ret) {
+			dev_err(dev, "Failed to adaptive scaling\n");
+			return ret;
 		}
 	}
 
-	cpu_clk = clk_get(dev, NULL);
-	if (IS_ERR_OR_NULL(cpu_clk)) {
-		dev_err(dev, "Failed to get clk\n");
-		goto parse_dt_out;
-	}
-	cpu_reg = regulator_get_optional(dev, "cpu");
-	if (IS_ERR_OR_NULL(cpu_reg)) {
-		dev_err(dev, "Failed to get cpu_reg\n");
-		goto put_clk;
-	}
-	ret = rockchip_get_pvtm_specific_value(dev, np, cpu_clk, cpu_reg,
-					       &cluster->pvtm);
-	if (!ret) {
-		ret = rockchip_get_volt_sel(np, "rockchip,pvtm-voltage-sel",
-					    cluster->pvtm, &pvtm_volt_sel);
-		if (!ret) {
-			dev_info(dev, "pvtm-sel=%d\n", pvtm_volt_sel);
-			if (cluster->volt_sel < 0 ||
-			    cluster->volt_sel > pvtm_volt_sel)
-				cluster->volt_sel = pvtm_volt_sel;
-		}
-	}
+	lkg_volt_sel = rockchip_of_get_lkg_volt_sel(dev, "cpu_leakage");
+	pvtm_volt_sel = rockchip_of_get_pvtm_volt_sel(dev, NULL, "cpu");
 
-	regulator_put(cpu_reg);
-put_clk:
-	clk_put(cpu_clk);
-parse_dt_out:
+	cluster->volt_sel = max(lkg_volt_sel, pvtm_volt_sel);
 
 	return 0;
 }

drivers/devfreq/rockchip_dmc.c

@@ -44,6 +44,7 @@
 #include <soc/rockchip/rockchip_dmc.h>
 #include <soc/rockchip/rockchip_sip.h>
 #include <soc/rockchip/rockchip-system-status.h>
+#include <soc/rockchip/rockchip_opp_select.h>
 #include <soc/rockchip/scpi.h>
 #include <uapi/drm/drm_mode.h>
 
@@ -2347,6 +2348,9 @@ static int rockchip_dmcfreq_probe(struct platform_device *pdev)
 	const struct of_device_id *match;
 	int (*init)(struct platform_device *pdev,
 		    struct rockchip_dmcfreq *data);
+#define MAX_PROP_NAME_LEN	3
+	char name[MAX_PROP_NAME_LEN];
+	int lkg_volt_sel;
 	int ret;
 
 	data = devm_kzalloc(dev, sizeof(struct rockchip_dmcfreq), GFP_KERNEL);
@@ -2392,6 +2396,14 @@ static int rockchip_dmcfreq_probe(struct platform_device *pdev)
 	 * We add a devfreq driver to our parent since it has a device tree node
 	 * with operating points.
 	 */
+	lkg_volt_sel = rockchip_of_get_lkg_volt_sel(dev, "ddr_leakage");
+	if (lkg_volt_sel >= 0) {
+		snprintf(name, MAX_PROP_NAME_LEN, "L%d", lkg_volt_sel);
+		ret = dev_pm_opp_set_prop_name(dev, name);
+		if (ret)
+			dev_err(dev, "Failed to set prop name\n");
+	}
+
 	if (dev_pm_opp_of_add_table(dev)) {
 		dev_err(dev, "Invalid operating-points in device tree.\n");
 		return -EINVAL;

drivers/gpu/arm/mali400/mali/linux/mali_kernel_linux.c

@@ -30,8 +30,9 @@
 #include <linux/of.h>
 #include <linux/clk.h>
 #include <linux/regulator/consumer.h>
-
 #include <linux/mali/mali_utgard.h>
+#include <soc/rockchip/rockchip_opp_select.h>
+
 #include "mali_kernel_common.h"
 #include "mali_session.h"
 #include "mali_kernel_core.h"
@@ -517,7 +518,10 @@ static int mali_probe(struct platform_device *pdev)
 {
 	int err;
 #ifdef CONFIG_MALI_DEVFREQ
+#define MAX_PROP_NAME_LEN	3
 	struct mali_device *mdev;
+	char name[MAX_PROP_NAME_LEN];
+	int lkg_volt_sel;
 #endif
 
 	MALI_DEBUG_PRINT(2, ("mali_probe(): Called for platform device %s\n", pdev->name));
@@ -563,6 +567,14 @@ static int mali_probe(struct platform_device *pdev)
 	mdev->dev = &pdev->dev;
 	dev_set_drvdata(mdev->dev, mdev);
 
+	lkg_volt_sel = rockchip_of_get_lkg_volt_sel(mdev->dev, "gpu_leakage");
+	if (lkg_volt_sel >= 0) {
+		snprintf(name, MAX_PROP_NAME_LEN, "L%d", lkg_volt_sel);
+		err = dev_pm_opp_set_prop_name(mdev->dev, name);
+		if (err)
+			dev_err(mdev->dev, "Failed to set prop name\n");
+	}
+
 	/*Initilization clock and regulator*/
 #if (LINUX_VERSION_CODE >= KERNEL_VERSION(3, 12, 0)) && defined(CONFIG_OF) \
                         && defined(CONFIG_REGULATOR)

drivers/gpu/arm/midgard/platform/rk/mali_kbase_config_rk.c

@@ -23,32 +23,10 @@
 #include <linux/nvmem-consumer.h>
 #include <linux/soc/rockchip/pvtm.h>
 #include <linux/thermal.h>
+#include <soc/rockchip/rockchip_opp_select.h>
 
 #include "mali_kbase_rk.h"
 
-#define MAX_PROP_NAME_LEN	3
-#define LEAKAGE_TABLE_END	~1
-#define LEAKAGE_INVALID		0xff
-
-struct pvtm_config {
-	unsigned int freq;
-	unsigned int volt;
-	unsigned int ch[2];
-	unsigned int sample_time;
-	unsigned int num;
-	unsigned int err;
-	unsigned int ref_temp;
-	int temp_prop[2];
-	const char *tz_name;
-	struct thermal_zone_device *tz;
-};
-
-struct volt_sel_table {
-	int min;
-	int max;
-	int sel;
-};
-
 /**
  * @file mali_kbase_config_rk.c
  * 对 platform_config_of_rk 的具体实现.
@@ -451,268 +429,20 @@ static void kbase_platform_rk_remove_sysfs_files(struct device *dev)
 	device_remove_file(dev, &dev_attr_utilisation);
 }
 
-static int rockchip_get_efuse_value(struct device_node *np, char *porp_name,
-				    int *value)
-{
-	struct nvmem_cell *cell;
-	unsigned char *buf;
-	size_t len;
-
-	cell = of_nvmem_cell_get(np, porp_name);
-	if (IS_ERR(cell))
-		return PTR_ERR(cell);
-
-	buf = (unsigned char *)nvmem_cell_read(cell, &len);
-
-	nvmem_cell_put(cell);
-
-	if (IS_ERR(buf))
-		return PTR_ERR(buf);
-
-	if (buf[0] == LEAKAGE_INVALID)
-		return -EINVAL;
-
-	*value = buf[0];
-
-	kfree(buf);
-
-	return 0;
-}
-
-static int rockchip_get_volt_sel_table(struct device_node *np, char *porp_name,
-				       struct volt_sel_table **table)
-{
-	struct volt_sel_table *sel_table;
-	const struct property *prop;
-	int count, i;
-
-	prop = of_find_property(np, porp_name, NULL);
-	if (!prop)
-		return -EINVAL;
-
-	if (!prop->value)
-		return -ENODATA;
-
-	count = of_property_count_u32_elems(np, porp_name);
-	if (count < 0)
-		return -EINVAL;
-
-	if (count % 3)
-		return -EINVAL;
-
-	sel_table = kzalloc(sizeof(*sel_table) * (count / 3 + 1), GFP_KERNEL);
-	if (!sel_table)
-		return -ENOMEM;
-
-	for (i = 0; i < count / 3; i++) {
-		of_property_read_u32_index(np, porp_name, 3 * i,
-					   &sel_table[i].min);
-		of_property_read_u32_index(np, porp_name, 3 * i + 1,
-					   &sel_table[i].max);
-		of_property_read_u32_index(np, porp_name, 3 * i + 2,
-					   &sel_table[i].sel);
-	}
-	sel_table[i].min = 0;
-	sel_table[i].max = 0;
-	sel_table[i].sel = LEAKAGE_TABLE_END;
-
-	*table = sel_table;
-
-	return 0;
-}
-
-static int rockchip_get_volt_sel(struct device_node *np, char *name,
-				 int value, int *sel)
-{
-	struct volt_sel_table *table;
-	int i, j = -1, ret;
-
-	ret = rockchip_get_volt_sel_table(np, name, &table);
-	if (ret)
-		return -EINVAL;
-
-	for (i = 0; table[i].sel != LEAKAGE_TABLE_END; i++) {
-		if (value >= table[i].min)
-			j = i;
-	}
-	if (j != -1)
-		*sel = table[j].sel;
-	else
-		ret = -EINVAL;
-
-	kfree(table);
-
-	return ret;
-}
-
-static int rockchip_parse_pvtm_config(struct device_node *np,
-				      struct pvtm_config *pvtm)
-{
-	if (!of_find_property(np, "rockchip,pvtm-voltage-sel", NULL))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-freq", &pvtm->freq))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-volt", &pvtm->volt))
-		return -EINVAL;
-	if (of_property_read_u32_array(np, "rockchip,pvtm-ch", pvtm->ch, 2))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-sample-time",
-				 &pvtm->sample_time))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-number", &pvtm->num))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-error", &pvtm->err))
-		return -EINVAL;
-	if (of_property_read_u32(np, "rockchip,pvtm-ref-temp", &pvtm->ref_temp))
-		return -EINVAL;
-	if (of_property_read_u32_array(np, "rockchip,pvtm-temp-prop",
-				       pvtm->temp_prop, 2))
-		return -EINVAL;
-	if (of_property_read_string(np, "rockchip,pvtm-thermal-zone",
-				    &pvtm->tz_name))
-		return -EINVAL;
-	pvtm->tz = thermal_zone_get_zone_by_name(pvtm->tz_name);
-	if (IS_ERR(pvtm->tz))
-		return -EINVAL;
-	if (!pvtm->tz->ops->get_temp)
-		return -EINVAL;
-
-	return 0;
-}
-
-static int rockchip_get_pvtm_specific_value(struct device *dev,
-					    struct device_node *np,
-					    struct clk *clk,
-					    struct regulator *reg,
-					    int *target_value)
-{
-	struct pvtm_config *pvtm;
-	unsigned long old_freq;
-	unsigned int old_volt;
-	int cur_temp, diff_temp;
-	int cur_value, total_value, avg_value, diff_value;
-	int min_value, max_value;
-	int ret = 0, i = 0, retry = 2;
-
-	pvtm = kzalloc(sizeof(*pvtm), GFP_KERNEL);
-	if (!pvtm)
-		return -ENOMEM;
-
-	ret = rockchip_parse_pvtm_config(np, pvtm);
-	if (ret)
-		goto pvtm_value_out;
-
-	old_freq = clk_get_rate(clk);
-	old_volt = regulator_get_voltage(reg);
-
-	/*
-	 * Set pvtm_freq to the lowest frequency in dts,
-	 * so change frequency first.
-	 */
-	ret = clk_set_rate(clk, pvtm->freq * 1000);
-	if (ret) {
-		dev_err(dev, "Failed to set pvtm freq\n");
-		goto pvtm_value_out;
-	}
-
-	ret = regulator_set_voltage(reg, pvtm->volt, pvtm->volt);
-	if (ret) {
-		dev_err(dev, "Failed to set pvtm_volt\n");
-		goto restore_clk;
-	}
-
-	/* The first few values may be fluctuant, if error is too big, retry*/
-	while (retry--) {
-		total_value = 0;
-		min_value = INT_MAX;
-		max_value = 0;
-
-		for (i = 0; i < pvtm->num; i++) {
-			cur_value = rockchip_get_pvtm_value(pvtm->ch[0],
-							    pvtm->ch[1],
-							    pvtm->sample_time);
-			if (!cur_value)
-				goto resetore_volt;
-			if (cur_value < min_value)
-				min_value = cur_value;
-			if (cur_value > max_value)
-				max_value = cur_value;
-			total_value += cur_value;
-		}
-		if (max_value - min_value < pvtm->err)
-			break;
-	}
-	avg_value = total_value / pvtm->num;
-
-	/*
-	 * As pvtm is influenced by temperature, compute difference between
-	 * current temperature and reference temperature
-	 */
-	pvtm->tz->ops->get_temp(pvtm->tz, &cur_temp);
-	diff_temp = (cur_temp / 1000 - pvtm->ref_temp);
-	diff_value = diff_temp *
-		(diff_temp < 0 ? pvtm->temp_prop[0] : pvtm->temp_prop[1]);
-	*target_value = avg_value + diff_value;
-
-	dev_info(dev, "temp=%d, pvtm=%d (%d + %d)\n",
-		 cur_temp, *target_value, avg_value, diff_value);
-
-resetore_volt:
-	regulator_set_voltage(reg, old_volt, old_volt);
-restore_clk:
-	clk_set_rate(clk, old_freq);
-pvtm_value_out:
-	kfree(pvtm);
-
-	return ret;
-}
-
 void kbase_platform_rk_set_opp_info(struct kbase_device *kbdev)
 {
-	struct device_node *np;
+#define MAX_PROP_NAME_LEN	3
 	char name[MAX_PROP_NAME_LEN];
-	int pvmt_value, leakage;
-	int lkg_volt_sel, pvtm_volt_sel, volt_sel = -1;
+	int lkg_volt_sel, pvtm_volt_sel, volt_sel;
 	int err = 0;
 
 	if (!kbdev)
 		return;
 
-	if (IS_ERR_OR_NULL(kbdev->regulator))
-		return;
-	if (IS_ERR_OR_NULL(kbdev->clock))
-		return;
-
-	np = of_parse_phandle(kbdev->dev->of_node, "operating-points-v2", 0);
-	if (!np) {
-		dev_warn(kbdev->dev, "OPP-v2 not supported\n");
-		return;
-	}
-
-	err = rockchip_get_efuse_value(np, "gpu_leakage", &leakage);
-	if (!err) {
-		dev_info(kbdev->dev, "leakage=%d\n", leakage);
-		err = rockchip_get_volt_sel(np, "rockchip,leakage-voltage-sel",
-					    leakage, &lkg_volt_sel);
-		if (!err) {
-			dev_info(kbdev->dev, "leakage-sel=%d\n", lkg_volt_sel);
-			volt_sel = lkg_volt_sel;
-		}
-	}
-
-	err = rockchip_get_pvtm_specific_value(kbdev->dev, np, kbdev->clock,
-					       kbdev->regulator,
-					       &pvmt_value);
-	if (!err) {
-		err = rockchip_get_volt_sel(np, "rockchip,pvtm-voltage-sel",
-					    pvmt_value, &pvtm_volt_sel);
-		if (!err) {
-			dev_info(kbdev->dev, "pvtm-sel=%d\n", pvtm_volt_sel);
-			if (volt_sel < 0 || volt_sel > pvtm_volt_sel)
-				volt_sel = pvtm_volt_sel;
-		}
-	}
+	lkg_volt_sel = rockchip_of_get_lkg_volt_sel(kbdev->dev, "gpu_leakage");
+	pvtm_volt_sel = rockchip_of_get_pvtm_volt_sel(kbdev->dev, NULL, "mali");
 
+	volt_sel = max(lkg_volt_sel, pvtm_volt_sel);
 	if (volt_sel >= 0) {
 		snprintf(name, MAX_PROP_NAME_LEN, "L%d", volt_sel);
 		err = dev_pm_opp_set_prop_name(kbdev->dev, name);

drivers/soc/rockchip/Makefile

@@ -14,3 +14,4 @@ obj-$(CONFIG_ROCK_CHIP_SOC_CAMERA) += rk_camera.o
 
 obj-y += rk_vendor_storage.o
 obj-y += rockchip-system-status.o
+obj-y += rockchip_opp_select.o

drivers/soc/rockchip/rockchip_opp_select.c

@@ -0,0 +1,343 @@
+/*
+ * Copyright (c) 2017 Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+#include <linux/clk.h>
+#include <linux/nvmem-consumer.h>
+#include <linux/regulator/consumer.h>
+#include <linux/slab.h>
+#include <linux/soc/rockchip/pvtm.h>
+#include <linux/thermal.h>
+#include <soc/rockchip/rockchip_opp_select.h>
+
+#define LEAKAGE_TABLE_END	~1
+#define LEAKAGE_INVALID		0xff
+
+struct volt_sel_table {
+	int min;
+	int max;
+	int sel;
+};
+
+struct pvtm_config {
+	unsigned int freq;
+	unsigned int volt;
+	unsigned int ch[2];
+	unsigned int sample_time;
+	unsigned int num;
+	unsigned int err;
+	unsigned int ref_temp;
+	int temp_prop[2];
+	const char *tz_name;
+	struct thermal_zone_device *tz;
+};
+
+static int rockchip_get_efuse_value(struct device_node *np, char *porp_name,
+				    int *value)
+{
+	struct nvmem_cell *cell;
+	unsigned char *buf;
+	size_t len;
+
+	cell = of_nvmem_cell_get(np, porp_name);
+	if (IS_ERR(cell))
+		return PTR_ERR(cell);
+
+	buf = (unsigned char *)nvmem_cell_read(cell, &len);
+
+	nvmem_cell_put(cell);
+
+	if (IS_ERR(buf))
+		return PTR_ERR(buf);
+
+	if (buf[0] == LEAKAGE_INVALID)
+		return -EINVAL;
+
+	*value = buf[0];
+
+	kfree(buf);
+
+	return 0;
+}
+
+static int rockchip_get_volt_sel_table(struct device_node *np, char *porp_name,
+				       struct volt_sel_table **table)
+{
+	struct volt_sel_table *sel_table;
+	const struct property *prop;
+	int count, i;
+
+	prop = of_find_property(np, porp_name, NULL);
+	if (!prop)
+		return -EINVAL;
+
+	if (!prop->value)
+		return -ENODATA;
+
+	count = of_property_count_u32_elems(np, porp_name);
+	if (count < 0)
+		return -EINVAL;
+
+	if (count % 3)
+		return -EINVAL;
+
+	sel_table = kzalloc(sizeof(*sel_table) * (count / 3 + 1), GFP_KERNEL);
+	if (!sel_table)
+		return -ENOMEM;
+
+	for (i = 0; i < count / 3; i++) {
+		of_property_read_u32_index(np, porp_name, 3 * i,
+					   &sel_table[i].min);
+		of_property_read_u32_index(np, porp_name, 3 * i + 1,
+					   &sel_table[i].max);
+		of_property_read_u32_index(np, porp_name, 3 * i + 2,
+					   &sel_table[i].sel);
+	}
+	sel_table[i].min = 0;
+	sel_table[i].max = 0;
+	sel_table[i].sel = LEAKAGE_TABLE_END;
+
+	*table = sel_table;
+
+	return 0;
+}
+
+static int rockchip_get_volt_sel(struct device_node *np, char *name,
+				 int value, int *sel)
+{
+	struct volt_sel_table *table;
+	int i, j = -1, ret;
+
+	ret = rockchip_get_volt_sel_table(np, name, &table);
+	if (ret)
+		return -EINVAL;
+
+	for (i = 0; table[i].sel != LEAKAGE_TABLE_END; i++) {
+		if (value >= table[i].min)
+			j = i;
+	}
+	if (j != -1)
+		*sel = table[j].sel;
+	else
+		ret = -EINVAL;
+
+	kfree(table);
+
+	return ret;
+}
+
+static int rockchip_parse_pvtm_config(struct device_node *np,
+				      struct pvtm_config *pvtm)
+{
+	if (!of_find_property(np, "rockchip,pvtm-voltage-sel", NULL))
+		return -EINVAL;
+	if (of_property_read_u32(np, "rockchip,pvtm-freq", &pvtm->freq))
+		return -EINVAL;
+	if (of_property_read_u32(np, "rockchip,pvtm-volt", &pvtm->volt))
+		return -EINVAL;
+	if (of_property_read_u32_array(np, "rockchip,pvtm-ch", pvtm->ch, 2))
+		return -EINVAL;
+	if (of_property_read_u32(np, "rockchip,pvtm-sample-time",
+				 &pvtm->sample_time))
+		return -EINVAL;
+	if (of_property_read_u32(np, "rockchip,pvtm-number", &pvtm->num))
+		return -EINVAL;
+	if (of_property_read_u32(np, "rockchip,pvtm-error", &pvtm->err))
+		return -EINVAL;
+	if (of_property_read_u32(np, "rockchip,pvtm-ref-temp", &pvtm->ref_temp))
+		return -EINVAL;
+	if (of_property_read_u32_array(np, "rockchip,pvtm-temp-prop",
+				       pvtm->temp_prop, 2))
+		return -EINVAL;
+	if (of_property_read_string(np, "rockchip,pvtm-thermal-zone",
+				    &pvtm->tz_name))
+		return -EINVAL;
+	pvtm->tz = thermal_zone_get_zone_by_name(pvtm->tz_name);
+	if (IS_ERR(pvtm->tz))
+		return -EINVAL;
+	if (!pvtm->tz->ops->get_temp)
+		return -EINVAL;
+
+	return 0;
+}
+
+static int rockchip_get_pvtm_specific_value(struct device *dev,
+					    struct device_node *np,
+					    struct clk *clk,
+					    struct regulator *reg,
+					    int *target_value)
+{
+	struct pvtm_config *pvtm;
+	unsigned long old_freq;
+	unsigned int old_volt;
+	int cur_temp, diff_temp;
+	int cur_value, total_value, avg_value, diff_value;
+	int min_value, max_value;
+	int ret = 0, i = 0, retry = 2;
+
+	pvtm = kzalloc(sizeof(*pvtm), GFP_KERNEL);
+	if (!pvtm)
+		return -ENOMEM;
+
+	ret = rockchip_parse_pvtm_config(np, pvtm);
+	if (ret)
+		goto pvtm_value_out;
+
+	old_freq = clk_get_rate(clk);
+	old_volt = regulator_get_voltage(reg);
+
+	/*
+	 * Set pvtm_freq to the lowest frequency in dts,
+	 * so change frequency first.
+	 */
+	ret = clk_set_rate(clk, pvtm->freq * 1000);
+	if (ret) {
+		dev_err(dev, "Failed to set pvtm freq\n");
+		goto pvtm_value_out;
+	}
+
+	ret = regulator_set_voltage(reg, pvtm->volt, pvtm->volt);
+	if (ret) {
+		dev_err(dev, "Failed to set pvtm_volt\n");
+		goto restore_clk;
+	}
+
+	/* The first few values may be fluctuant, if error is too big, retry*/
+	while (retry--) {
+		total_value = 0;
+		min_value = INT_MAX;
+		max_value = 0;
+
+		for (i = 0; i < pvtm->num; i++) {
+			cur_value = rockchip_get_pvtm_value(pvtm->ch[0],
+							    pvtm->ch[1],
+							    pvtm->sample_time);
+			if (!cur_value)
+				goto resetore_volt;
+			if (cur_value < min_value)
+				min_value = cur_value;
+			if (cur_value > max_value)
+				max_value = cur_value;
+			total_value += cur_value;
+		}
+		if (max_value - min_value < pvtm->err)
+			break;
+	}
+	avg_value = total_value / pvtm->num;
+
+	/*
+	 * As pvtm is influenced by temperature, compute difference between
+	 * current temperature and reference temperature
+	 */
+	pvtm->tz->ops->get_temp(pvtm->tz, &cur_temp);
+	diff_temp = (cur_temp / 1000 - pvtm->ref_temp);
+	diff_value = diff_temp *
+		(diff_temp < 0 ? pvtm->temp_prop[0] : pvtm->temp_prop[1]);
+	*target_value = avg_value + diff_value;
+
+	dev_info(dev, "temp=%d, pvtm=%d (%d + %d)\n",
+		 cur_temp, *target_value, avg_value, diff_value);
+
+resetore_volt:
+	regulator_set_voltage(reg, old_volt, old_volt);
+restore_clk:
+	clk_set_rate(clk, old_freq);
+pvtm_value_out:
+	kfree(pvtm);
+
+	return ret;
+}
+
+int rockchip_of_get_lkg_scale_sel(struct device *dev, char *name)
+{
+	struct device_node *np;
+	int leakage, volt_sel;
+	int ret;
+
+	np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
+	if (!np) {
+		dev_warn(dev, "OPP-v2 not supported\n");
+		return -ENOENT;
+	}
+
+	ret = rockchip_get_efuse_value(np, name, &leakage);
+	if (!ret) {
+		dev_info(dev, "%s=%d\n", name, leakage);
+		ret = rockchip_get_volt_sel(np, "rockchip,leakage-scaling-sel",
+					    leakage, &volt_sel);
+		if (!ret) {
+			dev_info(dev, "%s-scale-sel=%d\n", name, volt_sel);
+			return volt_sel;
+		}
+	}
+
+	return ret;
+}
+
+int rockchip_of_get_lkg_volt_sel(struct device *dev, char *name)
+{
+	struct device_node *np;
+	int leakage, volt_sel;
+	int ret;
+
+	np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
+	if (!np) {
+		dev_warn(dev, "OPP-v2 not supported\n");
+		return -ENOENT;
+	}
+
+	ret = rockchip_get_efuse_value(np, name, &leakage);
+	if (!ret) {
+		dev_info(dev, "%s=%d\n", name, leakage);
+		ret = rockchip_get_volt_sel(np, "rockchip,leakage-voltage-sel",
+					    leakage, &volt_sel);
+		if (!ret) {
+			dev_info(dev, "%s-volt-sel=%d\n", name, volt_sel);
+			return volt_sel;
+		}
+	}
+
+	return ret;
+}
+
+int rockchip_of_get_pvtm_volt_sel(struct device *dev,
+				  char *clk_name,
+				  char *reg_name)
+{
+	struct device_node *np;
+	struct regulator *reg;
+	struct clk *clk;
+	int pvtm, volt_sel;
+	int ret;
+
+	np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
+	if (!np) {
+		dev_warn(dev, "OPP-v2 not supported\n");
+		return -ENOENT;
+	}
+
+	clk = clk_get(dev, clk_name);
+	if (IS_ERR_OR_NULL(clk)) {
+		dev_err(dev, "Failed to get clk\n");
+		return PTR_ERR(clk);
+	}
+
+	reg = regulator_get_optional(dev, reg_name);
+	if (IS_ERR_OR_NULL(reg)) {
+		dev_err(dev, "Failed to get reg\n");
+		clk_put(clk);
+		return PTR_ERR(reg);
+	}
+
+	ret = rockchip_get_pvtm_specific_value(dev, np, clk, reg, &pvtm);
+	if (!ret)
+		ret = rockchip_get_volt_sel(np, "rockchip,pvtm-voltage-sel",
+					    pvtm, &volt_sel);
+
+	regulator_put(reg);
+	clk_put(clk);
+
+	return ret ? ret : volt_sel;
+}
+

include/soc/rockchip/rockchip_opp_select.h

@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2017 Fuzhou Rockchip Electronics Co., Ltd
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ */
+#ifndef __SOC_ROCKCHIP_OPP_SELECT_H
+#define __SOC_ROCKCHIP_OPP_SELECT_H
+
+int rockchip_of_get_lkg_scale_sel(struct device *dev, char *name);
+int rockchip_of_get_lkg_volt_sel(struct device *dev, char *name);
+int rockchip_of_get_pvtm_volt_sel(struct device *dev,
+				  char *clk_name,
+				  char *reg_name);
+
+#endif