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