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 
armada_37xx_cpu_freq_info_get(u32 freq)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  */
armada37xx_cpufreq_dvfs_setup(struct regmap * base,struct regmap * clk_base,u8 * divider)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(clk_base, ARMADA_37XX_CLK_TBG_SEL, &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(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  */
armada_37xx_avs_val_match(int target_vm)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  */
armada37xx_cpufreq_avs_configure(struct regmap * base,struct armada_37xx_dvfs * dvfs)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(base, ARMADA_37XX_AVS_CTL0, &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 
armada37xx_cpufreq_avs_setup(struct regmap * base,struct armada_37xx_dvfs * dvfs)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(base, ARMADA_37XX_AVS_CTL0,
315 			   ARMADA_37XX_AVS_ENABLE, 0);
316 
317 
318 	/* Enable low voltage mode */
319 	regmap_update_bits(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(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 		    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(base, ARMADA_37XX_AVS_CTL0,
335 			   ARMADA_37XX_AVS_ENABLE,
336 			   ARMADA_37XX_AVS_ENABLE);
337 
338 }
339 
armada37xx_cpufreq_disable_dvfs(struct regmap * base)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(base, reg, mask, 0);
346 }
347 
armada37xx_cpufreq_enable_dvfs(struct regmap * base)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(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(base, reg, mask, mask);
364 }
365 
armada37xx_cpufreq_suspend(struct cpufreq_policy * policy)366 static int armada37xx_cpufreq_suspend(struct cpufreq_policy *policy)
367 {
368 	struct armada37xx_cpufreq_state *state = armada37xx_cpufreq_state;
369 
370 	regmap_read(state->regmap, ARMADA_37XX_NB_L0L1, &state->nb_l0l1);
371 	regmap_read(state->regmap, ARMADA_37XX_NB_L2L3, &state->nb_l2l3);
372 	regmap_read(state->regmap, ARMADA_37XX_NB_CPU_LOAD,
373 		    &state->nb_cpu_load);
374 	regmap_read(state->regmap, ARMADA_37XX_NB_DYN_MOD, &state->nb_dyn_mod);
375 
376 	return 0;
377 }
378 
armada37xx_cpufreq_resume(struct cpufreq_policy * policy)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(state->regmap);
385 
386 	regmap_write(state->regmap, ARMADA_37XX_NB_L0L1, state->nb_l0l1);
387 	regmap_write(state->regmap, ARMADA_37XX_NB_L2L3, state->nb_l2l3);
388 	regmap_write(state->regmap, ARMADA_37XX_NB_CPU_LOAD,
389 		     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(state->regmap, ARMADA_37XX_NB_DYN_MOD, state->nb_dyn_mod);
397 
398 	return 0;
399 }
400 
armada37xx_cpufreq_driver_init(void)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("marvell,armada-3700-periph-clock-nb");
415 	if (IS_ERR(nb_clk_base))
416 		return -ENODEV;
417 
418 	nb_pm_base =
419 		syscon_regmap_lookup_by_compatible("marvell,armada-3700-nb-pm");
420 
421 	if (IS_ERR(nb_pm_base))
422 		return -ENODEV;
423 
424 	avs_base =
425 		syscon_regmap_lookup_by_compatible("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(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(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(0);
441 	if (!cpu_dev) {
442 		dev_err(cpu_dev, "Cannot get CPU\n");
443 		return -ENODEV;
444 	}
445 
446 	clk = clk_get(cpu_dev, NULL);
447 	if (IS_ERR(clk)) {
448 		dev_err(cpu_dev, "Cannot get clock for CPU0\n");
449 		return PTR_ERR(clk);
450 	}
451 
452 	parent = clk_get_parent(clk);
453 	if (IS_ERR(parent)) {
454 		dev_err(cpu_dev, "Cannot get parent clock for CPU0\n");
455 		clk_put(clk);
456 		return PTR_ERR(parent);
457 	}
458 
459 	/* Get parent CPU frequency */
460 	base_frequency =  clk_get_rate(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(base_frequency);
469 	if (!dvfs) {
470 		clk_put(clk);
471 		return -EINVAL;
472 	}
473 
474 	armada37xx_cpufreq_state = kmalloc(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(avs_base, dvfs);
484 	armada37xx_cpufreq_avs_setup(avs_base, dvfs);
485 
486 	armada37xx_cpufreq_dvfs_setup(nb_pm_base, nb_clk_base, 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(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(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, "cpufreq-dt", -1, &pdata,
508 					     sizeof(pdata));
509 	ret = PTR_ERR_OR_ZERO(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, dvfs);
516 	return 0;
517 
518 disable_dvfs:
519 	armada37xx_cpufreq_disable_dvfs(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(cpu_dev, freq);
525 	}
526 
527 	kfree(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 
armada37xx_cpufreq_driver_exit(void)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(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(armada37xx_cpufreq_state->cpu_dev, freq);
548 	}
549 
550 	kfree(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