1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * CPU frequency scaling support for Armada 37xx platform.
4  *
5  * Copyright (C) 2017 Marvell
6  *
7  * Gregory CLEMENT <gregory.clement@free-electrons.com>
8  */
9 
10 #include <linux/clk.h>
11 #include <linux/cpu.h>
12 #include <linux/cpufreq.h>
13 #include <linux/err.h>
14 #include <linux/interrupt.h>
15 #include <linux/io.h>
16 #include <linux/mfd/syscon.h>
17 #include <linux/module.h>
18 #include <linux/of_address.h>
19 #include <linux/of_device.h>
20 #include <linux/of_irq.h>
21 #include <linux/platform_device.h>
22 #include <linux/pm_opp.h>
23 #include <linux/regmap.h>
24 #include <linux/slab.h>
25 
26 #include "cpufreq-dt.h"
27 
28 /* Power management in North Bridge register set */
29 #define ARMADA_37XX_NB_L0L1	0x18
30 #define ARMADA_37XX_NB_L2L3	0x1C
31 #define  ARMADA_37XX_NB_TBG_DIV_OFF	13
32 #define  ARMADA_37XX_NB_TBG_DIV_MASK	0x7
33 #define  ARMADA_37XX_NB_CLK_SEL_OFF	11
34 #define  ARMADA_37XX_NB_CLK_SEL_MASK	0x1
35 #define  ARMADA_37XX_NB_CLK_SEL_TBG	0x1
36 #define  ARMADA_37XX_NB_TBG_SEL_OFF	9
37 #define  ARMADA_37XX_NB_TBG_SEL_MASK	0x3
38 #define  ARMADA_37XX_NB_VDD_SEL_OFF	6
39 #define  ARMADA_37XX_NB_VDD_SEL_MASK	0x3
40 #define  ARMADA_37XX_NB_CONFIG_SHIFT	16
41 #define ARMADA_37XX_NB_DYN_MOD	0x24
42 #define  ARMADA_37XX_NB_CLK_SEL_EN	BIT(26)
43 #define  ARMADA_37XX_NB_TBG_EN		BIT(28)
44 #define  ARMADA_37XX_NB_DIV_EN		BIT(29)
45 #define  ARMADA_37XX_NB_VDD_EN		BIT(30)
46 #define  ARMADA_37XX_NB_DFS_EN		BIT(31)
47 #define ARMADA_37XX_NB_CPU_LOAD 0x30
48 #define  ARMADA_37XX_NB_CPU_LOAD_MASK	0x3
49 #define  ARMADA_37XX_DVFS_LOAD_0	0
50 #define  ARMADA_37XX_DVFS_LOAD_1	1
51 #define  ARMADA_37XX_DVFS_LOAD_2	2
52 #define  ARMADA_37XX_DVFS_LOAD_3	3
53 
54 /* AVS register set */
55 #define ARMADA_37XX_AVS_CTL0		0x0
56 #define	 ARMADA_37XX_AVS_ENABLE		BIT(30)
57 #define	 ARMADA_37XX_AVS_HIGH_VDD_LIMIT	16
58 #define	 ARMADA_37XX_AVS_LOW_VDD_LIMIT	22
59 #define	 ARMADA_37XX_AVS_VDD_MASK	0x3F
60 #define ARMADA_37XX_AVS_CTL2		0x8
61 #define	 ARMADA_37XX_AVS_LOW_VDD_EN	BIT(6)
62 #define ARMADA_37XX_AVS_VSET(x)	    (0x1C + 4 * (x))
63 
64 /*
65  * On Armada 37xx the Power management manages 4 level of CPU load,
66  * each level can be associated with a CPU clock source, a CPU
67  * divider, a VDD level, etc...
68  */
69 #define LOAD_LEVEL_NR	4
70 
71 #define MIN_VOLT_MV 1000
72 
73 /*  AVS value for the corresponding voltage (in mV) */
74 static int avs_map[] = {
75 	747, 758, 770, 782, 793, 805, 817, 828, 840, 852, 863, 875, 887, 898,
76 	910, 922, 933, 945, 957, 968, 980, 992, 1003, 1015, 1027, 1038, 1050,
77 	1062, 1073, 1085, 1097, 1108, 1120, 1132, 1143, 1155, 1167, 1178, 1190,
78 	1202, 1213, 1225, 1237, 1248, 1260, 1272, 1283, 1295, 1307, 1318, 1330,
79 	1342
80 };
81 
82 struct armada37xx_cpufreq_state {
83 	struct regmap *regmap;
84 	u32 nb_l0l1;
85 	u32 nb_l2l3;
86 	u32 nb_dyn_mod;
87 	u32 nb_cpu_load;
88 };
89 
90 static struct armada37xx_cpufreq_state *armada37xx_cpufreq_state;
91 
92 struct armada_37xx_dvfs {
93 	u32 cpu_freq_max;
94 	u8 divider[LOAD_LEVEL_NR];
95 	u32 avs[LOAD_LEVEL_NR];
96 };
97 
98 static struct armada_37xx_dvfs armada_37xx_dvfs[] = {
99 	{.cpu_freq_max = 1200*1000*1000, .divider = {1, 2, 4, 6} },
100 	{.cpu_freq_max = 1000*1000*1000, .divider = {1, 2, 4, 5} },
101 	{.cpu_freq_max = 800*1000*1000,  .divider = {1, 2, 3, 4} },
102 	{.cpu_freq_max = 600*1000*1000,  .divider = {2, 4, 5, 6} },
103 };
104 
105 static struct armada_37xx_dvfs *armada_37xx_cpu_freq_info_get(u32 freq)
106 {
107 	int i;
108 
109 	for (i = 0; i < ARRAY_SIZE(armada_37xx_dvfs); i++) {
110 		if (freq == armada_37xx_dvfs[i].cpu_freq_max)
111 			return &armada_37xx_dvfs[i];
112 	}
113 
114 	pr_err("Unsupported CPU frequency %d MHz\n", freq/1000000);
115 	return NULL;
116 }
117 
118 /*
119  * Setup the four level managed by the hardware. Once the four level
120  * will be configured then the DVFS will be enabled.
121  */
122 static void __init armada37xx_cpufreq_dvfs_setup(struct regmap *base,
123 						 struct clk *clk, u8 *divider)
124 {
125 	int load_lvl;
126 	struct clk *parent;
127 
128 	for (load_lvl = 0; load_lvl < LOAD_LEVEL_NR; load_lvl++) {
129 		unsigned int reg, mask, val, offset = 0;
130 
131 		if (load_lvl <= ARMADA_37XX_DVFS_LOAD_1)
132 			reg = ARMADA_37XX_NB_L0L1;
133 		else
134 			reg = ARMADA_37XX_NB_L2L3;
135 
136 		if (load_lvl == ARMADA_37XX_DVFS_LOAD_0 ||
137 		    load_lvl == ARMADA_37XX_DVFS_LOAD_2)
138 			offset += ARMADA_37XX_NB_CONFIG_SHIFT;
139 
140 		/* Set cpu clock source, for all the level we use TBG */
141 		val = ARMADA_37XX_NB_CLK_SEL_TBG << ARMADA_37XX_NB_CLK_SEL_OFF;
142 		mask = (ARMADA_37XX_NB_CLK_SEL_MASK
143 			<< ARMADA_37XX_NB_CLK_SEL_OFF);
144 
145 		/*
146 		 * Set cpu divider based on the pre-computed array in
147 		 * order to have balanced step.
148 		 */
149 		val |= divider[load_lvl] << ARMADA_37XX_NB_TBG_DIV_OFF;
150 		mask |= (ARMADA_37XX_NB_TBG_DIV_MASK
151 			<< ARMADA_37XX_NB_TBG_DIV_OFF);
152 
153 		/* Set VDD divider which is actually the load level. */
154 		val |= load_lvl << ARMADA_37XX_NB_VDD_SEL_OFF;
155 		mask |= (ARMADA_37XX_NB_VDD_SEL_MASK
156 			<< ARMADA_37XX_NB_VDD_SEL_OFF);
157 
158 		val <<= offset;
159 		mask <<= offset;
160 
161 		regmap_update_bits(base, reg, mask, val);
162 	}
163 
164 	/*
165 	 * Set cpu clock source, for all the level we keep the same
166 	 * clock source that the one already configured. For this one
167 	 * we need to use the clock framework
168 	 */
169 	parent = clk_get_parent(clk);
170 	clk_set_parent(clk, parent);
171 }
172 
173 /*
174  * Find out the armada 37x supported AVS value whose voltage value is
175  * the round-up closest to the target voltage value.
176  */
177 static u32 armada_37xx_avs_val_match(int target_vm)
178 {
179 	u32 avs;
180 
181 	/* Find out the round-up closest supported voltage value */
182 	for (avs = 0; avs < ARRAY_SIZE(avs_map); avs++)
183 		if (avs_map[avs] >= target_vm)
184 			break;
185 
186 	/*
187 	 * If all supported voltages are smaller than target one,
188 	 * choose the largest supported voltage
189 	 */
190 	if (avs == ARRAY_SIZE(avs_map))
191 		avs = ARRAY_SIZE(avs_map) - 1;
192 
193 	return avs;
194 }
195 
196 /*
197  * For Armada 37xx soc, L0(VSET0) VDD AVS value is set to SVC revision
198  * value or a default value when SVC is not supported.
199  * - L0 can be read out from the register of AVS_CTRL_0 and L0 voltage
200  *   can be got from the mapping table of avs_map.
201  * - L1 voltage should be about 100mv smaller than L0 voltage
202  * - L2 & L3 voltage should be about 150mv smaller than L0 voltage.
203  * This function calculates L1 & L2 & L3 AVS values dynamically based
204  * on L0 voltage and fill all AVS values to the AVS value table.
205  */
206 static void __init armada37xx_cpufreq_avs_configure(struct regmap *base,
207 						struct armada_37xx_dvfs *dvfs)
208 {
209 	unsigned int target_vm;
210 	int load_level = 0;
211 	u32 l0_vdd_min;
212 
213 	if (base == NULL)
214 		return;
215 
216 	/* Get L0 VDD min value */
217 	regmap_read(base, ARMADA_37XX_AVS_CTL0, &l0_vdd_min);
218 	l0_vdd_min = (l0_vdd_min >> ARMADA_37XX_AVS_LOW_VDD_LIMIT) &
219 		ARMADA_37XX_AVS_VDD_MASK;
220 	if (l0_vdd_min >= ARRAY_SIZE(avs_map))  {
221 		pr_err("L0 VDD MIN %d is not correct.\n", l0_vdd_min);
222 		return;
223 	}
224 	dvfs->avs[0] = l0_vdd_min;
225 
226 	if (avs_map[l0_vdd_min] <= MIN_VOLT_MV) {
227 		/*
228 		 * If L0 voltage is smaller than 1000mv, then all VDD sets
229 		 * use L0 voltage;
230 		 */
231 		u32 avs_min = armada_37xx_avs_val_match(MIN_VOLT_MV);
232 
233 		for (load_level = 1; load_level < LOAD_LEVEL_NR; load_level++)
234 			dvfs->avs[load_level] = avs_min;
235 
236 		return;
237 	}
238 
239 	/*
240 	 * L1 voltage is equal to L0 voltage - 100mv and it must be
241 	 * larger than 1000mv
242 	 */
243 
244 	target_vm = avs_map[l0_vdd_min] - 100;
245 	target_vm = target_vm > MIN_VOLT_MV ? target_vm : MIN_VOLT_MV;
246 	dvfs->avs[1] = armada_37xx_avs_val_match(target_vm);
247 
248 	/*
249 	 * L2 & L3 voltage is equal to L0 voltage - 150mv and it must
250 	 * be larger than 1000mv
251 	 */
252 	target_vm = avs_map[l0_vdd_min] - 150;
253 	target_vm = target_vm > MIN_VOLT_MV ? target_vm : MIN_VOLT_MV;
254 	dvfs->avs[2] = dvfs->avs[3] = armada_37xx_avs_val_match(target_vm);
255 }
256 
257 static void __init armada37xx_cpufreq_avs_setup(struct regmap *base,
258 						struct armada_37xx_dvfs *dvfs)
259 {
260 	unsigned int avs_val = 0, freq;
261 	int load_level = 0;
262 
263 	if (base == NULL)
264 		return;
265 
266 	/* Disable AVS before the configuration */
267 	regmap_update_bits(base, ARMADA_37XX_AVS_CTL0,
268 			   ARMADA_37XX_AVS_ENABLE, 0);
269 
270 
271 	/* Enable low voltage mode */
272 	regmap_update_bits(base, ARMADA_37XX_AVS_CTL2,
273 			   ARMADA_37XX_AVS_LOW_VDD_EN,
274 			   ARMADA_37XX_AVS_LOW_VDD_EN);
275 
276 
277 	for (load_level = 1; load_level < LOAD_LEVEL_NR; load_level++) {
278 		freq = dvfs->cpu_freq_max / dvfs->divider[load_level];
279 
280 		avs_val = dvfs->avs[load_level];
281 		regmap_update_bits(base, ARMADA_37XX_AVS_VSET(load_level-1),
282 		    ARMADA_37XX_AVS_VDD_MASK << ARMADA_37XX_AVS_HIGH_VDD_LIMIT |
283 		    ARMADA_37XX_AVS_VDD_MASK << ARMADA_37XX_AVS_LOW_VDD_LIMIT,
284 		    avs_val << ARMADA_37XX_AVS_HIGH_VDD_LIMIT |
285 		    avs_val << ARMADA_37XX_AVS_LOW_VDD_LIMIT);
286 	}
287 
288 	/* Enable AVS after the configuration */
289 	regmap_update_bits(base, ARMADA_37XX_AVS_CTL0,
290 			   ARMADA_37XX_AVS_ENABLE,
291 			   ARMADA_37XX_AVS_ENABLE);
292 
293 }
294 
295 static void armada37xx_cpufreq_disable_dvfs(struct regmap *base)
296 {
297 	unsigned int reg = ARMADA_37XX_NB_DYN_MOD,
298 		mask = ARMADA_37XX_NB_DFS_EN;
299 
300 	regmap_update_bits(base, reg, mask, 0);
301 }
302 
303 static void __init armada37xx_cpufreq_enable_dvfs(struct regmap *base)
304 {
305 	unsigned int val, reg = ARMADA_37XX_NB_CPU_LOAD,
306 		mask = ARMADA_37XX_NB_CPU_LOAD_MASK;
307 
308 	/* Start with the highest load (0) */
309 	val = ARMADA_37XX_DVFS_LOAD_0;
310 	regmap_update_bits(base, reg, mask, val);
311 
312 	/* Now enable DVFS for the CPUs */
313 	reg = ARMADA_37XX_NB_DYN_MOD;
314 	mask =	ARMADA_37XX_NB_CLK_SEL_EN | ARMADA_37XX_NB_TBG_EN |
315 		ARMADA_37XX_NB_DIV_EN | ARMADA_37XX_NB_VDD_EN |
316 		ARMADA_37XX_NB_DFS_EN;
317 
318 	regmap_update_bits(base, reg, mask, mask);
319 }
320 
321 static int armada37xx_cpufreq_suspend(struct cpufreq_policy *policy)
322 {
323 	struct armada37xx_cpufreq_state *state = armada37xx_cpufreq_state;
324 
325 	regmap_read(state->regmap, ARMADA_37XX_NB_L0L1, &state->nb_l0l1);
326 	regmap_read(state->regmap, ARMADA_37XX_NB_L2L3, &state->nb_l2l3);
327 	regmap_read(state->regmap, ARMADA_37XX_NB_CPU_LOAD,
328 		    &state->nb_cpu_load);
329 	regmap_read(state->regmap, ARMADA_37XX_NB_DYN_MOD, &state->nb_dyn_mod);
330 
331 	return 0;
332 }
333 
334 static int armada37xx_cpufreq_resume(struct cpufreq_policy *policy)
335 {
336 	struct armada37xx_cpufreq_state *state = armada37xx_cpufreq_state;
337 
338 	/* Ensure DVFS is disabled otherwise the following registers are RO */
339 	armada37xx_cpufreq_disable_dvfs(state->regmap);
340 
341 	regmap_write(state->regmap, ARMADA_37XX_NB_L0L1, state->nb_l0l1);
342 	regmap_write(state->regmap, ARMADA_37XX_NB_L2L3, state->nb_l2l3);
343 	regmap_write(state->regmap, ARMADA_37XX_NB_CPU_LOAD,
344 		     state->nb_cpu_load);
345 
346 	/*
347 	 * NB_DYN_MOD register is the one that actually enable back DVFS if it
348 	 * was enabled before the suspend operation. This must be done last
349 	 * otherwise other registers are not writable.
350 	 */
351 	regmap_write(state->regmap, ARMADA_37XX_NB_DYN_MOD, state->nb_dyn_mod);
352 
353 	return 0;
354 }
355 
356 static int __init armada37xx_cpufreq_driver_init(void)
357 {
358 	struct cpufreq_dt_platform_data pdata;
359 	struct armada_37xx_dvfs *dvfs;
360 	struct platform_device *pdev;
361 	unsigned long freq;
362 	unsigned int cur_frequency;
363 	struct regmap *nb_pm_base, *avs_base;
364 	struct device *cpu_dev;
365 	int load_lvl, ret;
366 	struct clk *clk;
367 
368 	nb_pm_base =
369 		syscon_regmap_lookup_by_compatible("marvell,armada-3700-nb-pm");
370 
371 	if (IS_ERR(nb_pm_base))
372 		return -ENODEV;
373 
374 	avs_base =
375 		syscon_regmap_lookup_by_compatible("marvell,armada-3700-avs");
376 
377 	/* if AVS is not present don't use it but still try to setup dvfs */
378 	if (IS_ERR(avs_base)) {
379 		pr_info("Syscon failed for Adapting Voltage Scaling: skip it\n");
380 		avs_base = NULL;
381 	}
382 	/* Before doing any configuration on the DVFS first, disable it */
383 	armada37xx_cpufreq_disable_dvfs(nb_pm_base);
384 
385 	/*
386 	 * On CPU 0 register the operating points supported (which are
387 	 * the nominal CPU frequency and full integer divisions of
388 	 * it).
389 	 */
390 	cpu_dev = get_cpu_device(0);
391 	if (!cpu_dev) {
392 		dev_err(cpu_dev, "Cannot get CPU\n");
393 		return -ENODEV;
394 	}
395 
396 	clk = clk_get(cpu_dev, 0);
397 	if (IS_ERR(clk)) {
398 		dev_err(cpu_dev, "Cannot get clock for CPU0\n");
399 		return PTR_ERR(clk);
400 	}
401 
402 	/* Get nominal (current) CPU frequency */
403 	cur_frequency = clk_get_rate(clk);
404 	if (!cur_frequency) {
405 		dev_err(cpu_dev, "Failed to get clock rate for CPU\n");
406 		clk_put(clk);
407 		return -EINVAL;
408 	}
409 
410 	dvfs = armada_37xx_cpu_freq_info_get(cur_frequency);
411 	if (!dvfs) {
412 		clk_put(clk);
413 		return -EINVAL;
414 	}
415 
416 	armada37xx_cpufreq_state = kmalloc(sizeof(*armada37xx_cpufreq_state),
417 					   GFP_KERNEL);
418 	if (!armada37xx_cpufreq_state) {
419 		clk_put(clk);
420 		return -ENOMEM;
421 	}
422 
423 	armada37xx_cpufreq_state->regmap = nb_pm_base;
424 
425 	armada37xx_cpufreq_avs_configure(avs_base, dvfs);
426 	armada37xx_cpufreq_avs_setup(avs_base, dvfs);
427 
428 	armada37xx_cpufreq_dvfs_setup(nb_pm_base, clk, dvfs->divider);
429 	clk_put(clk);
430 
431 	for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR;
432 	     load_lvl++) {
433 		unsigned long u_volt = avs_map[dvfs->avs[load_lvl]] * 1000;
434 		freq = cur_frequency / dvfs->divider[load_lvl];
435 		ret = dev_pm_opp_add(cpu_dev, freq, u_volt);
436 		if (ret)
437 			goto remove_opp;
438 
439 
440 	}
441 
442 	/* Now that everything is setup, enable the DVFS at hardware level */
443 	armada37xx_cpufreq_enable_dvfs(nb_pm_base);
444 
445 	pdata.suspend = armada37xx_cpufreq_suspend;
446 	pdata.resume = armada37xx_cpufreq_resume;
447 
448 	pdev = platform_device_register_data(NULL, "cpufreq-dt", -1, &pdata,
449 					     sizeof(pdata));
450 	ret = PTR_ERR_OR_ZERO(pdev);
451 	if (ret)
452 		goto disable_dvfs;
453 
454 	return 0;
455 
456 disable_dvfs:
457 	armada37xx_cpufreq_disable_dvfs(nb_pm_base);
458 remove_opp:
459 	/* clean-up the already added opp before leaving */
460 	while (load_lvl-- > ARMADA_37XX_DVFS_LOAD_0) {
461 		freq = cur_frequency / dvfs->divider[load_lvl];
462 		dev_pm_opp_remove(cpu_dev, freq);
463 	}
464 
465 	kfree(armada37xx_cpufreq_state);
466 
467 	return ret;
468 }
469 /* late_initcall, to guarantee the driver is loaded after A37xx clock driver */
470 late_initcall(armada37xx_cpufreq_driver_init);
471 
472 MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>");
473 MODULE_DESCRIPTION("Armada 37xx cpufreq driver");
474 MODULE_LICENSE("GPL");
475