[linux.git] / drivers / cpuidle / cpuidle-psci.c

// SPDX-License-Identifier: GPL-2.0-only
/*
 * PSCI CPU idle driver.
 *
 * Copyright (C) 2019 ARM Ltd.
 * Author: Lorenzo Pieralisi <[email protected]>
 */

#define pr_fmt(fmt) "CPUidle PSCI: " fmt

#include <linux/cpuhotplug.h>
#include <linux/cpu_cooling.h>
#include <linux/cpuidle.h>
#include <linux/cpumask.h>
#include <linux/cpu_pm.h>
#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/of.h>
#include <linux/platform_device.h>
#include <linux/psci.h>
#include <linux/pm_domain.h>
#include <linux/pm_runtime.h>
#include <linux/slab.h>
#include <linux/string.h>
#include <linux/syscore_ops.h>

#include <asm/cpuidle.h>

#include "cpuidle-psci.h"
#include "dt_idle_states.h"
#include "dt_idle_genpd.h"

struct psci_cpuidle_data {
	u32 *psci_states;
	struct device *dev;
};

static DEFINE_PER_CPU_READ_MOSTLY(struct psci_cpuidle_data, psci_cpuidle_data);
static DEFINE_PER_CPU(u32, domain_state);
static bool psci_cpuidle_use_cpuhp;

void psci_set_domain_state(u32 state)
{
	__this_cpu_write(domain_state, state);
}

static inline u32 psci_get_domain_state(void)
{
	return __this_cpu_read(domain_state);
}

static __cpuidle int __psci_enter_domain_idle_state(struct cpuidle_device *dev,
						    struct cpuidle_driver *drv, int idx,
						    bool s2idle)
{
	struct psci_cpuidle_data *data = this_cpu_ptr(&psci_cpuidle_data);
	u32 *states = data->psci_states;
	struct device *pd_dev = data->dev;
	u32 state;
	int ret;

	ret = cpu_pm_enter();
	if (ret)
		return -1;

	/* Do runtime PM to manage a hierarchical CPU toplogy. */
	if (s2idle)
		dev_pm_genpd_suspend(pd_dev);
	else
		pm_runtime_put_sync_suspend(pd_dev);

	state = psci_get_domain_state();
	if (!state)
		state = states[idx];

	ret = psci_cpu_suspend_enter(state) ? -1 : idx;

	if (s2idle)
		dev_pm_genpd_resume(pd_dev);
	else
		pm_runtime_get_sync(pd_dev);

	cpu_pm_exit();

	/* Clear the domain state to start fresh when back from idle. */
	psci_set_domain_state(0);
	return ret;
}

static int psci_enter_domain_idle_state(struct cpuidle_device *dev,
					struct cpuidle_driver *drv, int idx)
{
	return __psci_enter_domain_idle_state(dev, drv, idx, false);
}

static int psci_enter_s2idle_domain_idle_state(struct cpuidle_device *dev,
					       struct cpuidle_driver *drv,
					       int idx)
{
	return __psci_enter_domain_idle_state(dev, drv, idx, true);
}

static int psci_idle_cpuhp_up(unsigned int cpu)
{
	struct device *pd_dev = __this_cpu_read(psci_cpuidle_data.dev);

	if (pd_dev)
		pm_runtime_get_sync(pd_dev);

	return 0;
}

static int psci_idle_cpuhp_down(unsigned int cpu)
{
	struct device *pd_dev = __this_cpu_read(psci_cpuidle_data.dev);

	if (pd_dev) {
		pm_runtime_put_sync(pd_dev);
		/* Clear domain state to start fresh at next online. */
		psci_set_domain_state(0);
	}

	return 0;
}

static void psci_idle_syscore_switch(bool suspend)
{
	bool cleared = false;
	struct device *dev;
	int cpu;

	for_each_possible_cpu(cpu) {
		dev = per_cpu_ptr(&psci_cpuidle_data, cpu)->dev;

		if (dev && suspend) {
			dev_pm_genpd_suspend(dev);
		} else if (dev) {
			dev_pm_genpd_resume(dev);

			/* Account for userspace having offlined a CPU. */
			if (pm_runtime_status_suspended(dev))
				pm_runtime_set_active(dev);

			/* Clear domain state to re-start fresh. */
			if (!cleared) {
				psci_set_domain_state(0);
				cleared = true;
			}
		}
	}
}

static int psci_idle_syscore_suspend(void)
{
	psci_idle_syscore_switch(true);
	return 0;
}

static void psci_idle_syscore_resume(void)
{
	psci_idle_syscore_switch(false);
}

static struct syscore_ops psci_idle_syscore_ops = {
	.suspend = psci_idle_syscore_suspend,
	.resume = psci_idle_syscore_resume,
};

static void psci_idle_init_cpuhp(void)
{
	int err;

	if (!psci_cpuidle_use_cpuhp)
		return;

	register_syscore_ops(&psci_idle_syscore_ops);

	err = cpuhp_setup_state_nocalls(CPUHP_AP_CPU_PM_STARTING,
					"cpuidle/psci:online",
					psci_idle_cpuhp_up,
					psci_idle_cpuhp_down);
	if (err)
		pr_warn("Failed %d while setup cpuhp state\n", err);
}

static __cpuidle int psci_enter_idle_state(struct cpuidle_device *dev,
					   struct cpuidle_driver *drv, int idx)
{
	u32 *state = __this_cpu_read(psci_cpuidle_data.psci_states);

	return CPU_PM_CPU_IDLE_ENTER_PARAM_RCU(psci_cpu_suspend_enter, idx, state[idx]);
}

static const struct of_device_id psci_idle_state_match[] = {
	{ .compatible = "arm,idle-state",
	  .data = psci_enter_idle_state },
	{ },
};

int psci_dt_parse_state_node(struct device_node *np, u32 *state)
{
	int err = of_property_read_u32(np, "arm,psci-suspend-param", state);

	if (err) {
		pr_warn("%pOF missing arm,psci-suspend-param property\n", np);
		return err;
	}

	if (!psci_power_state_is_valid(*state)) {
		pr_warn("Invalid PSCI power state %#x\n", *state);
		return -EINVAL;
	}

	return 0;
}

static int psci_dt_cpu_init_topology(struct cpuidle_driver *drv,
				     struct psci_cpuidle_data *data,
				     unsigned int state_count, int cpu)
{
	/* Currently limit the hierarchical topology to be used in OSI mode. */
	if (!psci_has_osi_support())
		return 0;

	if (IS_ENABLED(CONFIG_PREEMPT_RT))
		return 0;

	data->dev = dt_idle_attach_cpu(cpu, "psci");
	if (IS_ERR_OR_NULL(data->dev))
		return PTR_ERR_OR_ZERO(data->dev);

	/*
	 * Using the deepest state for the CPU to trigger a potential selection
	 * of a shared state for the domain, assumes the domain states are all
	 * deeper states.
	 */
	drv->states[state_count - 1].flags |= CPUIDLE_FLAG_RCU_IDLE;
	drv->states[state_count - 1].enter = psci_enter_domain_idle_state;
	drv->states[state_count - 1].enter_s2idle = psci_enter_s2idle_domain_idle_state;
	psci_cpuidle_use_cpuhp = true;

	return 0;
}

static int psci_dt_cpu_init_idle(struct device *dev, struct cpuidle_driver *drv,
				 struct device_node *cpu_node,
				 unsigned int state_count, int cpu)
{
	int i, ret = 0;
	u32 *psci_states;
	struct device_node *state_node;
	struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);

	state_count++; /* Add WFI state too */
	psci_states = devm_kcalloc(dev, state_count, sizeof(*psci_states),
				   GFP_KERNEL);
	if (!psci_states)
		return -ENOMEM;

	for (i = 1; i < state_count; i++) {
		state_node = of_get_cpu_state_node(cpu_node, i - 1);
		if (!state_node)
			break;

		ret = psci_dt_parse_state_node(state_node, &psci_states[i]);
		of_node_put(state_node);

		if (ret)
			return ret;

		pr_debug("psci-power-state %#x index %d\n", psci_states[i], i);
	}

	if (i != state_count)
		return -ENODEV;

	/* Initialize optional data, used for the hierarchical topology. */
	ret = psci_dt_cpu_init_topology(drv, data, state_count, cpu);
	if (ret < 0)
		return ret;

	/* Idle states parsed correctly, store them in the per-cpu struct. */
	data->psci_states = psci_states;
	return 0;
}

static int psci_cpu_init_idle(struct device *dev, struct cpuidle_driver *drv,
			      unsigned int cpu, unsigned int state_count)
{
	struct device_node *cpu_node;
	int ret;

	/*
	 * If the PSCI cpu_suspend function hook has not been initialized
	 * idle states must not be enabled, so bail out
	 */
	if (!psci_ops.cpu_suspend)
		return -EOPNOTSUPP;

	cpu_node = of_cpu_device_node_get(cpu);
	if (!cpu_node)
		return -ENODEV;

	ret = psci_dt_cpu_init_idle(dev, drv, cpu_node, state_count, cpu);

	of_node_put(cpu_node);

	return ret;
}

static void psci_cpu_deinit_idle(int cpu)
{
	struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);

	dt_idle_detach_cpu(data->dev);
	psci_cpuidle_use_cpuhp = false;
}

static int psci_idle_init_cpu(struct device *dev, int cpu)
{
	struct cpuidle_driver *drv;
	struct device_node *cpu_node;
	const char *enable_method;
	int ret = 0;

	cpu_node = of_cpu_device_node_get(cpu);
	if (!cpu_node)
		return -ENODEV;

	/*
	 * Check whether the enable-method for the cpu is PSCI, fail
	 * if it is not.
	 */
	enable_method = of_get_property(cpu_node, "enable-method", NULL);
	if (!enable_method || (strcmp(enable_method, "psci")))
		ret = -ENODEV;

	of_node_put(cpu_node);
	if (ret)
		return ret;

	drv = devm_kzalloc(dev, sizeof(*drv), GFP_KERNEL);
	if (!drv)
		return -ENOMEM;

	drv->name = "psci_idle";
	drv->owner = THIS_MODULE;
	drv->cpumask = (struct cpumask *)cpumask_of(cpu);

	/*
	 * PSCI idle states relies on architectural WFI to be represented as
	 * state index 0.
	 */
	drv->states[0].enter = psci_enter_idle_state;
	drv->states[0].exit_latency = 1;
	drv->states[0].target_residency = 1;
	drv->states[0].power_usage = UINT_MAX;
	strcpy(drv->states[0].name, "WFI");
	strcpy(drv->states[0].desc, "ARM WFI");

	/*
	 * If no DT idle states are detected (ret == 0) let the driver
	 * initialization fail accordingly since there is no reason to
	 * initialize the idle driver if only wfi is supported, the
	 * default archictectural back-end already executes wfi
	 * on idle entry.
	 */
	ret = dt_init_idle_driver(drv, psci_idle_state_match, 1);
	if (ret <= 0)
		return ret ? : -ENODEV;

	/*
	 * Initialize PSCI idle states.
	 */
	ret = psci_cpu_init_idle(dev, drv, cpu, ret);
	if (ret) {
		pr_err("CPU %d failed to PSCI idle\n", cpu);
		return ret;
	}

	ret = cpuidle_register(drv, NULL);
	if (ret)
		goto deinit;

	cpuidle_cooling_register(drv);

	return 0;
deinit:
	psci_cpu_deinit_idle(cpu);
	return ret;
}

/*
 * psci_idle_probe - Initializes PSCI cpuidle driver
 *
 * Initializes PSCI cpuidle driver for all CPUs, if any CPU fails
 * to register cpuidle driver then rollback to cancel all CPUs
 * registration.
 */
static int psci_cpuidle_probe(struct platform_device *pdev)
{
	int cpu, ret;
	struct cpuidle_driver *drv;
	struct cpuidle_device *dev;

	for_each_possible_cpu(cpu) {
		ret = psci_idle_init_cpu(&pdev->dev, cpu);
		if (ret)
			goto out_fail;
	}

	psci_idle_init_cpuhp();
	return 0;

out_fail:
	while (--cpu >= 0) {
		dev = per_cpu(cpuidle_devices, cpu);
		drv = cpuidle_get_cpu_driver(dev);
		cpuidle_unregister(drv);
		psci_cpu_deinit_idle(cpu);
	}

	return ret;
}

static struct platform_driver psci_cpuidle_driver = {
	.probe = psci_cpuidle_probe,
	.driver = {
		.name = "psci-cpuidle",
	},
};

static int __init psci_idle_init(void)
{
	struct platform_device *pdev;
	int ret;

	ret = platform_driver_register(&psci_cpuidle_driver);
	if (ret)
		return ret;

	pdev = platform_device_register_simple("psci-cpuidle", -1, NULL, 0);
	if (IS_ERR(pdev)) {
		platform_driver_unregister(&psci_cpuidle_driver);
		return PTR_ERR(pdev);
	}

	return 0;
}
device_initcall(psci_idle_init);
Commit	Line	Data
81d549e0 LP	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 <[email protected]>
	7	*/
	8
	9	#define pr_fmt(fmt) "CPUidle PSCI: " fmt
	10
9c6ceecb	11	#include <linux/cpuhotplug.h>
fc7a3d9e	12	#include <linux/cpu_cooling.h>
81d549e0 LP	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>
166bf835	19	#include <linux/platform_device.h>
81d549e0	20	#include <linux/psci.h>
670c90de	21	#include <linux/pm_domain.h>
ce85aef5	22	#include <linux/pm_runtime.h>
81d549e0	23	#include <linux/slab.h>
166bf835	24	#include <linux/string.h>
171b66e2	25	#include <linux/syscore_ops.h>
81d549e0 LP	26
	27	#include <asm/cpuidle.h>
	28
8554951a	29	#include "cpuidle-psci.h"
81d549e0	30	#include "dt_idle_states.h"
d72d7d6c	31	#include "dt_idle_genpd.h"
81d549e0	32
8554951a UH	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);
a0cf3194	39	static DEFINE_PER_CPU(u32, domain_state);
166bf835	40	static bool psci_cpuidle_use_cpuhp;
a0cf3194	41
a65a397f	42	void psci_set_domain_state(u32 state)
a0cf3194 UH	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
69e26b4f PZ	52	static __cpuidle int __psci_enter_domain_idle_state(struct cpuidle_device *dev,
	53	struct cpuidle_driver *drv, int idx,
	54	bool s2idle)
a0cf3194 UH	55	{
	56	struct psci_cpuidle_data *data = this_cpu_ptr(&psci_cpuidle_data);
	57	u32 *states = data->psci_states;
ce85aef5 UH	58	struct device *pd_dev = data->dev;
ce85aef5 UH	59	u32 state;
a0cf3194 UH	60	int ret;
a0cf3194 UH	61
8b7ce5e4 UH	62	ret = cpu_pm_enter();
	63	if (ret)
	64	return -1;
	65
ce85aef5	66	/* Do runtime PM to manage a hierarchical CPU toplogy. */
670c90de UH	67	if (s2idle)
	68	dev_pm_genpd_suspend(pd_dev);
	69	else
	70	pm_runtime_put_sync_suspend(pd_dev);
e038f7b8	71
ce85aef5	72	state = psci_get_domain_state();
a0cf3194 UH	73	if (!state)
	74	state = states[idx];
	75
8b7ce5e4	76	ret = psci_cpu_suspend_enter(state) ? -1 : idx;
a0cf3194	77
670c90de UH	78	if (s2idle)
	79	dev_pm_genpd_resume(pd_dev);
	80	else
	81	pm_runtime_get_sync(pd_dev);
ce85aef5	82
8b7ce5e4 UH	83	cpu_pm_exit();
8b7ce5e4 UH	84
a0cf3194 UH	85	/* Clear the domain state to start fresh when back from idle. */
	86	psci_set_domain_state(0);
	87	return ret;
	88	}
9ffeb6d0	89
670c90de UH	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
9c6ceecb UH	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
171b66e2 UH	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
166bf835	169	static void psci_idle_init_cpuhp(void)
9c6ceecb UH	170	{
	171	int err;
	172
	173	if (!psci_cpuidle_use_cpuhp)
	174	return;
	175
171b66e2 UH	176	register_syscore_ops(&psci_idle_syscore_ops);
171b66e2 UH	177
9c6ceecb UH	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
69e26b4f PZ	186	static __cpuidle int psci_enter_idle_state(struct cpuidle_device *dev,
69e26b4f PZ	187	struct cpuidle_driver *drv, int idx)
81d549e0	188	{
8554951a	189	u32 *state = __this_cpu_read(psci_cpuidle_data.psci_states);
9ffeb6d0	190
19235e47	191	return CPU_PM_CPU_IDLE_ENTER_PARAM_RCU(psci_cpu_suspend_enter, idx, state[idx]);
81d549e0 LP	192	}
81d549e0 LP	193
166bf835	194	static const struct of_device_id psci_idle_state_match[] = {
81d549e0 LP	195	{ .compatible = "arm,idle-state",
	196	.data = psci_enter_idle_state },
	197	{ },
	198	};
	199
166bf835	200	int psci_dt_parse_state_node(struct device_node np, u32 state)
9ffeb6d0 LP	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
166bf835 UH	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)
7fbee48e UH	220	{
	221	/* Currently limit the hierarchical topology to be used in OSI mode. */
	222	if (!psci_has_osi_support())
	223	return 0;
	224
f9901f64 KK	225	if (IS_ENABLED(CONFIG_PREEMPT_RT))
	226	return 0;
	227
d72d7d6c	228	data->dev = dt_idle_attach_cpu(cpu, "psci");
7fbee48e UH	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	*/
e038f7b8	237	drv->states[state_count - 1].flags \|= CPUIDLE_FLAG_RCU_IDLE;
7fbee48e	238	drv->states[state_count - 1].enter = psci_enter_domain_idle_state;
670c90de	239	drv->states[state_count - 1].enter_s2idle = psci_enter_s2idle_domain_idle_state;
7fbee48e UH	240	psci_cpuidle_use_cpuhp = true;
	241
	242	return 0;
	243	}
	244
166bf835 UH	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)
9ffeb6d0	248	{
1595e4b0	249	int i, ret = 0;
9ffeb6d0 LP	250	u32 *psci_states;
9ffeb6d0 LP	251	struct device_node *state_node;
8554951a	252	struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);
9ffeb6d0	253
1595e4b0	254	state_count++; /* Add WFI state too */
166bf835 UH	255	psci_states = devm_kcalloc(dev, state_count, sizeof(*psci_states),
166bf835 UH	256	GFP_KERNEL);
9ffeb6d0 LP	257	if (!psci_states)
	258	return -ENOMEM;
	259
1595e4b0	260	for (i = 1; i < state_count; i++) {
f08cfbfa	261	state_node = of_get_cpu_state_node(cpu_node, i - 1);
1595e4b0 UH	262	if (!state_node)
	263	break;
	264
9ffeb6d0 LP	265	ret = psci_dt_parse_state_node(state_node, &psci_states[i]);
	266	of_node_put(state_node);
	267
	268	if (ret)
166bf835	269	return ret;
9ffeb6d0 LP	270
	271	pr_debug("psci-power-state %#x index %d\n", psci_states[i], i);
	272	}
	273
166bf835 UH	274	if (i != state_count)
166bf835 UH	275	return -ENODEV;
1595e4b0	276
7fbee48e UH	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)
166bf835	280	return ret;
8554951a UH	281
	282	/* Idle states parsed correctly, store them in the per-cpu struct. */
	283	data->psci_states = psci_states;
9ffeb6d0	284	return 0;
9ffeb6d0 LP	285	}
9ffeb6d0 LP	286
166bf835 UH	287	static int psci_cpu_init_idle(struct device dev, struct cpuidle_driver drv,
166bf835 UH	288	unsigned int cpu, unsigned int state_count)
9ffeb6d0 LP	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
166bf835	304	ret = psci_dt_cpu_init_idle(dev, drv, cpu_node, state_count, cpu);
9ffeb6d0 LP	305
	306	of_node_put(cpu_node);
	307
	308	return ret;
	309	}
	310
166bf835 UH	311	static void psci_cpu_deinit_idle(int cpu)
	312	{
	313	struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu);
	314
d72d7d6c	315	dt_idle_detach_cpu(data->dev);
166bf835 UH	316	psci_cpuidle_use_cpuhp = false;
	317	}
	318
	319	static int psci_idle_init_cpu(struct device *dev, int cpu)
81d549e0 LP	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
166bf835	342	drv = devm_kzalloc(dev, sizeof(*drv), GFP_KERNEL);
81d549e0 LP	343	if (!drv)
	344	return -ENOMEM;
	345
166bf835 UH	346	drv->name = "psci_idle";
166bf835 UH	347	drv->owner = THIS_MODULE;
81d549e0 LP	348	drv->cpumask = (struct cpumask *)cpumask_of(cpu);
	349
	350	/*
166bf835 UH	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	/*
81d549e0 LP	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);
166bf835 UH	369	if (ret <= 0)
166bf835 UH	370	return ret ? : -ENODEV;
81d549e0 LP	371
	372	/*
	373	* Initialize PSCI idle states.
	374	*/
166bf835	375	ret = psci_cpu_init_idle(dev, drv, cpu, ret);
81d549e0 LP	376	if (ret) {
81d549e0 LP	377	pr_err("CPU %d failed to PSCI idle\n", cpu);
166bf835	378	return ret;
81d549e0 LP	379	}
	380
	381	ret = cpuidle_register(drv, NULL);
	382	if (ret)
166bf835	383	goto deinit;
81d549e0	384
fc7a3d9e DL	385	cpuidle_cooling_register(drv);
fc7a3d9e DL	386
81d549e0	387	return 0;
166bf835 UH	388	deinit:
166bf835 UH	389	psci_cpu_deinit_idle(cpu);
81d549e0 LP	390	return ret;
	391	}
	392
	393	/*
166bf835	394	* psci_idle_probe - Initializes PSCI cpuidle driver
81d549e0 LP	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	*/
166bf835	400	static int psci_cpuidle_probe(struct platform_device *pdev)
81d549e0 LP	401	{
	402	int cpu, ret;
	403	struct cpuidle_driver *drv;
	404	struct cpuidle_device *dev;
	405
	406	for_each_possible_cpu(cpu) {
166bf835	407	ret = psci_idle_init_cpu(&pdev->dev, cpu);
81d549e0 LP	408	if (ret)
	409	goto out_fail;
	410	}
	411
9c6ceecb	412	psci_idle_init_cpuhp();
81d549e0 LP	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);
166bf835	420	psci_cpu_deinit_idle(cpu);
81d549e0 LP	421	}
	422
	423	return ret;
	424	}
166bf835 UH	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	}
81d549e0	450	device_initcall(psci_idle_init);