~hc/RK356X_SDK_RELEASE.git

..	..	@@ -3,6 +3,7 @@
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>
..	..	@@ -11,6 +12,7 @@
11	12	#include <linux/nvmem-consumer.h>
12	13	#include <linux/regmap.h>
13	14	#include <linux/regulator/consumer.h>
	15	+#include <linux/rockchip/rockchip_sip.h>
14	16	#include <linux/slab.h>
15	17	#include <linux/soc/rockchip/pvtm.h>
16	18	#include <linux/thermal.h>
..	..	@@ -714,6 +716,416 @@
714	716	}
715	717	EXPORT_SYMBOL(rockchip_of_get_lkg_sel);
716	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	+
717	1129	static int rockchip_get_pvtm_pvtpll(struct device dev, struct device_node np,
718	1130	char *reg_name)
719	1131	{
..	..	@@ -725,6 +1137,11 @@
725	1137	int cur_temp, diff_temp, prop_temp, diff_value;
726	1138	int pvtm_value = 0;
727	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	+ }
728	1145
729	1146	pvtm = kzalloc(sizeof(*pvtm), GFP_KERNEL);
730	1147	if (!pvtm)
..	..	@@ -838,12 +1255,13 @@
838	1255	}
839	1256
840	1257	void rockchip_of_get_pvtm_sel(struct device dev, struct device_node np,
841		- char *reg_name, int process,
	1258	+ char *reg_name, int bin, int process,
842	1259	int volt_sel, int scale_sel)
843	1260	{
844	1261	struct property *prop = NULL;
845	1262	char name[NAME_MAX];
846	1263	int pvtm, ret;
	1264	+ u32 hw = 0;
847	1265
848	1266	if (of_property_read_bool(np, "rockchip,pvtm-pvtpll"))
849	1267	pvtm = rockchip_get_pvtm_pvtpll(dev, np, reg_name);
..	..	@@ -858,6 +1276,17 @@
858	1276	snprintf(name, sizeof(name),
859	1277	"rockchip,p%d-pvtm-voltage-sel", process);
860	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	+ }
861	1290	}
862	1291	if (!prop)
863	1292	sprintf(name, "rockchip,pvtm-voltage-sel");
..	..	@@ -868,6 +1297,7 @@
868	1297	next:
869	1298	if (!scale_sel)
870	1299	return;
	1300	+ prop = NULL;
871	1301	if (process >= 0) {
872	1302	snprintf(name, sizeof(name),
873	1303	"rockchip,p%d-pvtm-scaling-sel", process);
..	..	@@ -911,37 +1341,99 @@
911	1341	}
912	1342	EXPORT_SYMBOL(rockchip_of_get_bin_volt_sel);
913	1343
914		-void rockchip_get_soc_info(struct device *dev,
915		- const struct of_device_id *matches,
916		- int bin, int process)
	1344	+void rockchip_get_opp_data(const struct of_device_id *matches,
	1345	+ struct rockchip_opp_info *info)
917	1346	{
918	1347	const struct of_device_id *match;
919		- struct device_node *np;
920	1348	struct device_node *node;
921		- int (get_soc_info)(struct device dev, struct device_node *np,
922		- int bin, int process);
923		- int ret = 0;
924		-
925		- if (!matches)
926		- return;
927		-
928		- np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
929		- if (!np) {
930		- dev_warn(dev, "OPP-v2 not supported\n");
931		- return;
932		- }
933	1349
934	1350	node = of_find_node_by_path("/");
935	1351	match = of_match_node(matches, node);
936		- if (match && match->data) {
937		- get_soc_info = match->data;
938		- ret = get_soc_info(dev, np, bin, process);
939		- if (ret)
940		- 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);
941	1389	}
942	1390
943		- of_node_put(node);
944		- 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;
945	1437	}
946	1438	EXPORT_SYMBOL(rockchip_get_soc_info);
947	1439
..	..	@@ -962,7 +1454,7 @@
962	1454
963	1455	rockchip_of_get_lkg_sel(dev, np, lkg_name, process,
964	1456	&lkg_volt_sel, &lkg_scale);
965		- rockchip_of_get_pvtm_sel(dev, np, reg_name, process,
	1457	+ rockchip_of_get_pvtm_sel(dev, np, reg_name, bin, process,
966	1458	&pvtm_volt_sel, &pvtm_scale);
967	1459	rockchip_of_get_bin_sel(dev, np, bin, &bin_scale);
968	1460	rockchip_of_get_bin_volt_sel(dev, np, bin, &bin_volt_sel);
..	..	@@ -1000,6 +1492,42 @@
1000	1492	}
1001	1493	EXPORT_SYMBOL(rockchip_set_opp_prop_name);
1002	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	+
1003	1531	static int rockchip_adjust_opp_by_irdrop(struct device *dev,
1004	1532	struct device_node *np,
1005	1533	unsigned long *safe_rate,
..	..	@@ -1026,6 +1554,8 @@
1026	1554
1027	1555	mutex_lock(&opp_table->lock);
1028	1556	list_for_each_entry(opp, &opp_table->opp_list, node) {
	1557	+ if (!opp->available)
	1558	+ continue;
1029	1559	if (!irdrop_table) {
1030	1560	delta_irdrop = 0;
1031	1561	} else {
..	..	@@ -1094,6 +1624,8 @@
1094	1624
1095	1625	mutex_lock(&opp_table->lock);
1096	1626	list_for_each_entry(opp, &opp_table->opp_list, node) {
	1627	+ if (!opp->available)
	1628	+ continue;
1097	1629	if (opp->supplies->u_volt < vmin) {
1098	1630	opp->supplies->u_volt = vmin;
1099	1631	opp->supplies->u_volt_min = vmin;
..	..	@@ -1131,9 +1663,14 @@
1131	1663	if (opp->rate > opp_info.max_freq * 1000000)
1132	1664	continue;
1133	1665
1134		- opp->supplies->u_volt += opp_info.volt * 1000;
1135		- if (opp->supplies->u_volt > opp->supplies->u_volt_max)
1136		- opp->supplies->u_volt = opp->supplies->u_volt_max;
	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	+ }
1137	1674	}
1138	1675	mutex_unlock(&opp_table->lock);
1139	1676
..	..	@@ -1161,7 +1698,7 @@
1161	1698	goto out;
1162	1699	}
1163	1700	if (opp->rate > scale_rate)
1164		- dev_pm_opp_remove(dev, opp->rate);
	1701	+ dev_pm_opp_disable(dev, opp->rate);
1165	1702	dev_pm_opp_put(opp);
1166	1703	}
1167	1704	out:
..	..	@@ -1191,6 +1728,10 @@
1191	1728	rockchip_adjust_opp_by_irdrop(dev, np, &safe_rate, &max_rate);
1192	1729
1193	1730	dev_info(dev, "avs=%d\n", avs);
	1731	+
	1732	+ if (!safe_rate && !scale)
	1733	+ goto out_np;
	1734	+
1194	1735	clk = of_clk_get_by_name(np, NULL);
1195	1736	if (IS_ERR(clk)) {
1196	1737	if (!safe_rate)
..	..	@@ -1204,14 +1745,14 @@
1204	1745
1205	1746	if (safe_rate)
1206	1747	irdrop_scale = rockchip_pll_clk_rate_to_scale(clk, safe_rate);
1207		- if (max_rate)
1208		- opp_scale = rockchip_pll_clk_rate_to_scale(clk, max_rate);
1209	1748	target_scale = max(irdrop_scale, scale);
1210	1749	if (target_scale <= 0)
1211	1750	goto out_clk;
1212	1751	dev_dbg(dev, "target_scale=%d, irdrop_scale=%d, scale=%d\n",
1213	1752	target_scale, irdrop_scale, scale);
1214	1753
	1754	+ if (max_rate)
	1755	+ opp_scale = rockchip_pll_clk_rate_to_scale(clk, max_rate);
1215	1756	if (avs == AVS_SCALING_RATE) {
1216	1757	ret = rockchip_pll_clk_adaptive_scaling(clk, target_scale);
1217	1758	if (ret)
..	..	@@ -1256,14 +1797,219 @@
1256	1797	}
1257	1798	EXPORT_SYMBOL(rockchip_adjust_power_scale);
1258	1799
1259		-int rockchip_init_opp_table(struct device *dev,
1260		- 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,
1261	2006	char lkg_name, char reg_name)
1262	2007	{
1263	2008	struct device_node *np;
1264	2009	int bin = -EINVAL, process = -EINVAL;
1265	2010	int scale = 0, volt_sel = -EINVAL;
1266	2011	int ret = 0;
	2012	+ u32 freq;
1267	2013
1268	2014	/* Get OPP descriptor node */
1269	2015	np = of_parse_phandle(dev->of_node, "operating-points-v2", 0);
..	..	@@ -1271,23 +2017,90 @@
1271	2017	dev_dbg(dev, "Failed to find operating-points-v2\n");
1272	2018	return -ENOENT;
1273	2019	}
1274		- of_node_put(np);
	2020	+ if (!info)
	2021	+ goto next;
	2022	+ info->dev = dev;
1275	2023
1276		- 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);
1277	2054	rockchip_get_scale_volt_sel(dev, lkg_name, reg_name, bin, process,
1278	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);
1279	2058	rockchip_set_opp_prop_name(dev, process, volt_sel);
	2059	+ rockchip_set_opp_supported_hw(dev, np, bin, volt_sel);
1280	2060	ret = dev_pm_opp_of_add_table(dev);
1281	2061	if (ret) {
1282	2062	dev_err(dev, "Invalid operating-points in device tree.\n");
1283		- return ret;
	2063	+ goto dis_opp_clk;
1284	2064	}
1285	2065	rockchip_adjust_power_scale(dev, scale);
	2066	+ rockchip_pvtpll_calibrate_opp(info);
	2067	+ rockchip_pvtpll_add_length(info);
1286	2068
1287		- 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;
1288	2076	}
1289	2077	EXPORT_SYMBOL(rockchip_init_opp_table);
1290	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	+
1291	2104	MODULE_DESCRIPTION("ROCKCHIP OPP Select");
1292	2105	MODULE_AUTHOR("Finley Xiao <finley.xiao@rock-chips.com>, Liang Chen <cl@rock-chips.com>");
1293	2106	MODULE_LICENSE("GPL");