xref: /optee_os/core/drivers/stm32_cpu_opp.c (revision 5d5d7d0b1c038a6836be9f0b38585f5aa6a4dd01) 
1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3  * Copyright (c) 2022-2024, STMicroelectronics
4  */
5 
6 #include <assert.h>
7 #include <config.h>
8 #include <drivers/clk.h>
9 #include <drivers/clk_dt.h>
10 #include <drivers/regulator.h>
11 #include <drivers/stm32_cpu_opp.h>
12 #ifdef CFG_STM32MP13
13 #include <drivers/stm32mp1_pwr.h>
14 #endif
15 #include <initcall.h>
16 #include <io.h>
17 #include <keep.h>
18 #include <kernel/dt.h>
19 #include <kernel/dt_driver.h>
20 #include <kernel/mutex.h>
21 #include <kernel/panic.h>
22 #include <kernel/pm.h>
23 #include <libfdt.h>
24 #include <stm32_util.h>
25 #include <trace.h>
26 
27 /*
28  * struct cpu_dvfs - CPU DVFS registered operating point
29  * @freq_khz: CPU frequency in kilohertz (kHz)
30  * @volt_uv: CPU voltage level in microvolts (uV)
31  */
32 struct cpu_dvfs {
33 	unsigned int freq_khz;
34 	int volt_uv;
35 };
36 
37 /*
38  * struct cpu_opp - CPU operating point
39  *
40  * @current_opp: Index of current CPU operating point in @dvfs array
41  * @opp_count: Number of cells of @dvfs
42  * @clock: CPU clock handle
43  * @regul: CPU regulator supply handle
44  * @dvfs: Array of the supported CPU operating points
45  * @sustained_freq_khz: Max long term sustainable frequency in kHz
46  */
47 struct cpu_opp {
48 	unsigned int current_opp;
49 	unsigned int opp_count;
50 	struct clk *clock;
51 	struct regulator *regul;
52 	struct cpu_dvfs *dvfs;
53 	unsigned int sustained_freq_khz;
54 };
55 
56 static struct cpu_opp cpu_opp;
57 
58 /* Mutex for protecting CPU OPP changes */
59 static struct mutex cpu_opp_mu = MUTEX_INITIALIZER;
60 
61 unsigned int stm32_cpu_opp_count(void)
62 {
63 	return cpu_opp.opp_count;
64 }
65 
66 unsigned int stm32_cpu_opp_sustained_level(void)
67 {
68 	return cpu_opp.sustained_freq_khz;
69 }
70 
71 /* Perf level relates straight to CPU frequency in kHz */
72 unsigned int stm32_cpu_opp_level(unsigned int opp)
73 {
74 	assert(opp < cpu_opp.opp_count);
75 
76 	return cpu_opp.dvfs[opp].freq_khz;
77 }
78 
79 static TEE_Result set_opp_clk_rate(unsigned int opp)
80 {
81 	return clk_set_rate(cpu_opp.clock, cpu_opp.dvfs[opp].freq_khz * 1000);
82 }
83 
84 static TEE_Result set_opp_voltage(unsigned int opp)
85 {
86 	return regulator_set_voltage(cpu_opp.regul, cpu_opp.dvfs[opp].volt_uv);
87 }
88 
89 /*
90  * This function returns true if the given OPP voltage can be managed.
91  * If the exact voltage value is not supported by the regulator,
92  * the function may adjust the input parameter volt_uv to a higher
93  * supported value and still return true.
94  */
95 static bool opp_voltage_is_supported(struct regulator *regul, uint32_t *volt_uv)
96 {
97 	int target_volt_uv = 0;
98 	int new_volt_uv = 0;
99 	int min_uv = 0;
100 	int max_uv = 0;
101 	struct regulator_voltages_desc *desc = NULL;
102 	const int *levels = NULL;
103 	TEE_Result res = TEE_ERROR_GENERIC;
104 
105 	assert(*volt_uv < INT_MAX);
106 	target_volt_uv = (int)*volt_uv;
107 
108 	res = regulator_supported_voltages(regul, &desc, &levels);
109 	if (res) {
110 		regulator_get_range(regul, &min_uv, &max_uv);
111 		if (target_volt_uv > max_uv)
112 			return false;
113 		if (target_volt_uv < min_uv)
114 			*volt_uv = min_uv;
115 		return true;
116 	}
117 
118 	if (desc->type == VOLTAGE_TYPE_FULL_LIST) {
119 		unsigned int i = 0;
120 
121 		for (i = 0 ; i < desc->num_levels; i++) {
122 			if (levels[i] >= target_volt_uv) {
123 				new_volt_uv = levels[i];
124 				break;
125 			}
126 		}
127 		if (new_volt_uv == 0)
128 			return false;
129 
130 	} else if (desc->type == VOLTAGE_TYPE_INCREMENT) {
131 		int min = levels[0];
132 		int max = levels[1];
133 		int step = levels[2];
134 
135 		if (new_volt_uv > max)
136 			return false;
137 
138 		new_volt_uv = min +
139 			      DIV_ROUND_UP(target_volt_uv - min, step) * step;
140 	} else {
141 		return false;
142 	}
143 
144 	*volt_uv = new_volt_uv;
145 
146 	return true;
147 }
148 
149 static TEE_Result set_clock_then_voltage(unsigned int opp)
150 {
151 	TEE_Result res = TEE_ERROR_GENERIC;
152 
153 	res = set_opp_clk_rate(opp);
154 	if (res)
155 		return res;
156 
157 #ifdef CFG_STM32MP13
158 	if (cpu_opp.dvfs[opp].volt_uv <= PWR_MPU_RAM_LOW_SPEED_THRESHOLD)
159 		io_setbits32(stm32_pwr_base() + PWR_CR1_OFF,
160 			     PWR_CR1_MPU_RAM_LOW_SPEED);
161 #endif
162 
163 	res = set_opp_voltage(opp);
164 	if (res) {
165 		/* Restore previous OPP */
166 #ifdef CFG_STM32MP13
167 		if (cpu_opp.dvfs[cpu_opp.current_opp].volt_uv >
168 		    PWR_MPU_RAM_LOW_SPEED_THRESHOLD)
169 			io_clrbits32(stm32_pwr_base() + PWR_CR1_OFF,
170 				     PWR_CR1_MPU_RAM_LOW_SPEED);
171 #endif
172 
173 		if (set_opp_clk_rate(cpu_opp.current_opp))
174 			panic();
175 
176 		return res;
177 	}
178 
179 	return TEE_SUCCESS;
180 }
181 
182 static TEE_Result set_voltage_then_clock(unsigned int opp)
183 {
184 	TEE_Result res = TEE_ERROR_GENERIC;
185 
186 	res = set_opp_voltage(opp);
187 	if (res)
188 		return res;
189 
190 #ifdef CFG_STM32MP13
191 	if (cpu_opp.dvfs[opp].volt_uv > PWR_MPU_RAM_LOW_SPEED_THRESHOLD)
192 		io_clrbits32(stm32_pwr_base() + PWR_CR1_OFF,
193 			     PWR_CR1_MPU_RAM_LOW_SPEED);
194 #endif
195 
196 	res = set_opp_clk_rate(opp);
197 	if (res) {
198 		/* Restore previous OPP */
199 #ifdef CFG_STM32MP13
200 		if (cpu_opp.dvfs[cpu_opp.current_opp].volt_uv <=
201 		    PWR_MPU_RAM_LOW_SPEED_THRESHOLD)
202 			io_setbits32(stm32_pwr_base() + PWR_CR1_OFF,
203 				     PWR_CR1_MPU_RAM_LOW_SPEED);
204 #endif
205 
206 		if (set_opp_voltage(cpu_opp.current_opp))
207 			panic();
208 
209 		return res;
210 	}
211 
212 	return TEE_SUCCESS;
213 }
214 
215 TEE_Result stm32_cpu_opp_set_level(unsigned int level)
216 {
217 	unsigned int current_level = 0;
218 	TEE_Result res = TEE_ERROR_GENERIC;
219 	unsigned int opp = 0;
220 
221 	DMSG("Set OPP to %ukHz", level);
222 
223 	if (!cpu_opp.opp_count)
224 		return TEE_ERROR_GENERIC;
225 
226 	mutex_lock(&cpu_opp_mu);
227 
228 	current_level = stm32_cpu_opp_level(cpu_opp.current_opp);
229 
230 	if (level == current_level) {
231 		mutex_unlock(&cpu_opp_mu);
232 		return TEE_SUCCESS;
233 	}
234 
235 	for (opp = 0; opp < cpu_opp.opp_count; opp++)
236 		if (level == cpu_opp.dvfs[opp].freq_khz)
237 			break;
238 
239 	if (opp == cpu_opp.opp_count) {
240 		mutex_unlock(&cpu_opp_mu);
241 		return TEE_ERROR_BAD_PARAMETERS;
242 	}
243 
244 	if (level < current_level)
245 		res = set_clock_then_voltage(opp);
246 	else
247 		res = set_voltage_then_clock(opp);
248 
249 	if (res)
250 		EMSG("Failed to set OPP to %ukHz", level);
251 	else
252 		cpu_opp.current_opp = opp;
253 
254 	mutex_unlock(&cpu_opp_mu);
255 
256 	return res;
257 }
258 
259 TEE_Result stm32_cpu_opp_read_level(unsigned int *level)
260 {
261 	if (!cpu_opp.opp_count) {
262 		EMSG("No CPU OPP defined");
263 		return TEE_ERROR_GENERIC;
264 	}
265 
266 	*level = stm32_cpu_opp_level(cpu_opp.current_opp);
267 
268 	return TEE_SUCCESS;
269 }
270 
271 static TEE_Result stm32_cpu_opp_is_supported(const void *fdt, int subnode)
272 {
273 	const fdt32_t *cuint32 = NULL;
274 	uint32_t opp = 0;
275 
276 	cuint32 = fdt_getprop(fdt, subnode, "opp-supported-hw", NULL);
277 	if (!cuint32) {
278 		DMSG("Can't find property opp-supported-hw");
279 		return TEE_ERROR_GENERIC;
280 	}
281 
282 	opp = fdt32_to_cpu(*cuint32);
283 	if (!stm32mp_supports_cpu_opp(opp)) {
284 		DMSG("Not supported opp-supported-hw %#"PRIx32, opp);
285 		return TEE_ERROR_NOT_SUPPORTED;
286 	}
287 
288 	return TEE_SUCCESS;
289 }
290 
291 static TEE_Result cpu_opp_pm(enum pm_op op, unsigned int pm_hint,
292 			     const struct pm_callback_handle *hdl __unused)
293 {
294 	assert(op == PM_OP_SUSPEND || op == PM_OP_RESUME);
295 
296 	/* nothing to do if RCC clock tree is not lost */
297 	if (!PM_HINT_IS_STATE(pm_hint, CONTEXT))
298 		return TEE_SUCCESS;
299 
300 	if (op == PM_OP_RESUME) {
301 		unsigned long clk_cpu = 0;
302 		unsigned int opp = cpu_opp.current_opp;
303 
304 		DMSG("Resume to OPP %u", opp);
305 
306 		clk_cpu = clk_get_rate(cpu_opp.clock);
307 		assert(clk_cpu);
308 		if (cpu_opp.dvfs[opp].freq_khz * 1000 >= clk_cpu)
309 			return set_voltage_then_clock(opp);
310 		else
311 			return set_clock_then_voltage(opp);
312 	}
313 
314 	return TEE_SUCCESS;
315 }
316 DECLARE_KEEP_PAGER(cpu_opp_pm);
317 
318 static TEE_Result stm32_cpu_opp_get_dt_subnode(const void *fdt, int node)
319 {
320 	const fdt64_t *cuint64 = NULL;
321 	const fdt32_t *cuint32 = NULL;
322 	uint64_t freq_hz = 0;
323 	uint64_t freq_khz = 0;
324 	uint64_t freq_khz_opp_def = 0;
325 	uint32_t volt_uv = 0;
326 	unsigned long clk_cpu = 0;
327 	unsigned int i = 0;
328 	int subnode = 0;
329 	TEE_Result res = TEE_ERROR_GENERIC;
330 
331 	cpu_opp.dvfs = calloc(CFG_STM32MP_OPP_COUNT, sizeof(*cpu_opp.dvfs));
332 	if (!cpu_opp.dvfs)
333 		return TEE_ERROR_OUT_OF_MEMORY;
334 
335 	cpu_opp.opp_count = CFG_STM32MP_OPP_COUNT;
336 
337 	fdt_for_each_subnode(subnode, fdt, node) {
338 		cuint64 = fdt_getprop(fdt, subnode, "opp-hz", NULL);
339 		if (!cuint64) {
340 			EMSG("Missing opp-hz in node %s",
341 			     fdt_get_name(fdt, subnode, NULL));
342 			res = TEE_ERROR_GENERIC;
343 			goto err;
344 		}
345 
346 		freq_hz = fdt64_to_cpu(*cuint64);
347 		freq_khz = freq_hz / ULL(1000);
348 		if (freq_khz > (uint64_t)UINT32_MAX) {
349 			EMSG("Invalid opp-hz %"PRIu64" in node %s",
350 			     freq_khz, fdt_get_name(fdt, subnode, NULL));
351 			res = TEE_ERROR_GENERIC;
352 			goto err;
353 		}
354 
355 		cuint32 = fdt_getprop(fdt, subnode, "opp-microvolt", NULL);
356 		if (!cuint32) {
357 			EMSG("Missing opp-microvolt in node %s",
358 			     fdt_get_name(fdt, subnode, NULL));
359 			res = TEE_ERROR_GENERIC;
360 			goto err;
361 		}
362 
363 		volt_uv = fdt32_to_cpu(*cuint32);
364 
365 		/* skip OPP when the SOC does not support it */
366 		if (stm32_cpu_opp_is_supported(fdt, subnode) != TEE_SUCCESS) {
367 			DMSG("Skip SoC OPP %"PRIu64"kHz/%"PRIu32"uV",
368 			     freq_khz, volt_uv);
369 			cpu_opp.opp_count--;
370 			continue;
371 		}
372 
373 		/* skip OPP when voltage is not supported */
374 		if (!opp_voltage_is_supported(cpu_opp.regul, &volt_uv)) {
375 			DMSG("Skip volt OPP %"PRIu64"kHz/%"PRIu32"uV",
376 			     freq_khz, volt_uv);
377 			cpu_opp.opp_count--;
378 			continue;
379 		}
380 
381 		if (i == cpu_opp.opp_count) {
382 			EMSG("Too many OPP defined in node %s",
383 			     fdt_get_name(fdt, node, NULL));
384 			res = TEE_ERROR_GENERIC;
385 			goto err;
386 		}
387 
388 		cpu_opp.dvfs[i].freq_khz = freq_khz;
389 		cpu_opp.dvfs[i].volt_uv = volt_uv;
390 
391 		DMSG("Found OPP %u (%"PRIu64"kHz/%"PRIu32"uV) from DT",
392 		     i, freq_khz, volt_uv);
393 
394 		/* Select the max "st,opp-default" node as current OPP */
395 		if (fdt_getprop(fdt, subnode, "st,opp-default", NULL) &&
396 		    freq_khz > freq_khz_opp_def) {
397 			cpu_opp.current_opp = i;
398 			freq_khz_opp_def = freq_khz;
399 		}
400 
401 		i++;
402 	}
403 
404 	/* At least one OPP node shall have a "st,opp-default" property */
405 	if (freq_khz_opp_def == 0) {
406 		EMSG("No default OPP found");
407 		res = TEE_ERROR_GENERIC;
408 		goto err;
409 	}
410 
411 	/* Use the highest default OPP as sustained freq */
412 	cpu_opp.sustained_freq_khz = freq_khz_opp_def;
413 
414 	/* Apply the current OPP */
415 	DMSG("Set OPP to %"PRIu64"kHz", freq_khz_opp_def);
416 	clk_cpu = clk_get_rate(cpu_opp.clock);
417 	assert(clk_cpu);
418 	if (freq_khz_opp_def * ULL(1000) > clk_cpu)
419 		res = set_voltage_then_clock(cpu_opp.current_opp);
420 	else
421 		res = set_clock_then_voltage(cpu_opp.current_opp);
422 
423 	if (res) {
424 		EMSG("Failed to set default OPP %u", cpu_opp.current_opp);
425 		goto err;
426 	}
427 
428 	register_pm_driver_cb(cpu_opp_pm, NULL, "cpu-opp");
429 
430 	return TEE_SUCCESS;
431 
432 err:
433 	free(cpu_opp.dvfs);
434 	cpu_opp.dvfs = NULL;
435 	cpu_opp.opp_count = 0;
436 
437 	return res;
438 }
439 
440 static TEE_Result
441 stm32_cpu_init(const void *fdt, int node, const void *compat_data __unused)
442 {
443 	const fdt32_t *cuint = NULL;
444 	int opp_node = 0;
445 	int len = 0;
446 	uint32_t phandle = 0;
447 	TEE_Result res = TEE_SUCCESS;
448 
449 	cuint = fdt_getprop(fdt, node, "operating-points-v2", &len);
450 	if (!cuint || len != sizeof(uint32_t)) {
451 		DMSG("No CPU operating points");
452 		return TEE_SUCCESS;
453 	}
454 
455 	res = clk_dt_get_by_index(fdt, node, 0, &cpu_opp.clock);
456 	if (res)
457 		return res;
458 
459 	res = regulator_dt_get_supply(fdt, node, "cpu", &cpu_opp.regul);
460 	if (res)
461 		return res;
462 
463 	phandle = fdt32_to_cpu(*cuint);
464 	opp_node = fdt_node_offset_by_phandle(fdt, phandle);
465 
466 	return stm32_cpu_opp_get_dt_subnode(fdt, opp_node);
467 }
468 
469 static const struct dt_device_match stm32_cpu_match_table[] = {
470 	{ .compatible = "arm,cortex-a7" },
471 	{ .compatible = "arm,cortex-a35" },
472 	{ }
473 };
474 
475 DEFINE_DT_DRIVER(stm32_cpu_dt_driver) = {
476 	.name = "stm32-cpu",
477 	.match_table = stm32_cpu_match_table,
478 	.probe = &stm32_cpu_init,
479 };
480 
481 static TEE_Result stm32_cpu_initcall(void)
482 {
483 	TEE_Result res = TEE_ERROR_GENERIC;
484 	const void *fdt = get_embedded_dt();
485 	int node = fdt_path_offset(fdt, "/cpus/cpu@0");
486 
487 	if (node < 0) {
488 		EMSG("cannot find /cpus/cpu@0 node");
489 		panic();
490 	}
491 
492 	res = dt_driver_maybe_add_probe_node(fdt, node);
493 	if (res) {
494 		EMSG("Failed on node %s with %#"PRIx32,
495 		     fdt_get_name(fdt, node, NULL), res);
496 		panic();
497 	}
498 
499 	return TEE_SUCCESS;
500 }
501 
502 early_init(stm32_cpu_initcall);
503 
504