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