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 static TEE_Result set_clock_then_voltage(unsigned int opp) 89 { 90 TEE_Result res = TEE_ERROR_GENERIC; 91 92 res = set_opp_clk_rate(opp); 93 if (res) 94 return res; 95 96 #ifdef CFG_STM32MP13 97 if (cpu_opp.dvfs[opp].volt_uv <= PWR_MPU_RAM_LOW_SPEED_THRESHOLD) 98 io_setbits32(stm32_pwr_base() + PWR_CR1_OFF, 99 PWR_CR1_MPU_RAM_LOW_SPEED); 100 #endif 101 102 res = set_opp_voltage(opp); 103 if (res) { 104 /* Restore previous OPP */ 105 #ifdef CFG_STM32MP13 106 if (cpu_opp.dvfs[cpu_opp.current_opp].volt_uv > 107 PWR_MPU_RAM_LOW_SPEED_THRESHOLD) 108 io_clrbits32(stm32_pwr_base() + PWR_CR1_OFF, 109 PWR_CR1_MPU_RAM_LOW_SPEED); 110 #endif 111 112 if (set_opp_clk_rate(cpu_opp.current_opp)) 113 panic(); 114 115 return res; 116 } 117 118 return TEE_SUCCESS; 119 } 120 121 static TEE_Result set_voltage_then_clock(unsigned int opp) 122 { 123 TEE_Result res = TEE_ERROR_GENERIC; 124 125 res = set_opp_voltage(opp); 126 if (res) 127 return res; 128 129 #ifdef CFG_STM32MP13 130 if (cpu_opp.dvfs[opp].volt_uv > PWR_MPU_RAM_LOW_SPEED_THRESHOLD) 131 io_clrbits32(stm32_pwr_base() + PWR_CR1_OFF, 132 PWR_CR1_MPU_RAM_LOW_SPEED); 133 #endif 134 135 res = set_opp_clk_rate(opp); 136 if (res) { 137 /* Restore previous OPP */ 138 #ifdef CFG_STM32MP13 139 if (cpu_opp.dvfs[cpu_opp.current_opp].volt_uv <= 140 PWR_MPU_RAM_LOW_SPEED_THRESHOLD) 141 io_setbits32(stm32_pwr_base() + PWR_CR1_OFF, 142 PWR_CR1_MPU_RAM_LOW_SPEED); 143 #endif 144 145 if (set_opp_voltage(cpu_opp.current_opp)) 146 panic(); 147 148 return res; 149 } 150 151 return TEE_SUCCESS; 152 } 153 154 TEE_Result stm32_cpu_opp_set_level(unsigned int level) 155 { 156 unsigned int current_level = 0; 157 TEE_Result res = TEE_ERROR_GENERIC; 158 unsigned int opp = 0; 159 160 DMSG("Set OPP to %ukHz", level); 161 162 if (!cpu_opp.opp_count) 163 return TEE_ERROR_GENERIC; 164 165 mutex_lock(&cpu_opp_mu); 166 167 current_level = stm32_cpu_opp_level(cpu_opp.current_opp); 168 169 if (level == current_level) { 170 mutex_unlock(&cpu_opp_mu); 171 return TEE_SUCCESS; 172 } 173 174 for (opp = 0; opp < cpu_opp.opp_count; opp++) 175 if (level == cpu_opp.dvfs[opp].freq_khz) 176 break; 177 178 if (opp == cpu_opp.opp_count) { 179 mutex_unlock(&cpu_opp_mu); 180 return TEE_ERROR_BAD_PARAMETERS; 181 } 182 183 if (level < current_level) 184 res = set_clock_then_voltage(opp); 185 else 186 res = set_voltage_then_clock(opp); 187 188 if (res) 189 EMSG("Failed to set OPP to %ukHz", level); 190 else 191 cpu_opp.current_opp = opp; 192 193 mutex_unlock(&cpu_opp_mu); 194 195 return res; 196 } 197 198 TEE_Result stm32_cpu_opp_read_level(unsigned int *level) 199 { 200 if (!cpu_opp.opp_count) { 201 EMSG("No CPU OPP defined"); 202 return TEE_ERROR_GENERIC; 203 } 204 205 *level = stm32_cpu_opp_level(cpu_opp.current_opp); 206 207 return TEE_SUCCESS; 208 } 209 210 static TEE_Result cpu_opp_pm(enum pm_op op, unsigned int pm_hint, 211 const struct pm_callback_handle *hdl __unused) 212 { 213 assert(op == PM_OP_SUSPEND || op == PM_OP_RESUME); 214 215 /* nothing to do if RCC clock tree is not lost */ 216 if (!PM_HINT_IS_STATE(pm_hint, CONTEXT)) 217 return TEE_SUCCESS; 218 219 if (op == PM_OP_RESUME) { 220 unsigned long clk_cpu = 0; 221 unsigned int opp = cpu_opp.current_opp; 222 223 DMSG("Resume to OPP %u", opp); 224 225 clk_cpu = clk_get_rate(cpu_opp.clock); 226 assert(clk_cpu); 227 if (cpu_opp.dvfs[opp].freq_khz * 1000 >= clk_cpu) 228 return set_voltage_then_clock(opp); 229 else 230 return set_clock_then_voltage(opp); 231 } 232 233 return TEE_SUCCESS; 234 } 235 DECLARE_KEEP_PAGER(cpu_opp_pm); 236 237 static TEE_Result stm32_cpu_opp_get_dt_subnode(const void *fdt, int node) 238 { 239 const fdt64_t *cuint64 = NULL; 240 const fdt32_t *cuint32 = NULL; 241 uint64_t freq_hz = 0; 242 uint64_t freq_khz = 0; 243 uint64_t freq_khz_opp_def = 0; 244 uint32_t volt_uv = 0; 245 unsigned long clk_cpu = 0; 246 unsigned int i = 0; 247 int subnode = 0; 248 TEE_Result res = TEE_ERROR_GENERIC; 249 250 cpu_opp.dvfs = calloc(CFG_STM32MP_OPP_COUNT, sizeof(*cpu_opp.dvfs)); 251 if (!cpu_opp.dvfs) 252 return TEE_ERROR_OUT_OF_MEMORY; 253 254 cpu_opp.opp_count = CFG_STM32MP_OPP_COUNT; 255 256 fdt_for_each_subnode(subnode, fdt, node) { 257 cuint64 = fdt_getprop(fdt, subnode, "opp-hz", NULL); 258 if (!cuint64) { 259 EMSG("Missing opp-hz in node %s", 260 fdt_get_name(fdt, subnode, NULL)); 261 res = TEE_ERROR_GENERIC; 262 goto err; 263 } 264 265 freq_hz = fdt64_to_cpu(*cuint64); 266 freq_khz = freq_hz / ULL(1000); 267 if (freq_khz > (uint64_t)UINT32_MAX) { 268 EMSG("Invalid opp-hz %"PRIu64" in node %s", 269 freq_khz, fdt_get_name(fdt, subnode, NULL)); 270 res = TEE_ERROR_GENERIC; 271 goto err; 272 } 273 274 cuint32 = fdt_getprop(fdt, subnode, "opp-microvolt", NULL); 275 if (!cuint32) { 276 EMSG("Missing opp-microvolt in node %s", 277 fdt_get_name(fdt, subnode, NULL)); 278 res = TEE_ERROR_GENERIC; 279 goto err; 280 } 281 282 volt_uv = fdt32_to_cpu(*cuint32); 283 284 if (i == cpu_opp.opp_count) { 285 EMSG("Too many OPP defined in node %s", 286 fdt_get_name(fdt, node, NULL)); 287 res = TEE_ERROR_GENERIC; 288 goto err; 289 } 290 291 cpu_opp.dvfs[i].freq_khz = freq_khz; 292 cpu_opp.dvfs[i].volt_uv = volt_uv; 293 294 DMSG("Found OPP %u (%"PRIu64"kHz/%"PRIu32"uV) from DT", 295 i, freq_khz, volt_uv); 296 297 /* Select the max "st,opp-default" node as current OPP */ 298 if (fdt_getprop(fdt, subnode, "st,opp-default", NULL) && 299 freq_khz > freq_khz_opp_def) { 300 cpu_opp.current_opp = i; 301 freq_khz_opp_def = freq_khz; 302 } 303 304 i++; 305 } 306 307 /* At least one OPP node shall have a "st,opp-default" property */ 308 if (freq_khz_opp_def == 0) { 309 EMSG("No default OPP found"); 310 res = TEE_ERROR_GENERIC; 311 goto err; 312 } 313 314 /* Use the highest default OPP as sustained freq */ 315 cpu_opp.sustained_freq_khz = freq_khz_opp_def; 316 317 /* Apply the current OPP */ 318 DMSG("Set OPP to %"PRIu64"kHz", freq_khz_opp_def); 319 clk_cpu = clk_get_rate(cpu_opp.clock); 320 assert(clk_cpu); 321 if (freq_khz_opp_def * ULL(1000) > clk_cpu) 322 res = set_voltage_then_clock(cpu_opp.current_opp); 323 else 324 res = set_clock_then_voltage(cpu_opp.current_opp); 325 326 if (res) { 327 EMSG("Failed to set default OPP %u", cpu_opp.current_opp); 328 goto err; 329 } 330 331 register_pm_driver_cb(cpu_opp_pm, NULL, "cpu-opp"); 332 333 return TEE_SUCCESS; 334 335 err: 336 free(cpu_opp.dvfs); 337 cpu_opp.dvfs = NULL; 338 cpu_opp.opp_count = 0; 339 340 return res; 341 } 342 343 static TEE_Result 344 stm32_cpu_init(const void *fdt, int node, const void *compat_data __unused) 345 { 346 const fdt32_t *cuint = NULL; 347 int opp_node = 0; 348 int len = 0; 349 uint32_t phandle = 0; 350 TEE_Result res = TEE_SUCCESS; 351 352 cuint = fdt_getprop(fdt, node, "operating-points-v2", &len); 353 if (!cuint || len != sizeof(uint32_t)) { 354 DMSG("No CPU operating points"); 355 return TEE_SUCCESS; 356 } 357 358 res = clk_dt_get_by_index(fdt, node, 0, &cpu_opp.clock); 359 if (res) 360 return res; 361 362 res = regulator_dt_get_supply(fdt, node, "cpu", &cpu_opp.regul); 363 if (res) 364 return res; 365 366 phandle = fdt32_to_cpu(*cuint); 367 opp_node = fdt_node_offset_by_phandle(fdt, phandle); 368 369 return stm32_cpu_opp_get_dt_subnode(fdt, opp_node); 370 } 371 372 static const struct dt_device_match stm32_cpu_match_table[] = { 373 { .compatible = "arm,cortex-a7" }, 374 { .compatible = "arm,cortex-a35" }, 375 { } 376 }; 377 378 DEFINE_DT_DRIVER(stm32_cpu_dt_driver) = { 379 .name = "stm32-cpu", 380 .match_table = stm32_cpu_match_table, 381 .probe = &stm32_cpu_init, 382 }; 383 384 static TEE_Result stm32_cpu_initcall(void) 385 { 386 TEE_Result res = TEE_ERROR_GENERIC; 387 const void *fdt = get_embedded_dt(); 388 int node = fdt_path_offset(fdt, "/cpus/cpu@0"); 389 390 if (node < 0) { 391 EMSG("cannot find /cpus/cpu@0 node"); 392 panic(); 393 } 394 395 res = dt_driver_maybe_add_probe_node(fdt, node); 396 if (res) { 397 EMSG("Failed on node %s with %#"PRIx32, 398 fdt_get_name(fdt, node, NULL), res); 399 panic(); 400 } 401 402 return TEE_SUCCESS; 403 } 404 405 early_init(stm32_cpu_initcall); 406 407