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