]>
Commit | Line | Data |
---|---|---|
92ce45fb GC |
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 | ||
9c28d6df MR |
26 | #include "cpufreq-dt.h" |
27 | ||
22592df1 MB |
28 | /* Clk register set */ |
29 | #define ARMADA_37XX_CLK_TBG_SEL 0 | |
30 | #define ARMADA_37XX_CLK_TBG_SEL_CPU_OFF 22 | |
31 | ||
92ce45fb GC |
32 | /* Power management in North Bridge register set */ |
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 | ||
1c352823 GC |
58 | /* AVS register set */ |
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 | ||
92ce45fb GC |
68 | /* |
69 | * On Armada 37xx the Power management manages 4 level of CPU load, | |
70 | * each level can be associated with a CPU clock source, a CPU | |
71 | * divider, a VDD level, etc... | |
72 | */ | |
73 | #define LOAD_LEVEL_NR 4 | |
74 | ||
1c352823 | 75 | #define MIN_VOLT_MV 1000 |
d118ac20 PR |
76 | #define MIN_VOLT_MV_FOR_L1_1000MHZ 1108 |
77 | #define MIN_VOLT_MV_FOR_L1_1200MHZ 1155 | |
1c352823 GC |
78 | |
79 | /* AVS value for the corresponding voltage (in mV) */ | |
80 | static 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 | ||
9c28d6df | 88 | struct armada37xx_cpufreq_state { |
dbbd49ba PR |
89 | struct platform_device *pdev; |
90 | struct device *cpu_dev; | |
9c28d6df MR |
91 | struct regmap *regmap; |
92 | u32 nb_l0l1; | |
93 | u32 nb_l2l3; | |
94 | u32 nb_dyn_mod; | |
95 | u32 nb_cpu_load; | |
96 | }; | |
97 | ||
98 | static struct armada37xx_cpufreq_state *armada37xx_cpufreq_state; | |
99 | ||
92ce45fb GC |
100 | struct armada_37xx_dvfs { |
101 | u32 cpu_freq_max; | |
102 | u8 divider[LOAD_LEVEL_NR]; | |
1c352823 | 103 | u32 avs[LOAD_LEVEL_NR]; |
92ce45fb GC |
104 | }; |
105 | ||
106 | static struct armada_37xx_dvfs armada_37xx_dvfs[] = { | |
484f2b7c MB |
107 | /* |
108 | * The cpufreq scaling for 1.2 GHz variant of the SOC is currently | |
109 | * unstable because we do not know how to configure it properly. | |
110 | */ | |
111 | /* {.cpu_freq_max = 1200*1000*1000, .divider = {1, 2, 4, 6} }, */ | |
92ce45fb GC |
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 | ||
117 | static 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 | * Setup the four level managed by the hardware. Once the four level | |
132 | * will be configured then the DVFS will be enabled. | |
133 | */ | |
134 | static void __init armada37xx_cpufreq_dvfs_setup(struct regmap *base, | |
22592df1 | 135 | struct regmap *clk_base, u8 *divider) |
92ce45fb | 136 | { |
22592df1 | 137 | u32 cpu_tbg_sel; |
92ce45fb | 138 | int load_lvl; |
22592df1 MB |
139 | |
140 | /* Determine to which TBG clock is CPU connected */ | |
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; | |
92ce45fb GC |
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 | /* Set cpu clock source, for all the level we use TBG */ | |
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 | ||
22592df1 MB |
162 | /* Set TBG index, for all levels we use the same TBG */ |
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 | ||
92ce45fb GC |
167 | /* |
168 | * Set cpu divider based on the pre-computed array in | |
169 | * order to have balanced step. | |
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 | /* Set VDD divider which is actually the load level. */ | |
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 | } | |
92ce45fb GC |
185 | } |
186 | ||
1c352823 GC |
187 | /* |
188 | * Find out the armada 37x supported AVS value whose voltage value is | |
189 | * the round-up closest to the target voltage value. | |
190 | */ | |
191 | static u32 armada_37xx_avs_val_match(int target_vm) | |
192 | { | |
193 | u32 avs; | |
194 | ||
195 | /* Find out the round-up closest supported voltage value */ | |
196 | for (avs = 0; avs < ARRAY_SIZE(avs_map); avs++) | |
197 | if (avs_map[avs] >= target_vm) | |
198 | break; | |
199 | ||
200 | /* | |
201 | * If all supported voltages are smaller than target one, | |
202 | * choose the largest supported voltage | |
203 | */ | |
204 | if (avs == ARRAY_SIZE(avs_map)) | |
205 | avs = ARRAY_SIZE(avs_map) - 1; | |
206 | ||
207 | return avs; | |
208 | } | |
209 | ||
210 | /* | |
211 | * For Armada 37xx soc, L0(VSET0) VDD AVS value is set to SVC revision | |
212 | * value or a default value when SVC is not supported. | |
213 | * - L0 can be read out from the register of AVS_CTRL_0 and L0 voltage | |
214 | * can be got from the mapping table of avs_map. | |
215 | * - L1 voltage should be about 100mv smaller than L0 voltage | |
216 | * - L2 & L3 voltage should be about 150mv smaller than L0 voltage. | |
217 | * This function calculates L1 & L2 & L3 AVS values dynamically based | |
218 | * on L0 voltage and fill all AVS values to the AVS value table. | |
d118ac20 PR |
219 | * When base CPU frequency is 1000 or 1200 MHz then there is additional |
220 | * minimal avs value for load L1. | |
1c352823 GC |
221 | */ |
222 | static 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 | /* Get L0 VDD min value */ | |
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 | * If L0 voltage is smaller than 1000mv, then all VDD sets | |
245 | * use L0 voltage; | |
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 | ||
d118ac20 PR |
252 | /* |
253 | * Set the avs values for load L0 and L1 when base CPU frequency | |
254 | * is 1000/1200 MHz to its typical initial values according to | |
255 | * the Armada 3700 Hardware Specifications. | |
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 | ||
1c352823 GC |
265 | return; |
266 | } | |
267 | ||
268 | /* | |
269 | * L1 voltage is equal to L0 voltage - 100mv and it must be | |
270 | * larger than 1000mv | |
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 | * L2 & L3 voltage is equal to L0 voltage - 150mv and it must | |
279 | * be larger than 1000mv | |
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); | |
d118ac20 PR |
284 | |
285 | /* | |
286 | * Fix the avs value for load L1 when base CPU frequency is 1000/1200 MHz, | |
287 | * otherwise the CPU gets stuck when switching from load L1 to load L0. | |
288 | * Also ensure that avs value for load L1 is not higher than for L0. | |
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 | } | |
1c352823 GC |
304 | } |
305 | ||
306 | static void __init armada37xx_cpufreq_avs_setup(struct regmap *base, | |
307 | struct armada_37xx_dvfs *dvfs) | |
308 | { | |
036eb5c6 | 309 | unsigned int avs_val = 0; |
1c352823 GC |
310 | int load_level = 0; |
311 | ||
312 | if (base == NULL) | |
313 | return; | |
314 | ||
315 | /* Disable AVS before the configuration */ | |
316 | regmap_update_bits(base, ARMADA_37XX_AVS_CTL0, | |
317 | ARMADA_37XX_AVS_ENABLE, 0); | |
318 | ||
319 | ||
320 | /* Enable low voltage mode */ | |
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++) { | |
1c352823 GC |
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 | /* Enable AVS after the configuration */ | |
336 | regmap_update_bits(base, ARMADA_37XX_AVS_CTL0, | |
337 | ARMADA_37XX_AVS_ENABLE, | |
338 | ARMADA_37XX_AVS_ENABLE); | |
339 | ||
340 | } | |
341 | ||
9c28d6df | 342 | static void armada37xx_cpufreq_disable_dvfs(struct regmap *base) |
92ce45fb GC |
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 | ||
350 | static 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 | /* Start with the highest load (0) */ | |
356 | val = ARMADA_37XX_DVFS_LOAD_0; | |
357 | regmap_update_bits(base, reg, mask, val); | |
358 | ||
359 | /* Now enable DVFS for the CPUs */ | |
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 | ||
9c28d6df MR |
368 | static 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 | ||
381 | static int armada37xx_cpufreq_resume(struct cpufreq_policy *policy) | |
382 | { | |
383 | struct armada37xx_cpufreq_state *state = armada37xx_cpufreq_state; | |
384 | ||
385 | /* Ensure DVFS is disabled otherwise the following registers are RO */ | |
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 | * NB_DYN_MOD register is the one that actually enable back DVFS if it | |
395 | * was enabled before the suspend operation. This must be done last | |
396 | * otherwise other registers are not writable. | |
397 | */ | |
398 | regmap_write(state->regmap, ARMADA_37XX_NB_DYN_MOD, state->nb_dyn_mod); | |
399 | ||
400 | return 0; | |
401 | } | |
402 | ||
92ce45fb GC |
403 | static int __init armada37xx_cpufreq_driver_init(void) |
404 | { | |
9c28d6df | 405 | struct cpufreq_dt_platform_data pdata; |
92ce45fb GC |
406 | struct armada_37xx_dvfs *dvfs; |
407 | struct platform_device *pdev; | |
cfd84631 | 408 | unsigned long freq; |
5f23eb9d | 409 | unsigned int base_frequency; |
22592df1 | 410 | struct regmap *nb_clk_base, *nb_pm_base, *avs_base; |
92ce45fb GC |
411 | struct device *cpu_dev; |
412 | int load_lvl, ret; | |
8db82563 | 413 | struct clk *clk, *parent; |
92ce45fb | 414 | |
22592df1 MB |
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 | ||
92ce45fb GC |
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 | ||
1c352823 GC |
426 | avs_base = |
427 | syscon_regmap_lookup_by_compatible("marvell,armada-3700-avs"); | |
428 | ||
429 | /* if AVS is not present don't use it but still try to setup dvfs */ | |
430 | if (IS_ERR(avs_base)) { | |
431 | pr_info("Syscon failed for Adapting Voltage Scaling: skip it\n"); | |
432 | avs_base = NULL; | |
433 | } | |
92ce45fb GC |
434 | /* Before doing any configuration on the DVFS first, disable it */ |
435 | armada37xx_cpufreq_disable_dvfs(nb_pm_base); | |
436 | ||
437 | /* | |
438 | * On CPU 0 register the operating points supported (which are | |
439 | * the nominal CPU frequency and full integer divisions of | |
440 | * it). | |
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 | ||
8db82563 GC |
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 | /* Get parent CPU frequency */ | |
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 | ||
8bad3bf2 | 470 | dvfs = armada_37xx_cpu_freq_info_get(base_frequency); |
cfd84631 VK |
471 | if (!dvfs) { |
472 | clk_put(clk); | |
92ce45fb | 473 | return -EINVAL; |
cfd84631 | 474 | } |
92ce45fb | 475 | |
9c28d6df MR |
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 | ||
1c352823 GC |
485 | armada37xx_cpufreq_avs_configure(avs_base, dvfs); |
486 | armada37xx_cpufreq_avs_setup(avs_base, dvfs); | |
487 | ||
22592df1 | 488 | armada37xx_cpufreq_dvfs_setup(nb_pm_base, nb_clk_base, dvfs->divider); |
bbcc3285 | 489 | clk_put(clk); |
92ce45fb GC |
490 | |
491 | for (load_lvl = ARMADA_37XX_DVFS_LOAD_0; load_lvl < LOAD_LEVEL_NR; | |
492 | load_lvl++) { | |
1c352823 | 493 | unsigned long u_volt = avs_map[dvfs->avs[load_lvl]] * 1000; |
8db82563 | 494 | freq = base_frequency / dvfs->divider[load_lvl]; |
1c352823 | 495 | ret = dev_pm_opp_add(cpu_dev, freq, u_volt); |
cfd84631 VK |
496 | if (ret) |
497 | goto remove_opp; | |
1c352823 GC |
498 | |
499 | ||
92ce45fb GC |
500 | } |
501 | ||
502 | /* Now that everything is setup, enable the DVFS at hardware level */ | |
503 | armada37xx_cpufreq_enable_dvfs(nb_pm_base); | |
504 | ||
10470dec | 505 | memset(&pdata, 0, sizeof(pdata)); |
9c28d6df MR |
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)); | |
cfd84631 VK |
511 | ret = PTR_ERR_OR_ZERO(pdev); |
512 | if (ret) | |
513 | goto disable_dvfs; | |
514 | ||
dbbd49ba PR |
515 | armada37xx_cpufreq_state->cpu_dev = cpu_dev; |
516 | armada37xx_cpufreq_state->pdev = pdev; | |
517 | platform_set_drvdata(pdev, dvfs); | |
cfd84631 VK |
518 | return 0; |
519 | ||
520 | disable_dvfs: | |
521 | armada37xx_cpufreq_disable_dvfs(nb_pm_base); | |
522 | remove_opp: | |
523 | /* clean-up the already added opp before leaving */ | |
524 | while (load_lvl-- > ARMADA_37XX_DVFS_LOAD_0) { | |
92963903 | 525 | freq = base_frequency / dvfs->divider[load_lvl]; |
cfd84631 VK |
526 | dev_pm_opp_remove(cpu_dev, freq); |
527 | } | |
92ce45fb | 528 | |
9c28d6df MR |
529 | kfree(armada37xx_cpufreq_state); |
530 | ||
cfd84631 | 531 | return ret; |
92ce45fb GC |
532 | } |
533 | /* late_initcall, to guarantee the driver is loaded after A37xx clock driver */ | |
534 | late_initcall(armada37xx_cpufreq_driver_init); | |
535 | ||
dbbd49ba PR |
536 | static 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 | } | |
554 | module_exit(armada37xx_cpufreq_driver_exit); | |
555 | ||
c942d154 PR |
556 | static const struct of_device_id __maybe_unused armada37xx_cpufreq_of_match[] = { |
557 | { .compatible = "marvell,armada-3700-nb-pm" }, | |
558 | { }, | |
559 | }; | |
560 | MODULE_DEVICE_TABLE(of, armada37xx_cpufreq_of_match); | |
561 | ||
92ce45fb GC |
562 | MODULE_AUTHOR("Gregory CLEMENT <gregory.clement@free-electrons.com>"); |
563 | MODULE_DESCRIPTION("Armada 37xx cpufreq driver"); | |
564 | MODULE_LICENSE("GPL"); |