~hc/RK356X_SDK_RELEASE.git

..	..	@@ -3,12 +3,16 @@
3	3	*
4	4	* SPDX-License-Identifier: GPL-2.0+
5	5	*/
	6	+//#define DEBUG
6	7	#include <linux/clk.h>
7	8	#include <linux/cpufreq.h>
8	9	#include <linux/devfreq.h>
	10	+#include <linux/mfd/syscon.h>
9	11	#include <linux/module.h>
10	12	#include <linux/nvmem-consumer.h>
	13	+#include <linux/regmap.h>
11	14	#include <linux/regulator/consumer.h>
	15	+#include <linux/rockchip/rockchip_sip.h>
12	16	#include <linux/slab.h>
13	17	#include <linux/soc/rockchip/pvtm.h>
14	18	#include <linux/thermal.h>
..	..	@@ -51,15 +55,24 @@
51	55	unsigned int num;
52	56	unsigned int err;
53	57	unsigned int ref_temp;
	58	+ unsigned int offset;
54	59	int temp_prop[2];
55	60	const char *tz_name;
56	61	struct thermal_zone_device *tz;
	62	+ struct regmap *grf;
57	63	};
58	64
59	65	struct lkg_conversion_table {
60	66	int temp;
61	67	int conv;
62	68	};
	69	+
	70	+struct otp_opp_info {
	71	+ u16 min_freq;
	72	+ u16 max_freq;
	73	+ u8 volt;
	74	+ u8 length;
	75	+} __packed;
63	76
64	77	#define PVTM_CH_MAX 8
65	78	#define PVTM_SUB_CH_MAX 8
..	..	@@ -298,16 +311,8 @@
298	311	return -EINVAL;
299	312	if (of_property_read_u32(np, "rockchip,pvtm-volt", &pvtm->volt))
300	313	return -EINVAL;
301		- if (of_property_read_u32_array(np, "rockchip,pvtm-ch", pvtm->ch, 2))
302		- return -EINVAL;
303		- if (pvtm->ch[0] >= PVTM_CH_MAX \|\| pvtm->ch[1] >= PVTM_SUB_CH_MAX)
304		- return -EINVAL;
305	314	if (of_property_read_u32(np, "rockchip,pvtm-sample-time",
306	315	&pvtm->sample_time))
307		- return -EINVAL;
308		- if (of_property_read_u32(np, "rockchip,pvtm-number", &pvtm->num))
309		- return -EINVAL;
310		- if (of_property_read_u32(np, "rockchip,pvtm-error", &pvtm->err))
311	316	return -EINVAL;
312	317	if (of_property_read_u32(np, "rockchip,pvtm-ref-temp", &pvtm->ref_temp))
313	318	return -EINVAL;
..	..	@@ -324,6 +329,23 @@
324	329	if (IS_ERR(pvtm->tz))
325	330	return -EINVAL;
326	331	if (!pvtm->tz->ops->get_temp)
	332	+ return -EINVAL;
	333	+ if (of_property_read_bool(np, "rockchip,pvtm-pvtpll")) {
	334	+ if (of_property_read_u32(np, "rockchip,pvtm-offset",
	335	+ &pvtm->offset))
	336	+ return -EINVAL;
	337	+ pvtm->grf = syscon_regmap_lookup_by_phandle(np, "rockchip,grf");
	338	+ if (IS_ERR(pvtm->grf))
	339	+ return -EINVAL;
	340	+ return 0;
	341	+ }
	342	+ if (of_property_read_u32_array(np, "rockchip,pvtm-ch", pvtm->ch, 2))
	343	+ return -EINVAL;
	344	+ if (pvtm->ch[0] >= PVTM_CH_MAX \|\| pvtm->ch[1] >= PVTM_SUB_CH_MAX)
	345	+ return -EINVAL;
	346	+ if (of_property_read_u32(np, "rockchip,pvtm-number", &pvtm->num))
	347	+ return -EINVAL;
	348	+ if (of_property_read_u32(np, "rockchip,pvtm-error", &pvtm->err))
327	349	return -EINVAL;
328	350
329	351	return 0;
..	..	@@ -415,7 +437,7 @@
415	437	cur_temp, *target_value, avg_value, diff_value);
416	438
417	439	resetore_volt:
418		- regulator_set_voltage(reg, old_volt, old_volt);
	440	+ regulator_set_voltage(reg, old_volt, INT_MAX);
419	441	restore_clk:
420	442	clk_set_rate(clk, old_freq);
421	443	pvtm_value_out:
..	..	@@ -694,6 +716,496 @@
694	716	}
695	717	EXPORT_SYMBOL(rockchip_of_get_lkg_sel);
696	718
	719	+static unsigned long rockchip_pvtpll_get_rate(struct rockchip_opp_info *info)
	720	+{
	721	+ unsigned int rate0, rate1, delta;
	722	+ int i;
	723	+
	724	+#define MIN_STABLE_DELTA 3
	725	+ regmap_read(info->grf, info->pvtpll_avg_offset, &rate0);
	726	+ /* max delay 2ms */
	727	+ for (i = 0; i < 20; i++) {
	728	+ udelay(100);
	729	+ regmap_read(info->grf, info->pvtpll_avg_offset, &rate1);
	730	+ delta = abs(rate1 - rate0);
	731	+ rate0 = rate1;
	732	+ if (delta <= MIN_STABLE_DELTA)
	733	+ break;
	734	+ }
	735	+
	736	+ if (delta > MIN_STABLE_DELTA) {
	737	+ dev_err(info->dev, "%s: bad delta: %u\n", __func__, delta);
	738	+ return 0;
	739	+ }
	740	+
	741	+ return rate0 * 1000000;
	742	+}
	743	+
	744	+static int rockchip_pvtpll_parse_dt(struct rockchip_opp_info *info)
	745	+{
	746	+ struct device_node *np;
	747	+ int ret;
	748	+
	749	+ np = of_parse_phandle(info->dev->of_node, "operating-points-v2", 0);
	750	+ if (!np) {
	751	+ dev_warn(info->dev, "OPP-v2 not supported\n");
	752	+ return -ENOENT;
	753	+ }
	754	+
	755	+ ret = of_property_read_u32(np, "rockchip,pvtpll-avg-offset", &info->pvtpll_avg_offset);
	756	+ if (ret)
	757	+ goto out;
	758	+
	759	+ ret = of_property_read_u32(np, "rockchip,pvtpll-min-rate", &info->pvtpll_min_rate);
	760	+ if (ret)
	761	+ goto out;
	762	+
	763	+ ret = of_property_read_u32(np, "rockchip,pvtpll-volt-step", &info->pvtpll_volt_step);
	764	+out:
	765	+ of_node_put(np);
	766	+
	767	+ return ret;
	768	+}
	769	+
	770	+static int rockchip_init_pvtpll_info(struct rockchip_opp_info *info)
	771	+{
	772	+ struct opp_table *opp_table;
	773	+ struct dev_pm_opp *opp;
	774	+ int i = 0, max_count, ret;
	775	+
	776	+ ret = rockchip_pvtpll_parse_dt(info);
	777	+ if (ret)
	778	+ return ret;
	779	+
	780	+ max_count = dev_pm_opp_get_opp_count(info->dev);
	781	+ if (max_count <= 0)
	782	+ return max_count ? max_count : -ENODATA;
	783	+
	784	+ info->opp_table = kcalloc(max_count, sizeof(*info->opp_table), GFP_KERNEL);
	785	+ if (!info->opp_table)
	786	+ return -ENOMEM;
	787	+
	788	+ opp_table = dev_pm_opp_get_opp_table(info->dev);
	789	+ if (!opp_table) {
	790	+ kfree(info->opp_table);
	791	+ info->opp_table = NULL;
	792	+ return -ENOMEM;
	793	+ }
	794	+
	795	+ mutex_lock(&opp_table->lock);
	796	+ list_for_each_entry(opp, &opp_table->opp_list, node) {
	797	+ if (!opp->available)
	798	+ continue;
	799	+
	800	+ info->opp_table[i].u_volt = opp->supplies[0].u_volt;
	801	+ info->opp_table[i].u_volt_min = opp->supplies[0].u_volt_min;
	802	+ info->opp_table[i].u_volt_max = opp->supplies[0].u_volt_max;
	803	+ if (opp_table->regulator_count > 1) {
	804	+ info->opp_table[i].u_volt_mem = opp->supplies[1].u_volt;
	805	+ info->opp_table[i].u_volt_mem_min = opp->supplies[1].u_volt_min;
	806	+ info->opp_table[i].u_volt_mem_max = opp->supplies[1].u_volt_max;
	807	+ }
	808	+ info->opp_table[i++].rate = opp->rate;
	809	+ }
	810	+ mutex_unlock(&opp_table->lock);
	811	+
	812	+ dev_pm_opp_put_opp_table(opp_table);
	813	+
	814	+ return 0;
	815	+}
	816	+
	817	+static int rockchip_pvtpll_set_volt(struct device dev, struct regulator reg,
	818	+ int target_uV, int max_uV, char *reg_name)
	819	+{
	820	+ int ret = 0;
	821	+
	822	+ ret = regulator_set_voltage(reg, target_uV, max_uV);
	823	+ if (ret)
	824	+ dev_err(dev, "%s: failed to set %s voltage (%d %d uV): %d\n",
	825	+ __func__, reg_name, target_uV, max_uV, ret);
	826	+
	827	+ return ret;
	828	+}
	829	+
	830	+static int rockchip_pvtpll_set_clk(struct device dev, struct clk clk,
	831	+ unsigned long rate)
	832	+{
	833	+ int ret = 0;
	834	+
	835	+ ret = clk_set_rate(clk, rate);
	836	+ if (ret)
	837	+ dev_err(dev, "%s: failed to set rate %lu Hz, ret:%d\n",
	838	+ __func__, rate, ret);
	839	+
	840	+ return ret;
	841	+}
	842	+
	843	+void rockchip_pvtpll_calibrate_opp(struct rockchip_opp_info *info)
	844	+{
	845	+ struct opp_table *opp_table;
	846	+ struct dev_pm_opp *opp;
	847	+ struct regulator reg = NULL, reg_mem = NULL;
	848	+ unsigned long old_volt = 0, old_volt_mem = 0;
	849	+ unsigned long volt = 0, volt_mem = 0;
	850	+ unsigned long volt_min, volt_max, volt_mem_min, volt_mem_max;
	851	+ unsigned long rate, pvtpll_rate, old_rate, cur_rate, delta0, delta1;
	852	+ int i = 0, max_count, step, cur_step, ret;
	853	+
	854	+ if (!info \|\| !info->grf)
	855	+ return;
	856	+
	857	+ dev_dbg(info->dev, "calibrating opp ...\n");
	858	+ ret = rockchip_init_pvtpll_info(info);
	859	+ if (ret)
	860	+ return;
	861	+
	862	+ max_count = dev_pm_opp_get_opp_count(info->dev);
	863	+ if (max_count <= 0)
	864	+ return;
	865	+
	866	+ opp_table = dev_pm_opp_get_opp_table(info->dev);
	867	+ if (!opp_table)
	868	+ return;
	869	+
	870	+ if ((!opp_table->regulators) \|\| IS_ERR(opp_table->clk))
	871	+ goto out_put;
	872	+
	873	+ reg = opp_table->regulators[0];
	874	+ old_volt = regulator_get_voltage(reg);
	875	+ if (opp_table->regulator_count > 1) {
	876	+ reg_mem = opp_table->regulators[1];
	877	+ old_volt_mem = regulator_get_voltage(reg_mem);
	878	+ if (IS_ERR_VALUE(old_volt_mem))
	879	+ goto out_put;
	880	+ }
	881	+ old_rate = clk_get_rate(opp_table->clk);
	882	+ if (IS_ERR_VALUE(old_volt) \|\| IS_ERR_VALUE(old_rate))
	883	+ goto out_put;
	884	+ cur_rate = old_rate;
	885	+
	886	+ step = regulator_get_linear_step(reg);
	887	+ if (!step \|\| info->pvtpll_volt_step > step)
	888	+ step = info->pvtpll_volt_step;
	889	+
	890	+ if (old_rate > info->pvtpll_min_rate * 1000) {
	891	+ if (rockchip_pvtpll_set_clk(info->dev, opp_table->clk,
	892	+ info->pvtpll_min_rate * 1000))
	893	+ goto out_put;
	894	+ }
	895	+
	896	+ for (i = 0; i < max_count; i++) {
	897	+ rate = info->opp_table[i].rate;
	898	+ if (rate < 1000 * info->pvtpll_min_rate)
	899	+ continue;
	900	+
	901	+ volt = max(volt, info->opp_table[i].u_volt);
	902	+ volt_min = info->opp_table[i].u_volt_min;
	903	+ volt_max = info->opp_table[i].u_volt_max;
	904	+
	905	+ if (opp_table->regulator_count > 1) {
	906	+ volt_mem = max(volt_mem, info->opp_table[i].u_volt_mem);
	907	+ volt_mem_min = info->opp_table[i].u_volt_mem_min;
	908	+ volt_mem_max = info->opp_table[i].u_volt_mem_max;
	909	+ if (rockchip_pvtpll_set_volt(info->dev, reg_mem,
	910	+ volt_mem, volt_mem_max, "mem"))
	911	+ goto out;
	912	+ }
	913	+ if (rockchip_pvtpll_set_volt(info->dev, reg, volt, volt_max, "vdd"))
	914	+ goto out;
	915	+
	916	+ if (rockchip_pvtpll_set_clk(info->dev, opp_table->clk, rate))
	917	+ goto out;
	918	+ cur_rate = rate;
	919	+ pvtpll_rate = rockchip_pvtpll_get_rate(info);
	920	+ if (!pvtpll_rate)
	921	+ goto out;
	922	+ cur_step = (pvtpll_rate < rate) ? step : -step;
	923	+ delta1 = abs(pvtpll_rate - rate);
	924	+ do {
	925	+ delta0 = delta1;
	926	+ volt += cur_step;
	927	+ if ((volt < volt_min) \|\| (volt > volt_max))
	928	+ break;
	929	+ if (opp_table->regulator_count > 1) {
	930	+ if (volt > volt_mem_max)
	931	+ break;
	932	+ else if (volt < volt_mem_min)
	933	+ volt_mem = volt_mem_min;
	934	+ else
	935	+ volt_mem = volt;
	936	+ if (rockchip_pvtpll_set_volt(info->dev, reg_mem,
	937	+ volt_mem, volt_mem_max,
	938	+ "mem"))
	939	+ break;
	940	+ }
	941	+ if (rockchip_pvtpll_set_volt(info->dev, reg, volt,
	942	+ volt_max, "vdd"))
	943	+ break;
	944	+ pvtpll_rate = rockchip_pvtpll_get_rate(info);
	945	+ if (!pvtpll_rate)
	946	+ goto out;
	947	+ delta1 = abs(pvtpll_rate - rate);
	948	+ } while (delta1 < delta0);
	949	+
	950	+ volt -= cur_step;
	951	+ info->opp_table[i].u_volt = volt;
	952	+ if (opp_table->regulator_count > 1) {
	953	+ if (volt < volt_mem_min)
	954	+ volt_mem = volt_mem_min;
	955	+ else
	956	+ volt_mem = volt;
	957	+ info->opp_table[i].u_volt_mem = volt_mem;
	958	+ }
	959	+ }
	960	+
	961	+ i = 0;
	962	+ mutex_lock(&opp_table->lock);
	963	+ list_for_each_entry(opp, &opp_table->opp_list, node) {
	964	+ if (!opp->available)
	965	+ continue;
	966	+
	967	+ opp->supplies[0].u_volt = info->opp_table[i].u_volt;
	968	+ if (opp_table->regulator_count > 1)
	969	+ opp->supplies[1].u_volt = info->opp_table[i].u_volt_mem;
	970	+ i++;
	971	+ }
	972	+ mutex_unlock(&opp_table->lock);
	973	+ dev_info(info->dev, "opp calibration done\n");
	974	+out:
	975	+ if (cur_rate > old_rate)
	976	+ rockchip_pvtpll_set_clk(info->dev, opp_table->clk, old_rate);
	977	+ if (opp_table->regulator_count > 1)
	978	+ rockchip_pvtpll_set_volt(info->dev, reg_mem, old_volt_mem,
	979	+ INT_MAX, "mem");
	980	+ rockchip_pvtpll_set_volt(info->dev, reg, old_volt, INT_MAX, "vdd");
	981	+ if (cur_rate < old_rate)
	982	+ rockchip_pvtpll_set_clk(info->dev, opp_table->clk, old_rate);
	983	+out_put:
	984	+ dev_pm_opp_put_opp_table(opp_table);
	985	+}
	986	+EXPORT_SYMBOL(rockchip_pvtpll_calibrate_opp);
	987	+
	988	+void rockchip_pvtpll_add_length(struct rockchip_opp_info *info)
	989	+{
	990	+ struct device_node *np;
	991	+ struct opp_table *opp_table;
	992	+ struct dev_pm_opp *opp;
	993	+ unsigned long old_rate;
	994	+ unsigned int min_rate = 0, max_rate = 0, margin = 0;
	995	+ u32 opp_flag = 0;
	996	+ int ret;
	997	+
	998	+ if (!info)
	999	+ return;
	1000	+
	1001	+ np = of_parse_phandle(info->dev->of_node, "operating-points-v2", 0);
	1002	+ if (!np) {
	1003	+ dev_warn(info->dev, "OPP-v2 not supported\n");
	1004	+ return;
	1005	+ }
	1006	+
	1007	+ if (of_property_read_u32(np, "rockchip,pvtpll-len-min-rate", &min_rate))
	1008	+ goto out;
	1009	+ if (of_property_read_u32(np, "rockchip,pvtpll-len-max-rate", &max_rate))
	1010	+ goto out;
	1011	+ if (of_property_read_u32(np, "rockchip,pvtpll-len-margin", &margin))
	1012	+ goto out;
	1013	+
	1014	+ opp_table = dev_pm_opp_get_opp_table(info->dev);
	1015	+ if (!opp_table)
	1016	+ goto out;
	1017	+ old_rate = clk_get_rate(opp_table->clk);
	1018	+ opp_flag = OPP_ADD_LENGTH \| ((margin & OPP_LENGTH_MASK) << OPP_LENGTH_SHIFT);
	1019	+
	1020	+ mutex_lock(&opp_table->lock);
	1021	+ list_for_each_entry(opp, &opp_table->opp_list, node) {
	1022	+ if (opp->rate < min_rate * 1000 \|\| opp->rate > max_rate * 1000)
	1023	+ continue;
	1024	+ ret = clk_set_rate(opp_table->clk, opp->rate \| opp_flag);
	1025	+ if (ret) {
	1026	+ dev_err(info->dev,
	1027	+ "failed to change %lu len margin %d\n",
	1028	+ opp->rate, margin);
	1029	+ break;
	1030	+ }
	1031	+ }
	1032	+ mutex_unlock(&opp_table->lock);
	1033	+
	1034	+ clk_set_rate(opp_table->clk, old_rate);
	1035	+
	1036	+ dev_pm_opp_put_opp_table(opp_table);
	1037	+out:
	1038	+ of_node_put(np);
	1039	+}
	1040	+EXPORT_SYMBOL(rockchip_pvtpll_add_length);
	1041	+
	1042	+void rockchip_init_pvtpll_table(struct rockchip_opp_info *info, int bin)
	1043	+{
	1044	+ struct device_node *np = NULL;
	1045	+ struct property *prop = NULL;
	1046	+ struct of_phandle_args clkspec = { 0 };
	1047	+ struct arm_smccc_res res;
	1048	+ char prop_name[NAME_MAX];
	1049	+ u32 *value;
	1050	+ int count;
	1051	+ int ret, i;
	1052	+
	1053	+ if (!info)
	1054	+ return;
	1055	+
	1056	+ np = of_parse_phandle(info->dev->of_node, "operating-points-v2", 0);
	1057	+ if (!np) {
	1058	+ dev_warn(info->dev, "OPP-v2 not supported\n");
	1059	+ return;
	1060	+ }
	1061	+
	1062	+ ret = of_parse_phandle_with_args(info->dev->of_node, "clocks",
	1063	+ "#clock-cells", 0, &clkspec);
	1064	+ if (ret)
	1065	+ goto out;
	1066	+ info->pvtpll_clk_id = clkspec.args[0];
	1067	+ of_node_put(clkspec.np);
	1068	+
	1069	+ res = sip_smc_get_pvtpll_info(PVTPLL_GET_INFO, info->pvtpll_clk_id);
	1070	+ if (res.a0)
	1071	+ goto out;
	1072	+ if (!res.a1)
	1073	+ info->pvtpll_low_temp = true;
	1074	+
	1075	+ if (bin > 0) {
	1076	+ snprintf(prop_name, sizeof(prop_name),
	1077	+ "rockchip,pvtpll-table-B%d", bin);
	1078	+ prop = of_find_property(np, prop_name, NULL);
	1079	+ }
	1080	+ if (!prop)
	1081	+ sprintf(prop_name, "rockchip,pvtpll-table");
	1082	+
	1083	+ prop = of_find_property(np, prop_name, NULL);
	1084	+ if (!prop)
	1085	+ goto out;
	1086	+
	1087	+ count = of_property_count_u32_elems(np, prop_name);
	1088	+ if (count < 0) {
	1089	+ dev_err(info->dev, "%s: Invalid %s property (%d)\n",
	1090	+ __func__, prop_name, count);
	1091	+ goto out;
	1092	+ } else if (count % 5) {
	1093	+ dev_err(info->dev, "Invalid count of %s\n", prop_name);
	1094	+ goto out;
	1095	+ }
	1096	+
	1097	+ value = kmalloc_array(count, sizeof(*value), GFP_KERNEL);
	1098	+ if (!value)
	1099	+ goto out;
	1100	+ ret = of_property_read_u32_array(np, prop_name, value, count);
	1101	+ if (ret) {
	1102	+ dev_err(info->dev, "%s: error parsing %s: %d\n",
	1103	+ __func__, prop_name, ret);
	1104	+ goto free_value;
	1105	+ }
	1106	+
	1107	+ for (i = 0; i < count; i += 5) {
	1108	+ res = sip_smc_pvtpll_config(PVTPLL_ADJUST_TABLE,
	1109	+ info->pvtpll_clk_id, value[i],
	1110	+ value[i + 1], value[i + 2],
	1111	+ value[i + 3], value[i + 4]);
	1112	+ if (res.a0) {
	1113	+ dev_err(info->dev,
	1114	+ "%s: error cfg clk_id=%u %u %u %u %u %u (%d)\n",
	1115	+ __func__, info->pvtpll_clk_id, value[i],
	1116	+ value[i + 1], value[i + 2], value[i + 3],
	1117	+ value[i + 4], (int)res.a0);
	1118	+ goto free_value;
	1119	+ }
	1120	+ }
	1121	+
	1122	+free_value:
	1123	+ kfree(value);
	1124	+out:
	1125	+ of_node_put(np);
	1126	+}
	1127	+EXPORT_SYMBOL(rockchip_init_pvtpll_table);
	1128	+
	1129	+static int rockchip_get_pvtm_pvtpll(struct device dev, struct device_node np,
	1130	+ char *reg_name)
	1131	+{
	1132	+ struct regulator *reg;
	1133	+ struct clk *clk;
	1134	+ struct pvtm_config *pvtm;
	1135	+ unsigned long old_freq;
	1136	+ unsigned int old_volt;
	1137	+ int cur_temp, diff_temp, prop_temp, diff_value;
	1138	+ int pvtm_value = 0;
	1139	+ int ret = 0;
	1140	+
	1141	+ if (!rockchip_nvmem_cell_read_u16(np, "pvtm", (u16 *)&pvtm_value) && pvtm_value) {
	1142	+ dev_info(dev, "pvtm = %d, get from otp\n", pvtm_value);
	1143	+ return pvtm_value;
	1144	+ }
	1145	+
	1146	+ pvtm = kzalloc(sizeof(*pvtm), GFP_KERNEL);
	1147	+ if (!pvtm)
	1148	+ return -ENOMEM;
	1149	+
	1150	+ ret = rockchip_parse_pvtm_config(np, pvtm);
	1151	+ if (ret)
	1152	+ goto out;
	1153	+
	1154	+ clk = clk_get(dev, NULL);
	1155	+ if (IS_ERR_OR_NULL(clk)) {
	1156	+ dev_warn(dev, "Failed to get clk\n");
	1157	+ goto out;
	1158	+ }
	1159	+
	1160	+ reg = regulator_get_optional(dev, reg_name);
	1161	+ if (IS_ERR_OR_NULL(reg)) {
	1162	+ dev_warn(dev, "Failed to get reg\n");
	1163	+ clk_put(clk);
	1164	+ goto out;
	1165	+ }
	1166	+ old_freq = clk_get_rate(clk);
	1167	+ old_volt = regulator_get_voltage(reg);
	1168	+
	1169	+ ret = clk_set_rate(clk, pvtm->freq * 1000);
	1170	+ if (ret) {
	1171	+ dev_err(dev, "Failed to set pvtm freq\n");
	1172	+ goto put_reg;
	1173	+ }
	1174	+ ret = regulator_set_voltage(reg, pvtm->volt, INT_MAX);
	1175	+ if (ret) {
	1176	+ dev_err(dev, "Failed to set pvtm_volt\n");
	1177	+ goto restore_clk;
	1178	+ }
	1179	+ usleep_range(pvtm->sample_time, pvtm->sample_time + 100);
	1180	+
	1181	+ ret = regmap_read(pvtm->grf, pvtm->offset, &pvtm_value);
	1182	+ if (ret < 0) {
	1183	+ dev_err(dev, "failed to get pvtm from 0x%x\n", pvtm->offset);
	1184	+ goto resetore_volt;
	1185	+ }
	1186	+ pvtm->tz->ops->get_temp(pvtm->tz, &cur_temp);
	1187	+ diff_temp = (cur_temp / 1000 - pvtm->ref_temp);
	1188	+ if (diff_temp < 0)
	1189	+ prop_temp = pvtm->temp_prop[0];
	1190	+ else
	1191	+ prop_temp = pvtm->temp_prop[1];
	1192	+ diff_value = diff_temp * prop_temp / 1000;
	1193	+ pvtm_value += diff_value;
	1194	+
	1195	+ dev_info(dev, "pvtm=%d\n", pvtm_value);
	1196	+
	1197	+resetore_volt:
	1198	+ regulator_set_voltage(reg, old_volt, INT_MAX);
	1199	+restore_clk:
	1200	+ clk_set_rate(clk, old_freq);
	1201	+put_reg:
	1202	+ regulator_put(reg);
	1203	+ clk_put(clk);
	1204	+out:
	1205	+ kfree(pvtm);
	1206	+
	1207	+ return pvtm_value;
	1208	+}
697	1209
698	1210	static int rockchip_get_pvtm(struct device dev, struct device_node np,
699	1211	char *reg_name)
..	..	@@ -743,14 +1255,18 @@
743	1255	}
744	1256
745	1257	void rockchip_of_get_pvtm_sel(struct device dev, struct device_node np,
746		- char *reg_name, int process,
	1258	+ char *reg_name, int bin, int process,
747	1259	int volt_sel, int scale_sel)
748	1260	{
749	1261	struct property *prop = NULL;
750	1262	char name[NAME_MAX];
751	1263	int pvtm, ret;
	1264	+ u32 hw = 0;
752	1265
753		- pvtm = rockchip_get_pvtm(dev, np, reg_name);
	1266	+ if (of_property_read_bool(np, "rockchip,pvtm-pvtpll"))
	1267	+ pvtm = rockchip_get_pvtm_pvtpll(dev, np, reg_name);
	1268	+ else
	1269	+ pvtm = rockchip_get_pvtm(dev, np, reg_name);
754	1270	if (pvtm <= 0)
755	1271	return;
756	1272
..	..	@@ -760,6 +1276,17 @@
760	1276	snprintf(name, sizeof(name),
761	1277	"rockchip,p%d-pvtm-voltage-sel", process);
762	1278	prop = of_find_property(np, name, NULL);
	1279	+ } else if (bin > 0) {
	1280	+ of_property_read_u32(np, "rockchip,pvtm-hw", &hw);
	1281	+ if (hw && (hw & BIT(bin))) {
	1282	+ sprintf(name, "rockchip,pvtm-voltage-sel-hw");
	1283	+ prop = of_find_property(np, name, NULL);
	1284	+ }
	1285	+ if (!prop) {
	1286	+ snprintf(name, sizeof(name),
	1287	+ "rockchip,pvtm-voltage-sel-B%d", bin);
	1288	+ prop = of_find_property(np, name, NULL);
	1289	+ }
763	1290	}
764	1291	if (!prop)
765	1292	sprintf(name, "rockchip,pvtm-voltage-sel");
..	..	@@ -770,6 +1297,7 @@
770	1297	next:
771	1298	if (!scale_sel)
772	1299	return;
	1300	+ prop = NULL;
773	1301	if (process >= 0) {
774	1302	snprintf(name, sizeof(name),
775	1303	"rockchip,p%d-pvtm-scaling-sel", process);
..	..	@@ -813,37 +1341,99 @@
813	1341	}
814	1342	EXPORT_SYMBOL(rockchip_of_get_bin_volt_sel);
815	1343
816		-void rockchip_get_soc_info(struct device *dev,
817		- const struct of_device_id *matches,
818		- int bin, int process)
	1344	+void rockchip_get_opp_data(const struct of_device_id *matches,
	1345	+ struct rockchip_opp_info *info)
819	1346	{
820	1347	const struct of_device_id *match;
821		- struct device_node *np;
822	1348	struct device_node *node;
823		- int (get_soc_info)(struct device dev, struct device_node *np,
824		- int bin, int process);
825		- int ret = 0;
826		-
827		- if (!matches)
828		- return;
829		-
830		- np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
831		- if (!np) {
832		- dev_warn(dev, "OPP-v2 not supported\n");
833		- return;
834		- }
835	1349
836	1350	node = of_find_node_by_path("/");
837	1351	match = of_match_node(matches, node);
838		- if (match && match->data) {
839		- get_soc_info = match->data;
840		- ret = get_soc_info(dev, np, bin, process);
841		- if (ret)
842		- dev_err(dev, "Failed to get soc info\n");
	1352	+ if (match && match->data)
	1353	+ info->data = match->data;
	1354	+ of_node_put(node);
	1355	+}
	1356	+EXPORT_SYMBOL(rockchip_get_opp_data);
	1357	+
	1358	+int rockchip_get_volt_rm_table(struct device dev, struct device_node np,
	1359	+ char porp_name, struct volt_rm_table *table)
	1360	+{
	1361	+ struct volt_rm_table *rm_table;
	1362	+ const struct property *prop;
	1363	+ int count, i;
	1364	+
	1365	+ prop = of_find_property(np, porp_name, NULL);
	1366	+ if (!prop)
	1367	+ return -EINVAL;
	1368	+
	1369	+ if (!prop->value)
	1370	+ return -ENODATA;
	1371	+
	1372	+ count = of_property_count_u32_elems(np, porp_name);
	1373	+ if (count < 0)
	1374	+ return -EINVAL;
	1375	+
	1376	+ if (count % 2)
	1377	+ return -EINVAL;
	1378	+
	1379	+ rm_table = devm_kzalloc(dev, sizeof(rm_table) (count / 2 + 1),
	1380	+ GFP_KERNEL);
	1381	+ if (!rm_table)
	1382	+ return -ENOMEM;
	1383	+
	1384	+ for (i = 0; i < count / 2; i++) {
	1385	+ of_property_read_u32_index(np, porp_name, 2 * i,
	1386	+ &rm_table[i].volt);
	1387	+ of_property_read_u32_index(np, porp_name, 2 * i + 1,
	1388	+ &rm_table[i].rm);
843	1389	}
844	1390
845		- of_node_put(node);
846		- of_node_put(np);
	1391	+ rm_table[i].volt = 0;
	1392	+ rm_table[i].rm = VOLT_RM_TABLE_END;
	1393	+
	1394	+ *table = rm_table;
	1395	+
	1396	+ return 0;
	1397	+}
	1398	+EXPORT_SYMBOL(rockchip_get_volt_rm_table);
	1399	+
	1400	+int rockchip_get_soc_info(struct device dev, struct device_node np, int *bin,
	1401	+ int *process)
	1402	+{
	1403	+ u8 value = 0;
	1404	+ int ret = 0;
	1405	+
	1406	+ if (bin >= 0 \|\| process >= 0)
	1407	+ return 0;
	1408	+
	1409	+ if (of_property_match_string(np, "nvmem-cell-names",
	1410	+ "remark_spec_serial_number") >= 0)
	1411	+ rockchip_nvmem_cell_read_u8(np, "remark_spec_serial_number", &value);
	1412	+
	1413	+ if (!value && of_property_match_string(np, "nvmem-cell-names",
	1414	+ "specification_serial_number") >= 0) {
	1415	+ ret = rockchip_nvmem_cell_read_u8(np,
	1416	+ "specification_serial_number",
	1417	+ &value);
	1418	+ if (ret) {
	1419	+ dev_err(dev,
	1420	+ "Failed to get specification_serial_number\n");
	1421	+ return ret;
	1422	+ }
	1423	+ }
	1424	+
	1425	+ /* M */
	1426	+ if (value == 0xd)
	1427	+ *bin = 1;
	1428	+ /* J */
	1429	+ else if (value == 0xa)
	1430	+ *bin = 2;
	1431	+
	1432	+ if (*bin < 0)
	1433	+ *bin = 0;
	1434	+ dev_info(dev, "bin=%d\n", *bin);
	1435	+
	1436	+ return 0;
847	1437	}
848	1438	EXPORT_SYMBOL(rockchip_get_soc_info);
849	1439
..	..	@@ -864,7 +1454,7 @@
864	1454
865	1455	rockchip_of_get_lkg_sel(dev, np, lkg_name, process,
866	1456	&lkg_volt_sel, &lkg_scale);
867		- rockchip_of_get_pvtm_sel(dev, np, reg_name, process,
	1457	+ rockchip_of_get_pvtm_sel(dev, np, reg_name, bin, process,
868	1458	&pvtm_volt_sel, &pvtm_scale);
869	1459	rockchip_of_get_bin_sel(dev, np, bin, &bin_scale);
870	1460	rockchip_of_get_bin_volt_sel(dev, np, bin, &bin_volt_sel);
..	..	@@ -902,6 +1492,42 @@
902	1492	}
903	1493	EXPORT_SYMBOL(rockchip_set_opp_prop_name);
904	1494
	1495	+struct opp_table rockchip_set_opp_supported_hw(struct device dev,
	1496	+ struct device_node *np,
	1497	+ int bin, int volt_sel)
	1498	+{
	1499	+ struct opp_table *opp_table;
	1500	+ u32 supported_hw[2];
	1501	+ u32 version = 0, speed = 0;
	1502	+
	1503	+ if (!of_property_read_bool(np, "rockchip,supported-hw"))
	1504	+ return NULL;
	1505	+
	1506	+ opp_table = dev_pm_opp_get_opp_table(dev);
	1507	+ if (!opp_table)
	1508	+ return NULL;
	1509	+ if (opp_table->supported_hw) {
	1510	+ dev_pm_opp_put_opp_table(opp_table);
	1511	+ return NULL;
	1512	+ }
	1513	+ dev_pm_opp_put_opp_table(opp_table);
	1514	+
	1515	+ if (bin >= 0)
	1516	+ version = bin;
	1517	+ if (volt_sel >= 0)
	1518	+ speed = volt_sel;
	1519	+
	1520	+ /* SoC Version */
	1521	+ supported_hw[0] = BIT(version);
	1522	+ /* Speed Grade */
	1523	+ supported_hw[1] = BIT(speed);
	1524	+
	1525	+ dev_info(dev, "soc version=%d, speed=%d\n", version, speed);
	1526	+
	1527	+ return dev_pm_opp_set_supported_hw(dev, supported_hw, 2);
	1528	+}
	1529	+EXPORT_SYMBOL(rockchip_set_opp_supported_hw);
	1530	+
905	1531	static int rockchip_adjust_opp_by_irdrop(struct device *dev,
906	1532	struct device_node *np,
907	1533	unsigned long *safe_rate,
..	..	@@ -910,8 +1536,9 @@
910	1536	struct sel_table *irdrop_table = NULL;
911	1537	struct opp_table *opp_table;
912	1538	struct dev_pm_opp *opp;
	1539	+ unsigned long tmp_safe_rate = 0;
913	1540	int evb_irdrop = 0, board_irdrop, delta_irdrop;
914		- int tmp_safe_rate = 0, opp_rate, i, ret = 0;
	1541	+ int opp_rate, i, ret = 0;
915	1542	u32 max_volt = UINT_MAX;
916	1543	bool reach_max_volt = false;
917	1544
..	..	@@ -927,6 +1554,8 @@
927	1554
928	1555	mutex_lock(&opp_table->lock);
929	1556	list_for_each_entry(opp, &opp_table->opp_list, node) {
	1557	+ if (!opp->available)
	1558	+ continue;
930	1559	if (!irdrop_table) {
931	1560	delta_irdrop = 0;
932	1561	} else {
..	..	@@ -995,12 +1624,57 @@
995	1624
996	1625	mutex_lock(&opp_table->lock);
997	1626	list_for_each_entry(opp, &opp_table->opp_list, node) {
	1627	+ if (!opp->available)
	1628	+ continue;
998	1629	if (opp->supplies->u_volt < vmin) {
999	1630	opp->supplies->u_volt = vmin;
1000	1631	opp->supplies->u_volt_min = vmin;
1001	1632	}
1002	1633	}
1003	1634	mutex_unlock(&opp_table->lock);
	1635	+}
	1636	+
	1637	+static void rockchip_adjust_opp_by_otp(struct device *dev,
	1638	+ struct device_node *np)
	1639	+{
	1640	+ struct dev_pm_opp *opp;
	1641	+ struct opp_table *opp_table;
	1642	+ struct otp_opp_info opp_info = {};
	1643	+ int ret;
	1644	+
	1645	+ ret = rockchip_nvmem_cell_read_common(np, "opp-info", &opp_info,
	1646	+ sizeof(opp_info));
	1647	+ if (ret \|\| !opp_info.volt)
	1648	+ return;
	1649	+
	1650	+ dev_info(dev, "adjust opp-table by otp: min=%uM, max=%uM, volt=%umV\n",
	1651	+ opp_info.min_freq, opp_info.max_freq, opp_info.volt);
	1652	+
	1653	+ opp_table = dev_pm_opp_get_opp_table(dev);
	1654	+ if (!opp_table)
	1655	+ return;
	1656	+
	1657	+ mutex_lock(&opp_table->lock);
	1658	+ list_for_each_entry(opp, &opp_table->opp_list, node) {
	1659	+ if (!opp->available)
	1660	+ continue;
	1661	+ if (opp->rate < opp_info.min_freq * 1000000)
	1662	+ continue;
	1663	+ if (opp->rate > opp_info.max_freq * 1000000)
	1664	+ continue;
	1665	+
	1666	+ opp->supplies[0].u_volt += opp_info.volt * 1000;
	1667	+ if (opp->supplies[0].u_volt > opp->supplies[0].u_volt_max)
	1668	+ opp->supplies[0].u_volt = opp->supplies[0].u_volt_max;
	1669	+ if (opp_table->regulator_count > 1) {
	1670	+ opp->supplies[1].u_volt += opp_info.volt * 1000;
	1671	+ if (opp->supplies[1].u_volt > opp->supplies[1].u_volt_max)
	1672	+ opp->supplies[1].u_volt = opp->supplies[1].u_volt_max;
	1673	+ }
	1674	+ }
	1675	+ mutex_unlock(&opp_table->lock);
	1676	+
	1677	+ dev_pm_opp_put_opp_table(opp_table);
1004	1678	}
1005	1679
1006	1680	static int rockchip_adjust_opp_table(struct device *dev,
..	..	@@ -1024,7 +1698,7 @@
1024	1698	goto out;
1025	1699	}
1026	1700	if (opp->rate > scale_rate)
1027		- dev_pm_opp_remove(dev, opp->rate);
	1701	+ dev_pm_opp_disable(dev, opp->rate);
1028	1702	dev_pm_opp_put(opp);
1029	1703	}
1030	1704	out:
..	..	@@ -1049,10 +1723,15 @@
1049	1723	of_property_read_u32(np, "rockchip,avs-enable", &avs);
1050	1724	of_property_read_u32(np, "rockchip,avs", &avs);
1051	1725	of_property_read_u32(np, "rockchip,avs-scale", &avs_scale);
	1726	+ rockchip_adjust_opp_by_otp(dev, np);
1052	1727	rockchip_adjust_opp_by_mbist_vmin(dev, np);
1053	1728	rockchip_adjust_opp_by_irdrop(dev, np, &safe_rate, &max_rate);
1054	1729
1055	1730	dev_info(dev, "avs=%d\n", avs);
	1731	+
	1732	+ if (!safe_rate && !scale)
	1733	+ goto out_np;
	1734	+
1056	1735	clk = of_clk_get_by_name(np, NULL);
1057	1736	if (IS_ERR(clk)) {
1058	1737	if (!safe_rate)
..	..	@@ -1066,14 +1745,14 @@
1066	1745
1067	1746	if (safe_rate)
1068	1747	irdrop_scale = rockchip_pll_clk_rate_to_scale(clk, safe_rate);
1069		- if (max_rate)
1070		- opp_scale = rockchip_pll_clk_rate_to_scale(clk, max_rate);
1071	1748	target_scale = max(irdrop_scale, scale);
1072	1749	if (target_scale <= 0)
1073	1750	goto out_clk;
1074	1751	dev_dbg(dev, "target_scale=%d, irdrop_scale=%d, scale=%d\n",
1075	1752	target_scale, irdrop_scale, scale);
1076	1753
	1754	+ if (max_rate)
	1755	+ opp_scale = rockchip_pll_clk_rate_to_scale(clk, max_rate);
1077	1756	if (avs == AVS_SCALING_RATE) {
1078	1757	ret = rockchip_pll_clk_adaptive_scaling(clk, target_scale);
1079	1758	if (ret)
..	..	@@ -1118,14 +1797,219 @@
1118	1797	}
1119	1798	EXPORT_SYMBOL(rockchip_adjust_power_scale);
1120	1799
1121		-int rockchip_init_opp_table(struct device *dev,
1122		- const struct of_device_id *matches,
	1800	+int rockchip_get_read_margin(struct device *dev,
	1801	+ struct rockchip_opp_info *opp_info,
	1802	+ unsigned long volt, u32 *target_rm)
	1803	+{
	1804	+ int i;
	1805	+
	1806	+ if (!opp_info \|\| !opp_info->volt_rm_tbl)
	1807	+ return 0;
	1808	+
	1809	+ for (i = 0; opp_info->volt_rm_tbl[i].rm != VOLT_RM_TABLE_END; i++) {
	1810	+ if (volt >= opp_info->volt_rm_tbl[i].volt) {
	1811	+ opp_info->target_rm = opp_info->volt_rm_tbl[i].rm;
	1812	+ break;
	1813	+ }
	1814	+ }
	1815	+ *target_rm = opp_info->target_rm;
	1816	+
	1817	+ return 0;
	1818	+}
	1819	+EXPORT_SYMBOL(rockchip_get_read_margin);
	1820	+
	1821	+int rockchip_set_read_margin(struct device *dev,
	1822	+ struct rockchip_opp_info *opp_info, u32 rm,
	1823	+ bool is_set_rm)
	1824	+{
	1825	+ if (!is_set_rm \|\| !opp_info)
	1826	+ return 0;
	1827	+ if (!opp_info \|\| !opp_info->volt_rm_tbl)
	1828	+ return 0;
	1829	+ if (!opp_info->data \|\| !opp_info->data->set_read_margin)
	1830	+ return 0;
	1831	+ if (rm == opp_info->current_rm)
	1832	+ return 0;
	1833	+
	1834	+ return opp_info->data->set_read_margin(dev, opp_info, rm);
	1835	+}
	1836	+EXPORT_SYMBOL(rockchip_set_read_margin);
	1837	+
	1838	+int rockchip_init_read_margin(struct device *dev,
	1839	+ struct rockchip_opp_info *opp_info,
	1840	+ char *reg_name)
	1841	+{
	1842	+ struct clk *clk;
	1843	+ struct regulator *reg;
	1844	+ unsigned long cur_rate;
	1845	+ int cur_volt, ret = 0;
	1846	+ u32 target_rm = UINT_MAX;
	1847	+
	1848	+ reg = regulator_get_optional(dev, reg_name);
	1849	+ if (IS_ERR(reg)) {
	1850	+ ret = PTR_ERR(reg);
	1851	+ if (ret != -EPROBE_DEFER)
	1852	+ dev_err(dev, "%s: no regulator (%s) found: %d\n",
	1853	+ __func__, reg_name, ret);
	1854	+ return ret;
	1855	+ }
	1856	+ cur_volt = regulator_get_voltage(reg);
	1857	+ if (cur_volt < 0) {
	1858	+ ret = cur_volt;
	1859	+ if (ret != -EPROBE_DEFER)
	1860	+ dev_err(dev, "%s: failed to get (%s) volt: %d\n",
	1861	+ __func__, reg_name, ret);
	1862	+ goto out;
	1863	+ }
	1864	+
	1865	+ clk = clk_get(dev, NULL);
	1866	+ if (IS_ERR(clk)) {
	1867	+ ret = PTR_ERR(clk);
	1868	+ dev_err(dev, "%s: failed to get clk: %d\n", __func__, ret);
	1869	+ goto out;
	1870	+ }
	1871	+ cur_rate = clk_get_rate(clk);
	1872	+
	1873	+ rockchip_get_read_margin(dev, opp_info, cur_volt, &target_rm);
	1874	+ dev_dbg(dev, "cur_rate=%lu, threshold=%lu, cur_volt=%d, target_rm=%d\n",
	1875	+ cur_rate, opp_info->intermediate_threshold_freq,
	1876	+ cur_volt, target_rm);
	1877	+ if (opp_info->intermediate_threshold_freq &&
	1878	+ cur_rate > opp_info->intermediate_threshold_freq) {
	1879	+ clk_set_rate(clk, opp_info->intermediate_threshold_freq);
	1880	+ rockchip_set_read_margin(dev, opp_info, target_rm, true);
	1881	+ clk_set_rate(clk, cur_rate);
	1882	+ } else {
	1883	+ rockchip_set_read_margin(dev, opp_info, target_rm, true);
	1884	+ }
	1885	+
	1886	+ clk_put(clk);
	1887	+out:
	1888	+ regulator_put(reg);
	1889	+
	1890	+ return ret;
	1891	+}
	1892	+EXPORT_SYMBOL(rockchip_init_read_margin);
	1893	+
	1894	+int rockchip_set_intermediate_rate(struct device *dev,
	1895	+ struct rockchip_opp_info *opp_info,
	1896	+ struct clk *clk, unsigned long old_freq,
	1897	+ unsigned long new_freq, bool is_scaling_up,
	1898	+ bool is_set_clk)
	1899	+{
	1900	+ if (!is_set_clk)
	1901	+ return 0;
	1902	+ if (!opp_info \|\| !opp_info->volt_rm_tbl)
	1903	+ return 0;
	1904	+ if (!opp_info->data \|\| !opp_info->data->set_read_margin)
	1905	+ return 0;
	1906	+ if (opp_info->target_rm == opp_info->current_rm)
	1907	+ return 0;
	1908	+ /*
	1909	+ * There is no need to set intermediate rate if the new voltage
	1910	+ * and the current voltage are high voltage.
	1911	+ */
	1912	+ if ((opp_info->target_rm < opp_info->low_rm) &&
	1913	+ (opp_info->current_rm < opp_info->low_rm))
	1914	+ return 0;
	1915	+
	1916	+ if (is_scaling_up) {
	1917	+ /*
	1918	+ * If scaling up and the current frequency is less than
	1919	+ * or equal to intermediate threshold frequency, there is
	1920	+ * no need to set intermediate rate.
	1921	+ */
	1922	+ if (opp_info->intermediate_threshold_freq &&
	1923	+ old_freq <= opp_info->intermediate_threshold_freq)
	1924	+ return 0;
	1925	+ return clk_set_rate(clk, new_freq \| OPP_SCALING_UP_INTER);
	1926	+ }
	1927	+ /*
	1928	+ * If scaling down and the new frequency is less than or equal to
	1929	+ * intermediate threshold frequency , there is no need to set
	1930	+ * intermediate rate and set the new frequency directly.
	1931	+ */
	1932	+ if (opp_info->intermediate_threshold_freq &&
	1933	+ new_freq <= opp_info->intermediate_threshold_freq)
	1934	+ return clk_set_rate(clk, new_freq);
	1935	+
	1936	+ return clk_set_rate(clk, new_freq \| OPP_SCALING_DOWN_INTER);
	1937	+}
	1938	+EXPORT_SYMBOL(rockchip_set_intermediate_rate);
	1939	+
	1940	+static int rockchip_get_opp_clk(struct device dev, struct device_node np,
	1941	+ struct rockchip_opp_info *info)
	1942	+{
	1943	+ struct clk_bulk_data *clks;
	1944	+ struct of_phandle_args clkspec;
	1945	+ int ret = 0, num_clks = 0, i;
	1946	+
	1947	+ if (of_find_property(np, "rockchip,opp-clocks", NULL)) {
	1948	+ num_clks = of_count_phandle_with_args(np, "rockchip,opp-clocks",
	1949	+ "#clock-cells");
	1950	+ if (num_clks <= 0)
	1951	+ return 0;
	1952	+ clks = devm_kcalloc(dev, num_clks, sizeof(*clks), GFP_KERNEL);
	1953	+ if (!clks)
	1954	+ return -ENOMEM;
	1955	+ for (i = 0; i < num_clks; i++) {
	1956	+ ret = of_parse_phandle_with_args(np,
	1957	+ "rockchip,opp-clocks",
	1958	+ "#clock-cells", i,
	1959	+ &clkspec);
	1960	+ if (ret < 0) {
	1961	+ dev_err(dev, "%s: failed to parse opp clk %d\n",
	1962	+ np->name, i);
	1963	+ goto error;
	1964	+ }
	1965	+ clks[i].clk = of_clk_get_from_provider(&clkspec);
	1966	+ of_node_put(clkspec.np);
	1967	+ if (IS_ERR(clks[i].clk)) {
	1968	+ ret = PTR_ERR(clks[i].clk);
	1969	+ clks[i].clk = NULL;
	1970	+ dev_err(dev, "%s: failed to get opp clk %d\n",
	1971	+ np->name, i);
	1972	+ goto error;
	1973	+ }
	1974	+ }
	1975	+ } else {
	1976	+ num_clks = of_clk_get_parent_count(np);
	1977	+ if (num_clks <= 0)
	1978	+ return 0;
	1979	+ clks = devm_kcalloc(dev, num_clks, sizeof(*clks), GFP_KERNEL);
	1980	+ if (!clks)
	1981	+ return -ENOMEM;
	1982	+ for (i = 0; i < num_clks; i++) {
	1983	+ clks[i].clk = of_clk_get(np, i);
	1984	+ if (IS_ERR(clks[i].clk)) {
	1985	+ ret = PTR_ERR(clks[i].clk);
	1986	+ clks[i].clk = NULL;
	1987	+ dev_err(dev, "%s: failed to get clk %d\n",
	1988	+ np->name, i);
	1989	+ goto error;
	1990	+ }
	1991	+ }
	1992	+ }
	1993	+ info->clks = clks;
	1994	+ info->num_clks = num_clks;
	1995	+
	1996	+ return 0;
	1997	+error:
	1998	+ while (--i >= 0)
	1999	+ clk_put(clks[i].clk);
	2000	+ devm_kfree(dev, clks);
	2001	+
	2002	+ return ret;
	2003	+}
	2004	+
	2005	+int rockchip_init_opp_table(struct device dev, struct rockchip_opp_info info,
1123	2006	char lkg_name, char reg_name)
1124	2007	{
1125	2008	struct device_node *np;
1126	2009	int bin = -EINVAL, process = -EINVAL;
1127	2010	int scale = 0, volt_sel = -EINVAL;
1128	2011	int ret = 0;
	2012	+ u32 freq;
1129	2013
1130	2014	/* Get OPP descriptor node */
1131	2015	np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
..	..	@@ -1133,23 +2017,90 @@
1133	2017	dev_dbg(dev, "Failed to find operating-points-v2\n");
1134	2018	return -ENOENT;
1135	2019	}
1136		- of_node_put(np);
	2020	+ if (!info)
	2021	+ goto next;
	2022	+ info->dev = dev;
1137	2023
1138		- rockchip_get_soc_info(dev, matches, &bin, &process);
	2024	+ ret = rockchip_get_opp_clk(dev, np, info);
	2025	+ if (ret)
	2026	+ goto out;
	2027	+ if (info->clks) {
	2028	+ ret = clk_bulk_prepare_enable(info->num_clks, info->clks);
	2029	+ if (ret) {
	2030	+ dev_err(dev, "failed to enable opp clks\n");
	2031	+ goto out;
	2032	+ }
	2033	+ }
	2034	+ if (info->data && info->data->set_read_margin) {
	2035	+ info->current_rm = UINT_MAX;
	2036	+ info->grf = syscon_regmap_lookup_by_phandle(np, "rockchip,grf");
	2037	+ if (IS_ERR(info->grf))
	2038	+ info->grf = NULL;
	2039	+ rockchip_get_volt_rm_table(dev, np, "volt-mem-read-margin",
	2040	+ &info->volt_rm_tbl);
	2041	+ of_property_read_u32(np, "low-volt-mem-read-margin",
	2042	+ &info->low_rm);
	2043	+ if (!of_property_read_u32(np, "intermediate-threshold-freq",
	2044	+ &freq))
	2045	+ info->intermediate_threshold_freq = freq * 1000;
	2046	+ rockchip_init_read_margin(dev, info, reg_name);
	2047	+ }
	2048	+ if (info->data && info->data->get_soc_info)
	2049	+ info->data->get_soc_info(dev, np, &bin, &process);
	2050	+
	2051	+next:
	2052	+ rockchip_get_soc_info(dev, np, &bin, &process);
	2053	+ rockchip_init_pvtpll_table(info, bin);
1139	2054	rockchip_get_scale_volt_sel(dev, lkg_name, reg_name, bin, process,
1140	2055	&scale, &volt_sel);
	2056	+ if (info && info->data && info->data->set_soc_info)
	2057	+ info->data->set_soc_info(dev, np, bin, process, volt_sel);
1141	2058	rockchip_set_opp_prop_name(dev, process, volt_sel);
	2059	+ rockchip_set_opp_supported_hw(dev, np, bin, volt_sel);
1142	2060	ret = dev_pm_opp_of_add_table(dev);
1143	2061	if (ret) {
1144	2062	dev_err(dev, "Invalid operating-points in device tree.\n");
1145		- return ret;
	2063	+ goto dis_opp_clk;
1146	2064	}
1147	2065	rockchip_adjust_power_scale(dev, scale);
	2066	+ rockchip_pvtpll_calibrate_opp(info);
	2067	+ rockchip_pvtpll_add_length(info);
1148	2068
1149		- return 0;
	2069	+dis_opp_clk:
	2070	+ if (info && info->clks)
	2071	+ clk_bulk_disable_unprepare(info->num_clks, info->clks);
	2072	+out:
	2073	+ of_node_put(np);
	2074	+
	2075	+ return ret;
1150	2076	}
1151	2077	EXPORT_SYMBOL(rockchip_init_opp_table);
1152	2078
	2079	+void rockchip_uninit_opp_table(struct device dev, struct rockchip_opp_info info)
	2080	+{
	2081	+ struct opp_table *opp_table;
	2082	+
	2083	+ if (info) {
	2084	+ kfree(info->opp_table);
	2085	+ info->opp_table = NULL;
	2086	+ devm_kfree(dev, info->clks);
	2087	+ info->clks = NULL;
	2088	+ devm_kfree(dev, info->volt_rm_tbl);
	2089	+ info->volt_rm_tbl = NULL;
	2090	+ }
	2091	+
	2092	+ opp_table = dev_pm_opp_get_opp_table(dev);
	2093	+ if (IS_ERR(opp_table))
	2094	+ return;
	2095	+ dev_pm_opp_of_remove_table(dev);
	2096	+ if (opp_table->prop_name)
	2097	+ dev_pm_opp_put_prop_name(opp_table);
	2098	+ if (opp_table->supported_hw)
	2099	+ dev_pm_opp_put_supported_hw(opp_table);
	2100	+ dev_pm_opp_put_opp_table(opp_table);
	2101	+}
	2102	+EXPORT_SYMBOL(rockchip_uninit_opp_table);
	2103	+
1153	2104	MODULE_DESCRIPTION("ROCKCHIP OPP Select");
1154	2105	MODULE_AUTHOR("Finley Xiao <finley.xiao@rock-chips.com>, Liang Chen <cl@rock-chips.com>");
1155	2106	MODULE_LICENSE("GPL");