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
stm32_cpu_opp_count(void)61 unsigned int stm32_cpu_opp_count(void)
62 {
63 return cpu_opp.opp_count;
64 }
65
stm32_cpu_opp_sustained_level(void)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 */
stm32_cpu_opp_level(unsigned int opp)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
set_opp_clk_rate(unsigned int opp)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
set_opp_voltage(unsigned int opp)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 */
opp_voltage_is_supported(struct regulator * regul,uint32_t * volt_uv)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
set_clock_then_voltage(unsigned int opp)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
set_voltage_then_clock(unsigned int opp)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
stm32_cpu_opp_set_level(unsigned int level)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
stm32_cpu_opp_read_level(unsigned int * level)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
stm32_cpu_opp_is_supported(const void * fdt,int subnode)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
cpu_opp_pm(enum pm_op op,unsigned int pm_hint,const struct pm_callback_handle * hdl __unused)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
stm32_cpu_opp_get_dt_subnode(const void * fdt,int node)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
stm32_cpu_init(const void * fdt,int node,const void * compat_data __unused)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
stm32_cpu_initcall(void)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