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
 31struct psci_cpuidle_data {
 32	u32 *psci_states;
 33	struct device *dev;
 34};
 35
 36static DEFINE_PER_CPU_READ_MOSTLY(struct psci_cpuidle_data, psci_cpuidle_data);
 37static DEFINE_PER_CPU(u32, domain_state);
 38static bool psci_cpuidle_use_cpuhp;
 39
 40void psci_set_domain_state(u32 state)
 41{
 42	__this_cpu_write(domain_state, state);
 43}
 44
 45static inline u32 psci_get_domain_state(void)
 46{
 47	return __this_cpu_read(domain_state);
 48}
 49
 50static 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
 55static 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	RCU_NONIDLE(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	RCU_NONIDLE(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
 86static 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
 96static 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
109static 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
124static 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
132static const struct of_device_id psci_idle_state_match[] = {
133	{ .compatible = "arm,idle-state",
134	  .data = psci_enter_idle_state },
135	{ },
136};
137
138int 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
155static 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
178static 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
220static 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
244static 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
252static 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;
321deinit:
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 */
333static 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
348out_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
359static struct platform_driver psci_cpuidle_driver = {
360	.probe = psci_cpuidle_probe,
361	.driver = {
362		.name = "psci-cpuidle",
363	},
364};
365
366static 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}
383device_initcall(psci_idle_init);