修改led为gpio

hc

2024-10-12 a5969cabbb4660eab42b6ef0412cbbd1200cf14d

 kernel/drivers/clk/qcom/clk-rcg2.c
..
..
@@ -10,8 +10,10 @@
10
10
 #include <linux/export.h>
11
11
 #include <linux/clk-provider.h>
12
12
 #include <linux/delay.h>
13
+#include <linux/rational.h>
13
14
 #include <linux/regmap.h>
14
15
 #include <linux/math64.h>
16
+#include <linux/slab.h>
15
17
 
16
18
 #include <asm/div64.h>
17
19
 
..
..
@@ -40,6 +42,19 @@
40
42
 #define N_REG			0xc
41
43
 #define D_REG			0x10
42
44
 
45
+#define RCG_CFG_OFFSET(rcg)	((rcg)->cmd_rcgr + (rcg)->cfg_off + CFG_REG)
46
+#define RCG_M_OFFSET(rcg)	((rcg)->cmd_rcgr + (rcg)->cfg_off + M_REG)
47
+#define RCG_N_OFFSET(rcg)	((rcg)->cmd_rcgr + (rcg)->cfg_off + N_REG)
48
+#define RCG_D_OFFSET(rcg)	((rcg)->cmd_rcgr + (rcg)->cfg_off + D_REG)
49
+
50
+/* Dynamic Frequency Scaling */
51
+#define MAX_PERF_LEVEL		8
52
+#define SE_CMD_DFSR_OFFSET	0x14
53
+#define SE_CMD_DFS_EN		BIT(0)
54
+#define SE_PERF_DFSR(level)	(0x1c + 0x4 * (level))
55
+#define SE_PERF_M_DFSR(level)	(0x5c + 0x4 * (level))
56
+#define SE_PERF_N_DFSR(level)	(0x9c + 0x4 * (level))
57
+
43
58
 enum freq_policy {
44
59
 	FLOOR,
45
60
 	CEIL,
..
..
@@ -65,7 +80,7 @@
65
80
 	u32 cfg;
66
81
 	int i, ret;
67
82
 
68
-	ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
83
+	ret = regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);
69
84
 	if (ret)
70
85
 		goto err;
71
86
 
..
..
@@ -114,7 +129,7 @@
114
129
 	int ret;
115
130
 	u32 cfg = rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
116
131
 
117
-	ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
132
+	ret = regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg),
118
133
 				 CFG_SRC_SEL_MASK, cfg);
119
134
 	if (ret)
120
135
 		return ret;
..
..
@@ -153,13 +168,13 @@
153
168
 	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
154
169
 	u32 cfg, hid_div, m = 0, n = 0, mode = 0, mask;
155
170
 
156
-	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
171
+	regmap_read(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg), &cfg);
157
172
 
158
173
 	if (rcg->mnd_width) {
159
174
 		mask = BIT(rcg->mnd_width) - 1;
160
-		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + M_REG, &m);
175
+		regmap_read(rcg->clkr.regmap, RCG_M_OFFSET(rcg), &m);
161
176
 		m &= mask;
162
-		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + N_REG, &n);
177
+		regmap_read(rcg->clkr.regmap, RCG_N_OFFSET(rcg), &n);
163
178
 		n =  ~n;
164
179
 		n &= mask;
165
180
 		n += m;
..
..
@@ -192,7 +207,7 @@
192
207
 		break;
193
208
 	default:
194
209
 		return -EINVAL;
195
-	};
210
+	}
196
211
 
197
212
 	if (!f)
198
213
 		return -EINVAL;
..
..
@@ -249,7 +264,7 @@
249
264
 
250
265
 static int __clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f)
251
266
 {
252
-	u32 cfg, mask;
267
+	u32 cfg, mask, d_val, not2d_val, n_minus_m;
253
268
 	struct clk_hw *hw = &rcg->clkr.hw;
254
269
 	int ret, index = qcom_find_src_index(hw, rcg->parent_map, f->src);
255
270
 
..
..
@@ -259,17 +274,26 @@
259
274
 	if (rcg->mnd_width && f->n) {
260
275
 		mask = BIT(rcg->mnd_width) - 1;
261
276
 		ret = regmap_update_bits(rcg->clkr.regmap,
262
-				rcg->cmd_rcgr + M_REG, mask, f->m);
277
+				RCG_M_OFFSET(rcg), mask, f->m);
263
278
 		if (ret)
264
279
 			return ret;
265
280
 
266
281
 		ret = regmap_update_bits(rcg->clkr.regmap,
267
-				rcg->cmd_rcgr + N_REG, mask, ~(f->n - f->m));
282
+				RCG_N_OFFSET(rcg), mask, ~(f->n - f->m));
268
283
 		if (ret)
269
284
 			return ret;
270
285
 
286
+		/* Calculate 2d value */
287
+		d_val = f->n;
288
+
289
+		n_minus_m = f->n - f->m;
290
+		n_minus_m *= 2;
291
+
292
+		d_val = clamp_t(u32, d_val, f->m, n_minus_m);
293
+		not2d_val = ~d_val & mask;
294
+
271
295
 		ret = regmap_update_bits(rcg->clkr.regmap,
272
-				rcg->cmd_rcgr + D_REG, mask, ~f->n);
296
+				RCG_D_OFFSET(rcg), mask, not2d_val);
273
297
 		if (ret)
274
298
 			return ret;
275
299
 	}
..
..
@@ -280,8 +304,7 @@
280
304
 	cfg |= rcg->parent_map[index].cfg << CFG_SRC_SEL_SHIFT;
281
305
 	if (rcg->mnd_width && f->n && (f->m != f->n))
282
306
 		cfg |= CFG_MODE_DUAL_EDGE;
283
-
284
-	return regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
307
+	return regmap_update_bits(rcg->clkr.regmap, RCG_CFG_OFFSET(rcg),
285
308
 					mask, cfg);
286
309
 }
287
310
 
..
..
@@ -311,7 +334,7 @@
311
334
 		break;
312
335
 	default:
313
336
 		return -EINVAL;
314
-	};
337
+	}
315
338
 
316
339
 	if (!f)
317
340
 		return -EINVAL;
..
..
@@ -625,6 +648,7 @@
625
648
 	{ 2, 9 },
626
649
 	{ 4, 9 },
627
650
 	{ 1, 1 },
651
+	{ 2, 3 },
628
652
 	{ }
629
653
 };
630
654
 
..
..
@@ -934,3 +958,266 @@
934
958
 	.set_rate_and_parent = clk_rcg2_shared_set_rate_and_parent,
935
959
 };
936
960
 EXPORT_SYMBOL_GPL(clk_rcg2_shared_ops);
961
+
962
+/* Common APIs to be used for DFS based RCGR */
963
+static void clk_rcg2_dfs_populate_freq(struct clk_hw *hw, unsigned int l,
964
+				       struct freq_tbl *f)
965
+{
966
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
967
+	struct clk_hw *p;
968
+	unsigned long prate = 0;
969
+	u32 val, mask, cfg, mode, src;
970
+	int i, num_parents;
971
+
972
+	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(l), &cfg);
973
+
974
+	mask = BIT(rcg->hid_width) - 1;
975
+	f->pre_div = 1;
976
+	if (cfg & mask)
977
+		f->pre_div = cfg & mask;
978
+
979
+	src = cfg & CFG_SRC_SEL_MASK;
980
+	src >>= CFG_SRC_SEL_SHIFT;
981
+
982
+	num_parents = clk_hw_get_num_parents(hw);
983
+	for (i = 0; i < num_parents; i++) {
984
+		if (src == rcg->parent_map[i].cfg) {
985
+			f->src = rcg->parent_map[i].src;
986
+			p = clk_hw_get_parent_by_index(&rcg->clkr.hw, i);
987
+			prate = clk_hw_get_rate(p);
988
+		}
989
+	}
990
+
991
+	mode = cfg & CFG_MODE_MASK;
992
+	mode >>= CFG_MODE_SHIFT;
993
+	if (mode) {
994
+		mask = BIT(rcg->mnd_width) - 1;
995
+		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_M_DFSR(l),
996
+			    &val);
997
+		val &= mask;
998
+		f->m = val;
999
+
1000
+		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_N_DFSR(l),
1001
+			    &val);
1002
+		val = ~val;
1003
+		val &= mask;
1004
+		val += f->m;
1005
+		f->n = val;
1006
+	}
1007
+
1008
+	f->freq = calc_rate(prate, f->m, f->n, mode, f->pre_div);
1009
+}
1010
+
1011
+static int clk_rcg2_dfs_populate_freq_table(struct clk_rcg2 *rcg)
1012
+{
1013
+	struct freq_tbl *freq_tbl;
1014
+	int i;
1015
+
1016
+	/* Allocate space for 1 extra since table is NULL terminated */
1017
+	freq_tbl = kcalloc(MAX_PERF_LEVEL + 1, sizeof(*freq_tbl), GFP_KERNEL);
1018
+	if (!freq_tbl)
1019
+		return -ENOMEM;
1020
+	rcg->freq_tbl = freq_tbl;
1021
+
1022
+	for (i = 0; i < MAX_PERF_LEVEL; i++)
1023
+		clk_rcg2_dfs_populate_freq(&rcg->clkr.hw, i, freq_tbl + i);
1024
+
1025
+	return 0;
1026
+}
1027
+
1028
+static int clk_rcg2_dfs_determine_rate(struct clk_hw *hw,
1029
+				   struct clk_rate_request *req)
1030
+{
1031
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1032
+	int ret;
1033
+
1034
+	if (!rcg->freq_tbl) {
1035
+		ret = clk_rcg2_dfs_populate_freq_table(rcg);
1036
+		if (ret) {
1037
+			pr_err("Failed to update DFS tables for %s\n",
1038
+					clk_hw_get_name(hw));
1039
+			return ret;
1040
+		}
1041
+	}
1042
+
1043
+	return clk_rcg2_determine_rate(hw, req);
1044
+}
1045
+
1046
+static unsigned long
1047
+clk_rcg2_dfs_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
1048
+{
1049
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1050
+	u32 level, mask, cfg, m = 0, n = 0, mode, pre_div;
1051
+
1052
+	regmap_read(rcg->clkr.regmap,
1053
+		    rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &level);
1054
+	level &= GENMASK(4, 1);
1055
+	level >>= 1;
1056
+
1057
+	if (rcg->freq_tbl)
1058
+		return rcg->freq_tbl[level].freq;
1059
+
1060
+	/*
1061
+	 * Assume that parent_rate is actually the parent because
1062
+	 * we can't do any better at figuring it out when the table
1063
+	 * hasn't been populated yet. We only populate the table
1064
+	 * in determine_rate because we can't guarantee the parents
1065
+	 * will be registered with the framework until then.
1066
+	 */
1067
+	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + SE_PERF_DFSR(level),
1068
+		    &cfg);
1069
+
1070
+	mask = BIT(rcg->hid_width) - 1;
1071
+	pre_div = 1;
1072
+	if (cfg & mask)
1073
+		pre_div = cfg & mask;
1074
+
1075
+	mode = cfg & CFG_MODE_MASK;
1076
+	mode >>= CFG_MODE_SHIFT;
1077
+	if (mode) {
1078
+		mask = BIT(rcg->mnd_width) - 1;
1079
+		regmap_read(rcg->clkr.regmap,
1080
+			    rcg->cmd_rcgr + SE_PERF_M_DFSR(level), &m);
1081
+		m &= mask;
1082
+
1083
+		regmap_read(rcg->clkr.regmap,
1084
+			    rcg->cmd_rcgr + SE_PERF_N_DFSR(level), &n);
1085
+		n = ~n;
1086
+		n &= mask;
1087
+		n += m;
1088
+	}
1089
+
1090
+	return calc_rate(parent_rate, m, n, mode, pre_div);
1091
+}
1092
+
1093
+static const struct clk_ops clk_rcg2_dfs_ops = {
1094
+	.is_enabled = clk_rcg2_is_enabled,
1095
+	.get_parent = clk_rcg2_get_parent,
1096
+	.determine_rate = clk_rcg2_dfs_determine_rate,
1097
+	.recalc_rate = clk_rcg2_dfs_recalc_rate,
1098
+};
1099
+
1100
+static int clk_rcg2_enable_dfs(const struct clk_rcg_dfs_data *data,
1101
+			       struct regmap *regmap)
1102
+{
1103
+	struct clk_rcg2 *rcg = data->rcg;
1104
+	struct clk_init_data *init = data->init;
1105
+	u32 val;
1106
+	int ret;
1107
+
1108
+	ret = regmap_read(regmap, rcg->cmd_rcgr + SE_CMD_DFSR_OFFSET, &val);
1109
+	if (ret)
1110
+		return -EINVAL;
1111
+
1112
+	if (!(val & SE_CMD_DFS_EN))
1113
+		return 0;
1114
+
1115
+	/*
1116
+	 * Rate changes with consumer writing a register in
1117
+	 * their own I/O region
1118
+	 */
1119
+	init->flags |= CLK_GET_RATE_NOCACHE;
1120
+	init->ops = &clk_rcg2_dfs_ops;
1121
+
1122
+	rcg->freq_tbl = NULL;
1123
+
1124
+	return 0;
1125
+}
1126
+
1127
+int qcom_cc_register_rcg_dfs(struct regmap *regmap,
1128
+			     const struct clk_rcg_dfs_data *rcgs, size_t len)
1129
+{
1130
+	int i, ret;
1131
+
1132
+	for (i = 0; i < len; i++) {
1133
+		ret = clk_rcg2_enable_dfs(&rcgs[i], regmap);
1134
+		if (ret)
1135
+			return ret;
1136
+	}
1137
+
1138
+	return 0;
1139
+}
1140
+EXPORT_SYMBOL_GPL(qcom_cc_register_rcg_dfs);
1141
+
1142
+static int clk_rcg2_dp_set_rate(struct clk_hw *hw, unsigned long rate,
1143
+			unsigned long parent_rate)
1144
+{
1145
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1146
+	struct freq_tbl f = { 0 };
1147
+	u32 mask = BIT(rcg->hid_width) - 1;
1148
+	u32 hid_div, cfg;
1149
+	int i, num_parents = clk_hw_get_num_parents(hw);
1150
+	unsigned long num, den;
1151
+
1152
+	rational_best_approximation(parent_rate, rate,
1153
+			GENMASK(rcg->mnd_width - 1, 0),
1154
+			GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1155
+
1156
+	if (!num || !den)
1157
+		return -EINVAL;
1158
+
1159
+	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
1160
+	hid_div = cfg;
1161
+	cfg &= CFG_SRC_SEL_MASK;
1162
+	cfg >>= CFG_SRC_SEL_SHIFT;
1163
+
1164
+	for (i = 0; i < num_parents; i++) {
1165
+		if (cfg == rcg->parent_map[i].cfg) {
1166
+			f.src = rcg->parent_map[i].src;
1167
+			break;
1168
+		}
1169
+	}
1170
+
1171
+	f.pre_div = hid_div;
1172
+	f.pre_div >>= CFG_SRC_DIV_SHIFT;
1173
+	f.pre_div &= mask;
1174
+
1175
+	if (num != den) {
1176
+		f.m = num;
1177
+		f.n = den;
1178
+	} else {
1179
+		f.m = 0;
1180
+		f.n = 0;
1181
+	}
1182
+
1183
+	return clk_rcg2_configure(rcg, &f);
1184
+}
1185
+
1186
+static int clk_rcg2_dp_set_rate_and_parent(struct clk_hw *hw,
1187
+		unsigned long rate, unsigned long parent_rate, u8 index)
1188
+{
1189
+	return clk_rcg2_dp_set_rate(hw, rate, parent_rate);
1190
+}
1191
+
1192
+static int clk_rcg2_dp_determine_rate(struct clk_hw *hw,
1193
+				struct clk_rate_request *req)
1194
+{
1195
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
1196
+	unsigned long num, den;
1197
+	u64 tmp;
1198
+
1199
+	/* Parent rate is a fixed phy link rate */
1200
+	rational_best_approximation(req->best_parent_rate, req->rate,
1201
+			GENMASK(rcg->mnd_width - 1, 0),
1202
+			GENMASK(rcg->mnd_width - 1, 0), &den, &num);
1203
+
1204
+	if (!num || !den)
1205
+		return -EINVAL;
1206
+
1207
+	tmp = req->best_parent_rate * num;
1208
+	do_div(tmp, den);
1209
+	req->rate = tmp;
1210
+
1211
+	return 0;
1212
+}
1213
+
1214
+const struct clk_ops clk_dp_ops = {
1215
+	.is_enabled = clk_rcg2_is_enabled,
1216
+	.get_parent = clk_rcg2_get_parent,
1217
+	.set_parent = clk_rcg2_set_parent,
1218
+	.recalc_rate = clk_rcg2_recalc_rate,
1219
+	.set_rate = clk_rcg2_dp_set_rate,
1220
+	.set_rate_and_parent = clk_rcg2_dp_set_rate_and_parent,
1221
+	.determine_rate = clk_rcg2_dp_determine_rate,
1222
+};
1223
+EXPORT_SYMBOL_GPL(clk_dp_ops);