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