1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * PSCI CPU idle driver. 4 * 5 * Copyright (C) 2019 ARM Ltd. 6 * Author: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com> 7 */ 8 9 #define pr_fmt(fmt) "CPUidle PSCI: " fmt 10 11 #include <linux/cpuhotplug.h> 12 #include <linux/cpu_cooling.h> 13 #include <linux/cpuidle.h> 14 #include <linux/cpumask.h> 15 #include <linux/cpu_pm.h> 16 #include <linux/kernel.h> 17 #include <linux/module.h> 18 #include <linux/of.h> 19 #include <linux/of_device.h> 20 #include <linux/platform_device.h> 21 #include <linux/psci.h> 22 #include <linux/pm_domain.h> 23 #include <linux/pm_runtime.h> 24 #include <linux/slab.h> 25 #include <linux/string.h> 26 #include <linux/syscore_ops.h> 27 28 #include <asm/cpuidle.h> 29 30 #include "cpuidle-psci.h" 31 #include "dt_idle_states.h" 32 33 struct psci_cpuidle_data { 34 u32 *psci_states; 35 struct device *dev; 36 }; 37 38 static DEFINE_PER_CPU_READ_MOSTLY(struct psci_cpuidle_data, psci_cpuidle_data); 39 static DEFINE_PER_CPU(u32, domain_state); 40 static bool psci_cpuidle_use_cpuhp; 41 42 void psci_set_domain_state(u32 state) 43 { 44 __this_cpu_write(domain_state, state); 45 } 46 47 static inline u32 psci_get_domain_state(void) 48 { 49 return __this_cpu_read(domain_state); 50 } 51 52 static inline int psci_enter_state(int idx, u32 state) 53 { 54 return CPU_PM_CPU_IDLE_ENTER_PARAM(psci_cpu_suspend_enter, idx, state); 55 } 56 57 static int __psci_enter_domain_idle_state(struct cpuidle_device *dev, 58 struct cpuidle_driver *drv, int idx, 59 bool s2idle) 60 { 61 struct psci_cpuidle_data *data = this_cpu_ptr(&psci_cpuidle_data); 62 u32 *states = data->psci_states; 63 struct device *pd_dev = data->dev; 64 u32 state; 65 int ret; 66 67 ret = cpu_pm_enter(); 68 if (ret) 69 return -1; 70 71 /* Do runtime PM to manage a hierarchical CPU toplogy. */ 72 ct_irq_enter_irqson(); 73 if (s2idle) 74 dev_pm_genpd_suspend(pd_dev); 75 else 76 pm_runtime_put_sync_suspend(pd_dev); 77 ct_irq_exit_irqson(); 78 79 state = psci_get_domain_state(); 80 if (!state) 81 state = states[idx]; 82 83 ret = psci_cpu_suspend_enter(state) ? -1 : idx; 84 85 ct_irq_enter_irqson(); 86 if (s2idle) 87 dev_pm_genpd_resume(pd_dev); 88 else 89 pm_runtime_get_sync(pd_dev); 90 ct_irq_exit_irqson(); 91 92 cpu_pm_exit(); 93 94 /* Clear the domain state to start fresh when back from idle. */ 95 psci_set_domain_state(0); 96 return ret; 97 } 98 99 static int psci_enter_domain_idle_state(struct cpuidle_device *dev, 100 struct cpuidle_driver *drv, int idx) 101 { 102 return __psci_enter_domain_idle_state(dev, drv, idx, false); 103 } 104 105 static int psci_enter_s2idle_domain_idle_state(struct cpuidle_device *dev, 106 struct cpuidle_driver *drv, 107 int idx) 108 { 109 return __psci_enter_domain_idle_state(dev, drv, idx, true); 110 } 111 112 static int psci_idle_cpuhp_up(unsigned int cpu) 113 { 114 struct device *pd_dev = __this_cpu_read(psci_cpuidle_data.dev); 115 116 if (pd_dev) 117 pm_runtime_get_sync(pd_dev); 118 119 return 0; 120 } 121 122 static int psci_idle_cpuhp_down(unsigned int cpu) 123 { 124 struct device *pd_dev = __this_cpu_read(psci_cpuidle_data.dev); 125 126 if (pd_dev) { 127 pm_runtime_put_sync(pd_dev); 128 /* Clear domain state to start fresh at next online. */ 129 psci_set_domain_state(0); 130 } 131 132 return 0; 133 } 134 135 static void psci_idle_syscore_switch(bool suspend) 136 { 137 bool cleared = false; 138 struct device *dev; 139 int cpu; 140 141 for_each_possible_cpu(cpu) { 142 dev = per_cpu_ptr(&psci_cpuidle_data, cpu)->dev; 143 144 if (dev && suspend) { 145 dev_pm_genpd_suspend(dev); 146 } else if (dev) { 147 dev_pm_genpd_resume(dev); 148 149 /* Account for userspace having offlined a CPU. */ 150 if (pm_runtime_status_suspended(dev)) 151 pm_runtime_set_active(dev); 152 153 /* Clear domain state to re-start fresh. */ 154 if (!cleared) { 155 psci_set_domain_state(0); 156 cleared = true; 157 } 158 } 159 } 160 } 161 162 static int psci_idle_syscore_suspend(void) 163 { 164 psci_idle_syscore_switch(true); 165 return 0; 166 } 167 168 static void psci_idle_syscore_resume(void) 169 { 170 psci_idle_syscore_switch(false); 171 } 172 173 static struct syscore_ops psci_idle_syscore_ops = { 174 .suspend = psci_idle_syscore_suspend, 175 .resume = psci_idle_syscore_resume, 176 }; 177 178 static void psci_idle_init_cpuhp(void) 179 { 180 int err; 181 182 if (!psci_cpuidle_use_cpuhp) 183 return; 184 185 register_syscore_ops(&psci_idle_syscore_ops); 186 187 err = cpuhp_setup_state_nocalls(CPUHP_AP_CPU_PM_STARTING, 188 "cpuidle/psci:online", 189 psci_idle_cpuhp_up, 190 psci_idle_cpuhp_down); 191 if (err) 192 pr_warn("Failed %d while setup cpuhp state\n", err); 193 } 194 195 static int psci_enter_idle_state(struct cpuidle_device *dev, 196 struct cpuidle_driver *drv, int idx) 197 { 198 u32 *state = __this_cpu_read(psci_cpuidle_data.psci_states); 199 200 return psci_enter_state(idx, state[idx]); 201 } 202 203 static const struct of_device_id psci_idle_state_match[] = { 204 { .compatible = "arm,idle-state", 205 .data = psci_enter_idle_state }, 206 { }, 207 }; 208 209 int psci_dt_parse_state_node(struct device_node *np, u32 *state) 210 { 211 int err = of_property_read_u32(np, "arm,psci-suspend-param", state); 212 213 if (err) { 214 pr_warn("%pOF missing arm,psci-suspend-param property\n", np); 215 return err; 216 } 217 218 if (!psci_power_state_is_valid(*state)) { 219 pr_warn("Invalid PSCI power state %#x\n", *state); 220 return -EINVAL; 221 } 222 223 return 0; 224 } 225 226 static int psci_dt_cpu_init_topology(struct cpuidle_driver *drv, 227 struct psci_cpuidle_data *data, 228 unsigned int state_count, int cpu) 229 { 230 /* Currently limit the hierarchical topology to be used in OSI mode. */ 231 if (!psci_has_osi_support()) 232 return 0; 233 234 data->dev = psci_dt_attach_cpu(cpu); 235 if (IS_ERR_OR_NULL(data->dev)) 236 return PTR_ERR_OR_ZERO(data->dev); 237 238 /* 239 * Using the deepest state for the CPU to trigger a potential selection 240 * of a shared state for the domain, assumes the domain states are all 241 * deeper states. 242 */ 243 drv->states[state_count - 1].enter = psci_enter_domain_idle_state; 244 drv->states[state_count - 1].enter_s2idle = psci_enter_s2idle_domain_idle_state; 245 psci_cpuidle_use_cpuhp = true; 246 247 return 0; 248 } 249 250 static int psci_dt_cpu_init_idle(struct device *dev, struct cpuidle_driver *drv, 251 struct device_node *cpu_node, 252 unsigned int state_count, int cpu) 253 { 254 int i, ret = 0; 255 u32 *psci_states; 256 struct device_node *state_node; 257 struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu); 258 259 state_count++; /* Add WFI state too */ 260 psci_states = devm_kcalloc(dev, state_count, sizeof(*psci_states), 261 GFP_KERNEL); 262 if (!psci_states) 263 return -ENOMEM; 264 265 for (i = 1; i < state_count; i++) { 266 state_node = of_get_cpu_state_node(cpu_node, i - 1); 267 if (!state_node) 268 break; 269 270 ret = psci_dt_parse_state_node(state_node, &psci_states[i]); 271 of_node_put(state_node); 272 273 if (ret) 274 return ret; 275 276 pr_debug("psci-power-state %#x index %d\n", psci_states[i], i); 277 } 278 279 if (i != state_count) 280 return -ENODEV; 281 282 /* Initialize optional data, used for the hierarchical topology. */ 283 ret = psci_dt_cpu_init_topology(drv, data, state_count, cpu); 284 if (ret < 0) 285 return ret; 286 287 /* Idle states parsed correctly, store them in the per-cpu struct. */ 288 data->psci_states = psci_states; 289 return 0; 290 } 291 292 static int psci_cpu_init_idle(struct device *dev, struct cpuidle_driver *drv, 293 unsigned int cpu, unsigned int state_count) 294 { 295 struct device_node *cpu_node; 296 int ret; 297 298 /* 299 * If the PSCI cpu_suspend function hook has not been initialized 300 * idle states must not be enabled, so bail out 301 */ 302 if (!psci_ops.cpu_suspend) 303 return -EOPNOTSUPP; 304 305 cpu_node = of_cpu_device_node_get(cpu); 306 if (!cpu_node) 307 return -ENODEV; 308 309 ret = psci_dt_cpu_init_idle(dev, drv, cpu_node, state_count, cpu); 310 311 of_node_put(cpu_node); 312 313 return ret; 314 } 315 316 static void psci_cpu_deinit_idle(int cpu) 317 { 318 struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu); 319 320 psci_dt_detach_cpu(data->dev); 321 psci_cpuidle_use_cpuhp = false; 322 } 323 324 static int psci_idle_init_cpu(struct device *dev, int cpu) 325 { 326 struct cpuidle_driver *drv; 327 struct device_node *cpu_node; 328 const char *enable_method; 329 int ret = 0; 330 331 cpu_node = of_cpu_device_node_get(cpu); 332 if (!cpu_node) 333 return -ENODEV; 334 335 /* 336 * Check whether the enable-method for the cpu is PSCI, fail 337 * if it is not. 338 */ 339 enable_method = of_get_property(cpu_node, "enable-method", NULL); 340 if (!enable_method || (strcmp(enable_method, "psci"))) 341 ret = -ENODEV; 342 343 of_node_put(cpu_node); 344 if (ret) 345 return ret; 346 347 drv = devm_kzalloc(dev, sizeof(*drv), GFP_KERNEL); 348 if (!drv) 349 return -ENOMEM; 350 351 drv->name = "psci_idle"; 352 drv->owner = THIS_MODULE; 353 drv->cpumask = (struct cpumask *)cpumask_of(cpu); 354 355 /* 356 * PSCI idle states relies on architectural WFI to be represented as 357 * state index 0. 358 */ 359 drv->states[0].enter = psci_enter_idle_state; 360 drv->states[0].exit_latency = 1; 361 drv->states[0].target_residency = 1; 362 drv->states[0].power_usage = UINT_MAX; 363 strcpy(drv->states[0].name, "WFI"); 364 strcpy(drv->states[0].desc, "ARM WFI"); 365 366 /* 367 * If no DT idle states are detected (ret == 0) let the driver 368 * initialization fail accordingly since there is no reason to 369 * initialize the idle driver if only wfi is supported, the 370 * default archictectural back-end already executes wfi 371 * on idle entry. 372 */ 373 ret = dt_init_idle_driver(drv, psci_idle_state_match, 1); 374 if (ret <= 0) 375 return ret ? : -ENODEV; 376 377 /* 378 * Initialize PSCI idle states. 379 */ 380 ret = psci_cpu_init_idle(dev, drv, cpu, ret); 381 if (ret) { 382 pr_err("CPU %d failed to PSCI idle\n", cpu); 383 return ret; 384 } 385 386 ret = cpuidle_register(drv, NULL); 387 if (ret) 388 goto deinit; 389 390 cpuidle_cooling_register(drv); 391 392 return 0; 393 deinit: 394 psci_cpu_deinit_idle(cpu); 395 return ret; 396 } 397 398 /* 399 * psci_idle_probe - Initializes PSCI cpuidle driver 400 * 401 * Initializes PSCI cpuidle driver for all CPUs, if any CPU fails 402 * to register cpuidle driver then rollback to cancel all CPUs 403 * registration. 404 */ 405 static int psci_cpuidle_probe(struct platform_device *pdev) 406 { 407 int cpu, ret; 408 struct cpuidle_driver *drv; 409 struct cpuidle_device *dev; 410 411 for_each_possible_cpu(cpu) { 412 ret = psci_idle_init_cpu(&pdev->dev, cpu); 413 if (ret) 414 goto out_fail; 415 } 416 417 psci_idle_init_cpuhp(); 418 return 0; 419 420 out_fail: 421 while (--cpu >= 0) { 422 dev = per_cpu(cpuidle_devices, cpu); 423 drv = cpuidle_get_cpu_driver(dev); 424 cpuidle_unregister(drv); 425 psci_cpu_deinit_idle(cpu); 426 } 427 428 return ret; 429 } 430 431 static struct platform_driver psci_cpuidle_driver = { 432 .probe = psci_cpuidle_probe, 433 .driver = { 434 .name = "psci-cpuidle", 435 }, 436 }; 437 438 static int __init psci_idle_init(void) 439 { 440 struct platform_device *pdev; 441 int ret; 442 443 ret = platform_driver_register(&psci_cpuidle_driver); 444 if (ret) 445 return ret; 446 447 pdev = platform_device_register_simple("psci-cpuidle", -1, NULL, 0); 448 if (IS_ERR(pdev)) { 449 platform_driver_unregister(&psci_cpuidle_driver); 450 return PTR_ERR(pdev); 451 } 452 453 return 0; 454 } 455 device_initcall(psci_idle_init); 456