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