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 <lorenzo.pieralisi@arm.com> | |
7 | */ | |
8 | ||
9 | #define pr_fmt(fmt) "CPUidle PSCI: " fmt | |
10 | ||
11 | #include <linux/cpuidle.h> | |
12 | #include <linux/cpumask.h> | |
13 | #include <linux/cpu_pm.h> | |
14 | #include <linux/kernel.h> | |
15 | #include <linux/module.h> | |
16 | #include <linux/of.h> | |
17 | #include <linux/of_device.h> | |
18 | #include <linux/psci.h> | |
19 | #include <linux/slab.h> | |
20 | ||
21 | #include <asm/cpuidle.h> | |
22 | ||
8554951a | 23 | #include "cpuidle-psci.h" |
81d549e0 LP |
24 | #include "dt_idle_states.h" |
25 | ||
8554951a UH |
26 | struct psci_cpuidle_data { |
27 | u32 *psci_states; | |
28 | struct device *dev; | |
29 | }; | |
30 | ||
31 | static DEFINE_PER_CPU_READ_MOSTLY(struct psci_cpuidle_data, psci_cpuidle_data); | |
9ffeb6d0 | 32 | |
81d549e0 LP |
33 | static int psci_enter_idle_state(struct cpuidle_device *dev, |
34 | struct cpuidle_driver *drv, int idx) | |
35 | { | |
8554951a | 36 | u32 *state = __this_cpu_read(psci_cpuidle_data.psci_states); |
9ffeb6d0 LP |
37 | |
38 | return CPU_PM_CPU_IDLE_ENTER_PARAM(psci_cpu_suspend_enter, | |
4386aa86 | 39 | idx, state[idx]); |
81d549e0 LP |
40 | } |
41 | ||
42 | static struct cpuidle_driver psci_idle_driver __initdata = { | |
43 | .name = "psci_idle", | |
44 | .owner = THIS_MODULE, | |
45 | /* | |
46 | * PSCI idle states relies on architectural WFI to | |
47 | * be represented as state index 0. | |
48 | */ | |
49 | .states[0] = { | |
50 | .enter = psci_enter_idle_state, | |
51 | .exit_latency = 1, | |
52 | .target_residency = 1, | |
53 | .power_usage = UINT_MAX, | |
54 | .name = "WFI", | |
55 | .desc = "ARM WFI", | |
56 | } | |
57 | }; | |
58 | ||
59 | static const struct of_device_id psci_idle_state_match[] __initconst = { | |
60 | { .compatible = "arm,idle-state", | |
61 | .data = psci_enter_idle_state }, | |
62 | { }, | |
63 | }; | |
64 | ||
9ffeb6d0 LP |
65 | static int __init psci_dt_parse_state_node(struct device_node *np, u32 *state) |
66 | { | |
67 | int err = of_property_read_u32(np, "arm,psci-suspend-param", state); | |
68 | ||
69 | if (err) { | |
70 | pr_warn("%pOF missing arm,psci-suspend-param property\n", np); | |
71 | return err; | |
72 | } | |
73 | ||
74 | if (!psci_power_state_is_valid(*state)) { | |
75 | pr_warn("Invalid PSCI power state %#x\n", *state); | |
76 | return -EINVAL; | |
77 | } | |
78 | ||
79 | return 0; | |
80 | } | |
81 | ||
1595e4b0 UH |
82 | static int __init psci_dt_cpu_init_idle(struct device_node *cpu_node, |
83 | unsigned int state_count, int cpu) | |
9ffeb6d0 | 84 | { |
1595e4b0 | 85 | int i, ret = 0; |
9ffeb6d0 LP |
86 | u32 *psci_states; |
87 | struct device_node *state_node; | |
8554951a | 88 | struct psci_cpuidle_data *data = per_cpu_ptr(&psci_cpuidle_data, cpu); |
9ffeb6d0 | 89 | |
1595e4b0 UH |
90 | state_count++; /* Add WFI state too */ |
91 | psci_states = kcalloc(state_count, sizeof(*psci_states), GFP_KERNEL); | |
9ffeb6d0 LP |
92 | if (!psci_states) |
93 | return -ENOMEM; | |
94 | ||
1595e4b0 | 95 | for (i = 1; i < state_count; i++) { |
f08cfbfa | 96 | state_node = of_get_cpu_state_node(cpu_node, i - 1); |
1595e4b0 UH |
97 | if (!state_node) |
98 | break; | |
99 | ||
9ffeb6d0 LP |
100 | ret = psci_dt_parse_state_node(state_node, &psci_states[i]); |
101 | of_node_put(state_node); | |
102 | ||
103 | if (ret) | |
104 | goto free_mem; | |
105 | ||
106 | pr_debug("psci-power-state %#x index %d\n", psci_states[i], i); | |
107 | } | |
108 | ||
1595e4b0 UH |
109 | if (i != state_count) { |
110 | ret = -ENODEV; | |
111 | goto free_mem; | |
112 | } | |
113 | ||
8554951a UH |
114 | /* Currently limit the hierarchical topology to be used in OSI mode. */ |
115 | if (psci_has_osi_support()) { | |
116 | data->dev = psci_dt_attach_cpu(cpu); | |
117 | if (IS_ERR(data->dev)) { | |
118 | ret = PTR_ERR(data->dev); | |
119 | goto free_mem; | |
120 | } | |
121 | } | |
122 | ||
123 | /* Idle states parsed correctly, store them in the per-cpu struct. */ | |
124 | data->psci_states = psci_states; | |
9ffeb6d0 LP |
125 | return 0; |
126 | ||
127 | free_mem: | |
128 | kfree(psci_states); | |
129 | return ret; | |
130 | } | |
131 | ||
1595e4b0 | 132 | static __init int psci_cpu_init_idle(unsigned int cpu, unsigned int state_count) |
9ffeb6d0 LP |
133 | { |
134 | struct device_node *cpu_node; | |
135 | int ret; | |
136 | ||
137 | /* | |
138 | * If the PSCI cpu_suspend function hook has not been initialized | |
139 | * idle states must not be enabled, so bail out | |
140 | */ | |
141 | if (!psci_ops.cpu_suspend) | |
142 | return -EOPNOTSUPP; | |
143 | ||
144 | cpu_node = of_cpu_device_node_get(cpu); | |
145 | if (!cpu_node) | |
146 | return -ENODEV; | |
147 | ||
1595e4b0 | 148 | ret = psci_dt_cpu_init_idle(cpu_node, state_count, cpu); |
9ffeb6d0 LP |
149 | |
150 | of_node_put(cpu_node); | |
151 | ||
152 | return ret; | |
153 | } | |
154 | ||
81d549e0 LP |
155 | static int __init psci_idle_init_cpu(int cpu) |
156 | { | |
157 | struct cpuidle_driver *drv; | |
158 | struct device_node *cpu_node; | |
159 | const char *enable_method; | |
160 | int ret = 0; | |
161 | ||
162 | cpu_node = of_cpu_device_node_get(cpu); | |
163 | if (!cpu_node) | |
164 | return -ENODEV; | |
165 | ||
166 | /* | |
167 | * Check whether the enable-method for the cpu is PSCI, fail | |
168 | * if it is not. | |
169 | */ | |
170 | enable_method = of_get_property(cpu_node, "enable-method", NULL); | |
171 | if (!enable_method || (strcmp(enable_method, "psci"))) | |
172 | ret = -ENODEV; | |
173 | ||
174 | of_node_put(cpu_node); | |
175 | if (ret) | |
176 | return ret; | |
177 | ||
178 | drv = kmemdup(&psci_idle_driver, sizeof(*drv), GFP_KERNEL); | |
179 | if (!drv) | |
180 | return -ENOMEM; | |
181 | ||
182 | drv->cpumask = (struct cpumask *)cpumask_of(cpu); | |
183 | ||
184 | /* | |
185 | * Initialize idle states data, starting at index 1, since | |
186 | * by default idle state 0 is the quiescent state reached | |
187 | * by the cpu by executing the wfi instruction. | |
188 | * | |
189 | * If no DT idle states are detected (ret == 0) let the driver | |
190 | * initialization fail accordingly since there is no reason to | |
191 | * initialize the idle driver if only wfi is supported, the | |
192 | * default archictectural back-end already executes wfi | |
193 | * on idle entry. | |
194 | */ | |
195 | ret = dt_init_idle_driver(drv, psci_idle_state_match, 1); | |
196 | if (ret <= 0) { | |
197 | ret = ret ? : -ENODEV; | |
198 | goto out_kfree_drv; | |
199 | } | |
200 | ||
201 | /* | |
202 | * Initialize PSCI idle states. | |
203 | */ | |
1595e4b0 | 204 | ret = psci_cpu_init_idle(cpu, ret); |
81d549e0 LP |
205 | if (ret) { |
206 | pr_err("CPU %d failed to PSCI idle\n", cpu); | |
207 | goto out_kfree_drv; | |
208 | } | |
209 | ||
210 | ret = cpuidle_register(drv, NULL); | |
211 | if (ret) | |
212 | goto out_kfree_drv; | |
213 | ||
214 | return 0; | |
215 | ||
216 | out_kfree_drv: | |
217 | kfree(drv); | |
218 | return ret; | |
219 | } | |
220 | ||
221 | /* | |
222 | * psci_idle_init - Initializes PSCI cpuidle driver | |
223 | * | |
224 | * Initializes PSCI cpuidle driver for all CPUs, if any CPU fails | |
225 | * to register cpuidle driver then rollback to cancel all CPUs | |
226 | * registration. | |
227 | */ | |
228 | static int __init psci_idle_init(void) | |
229 | { | |
230 | int cpu, ret; | |
231 | struct cpuidle_driver *drv; | |
232 | struct cpuidle_device *dev; | |
233 | ||
234 | for_each_possible_cpu(cpu) { | |
235 | ret = psci_idle_init_cpu(cpu); | |
236 | if (ret) | |
237 | goto out_fail; | |
238 | } | |
239 | ||
240 | return 0; | |
241 | ||
242 | out_fail: | |
243 | while (--cpu >= 0) { | |
244 | dev = per_cpu(cpuidle_devices, cpu); | |
245 | drv = cpuidle_get_cpu_driver(dev); | |
246 | cpuidle_unregister(drv); | |
247 | kfree(drv); | |
248 | } | |
249 | ||
250 | return ret; | |
251 | } | |
252 | device_initcall(psci_idle_init); |