From 37f49e37ab4cb5d0bc4c60eb5c6d4dd57db767bb Mon Sep 17 00:00:00 2001
From: hc <hc@nodka.com>
Date: Fri, 10 May 2024 07:44:59 +0000
Subject: [PATCH] gmac get mac form eeprom

---
 kernel/drivers/soc/rockchip/rockchip_opp_select.c |  887 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 files changed, 850 insertions(+), 37 deletions(-)

diff --git a/kernel/drivers/soc/rockchip/rockchip_opp_select.c b/kernel/drivers/soc/rockchip/rockchip_opp_select.c
index 02bcf82..6c496ad 100644
--- a/kernel/drivers/soc/rockchip/rockchip_opp_select.c
+++ b/kernel/drivers/soc/rockchip/rockchip_opp_select.c
@@ -3,6 +3,7 @@
  *
  * SPDX-License-Identifier: GPL-2.0+
  */
+//#define DEBUG
 #include <linux/clk.h>
 #include <linux/cpufreq.h>
 #include <linux/devfreq.h>
@@ -11,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>
@@ -714,6 +716,416 @@
 }
 EXPORT_SYMBOL(rockchip_of_get_lkg_sel);
 
+static unsigned long rockchip_pvtpll_get_rate(struct rockchip_opp_info *info)
+{
+	unsigned int rate0, rate1, delta;
+	int i;
+
+#define MIN_STABLE_DELTA 3
+	regmap_read(info->grf, info->pvtpll_avg_offset, &rate0);
+	/* max delay 2ms */
+	for (i = 0; i < 20; i++) {
+		udelay(100);
+		regmap_read(info->grf, info->pvtpll_avg_offset, &rate1);
+		delta = abs(rate1 - rate0);
+		rate0 = rate1;
+		if (delta <= MIN_STABLE_DELTA)
+			break;
+	}
+
+	if (delta > MIN_STABLE_DELTA) {
+		dev_err(info->dev, "%s: bad delta: %u\n", __func__, delta);
+		return 0;
+	}
+
+	return rate0 * 1000000;
+}
+
+static int rockchip_pvtpll_parse_dt(struct rockchip_opp_info *info)
+{
+	struct device_node *np;
+	int ret;
+
+	np = of_parse_phandle(info->dev->of_node, "operating-points-v2", 0);
+	if (!np) {
+		dev_warn(info->dev, "OPP-v2 not supported\n");
+		return -ENOENT;
+	}
+
+	ret = of_property_read_u32(np, "rockchip,pvtpll-avg-offset", &info->pvtpll_avg_offset);
+	if (ret)
+		goto out;
+
+	ret = of_property_read_u32(np, "rockchip,pvtpll-min-rate", &info->pvtpll_min_rate);
+	if (ret)
+		goto out;
+
+	ret = of_property_read_u32(np, "rockchip,pvtpll-volt-step", &info->pvtpll_volt_step);
+out:
+	of_node_put(np);
+
+	return ret;
+}
+
+static int rockchip_init_pvtpll_info(struct rockchip_opp_info *info)
+{
+	struct opp_table *opp_table;
+	struct dev_pm_opp *opp;
+	int i = 0, max_count, ret;
+
+	ret = rockchip_pvtpll_parse_dt(info);
+	if (ret)
+		return ret;
+
+	max_count = dev_pm_opp_get_opp_count(info->dev);
+	if (max_count <= 0)
+		return max_count ? max_count : -ENODATA;
+
+	info->opp_table = kcalloc(max_count, sizeof(*info->opp_table), GFP_KERNEL);
+	if (!info->opp_table)
+		return -ENOMEM;
+
+	opp_table = dev_pm_opp_get_opp_table(info->dev);
+	if (!opp_table) {
+		kfree(info->opp_table);
+		info->opp_table = NULL;
+		return -ENOMEM;
+	}
+
+	mutex_lock(&opp_table->lock);
+	list_for_each_entry(opp, &opp_table->opp_list, node) {
+		if (!opp->available)
+			continue;
+
+		info->opp_table[i].u_volt = opp->supplies[0].u_volt;
+		info->opp_table[i].u_volt_min = opp->supplies[0].u_volt_min;
+		info->opp_table[i].u_volt_max = opp->supplies[0].u_volt_max;
+		if (opp_table->regulator_count > 1) {
+			info->opp_table[i].u_volt_mem = opp->supplies[1].u_volt;
+			info->opp_table[i].u_volt_mem_min = opp->supplies[1].u_volt_min;
+			info->opp_table[i].u_volt_mem_max = opp->supplies[1].u_volt_max;
+		}
+		info->opp_table[i++].rate = opp->rate;
+	}
+	mutex_unlock(&opp_table->lock);
+
+	dev_pm_opp_put_opp_table(opp_table);
+
+	return 0;
+}
+
+static int rockchip_pvtpll_set_volt(struct device *dev, struct regulator *reg,
+				    int target_uV, int max_uV, char *reg_name)
+{
+	int ret = 0;
+
+	ret = regulator_set_voltage(reg, target_uV, max_uV);
+	if (ret)
+		dev_err(dev, "%s: failed to set %s voltage (%d %d uV): %d\n",
+			__func__, reg_name, target_uV, max_uV, ret);
+
+	return ret;
+}
+
+static int rockchip_pvtpll_set_clk(struct device *dev, struct clk *clk,
+				   unsigned long rate)
+{
+	int ret = 0;
+
+	ret = clk_set_rate(clk, rate);
+	if (ret)
+		dev_err(dev, "%s: failed to set rate %lu Hz, ret:%d\n",
+			__func__, rate, ret);
+
+	return ret;
+}
+
+void rockchip_pvtpll_calibrate_opp(struct rockchip_opp_info *info)
+{
+	struct opp_table *opp_table;
+	struct dev_pm_opp *opp;
+	struct regulator *reg = NULL, *reg_mem = NULL;
+	unsigned long old_volt = 0, old_volt_mem = 0;
+	unsigned long volt = 0, volt_mem = 0;
+	unsigned long volt_min, volt_max, volt_mem_min, volt_mem_max;
+	unsigned long rate, pvtpll_rate, old_rate, cur_rate, delta0, delta1;
+	int i = 0, max_count, step, cur_step, ret;
+
+	if (!info || !info->grf)
+		return;
+
+	dev_dbg(info->dev, "calibrating opp ...\n");
+	ret = rockchip_init_pvtpll_info(info);
+	if (ret)
+		return;
+
+	max_count = dev_pm_opp_get_opp_count(info->dev);
+	if (max_count <= 0)
+		return;
+
+	opp_table = dev_pm_opp_get_opp_table(info->dev);
+	if (!opp_table)
+		return;
+
+	if ((!opp_table->regulators) || IS_ERR(opp_table->clk))
+		goto out_put;
+
+	reg = opp_table->regulators[0];
+	old_volt = regulator_get_voltage(reg);
+	if (opp_table->regulator_count > 1) {
+		reg_mem = opp_table->regulators[1];
+		old_volt_mem = regulator_get_voltage(reg_mem);
+		if (IS_ERR_VALUE(old_volt_mem))
+			goto out_put;
+	}
+	old_rate = clk_get_rate(opp_table->clk);
+	if (IS_ERR_VALUE(old_volt) || IS_ERR_VALUE(old_rate))
+		goto out_put;
+	cur_rate = old_rate;
+
+	step = regulator_get_linear_step(reg);
+	if (!step || info->pvtpll_volt_step > step)
+		step = info->pvtpll_volt_step;
+
+	if (old_rate > info->pvtpll_min_rate * 1000) {
+		if (rockchip_pvtpll_set_clk(info->dev, opp_table->clk,
+					    info->pvtpll_min_rate * 1000))
+			goto out_put;
+	}
+
+	for (i = 0; i < max_count; i++) {
+		rate = info->opp_table[i].rate;
+		if (rate < 1000 * info->pvtpll_min_rate)
+			continue;
+
+		volt = max(volt, info->opp_table[i].u_volt);
+		volt_min = info->opp_table[i].u_volt_min;
+		volt_max = info->opp_table[i].u_volt_max;
+
+		if (opp_table->regulator_count > 1) {
+			volt_mem = max(volt_mem, info->opp_table[i].u_volt_mem);
+			volt_mem_min = info->opp_table[i].u_volt_mem_min;
+			volt_mem_max = info->opp_table[i].u_volt_mem_max;
+			if (rockchip_pvtpll_set_volt(info->dev, reg_mem,
+						     volt_mem, volt_mem_max, "mem"))
+				goto out;
+		}
+		if (rockchip_pvtpll_set_volt(info->dev, reg, volt, volt_max, "vdd"))
+			goto out;
+
+		if (rockchip_pvtpll_set_clk(info->dev, opp_table->clk, rate))
+			goto out;
+		cur_rate = rate;
+		pvtpll_rate = rockchip_pvtpll_get_rate(info);
+		if (!pvtpll_rate)
+			goto out;
+		cur_step = (pvtpll_rate < rate) ? step : -step;
+		delta1 = abs(pvtpll_rate - rate);
+		do {
+			delta0 = delta1;
+			volt += cur_step;
+			if ((volt < volt_min) || (volt > volt_max))
+				break;
+			if (opp_table->regulator_count > 1) {
+				if (volt > volt_mem_max)
+					break;
+				else if (volt < volt_mem_min)
+					volt_mem = volt_mem_min;
+				else
+					volt_mem = volt;
+				if (rockchip_pvtpll_set_volt(info->dev, reg_mem,
+							     volt_mem, volt_mem_max,
+							     "mem"))
+					break;
+			}
+			if (rockchip_pvtpll_set_volt(info->dev, reg, volt,
+						     volt_max, "vdd"))
+				break;
+			pvtpll_rate = rockchip_pvtpll_get_rate(info);
+			if (!pvtpll_rate)
+				goto out;
+			delta1 = abs(pvtpll_rate - rate);
+		} while (delta1 < delta0);
+
+		volt -= cur_step;
+		info->opp_table[i].u_volt = volt;
+		if (opp_table->regulator_count > 1) {
+			if (volt < volt_mem_min)
+				volt_mem = volt_mem_min;
+			else
+				volt_mem = volt;
+			info->opp_table[i].u_volt_mem = volt_mem;
+		}
+	}
+
+	i = 0;
+	mutex_lock(&opp_table->lock);
+	list_for_each_entry(opp, &opp_table->opp_list, node) {
+		if (!opp->available)
+			continue;
+
+		opp->supplies[0].u_volt = info->opp_table[i].u_volt;
+		if (opp_table->regulator_count > 1)
+			opp->supplies[1].u_volt = info->opp_table[i].u_volt_mem;
+		i++;
+	}
+	mutex_unlock(&opp_table->lock);
+	dev_info(info->dev, "opp calibration done\n");
+out:
+	if (cur_rate > old_rate)
+		rockchip_pvtpll_set_clk(info->dev, opp_table->clk, old_rate);
+	if (opp_table->regulator_count > 1)
+		rockchip_pvtpll_set_volt(info->dev, reg_mem, old_volt_mem,
+					 INT_MAX, "mem");
+	rockchip_pvtpll_set_volt(info->dev, reg, old_volt, INT_MAX, "vdd");
+	if (cur_rate < old_rate)
+		rockchip_pvtpll_set_clk(info->dev, opp_table->clk, old_rate);
+out_put:
+	dev_pm_opp_put_opp_table(opp_table);
+}
+EXPORT_SYMBOL(rockchip_pvtpll_calibrate_opp);
+
+void rockchip_pvtpll_add_length(struct rockchip_opp_info *info)
+{
+	struct device_node *np;
+	struct opp_table *opp_table;
+	struct dev_pm_opp *opp;
+	unsigned long old_rate;
+	unsigned int min_rate = 0, max_rate = 0, margin = 0;
+	u32 opp_flag = 0;
+	int ret;
+
+	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;
+	}
+
+	if (of_property_read_u32(np, "rockchip,pvtpll-len-min-rate", &min_rate))
+		goto out;
+	if (of_property_read_u32(np, "rockchip,pvtpll-len-max-rate", &max_rate))
+		goto out;
+	if (of_property_read_u32(np, "rockchip,pvtpll-len-margin", &margin))
+		goto out;
+
+	opp_table = dev_pm_opp_get_opp_table(info->dev);
+	if (!opp_table)
+		goto out;
+	old_rate = clk_get_rate(opp_table->clk);
+	opp_flag = OPP_ADD_LENGTH | ((margin & OPP_LENGTH_MASK) << OPP_LENGTH_SHIFT);
+
+	mutex_lock(&opp_table->lock);
+	list_for_each_entry(opp, &opp_table->opp_list, node) {
+		if (opp->rate < min_rate * 1000 || opp->rate > max_rate * 1000)
+			continue;
+		ret = clk_set_rate(opp_table->clk, opp->rate | opp_flag);
+		if (ret) {
+			dev_err(info->dev,
+				"failed to change %lu len margin %d\n",
+				opp->rate, margin);
+			break;
+		}
+	}
+	mutex_unlock(&opp_table->lock);
+
+	clk_set_rate(opp_table->clk, old_rate);
+
+	dev_pm_opp_put_opp_table(opp_table);
+out:
+	of_node_put(np);
+}
+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)
 {
@@ -725,6 +1137,11 @@
 	int cur_temp, diff_temp, prop_temp, diff_value;
 	int pvtm_value = 0;
 	int ret = 0;
+
+	if (!rockchip_nvmem_cell_read_u16(np, "pvtm", (u16 *)&pvtm_value) && pvtm_value) {
+		dev_info(dev, "pvtm = %d, get from otp\n", pvtm_value);
+		return pvtm_value;
+	}
 
 	pvtm = kzalloc(sizeof(*pvtm), GFP_KERNEL);
 	if (!pvtm)
@@ -838,12 +1255,13 @@
 }
 
 void rockchip_of_get_pvtm_sel(struct device *dev, struct device_node *np,
-			      char *reg_name, int process,
+			      char *reg_name, int bin, int process,
 			      int *volt_sel, int *scale_sel)
 {
 	struct property *prop = NULL;
 	char name[NAME_MAX];
 	int pvtm, ret;
+	u32 hw = 0;
 
 	if (of_property_read_bool(np, "rockchip,pvtm-pvtpll"))
 		pvtm = rockchip_get_pvtm_pvtpll(dev, np, reg_name);
@@ -858,6 +1276,17 @@
 		snprintf(name, sizeof(name),
 			 "rockchip,p%d-pvtm-voltage-sel", process);
 		prop = of_find_property(np, name, NULL);
+	} else if (bin > 0) {
+		of_property_read_u32(np, "rockchip,pvtm-hw", &hw);
+		if (hw && (hw & BIT(bin))) {
+			sprintf(name, "rockchip,pvtm-voltage-sel-hw");
+			prop = of_find_property(np, name, NULL);
+		}
+		if (!prop) {
+			snprintf(name, sizeof(name),
+				 "rockchip,pvtm-voltage-sel-B%d", bin);
+			prop = of_find_property(np, name, NULL);
+		}
 	}
 	if (!prop)
 		sprintf(name, "rockchip,pvtm-voltage-sel");
@@ -868,6 +1297,7 @@
 next:
 	if (!scale_sel)
 		return;
+	prop = NULL;
 	if (process >= 0) {
 		snprintf(name, sizeof(name),
 			 "rockchip,p%d-pvtm-scaling-sel", process);
@@ -911,37 +1341,99 @@
 }
 EXPORT_SYMBOL(rockchip_of_get_bin_volt_sel);
 
-void rockchip_get_soc_info(struct device *dev,
-			   const struct of_device_id *matches,
-			   int *bin, int *process)
+void rockchip_get_opp_data(const struct of_device_id *matches,
+			   struct rockchip_opp_info *info)
 {
 	const struct of_device_id *match;
-	struct device_node *np;
 	struct device_node *node;
-	int (*get_soc_info)(struct device *dev, struct device_node *np,
-			    int *bin, int *process);
-	int ret = 0;
-
-	if (!matches)
-		return;
-
-	np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
-	if (!np) {
-		dev_warn(dev, "OPP-v2 not supported\n");
-		return;
-	}
 
 	node = of_find_node_by_path("/");
 	match = of_match_node(matches, node);
-	if (match && match->data) {
-		get_soc_info = match->data;
-		ret = get_soc_info(dev, np, bin, process);
-		if (ret)
-			dev_err(dev, "Failed to get soc info\n");
+	if (match && match->data)
+		info->data = match->data;
+	of_node_put(node);
+}
+EXPORT_SYMBOL(rockchip_get_opp_data);
+
+int rockchip_get_volt_rm_table(struct device *dev, struct device_node *np,
+			       char *porp_name, struct volt_rm_table **table)
+{
+	struct volt_rm_table *rm_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 % 2)
+		return -EINVAL;
+
+	rm_table = devm_kzalloc(dev, sizeof(*rm_table) * (count / 2 + 1),
+				GFP_KERNEL);
+	if (!rm_table)
+		return -ENOMEM;
+
+	for (i = 0; i < count / 2; i++) {
+		of_property_read_u32_index(np, porp_name, 2 * i,
+					   &rm_table[i].volt);
+		of_property_read_u32_index(np, porp_name, 2 * i + 1,
+					   &rm_table[i].rm);
 	}
 
-	of_node_put(node);
-	of_node_put(np);
+	rm_table[i].volt = 0;
+	rm_table[i].rm = VOLT_RM_TABLE_END;
+
+	*table = rm_table;
+
+	return 0;
+}
+EXPORT_SYMBOL(rockchip_get_volt_rm_table);
+
+int rockchip_get_soc_info(struct device *dev, struct device_node *np, int *bin,
+			  int *process)
+{
+	u8 value = 0;
+	int ret = 0;
+
+	if (*bin >= 0 || *process >= 0)
+		return 0;
+
+	if (of_property_match_string(np, "nvmem-cell-names",
+				     "remark_spec_serial_number") >= 0)
+		rockchip_nvmem_cell_read_u8(np, "remark_spec_serial_number", &value);
+
+	if (!value && of_property_match_string(np, "nvmem-cell-names",
+					       "specification_serial_number") >= 0) {
+		ret = rockchip_nvmem_cell_read_u8(np,
+						  "specification_serial_number",
+						  &value);
+		if (ret) {
+			dev_err(dev,
+				"Failed to get specification_serial_number\n");
+			return ret;
+		}
+	}
+
+	/* M */
+	if (value == 0xd)
+		*bin = 1;
+	/* J */
+	else if (value == 0xa)
+		*bin = 2;
+
+	if (*bin < 0)
+		*bin = 0;
+	dev_info(dev, "bin=%d\n", *bin);
+
+	return 0;
 }
 EXPORT_SYMBOL(rockchip_get_soc_info);
 
@@ -962,7 +1454,7 @@
 
 	rockchip_of_get_lkg_sel(dev, np, lkg_name, process,
 				&lkg_volt_sel, &lkg_scale);
-	rockchip_of_get_pvtm_sel(dev, np, reg_name, process,
+	rockchip_of_get_pvtm_sel(dev, np, reg_name, bin, process,
 				 &pvtm_volt_sel, &pvtm_scale);
 	rockchip_of_get_bin_sel(dev, np, bin, &bin_scale);
 	rockchip_of_get_bin_volt_sel(dev, np, bin, &bin_volt_sel);
@@ -1000,6 +1492,42 @@
 }
 EXPORT_SYMBOL(rockchip_set_opp_prop_name);
 
+struct opp_table *rockchip_set_opp_supported_hw(struct device *dev,
+						struct device_node *np,
+						int bin, int volt_sel)
+{
+	struct opp_table *opp_table;
+	u32 supported_hw[2];
+	u32 version = 0, speed = 0;
+
+	if (!of_property_read_bool(np, "rockchip,supported-hw"))
+		return NULL;
+
+	opp_table = dev_pm_opp_get_opp_table(dev);
+	if (!opp_table)
+		return NULL;
+	if (opp_table->supported_hw) {
+		dev_pm_opp_put_opp_table(opp_table);
+		return NULL;
+	}
+	dev_pm_opp_put_opp_table(opp_table);
+
+	if (bin >= 0)
+		version = bin;
+	if (volt_sel >= 0)
+		speed = volt_sel;
+
+	/* SoC Version */
+	supported_hw[0] = BIT(version);
+	/* Speed Grade */
+	supported_hw[1] = BIT(speed);
+
+	dev_info(dev, "soc version=%d, speed=%d\n", version, speed);
+
+	return dev_pm_opp_set_supported_hw(dev, supported_hw, 2);
+}
+EXPORT_SYMBOL(rockchip_set_opp_supported_hw);
+
 static int rockchip_adjust_opp_by_irdrop(struct device *dev,
 					 struct device_node *np,
 					 unsigned long *safe_rate,
@@ -1026,6 +1554,8 @@
 
 	mutex_lock(&opp_table->lock);
 	list_for_each_entry(opp, &opp_table->opp_list, node) {
+		if (!opp->available)
+			continue;
 		if (!irdrop_table) {
 			delta_irdrop = 0;
 		} else {
@@ -1094,6 +1624,8 @@
 
 	mutex_lock(&opp_table->lock);
 	list_for_each_entry(opp, &opp_table->opp_list, node) {
+		if (!opp->available)
+			continue;
 		if (opp->supplies->u_volt < vmin) {
 			opp->supplies->u_volt = vmin;
 			opp->supplies->u_volt_min = vmin;
@@ -1131,9 +1663,14 @@
 		if (opp->rate > opp_info.max_freq * 1000000)
 			continue;
 
-		opp->supplies->u_volt += opp_info.volt * 1000;
-		if (opp->supplies->u_volt > opp->supplies->u_volt_max)
-			opp->supplies->u_volt = opp->supplies->u_volt_max;
+		opp->supplies[0].u_volt += opp_info.volt * 1000;
+		if (opp->supplies[0].u_volt > opp->supplies[0].u_volt_max)
+			opp->supplies[0].u_volt = opp->supplies[0].u_volt_max;
+		if (opp_table->regulator_count > 1) {
+			opp->supplies[1].u_volt += opp_info.volt * 1000;
+			if (opp->supplies[1].u_volt > opp->supplies[1].u_volt_max)
+				opp->supplies[1].u_volt = opp->supplies[1].u_volt_max;
+		}
 	}
 	mutex_unlock(&opp_table->lock);
 
@@ -1161,7 +1698,7 @@
 			goto out;
 		}
 		if (opp->rate > scale_rate)
-			dev_pm_opp_remove(dev, opp->rate);
+			dev_pm_opp_disable(dev, opp->rate);
 		dev_pm_opp_put(opp);
 	}
 out:
@@ -1191,6 +1728,10 @@
 	rockchip_adjust_opp_by_irdrop(dev, np, &safe_rate, &max_rate);
 
 	dev_info(dev, "avs=%d\n", avs);
+
+	if (!safe_rate && !scale)
+		goto out_np;
+
 	clk = of_clk_get_by_name(np, NULL);
 	if (IS_ERR(clk)) {
 		if (!safe_rate)
@@ -1204,14 +1745,14 @@
 
 	if (safe_rate)
 		irdrop_scale = rockchip_pll_clk_rate_to_scale(clk, safe_rate);
-	if (max_rate)
-		opp_scale = rockchip_pll_clk_rate_to_scale(clk, max_rate);
 	target_scale = max(irdrop_scale, scale);
 	if (target_scale <= 0)
 		goto out_clk;
 	dev_dbg(dev, "target_scale=%d, irdrop_scale=%d, scale=%d\n",
 		target_scale, irdrop_scale, scale);
 
+	if (max_rate)
+		opp_scale = rockchip_pll_clk_rate_to_scale(clk, max_rate);
 	if (avs == AVS_SCALING_RATE) {
 		ret = rockchip_pll_clk_adaptive_scaling(clk, target_scale);
 		if (ret)
@@ -1256,14 +1797,219 @@
 }
 EXPORT_SYMBOL(rockchip_adjust_power_scale);
 
-int rockchip_init_opp_table(struct device *dev,
-			    const struct of_device_id *matches,
+int rockchip_get_read_margin(struct device *dev,
+			     struct rockchip_opp_info *opp_info,
+			     unsigned long volt, u32 *target_rm)
+{
+	int i;
+
+	if (!opp_info || !opp_info->volt_rm_tbl)
+		return 0;
+
+	for (i = 0; opp_info->volt_rm_tbl[i].rm != VOLT_RM_TABLE_END; i++) {
+		if (volt >= opp_info->volt_rm_tbl[i].volt) {
+			opp_info->target_rm = opp_info->volt_rm_tbl[i].rm;
+			break;
+		}
+	}
+	*target_rm = opp_info->target_rm;
+
+	return 0;
+}
+EXPORT_SYMBOL(rockchip_get_read_margin);
+
+int rockchip_set_read_margin(struct device *dev,
+			     struct rockchip_opp_info *opp_info, u32 rm,
+			     bool is_set_rm)
+{
+	if (!is_set_rm || !opp_info)
+		return 0;
+	if (!opp_info || !opp_info->volt_rm_tbl)
+		return 0;
+	if (!opp_info->data || !opp_info->data->set_read_margin)
+		return 0;
+	if (rm == opp_info->current_rm)
+		return 0;
+
+	return opp_info->data->set_read_margin(dev, opp_info, rm);
+}
+EXPORT_SYMBOL(rockchip_set_read_margin);
+
+int rockchip_init_read_margin(struct device *dev,
+			      struct rockchip_opp_info *opp_info,
+			      char *reg_name)
+{
+	struct clk *clk;
+	struct regulator *reg;
+	unsigned long cur_rate;
+	int cur_volt, ret = 0;
+	u32 target_rm = UINT_MAX;
+
+	reg = regulator_get_optional(dev, reg_name);
+	if (IS_ERR(reg)) {
+		ret = PTR_ERR(reg);
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev, "%s: no regulator (%s) found: %d\n",
+				__func__, reg_name, ret);
+		return ret;
+	}
+	cur_volt = regulator_get_voltage(reg);
+	if (cur_volt < 0) {
+		ret = cur_volt;
+		if (ret != -EPROBE_DEFER)
+			dev_err(dev, "%s: failed to get (%s) volt: %d\n",
+				__func__, reg_name, ret);
+		goto out;
+	}
+
+	clk = clk_get(dev, NULL);
+	if (IS_ERR(clk)) {
+		ret = PTR_ERR(clk);
+		dev_err(dev, "%s: failed to get clk: %d\n", __func__, ret);
+		goto out;
+	}
+	cur_rate = clk_get_rate(clk);
+
+	rockchip_get_read_margin(dev, opp_info, cur_volt, &target_rm);
+	dev_dbg(dev, "cur_rate=%lu, threshold=%lu, cur_volt=%d, target_rm=%d\n",
+		cur_rate, opp_info->intermediate_threshold_freq,
+		cur_volt, target_rm);
+	if (opp_info->intermediate_threshold_freq &&
+	    cur_rate > opp_info->intermediate_threshold_freq) {
+		clk_set_rate(clk, opp_info->intermediate_threshold_freq);
+		rockchip_set_read_margin(dev, opp_info, target_rm, true);
+		clk_set_rate(clk, cur_rate);
+	} else {
+		rockchip_set_read_margin(dev, opp_info, target_rm, true);
+	}
+
+	clk_put(clk);
+out:
+	regulator_put(reg);
+
+	return ret;
+}
+EXPORT_SYMBOL(rockchip_init_read_margin);
+
+int rockchip_set_intermediate_rate(struct device *dev,
+				   struct rockchip_opp_info *opp_info,
+				   struct clk *clk, unsigned long old_freq,
+				   unsigned long new_freq, bool is_scaling_up,
+				   bool is_set_clk)
+{
+	if (!is_set_clk)
+		return 0;
+	if (!opp_info || !opp_info->volt_rm_tbl)
+		return 0;
+	if (!opp_info->data || !opp_info->data->set_read_margin)
+		return 0;
+	if (opp_info->target_rm == opp_info->current_rm)
+		return 0;
+	/*
+	 * There is no need to set intermediate rate if the new voltage
+	 * and the current voltage are high voltage.
+	 */
+	if ((opp_info->target_rm < opp_info->low_rm) &&
+	    (opp_info->current_rm < opp_info->low_rm))
+		return 0;
+
+	if (is_scaling_up) {
+		/*
+		 * If scaling up and the current frequency is less than
+		 * or equal to intermediate threshold frequency, there is
+		 * no need to set intermediate rate.
+		 */
+		if (opp_info->intermediate_threshold_freq &&
+		    old_freq <= opp_info->intermediate_threshold_freq)
+			return 0;
+		return clk_set_rate(clk, new_freq | OPP_SCALING_UP_INTER);
+	}
+	/*
+	 * If scaling down and the new frequency is less than or equal to
+	 * intermediate threshold frequency , there is no need to set
+	 * intermediate rate and set the new frequency directly.
+	 */
+	if (opp_info->intermediate_threshold_freq &&
+	    new_freq <= opp_info->intermediate_threshold_freq)
+		return clk_set_rate(clk, new_freq);
+
+	return clk_set_rate(clk, new_freq | OPP_SCALING_DOWN_INTER);
+}
+EXPORT_SYMBOL(rockchip_set_intermediate_rate);
+
+static int rockchip_get_opp_clk(struct device *dev, struct device_node *np,
+				struct rockchip_opp_info *info)
+{
+	struct clk_bulk_data *clks;
+	struct of_phandle_args clkspec;
+	int ret = 0, num_clks = 0, i;
+
+	if (of_find_property(np, "rockchip,opp-clocks", NULL)) {
+		num_clks = of_count_phandle_with_args(np, "rockchip,opp-clocks",
+						      "#clock-cells");
+		if (num_clks <= 0)
+			return 0;
+		clks = devm_kcalloc(dev, num_clks, sizeof(*clks), GFP_KERNEL);
+		if (!clks)
+			return -ENOMEM;
+		for (i = 0; i < num_clks; i++) {
+			ret = of_parse_phandle_with_args(np,
+							 "rockchip,opp-clocks",
+							 "#clock-cells", i,
+							 &clkspec);
+			if (ret < 0) {
+				dev_err(dev, "%s: failed to parse opp clk %d\n",
+					np->name, i);
+				goto error;
+			}
+			clks[i].clk = of_clk_get_from_provider(&clkspec);
+			of_node_put(clkspec.np);
+			if (IS_ERR(clks[i].clk)) {
+				ret = PTR_ERR(clks[i].clk);
+				clks[i].clk = NULL;
+				dev_err(dev, "%s: failed to get opp clk %d\n",
+					np->name, i);
+				goto error;
+			}
+		}
+	} else {
+		num_clks = of_clk_get_parent_count(np);
+		if (num_clks <= 0)
+			return 0;
+		clks = devm_kcalloc(dev, num_clks, sizeof(*clks), GFP_KERNEL);
+		if (!clks)
+			return -ENOMEM;
+		for (i = 0; i < num_clks; i++) {
+			clks[i].clk = of_clk_get(np, i);
+			if (IS_ERR(clks[i].clk)) {
+				ret = PTR_ERR(clks[i].clk);
+				clks[i].clk = NULL;
+				dev_err(dev, "%s: failed to get clk %d\n",
+					np->name, i);
+				goto error;
+			}
+		}
+	}
+	info->clks = clks;
+	info->num_clks = num_clks;
+
+	return 0;
+error:
+	while (--i >= 0)
+		clk_put(clks[i].clk);
+	devm_kfree(dev, clks);
+
+	return ret;
+}
+
+int rockchip_init_opp_table(struct device *dev, struct rockchip_opp_info *info,
 			    char *lkg_name, char *reg_name)
 {
 	struct device_node *np;
 	int bin = -EINVAL, process = -EINVAL;
 	int scale = 0, volt_sel = -EINVAL;
 	int ret = 0;
+	u32 freq;
 
 	/* Get OPP descriptor node */
 	np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
@@ -1271,23 +2017,90 @@
 		dev_dbg(dev, "Failed to find operating-points-v2\n");
 		return -ENOENT;
 	}
-	of_node_put(np);
+	if (!info)
+		goto next;
+	info->dev = dev;
 
-	rockchip_get_soc_info(dev, matches, &bin, &process);
+	ret = rockchip_get_opp_clk(dev, np, info);
+	if (ret)
+		goto out;
+	if (info->clks) {
+		ret = clk_bulk_prepare_enable(info->num_clks, info->clks);
+		if (ret) {
+			dev_err(dev, "failed to enable opp clks\n");
+			goto out;
+		}
+	}
+	if (info->data && info->data->set_read_margin) {
+		info->current_rm = UINT_MAX;
+		info->grf = syscon_regmap_lookup_by_phandle(np, "rockchip,grf");
+		if (IS_ERR(info->grf))
+			info->grf = NULL;
+		rockchip_get_volt_rm_table(dev, np, "volt-mem-read-margin",
+					   &info->volt_rm_tbl);
+		of_property_read_u32(np, "low-volt-mem-read-margin",
+				     &info->low_rm);
+		if (!of_property_read_u32(np, "intermediate-threshold-freq",
+					  &freq))
+			info->intermediate_threshold_freq = freq * 1000;
+		rockchip_init_read_margin(dev, info, reg_name);
+	}
+	if (info->data && info->data->get_soc_info)
+		info->data->get_soc_info(dev, np, &bin, &process);
+
+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)
+		info->data->set_soc_info(dev, np, bin, process, volt_sel);
 	rockchip_set_opp_prop_name(dev, process, volt_sel);
+	rockchip_set_opp_supported_hw(dev, np, bin, volt_sel);
 	ret = dev_pm_opp_of_add_table(dev);
 	if (ret) {
 		dev_err(dev, "Invalid operating-points in device tree.\n");
-		return ret;
+		goto dis_opp_clk;
 	}
 	rockchip_adjust_power_scale(dev, scale);
+	rockchip_pvtpll_calibrate_opp(info);
+	rockchip_pvtpll_add_length(info);
 
-	return 0;
+dis_opp_clk:
+	if (info && info->clks)
+		clk_bulk_disable_unprepare(info->num_clks, info->clks);
+out:
+	of_node_put(np);
+
+	return ret;
 }
 EXPORT_SYMBOL(rockchip_init_opp_table);
 
+void rockchip_uninit_opp_table(struct device *dev, struct rockchip_opp_info *info)
+{
+	struct opp_table *opp_table;
+
+	if (info) {
+		kfree(info->opp_table);
+		info->opp_table = NULL;
+		devm_kfree(dev, info->clks);
+		info->clks = NULL;
+		devm_kfree(dev, info->volt_rm_tbl);
+		info->volt_rm_tbl = NULL;
+	}
+
+	opp_table = dev_pm_opp_get_opp_table(dev);
+	if (IS_ERR(opp_table))
+		return;
+	dev_pm_opp_of_remove_table(dev);
+	if (opp_table->prop_name)
+		dev_pm_opp_put_prop_name(opp_table);
+	if (opp_table->supported_hw)
+		dev_pm_opp_put_supported_hw(opp_table);
+	dev_pm_opp_put_opp_table(opp_table);
+}
+EXPORT_SYMBOL(rockchip_uninit_opp_table);
+
 MODULE_DESCRIPTION("ROCKCHIP OPP Select");
 MODULE_AUTHOR("Finley Xiao <finley.xiao@rock-chips.com>, Liang Chen <cl@rock-chips.com>");
 MODULE_LICENSE("GPL");

--
Gitblit v1.6.2