1
2
3
4
5
6
7
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
29#define ARMADA_37XX_CLK_TBG_SEL 0
30#define ARMADA_37XX_CLK_TBG_SEL_CPU_OFF 22
31
32
33#define ARMADA_37XX_NB_L0L1 0x18
34#define ARMADA_37XX_NB_L2L3 0x1C
35#define ARMADA_37XX_NB_TBG_DIV_OFF 13
36#define ARMADA_37XX_NB_TBG_DIV_MASK 0x7
37#define ARMADA_37XX_NB_CLK_SEL_OFF 11
38#define ARMADA_37XX_NB_CLK_SEL_MASK 0x1
39#define ARMADA_37XX_NB_CLK_SEL_TBG 0x1
40#define ARMADA_37XX_NB_TBG_SEL_OFF 9
41#define ARMADA_37XX_NB_TBG_SEL_MASK 0x3
42#define ARMADA_37XX_NB_VDD_SEL_OFF 6
43#define ARMADA_37XX_NB_VDD_SEL_MASK 0x3
44#define ARMADA_37XX_NB_CONFIG_SHIFT 16
45#define ARMADA_37XX_NB_DYN_MOD 0x24
46#define ARMADA_37XX_NB_CLK_SEL_EN BIT(26)
47#define ARMADA_37XX_NB_TBG_EN BIT(28)
48#define ARMADA_37XX_NB_DIV_EN BIT(29)
49#define ARMADA_37XX_NB_VDD_EN BIT(30)
50#define ARMADA_37XX_NB_DFS_EN BIT(31)
51#define ARMADA_37XX_NB_CPU_LOAD 0x30
52#define ARMADA_37XX_NB_CPU_LOAD_MASK 0x3
53#define ARMADA_37XX_DVFS_LOAD_0 0
54#define ARMADA_37XX_DVFS_LOAD_1 1
55#define ARMADA_37XX_DVFS_LOAD_2 2
56#define ARMADA_37XX_DVFS_LOAD_3 3
57
58
59#define ARMADA_37XX_AVS_CTL0 0x0
60#define ARMADA_37XX_AVS_ENABLE BIT(30)
61#define ARMADA_37XX_AVS_HIGH_VDD_LIMIT 16
62#define ARMADA_37XX_AVS_LOW_VDD_LIMIT 22
63#define ARMADA_37XX_AVS_VDD_MASK 0x3F
64#define ARMADA_37XX_AVS_CTL2 0x8
65#define ARMADA_37XX_AVS_LOW_VDD_EN BIT(6)
66#define ARMADA_37XX_AVS_VSET(x) (0x1C + 4 * (x))
67
68
69
70
71
72
73#define LOAD_LEVEL_NR 4
74
75#define MIN_VOLT_MV 1000
76#define MIN_VOLT_MV_FOR_L1_1000MHZ 1108
77#define MIN_VOLT_MV_FOR_L1_1200MHZ 1155
78
79
80static int avs_map[] = {
81 747, 758, 770, 782, 793, 805, 817, 828, 840, 852, 863, 875, 887, 898,
82 910, 922, 933, 945, 957, 968, 980, 992, 1003, 1015, 1027, 1038, 1050,
83 1062, 1073, 1085, 1097, 1108, 1120, 1132, 1143, 1155, 1167, 1178, 1190,
84 1202, 1213, 1225, 1237, 1248, 1260, 1272, 1283, 1295, 1307, 1318, 1330,
85 1342
86};
87
88struct armada37xx_cpufreq_state {
89 struct platform_device *pdev;
90 struct device *cpu_dev;
91 struct regmap *regmap;
92 u32 nb_l0l1;
93 u32 nb_l2l3;
94 u32 nb_dyn_mod;
95 u32 nb_cpu_load;
96};
97
98static struct armada37xx_cpufreq_state *armada37xx_cpufreq_state;
99
100struct armada_37xx_dvfs {
101 u32 cpu_freq_max;
102 u8 divider[LOAD_LEVEL_NR];
103 u32 avs[LOAD_LEVEL_NR];
104};
105
106static struct armada_37xx_dvfs armada_37xx_dvfs[] = {
107
108
109
110
111
112 {.cpu_freq_max = 1000*1000*1000, .divider = {1, 2, 4, 5} },
113 {.cpu_freq_max = 800*1000*1000, .divider = {1, 2, 3, 4} },
114 {.cpu_freq_max = 600*1000*1000, .divider = {2, 4, 5, 6} },
115};
116
117static struct armada_37xx_dvfs *armada_37xx_cpu_freq_info_get(u32 freq)
118{
119 int i;
120
121 for (i = 0; i < ARRAY_SIZE(armada_37xx_dvfs); i++) {
122 if (freq == armada_37xx_dvfs[i].cpu_freq_max)
123 return &armada_37xx_dvfs[i];
124 }
125
126 pr_err("Unsupported CPU frequency %d MHz\n", freq/1000000);
127 return NULL;
128}
129
130
131
132
133
134static void __init armada37xx_cpufreq_dvfs_setup(struct regmap *base,
135 struct regmap *clk_base, u8 *divider)
136{
137 u32 cpu_tbg_sel;
138 int load_lvl;
139
140
141 regmap_read(clk_base, ARMADA_37XX_CLK_TBG_SEL, &cpu_tbg_sel);
142 cpu_tbg_sel >>= ARMADA_37XX_CLK_TBG_SEL_CPU_OFF;
143 cpu_tbg_sel &= ARMADA_37XX_NB_TBG_SEL_MASK;
144
145 for (load_lvl = 0; load_lvl < LOAD_LEVEL_NR; load_lvl++) {
146 unsigned int reg, mask, val, offset = 0;
147
148 if (load_lvl <= ARMADA_37XX_DVFS_LOAD_1)
149 reg = ARMADA_37XX_NB_L0L1;
150 else
151 reg = ARMADA_37XX_NB_L2L3;
152
153 if (load_lvl == ARMADA_37XX_DVFS_LOAD_0 ||
154 load_lvl == ARMADA_37XX_DVFS_LOAD_2)
155 offset += ARMADA_37XX_NB_CONFIG_SHIFT;
156
157
158 val = ARMADA_37XX_NB_CLK_SEL_TBG << ARMADA_37XX_NB_CLK_SEL_OFF;
159 mask = (ARMADA_37XX_NB_CLK_SEL_MASK
160 << ARMADA_37XX_NB_CLK_SEL_OFF);
161
162
163 val = cpu_tbg_sel << ARMADA_37XX_NB_TBG_SEL_OFF;
164 mask = (ARMADA_37XX_NB_TBG_SEL_MASK
165 << ARMADA_37XX_NB_TBG_SEL_OFF);
166
167
168
169
170
171 val |= divider[load_lvl] << ARMADA_37XX_NB_TBG_DIV_OFF;
172 mask |= (ARMADA_37XX_NB_TBG_DIV_MASK
173 << ARMADA_37XX_NB_TBG_DIV_OFF);
174
175
176 val |= load_lvl << ARMADA_37XX_NB_VDD_SEL_OFF;
177 mask |= (ARMADA_37XX_NB_VDD_SEL_MASK
178 << ARMADA_37XX_NB_VDD_SEL_OFF);
179
180 val <<= offset;
181 mask <<= offset;
182
183 regmap_update_bits(base, reg, mask, val);
184 }
185}
186
187
188
189
190
191static u32 armada_37xx_avs_val_match(int target_vm)
192{
193 u32 avs;
194
195
196 for (avs = 0; avs < ARRAY_SIZE(avs_map); avs++)
197 if (avs_map[avs] >= target_vm)
198 break;
199
200
201
202
203
204 if (avs == ARRAY_SIZE(avs_map))
205 avs = ARRAY_SIZE(avs_map) - 1;
206
207 return avs;
208}
209
210
211
212
213
214
215
216
217
218
219
220
221
222static void __init armada37xx_cpufreq_avs_configure(struct regmap *base,
223 struct armada_37xx_dvfs *dvfs)
224{
225 unsigned int target_vm;
226 int load_level = 0;
227 u32 l0_vdd_min;
228
229 if (base == NULL)
230 return;
231
232
233 regmap_read(base, ARMADA_37XX_AVS_CTL0, &l0_vdd_min);
234 l0_vdd_min = (l0_vdd_min >> ARMADA_37XX_AVS_LOW_VDD_LIMIT) &
235 ARMADA_37XX_AVS_VDD_MASK;
236 if (l0_vdd_min >= ARRAY_SIZE(avs_map)) {
237 pr_err("L0 VDD MIN %d is not correct.\n", l0_vdd_min);
238 return;
239 }
240 dvfs->avs[0] = l0_vdd_min;
241
242 if (avs_map[l0_vdd_min] <= MIN_VOLT_MV) {
243
244
245
246
247 u32 avs_min = armada_37xx_avs_val_match(MIN_VOLT_MV);
248
249 for (load_level = 1; load_level < LOAD_LEVEL_NR; load_level++)
250 dvfs->avs[load_level] = avs_min;
251
252
253
254
255
256
257 if (dvfs->cpu_freq_max >= 1000*1000*1000) {
258 if (dvfs->cpu_freq_max >= 1200*1000*1000)
259 avs_min = armada_37xx_avs_val_match(MIN_VOLT_MV_FOR_L1_1200MHZ);
260 else
261 avs_min = armada_37xx_avs_val_match(MIN_VOLT_MV_FOR_L1_1000MHZ);
262 dvfs->avs[0] = dvfs->avs[1] = avs_min;
263 }
264
265 return;
266 }
267
268
269
270
271
272
273 target_vm = avs_map[l0_vdd_min] - 100;
274 target_vm = target_vm > MIN_VOLT_MV ? target_vm : MIN_VOLT_MV;
275 dvfs->avs[1] = armada_37xx_avs_val_match(target_vm);
276
277
278
279
280
281 target_vm = avs_map[l0_vdd_min] - 150;
282 target_vm = target_vm > MIN_VOLT_MV ? target_vm : MIN_VOLT_MV;
283 dvfs->avs[2] = dvfs->avs[3] = armada_37xx_avs_val_match(target_vm);
284
285
286
287
288
289
290 if (dvfs->cpu_freq_max >= 1000*1000*1000) {
291 u32 avs_min_l1;
292
293 if (dvfs->cpu_freq_max >= 1200*1000*1000)
294 avs_min_l1 = armada_37xx_avs_val_match(MIN_VOLT_MV_FOR_L1_1200MHZ);
295 else
296 avs_min_l1 = armada_37xx_avs_val_match(MIN_VOLT_MV_FOR_L1_1000MHZ);
297
298 if (avs_min_l1 > dvfs->avs[0])
299 avs_min_l1 = dvfs->avs[0];
300
301 if (dvfs->avs[1] < avs_min_l1)
302 dvfs->avs[1] = avs_min_l1;
303 }
304}
305
306static void __init armada37xx_cpufreq_avs_setup(struct regmap *base,
307 struct armada_37xx_dvfs *dvfs)
308{
309 unsigned int avs_val = 0;
310 int load_level = 0;
311
312 if (base == NULL)
313 return;
314
315
316 regmap_update_bits(base, ARMADA_37XX_AVS_CTL0,
317 ARMADA_37XX_AVS_ENABLE, 0);
318
319
320
321 regmap_update_bits(base, ARMADA_37XX_AVS_CTL2,
322 ARMADA_37XX_AVS_LOW_VDD_EN,
323 ARMADA_37XX_AVS_LOW_VDD_EN);
324
325
326 for (load_level = 1; load_level < LOAD_LEVEL_NR; load_level++) {
327 avs_val = dvfs->avs[load_level];
328 regmap_update_bits(base, ARMADA_37XX_AVS_VSET(load_level-1),
329 ARMADA_37XX_AVS_VDD_MASK << ARMADA_37XX_AVS_HIGH_VDD_LIMIT |
330 ARMADA_37XX_AVS_VDD_MASK << ARMADA_37XX_AVS_LOW_VDD_LIMIT,
331 avs_val << ARMADA_37XX_AVS_HIGH_VDD_LIMIT |
332 avs_val << ARMADA_37XX_AVS_LOW_VDD_LIMIT);
333 }
334
335
336 regmap_update_bits(base, ARMADA_37XX_AVS_CTL0,
337 ARMADA_37XX_AVS_ENABLE,
338 ARMADA_37XX_AVS_ENABLE);
339
340}
341
342static void armada37xx_cpufreq_disable_dvfs(struct regmap *base)
343{
344 unsigned int reg = ARMADA_37XX_NB_DYN_MOD,
345 mask = ARMADA_37XX_NB_DFS_EN;
346
347 regmap_update_bits(base, reg, mask, 0);
348}
349
350static void __init armada37xx_cpufreq_enable_dvfs(struct regmap *base)
351{
352 unsigned int val, reg = ARMADA_37XX_NB_CPU_LOAD,
353 mask = ARMADA_37XX_NB_CPU_LOAD_MASK;
354
355
356 val = ARMADA_37XX_DVFS_LOAD_0;
357 regmap_update_bits(base, reg, mask, val);
358
359
360 reg = ARMADA_37XX_NB_DYN_MOD;
361 mask = ARMADA_37XX_NB_CLK_SEL_EN | ARMADA_37XX_NB_TBG_EN |
362 ARMADA_37XX_NB_DIV_EN | ARMADA_37XX_NB_VDD_EN |
363 ARMADA_37XX_NB_DFS_EN;
364
365 regmap_update_bits(base, reg, mask, mask);
366}
367
368static int armada37xx_cpufreq_suspend(struct cpufreq_policy *policy)
369{
370 struct armada37xx_cpufreq_state *state = armada37xx_cpufreq_state;
371
372 regmap_read(state->regmap, ARMADA_37XX_NB_L0L1, &state->nb_l0l1);
373 regmap_read(state->regmap, ARMADA_37XX_NB_L2L3, &state->nb_l2l3);
374 regmap_read(state->regmap, ARMADA_37XX_NB_CPU_LOAD,
375 &state->nb_cpu_load);
376 regmap_read(state->regmap, ARMADA_37XX_NB_DYN_MOD, &state->nb_dyn_mod);
377
378 return 0;
379}
380
381static int armada37xx_cpufreq_resume(struct cpufreq_policy *policy)
382{
383 struct armada37xx_cpufreq_state *state = armada37xx_cpufreq_state;
384
385
386 armada37xx_cpufreq_disable_dvfs(state->regmap);
387
388 regmap_write(state->regmap, ARMADA_37XX_NB_L0L1, state->nb_l0l1);
389 regmap_write(state->regmap, ARMADA_37XX_NB_L2L3, state->nb_l2l3);
390 regmap_write(state->regmap, ARMADA_37XX_NB_CPU_LOAD,
391 state->nb_cpu_load);
392
393
394
395
396
397
398 regmap_write(state->regmap, ARMADA_37XX_NB_DYN_MOD, state->nb_dyn_mod);
399
400 return 0;
401}
402
403static int __init armada37xx_cpufreq_driver_init(void)
404{
405 struct cpufreq_dt_platform_data pdata;
406 struct armada_37xx_dvfs *dvfs;
407 struct platform_device *pdev;
408 unsigned long freq;
409 unsigned int base_frequency;
410 struct regmap *nb_clk_base, *nb_pm_base, *avs_base;
411 struct device *cpu_dev;
412 int load_lvl, ret;
413 struct clk *clk, *parent;
414
415 nb_clk_base =
416 syscon_regmap_lookup_by_compatible("marvell,armada-3700-periph-clock-nb");
417 if (IS_ERR(nb_clk_base))
418 return -ENODEV;
419
420 nb_pm_base =
421 syscon_regmap_lookup_by_compatible("marvell,armada-3700-nb-pm");
422
423 if (IS_ERR(nb_pm_base))
424 return -ENODEV;
425
426 avs_base =
427 syscon_regmap_lookup_by_compatible("marvell,armada-3700-avs");
428
429
430 if (IS_ERR(avs_base)) {
431 pr_info("Syscon failed for Adapting Voltage Scaling: skip it\n");
432 avs_base = NULL;
433 }
434
435 armada37xx_cpufreq_disable_dvfs(nb_pm_base);
436
437
438
439
440
441
442 cpu_dev = get_cpu_device(0);
443 if (!cpu_dev) {
444 dev_err(cpu_dev, "Cannot get CPU\n");
445 return -ENODEV;
446 }
447
448 clk = clk_get(cpu_dev, 0);
449 if (IS_ERR(clk)) {
450 dev_err(cpu_dev, "Cannot get clock for CPU0\n");
451 return PTR_ERR(clk);
452 }
453
454 parent = clk_get_parent(clk);
455 if (IS_ERR(parent)) {
456 dev_err(cpu_dev, "Cannot get parent clock for CPU0\n");
457 clk_put(clk);
458 return PTR_ERR(parent);
459 }
460
461
462 base_frequency = clk_get_rate(parent);
463
464 if (!base_frequency) {
465 dev_err(cpu_dev, "Failed to get parent clock rate for CPU\n");
466 clk_put(clk);
467 return -EINVAL;
468 }
469
470 dvfs = armada_37xx_cpu_freq_info_get(base_frequency);
471 if (!dvfs) {
472 clk_put(clk);
473 return -EINVAL;
474 }
475
476 armada37xx_cpufreq_state = kmalloc(sizeof(*armada37xx_cpufreq_state),
477 GFP_KERNEL);
478 if (!armada37xx_cpufreq_state) {
479 clk_put(clk);
480 return -ENOMEM;
481 }
482
483 armada37xx_cpufreq_state->regmap = nb_pm_base;
484
485 armada37xx_cpufreq_avs_configure(avs_base, dvfs);
486 armada37xx_cpufreq_avs_setup(avs_base, dvfs);
487
488 armada37xx_cpufreq_dvfs_setup(nb_pm_base, nb_clk_base, dvfs->divider);
489 clk_put(clk);
490
491 for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR;
492 load_lvl++) {
493 unsigned long u_volt = avs_map[dvfs->avs[load_lvl]] * 1000;
494 freq = base_frequency / dvfs->divider[load_lvl];
495 ret = dev_pm_opp_add(cpu_dev, freq, u_volt);
496 if (ret)
497 goto remove_opp;
498
499
500 }
501
502
503 armada37xx_cpufreq_enable_dvfs(nb_pm_base);
504
505 memset(&pdata, 0, sizeof(pdata));
506 pdata.suspend = armada37xx_cpufreq_suspend;
507 pdata.resume = armada37xx_cpufreq_resume;
508
509 pdev = platform_device_register_data(NULL, "cpufreq-dt", -1, &pdata,
510 sizeof(pdata));
511 ret = PTR_ERR_OR_ZERO(pdev);
512 if (ret)
513 goto disable_dvfs;
514
515 armada37xx_cpufreq_state->cpu_dev = cpu_dev;
516 armada37xx_cpufreq_state->pdev = pdev;
517 platform_set_drvdata(pdev, dvfs);
518 return 0;
519
520disable_dvfs:
521 armada37xx_cpufreq_disable_dvfs(nb_pm_base);
522remove_opp:
523
524 while (load_lvl-- > ARMADA_37XX_DVFS_LOAD_0) {
525 freq = base_frequency / dvfs->divider[load_lvl];
526 dev_pm_opp_remove(cpu_dev, freq);
527 }
528
529 kfree(armada37xx_cpufreq_state);
530
531 return ret;
532}
533
534late_initcall(armada37xx_cpufreq_driver_init);
535
536static void __exit armada37xx_cpufreq_driver_exit(void)
537{
538 struct platform_device *pdev = armada37xx_cpufreq_state->pdev;
539 struct armada_37xx_dvfs *dvfs = platform_get_drvdata(pdev);
540 unsigned long freq;
541 int load_lvl;
542
543 platform_device_unregister(pdev);
544
545 armada37xx_cpufreq_disable_dvfs(armada37xx_cpufreq_state->regmap);
546
547 for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR; load_lvl++) {
548 freq = dvfs->cpu_freq_max / dvfs->divider[load_lvl];
549 dev_pm_opp_remove(armada37xx_cpufreq_state->cpu_dev, freq);
550 }
551
552 kfree(armada37xx_cpufreq_state);
553}
554module_exit(armada37xx_cpufreq_driver_exit);
555
556static const struct of_device_id __maybe_unused armada37xx_cpufreq_of_match[] = {
557 { .compatible = "marvell,armada-3700-nb-pm" },
558 { },
559};
560MODULE_DEVICE_TABLE(of, armada37xx_cpufreq_of_match);
561
562MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>");
563MODULE_DESCRIPTION("Armada 37xx cpufreq driver");
564MODULE_LICENSE("GPL");
565