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