Commit | Line | Data |
---|---|---|
a5e0454c UH |
1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* | |
3 | * PM domains for CPUs via genpd - managed by cpuidle-psci. | |
4 | * | |
5 | * Copyright (C) 2019 Linaro Ltd. | |
6 | * Author: Ulf Hansson <ulf.hansson@linaro.org> | |
7 | * | |
8 | */ | |
9 | ||
a65a397f UH |
10 | #define pr_fmt(fmt) "CPUidle PSCI: " fmt |
11 | ||
a5e0454c UH |
12 | #include <linux/cpu.h> |
13 | #include <linux/device.h> | |
14 | #include <linux/kernel.h> | |
15 | #include <linux/pm_domain.h> | |
16 | #include <linux/pm_runtime.h> | |
a65a397f UH |
17 | #include <linux/psci.h> |
18 | #include <linux/slab.h> | |
19 | #include <linux/string.h> | |
a5e0454c UH |
20 | |
21 | #include "cpuidle-psci.h" | |
22 | ||
a65a397f UH |
23 | struct psci_pd_provider { |
24 | struct list_head link; | |
25 | struct device_node *node; | |
26 | }; | |
27 | ||
28 | static LIST_HEAD(psci_pd_providers); | |
29 | static bool osi_mode_enabled __initdata; | |
30 | ||
31 | static int psci_pd_power_off(struct generic_pm_domain *pd) | |
32 | { | |
33 | struct genpd_power_state *state = &pd->states[pd->state_idx]; | |
34 | u32 *pd_state; | |
35 | ||
36 | if (!state->data) | |
37 | return 0; | |
38 | ||
39 | /* OSI mode is enabled, set the corresponding domain state. */ | |
40 | pd_state = state->data; | |
41 | psci_set_domain_state(*pd_state); | |
42 | ||
43 | return 0; | |
44 | } | |
45 | ||
46 | static int __init psci_pd_parse_state_nodes(struct genpd_power_state *states, | |
47 | int state_count) | |
48 | { | |
49 | int i, ret; | |
50 | u32 psci_state, *psci_state_buf; | |
51 | ||
52 | for (i = 0; i < state_count; i++) { | |
53 | ret = psci_dt_parse_state_node(to_of_node(states[i].fwnode), | |
54 | &psci_state); | |
55 | if (ret) | |
56 | goto free_state; | |
57 | ||
58 | psci_state_buf = kmalloc(sizeof(u32), GFP_KERNEL); | |
59 | if (!psci_state_buf) { | |
60 | ret = -ENOMEM; | |
61 | goto free_state; | |
62 | } | |
63 | *psci_state_buf = psci_state; | |
64 | states[i].data = psci_state_buf; | |
65 | } | |
66 | ||
67 | return 0; | |
68 | ||
69 | free_state: | |
70 | i--; | |
71 | for (; i >= 0; i--) | |
72 | kfree(states[i].data); | |
73 | return ret; | |
74 | } | |
75 | ||
76 | static int __init psci_pd_parse_states(struct device_node *np, | |
77 | struct genpd_power_state **states, int *state_count) | |
78 | { | |
79 | int ret; | |
80 | ||
81 | /* Parse the domain idle states. */ | |
82 | ret = of_genpd_parse_idle_states(np, states, state_count); | |
83 | if (ret) | |
84 | return ret; | |
85 | ||
86 | /* Fill out the PSCI specifics for each found state. */ | |
87 | ret = psci_pd_parse_state_nodes(*states, *state_count); | |
88 | if (ret) | |
89 | kfree(*states); | |
90 | ||
91 | return ret; | |
92 | } | |
93 | ||
94 | static void psci_pd_free_states(struct genpd_power_state *states, | |
95 | unsigned int state_count) | |
96 | { | |
97 | int i; | |
98 | ||
99 | for (i = 0; i < state_count; i++) | |
100 | kfree(states[i].data); | |
101 | kfree(states); | |
102 | } | |
103 | ||
104 | static int __init psci_pd_init(struct device_node *np) | |
105 | { | |
106 | struct generic_pm_domain *pd; | |
107 | struct psci_pd_provider *pd_provider; | |
108 | struct dev_power_governor *pd_gov; | |
109 | struct genpd_power_state *states = NULL; | |
110 | int ret = -ENOMEM, state_count = 0; | |
111 | ||
112 | pd = kzalloc(sizeof(*pd), GFP_KERNEL); | |
113 | if (!pd) | |
114 | goto out; | |
115 | ||
116 | pd_provider = kzalloc(sizeof(*pd_provider), GFP_KERNEL); | |
117 | if (!pd_provider) | |
118 | goto free_pd; | |
119 | ||
120 | pd->name = kasprintf(GFP_KERNEL, "%pOF", np); | |
121 | if (!pd->name) | |
122 | goto free_pd_prov; | |
123 | ||
124 | /* | |
125 | * Parse the domain idle states and let genpd manage the state selection | |
126 | * for those being compatible with "domain-idle-state". | |
127 | */ | |
128 | ret = psci_pd_parse_states(np, &states, &state_count); | |
129 | if (ret) | |
130 | goto free_name; | |
131 | ||
132 | pd->free_states = psci_pd_free_states; | |
133 | pd->name = kbasename(pd->name); | |
134 | pd->power_off = psci_pd_power_off; | |
135 | pd->states = states; | |
136 | pd->state_count = state_count; | |
137 | pd->flags |= GENPD_FLAG_IRQ_SAFE | GENPD_FLAG_CPU_DOMAIN; | |
138 | ||
139 | /* Use governor for CPU PM domains if it has some states to manage. */ | |
140 | pd_gov = state_count > 0 ? &pm_domain_cpu_gov : NULL; | |
141 | ||
142 | ret = pm_genpd_init(pd, pd_gov, false); | |
143 | if (ret) { | |
144 | psci_pd_free_states(states, state_count); | |
145 | goto free_name; | |
146 | } | |
147 | ||
148 | ret = of_genpd_add_provider_simple(np, pd); | |
149 | if (ret) | |
150 | goto remove_pd; | |
151 | ||
152 | pd_provider->node = of_node_get(np); | |
153 | list_add(&pd_provider->link, &psci_pd_providers); | |
154 | ||
155 | pr_debug("init PM domain %s\n", pd->name); | |
156 | return 0; | |
157 | ||
158 | remove_pd: | |
159 | pm_genpd_remove(pd); | |
160 | free_name: | |
161 | kfree(pd->name); | |
162 | free_pd_prov: | |
163 | kfree(pd_provider); | |
164 | free_pd: | |
165 | kfree(pd); | |
166 | out: | |
167 | pr_err("failed to init PM domain ret=%d %pOF\n", ret, np); | |
168 | return ret; | |
169 | } | |
170 | ||
171 | static void __init psci_pd_remove(void) | |
172 | { | |
173 | struct psci_pd_provider *pd_provider, *it; | |
174 | struct generic_pm_domain *genpd; | |
175 | ||
176 | list_for_each_entry_safe(pd_provider, it, &psci_pd_providers, link) { | |
177 | of_genpd_del_provider(pd_provider->node); | |
178 | ||
179 | genpd = of_genpd_remove_last(pd_provider->node); | |
180 | if (!IS_ERR(genpd)) | |
181 | kfree(genpd); | |
182 | ||
183 | of_node_put(pd_provider->node); | |
184 | list_del(&pd_provider->link); | |
185 | kfree(pd_provider); | |
186 | } | |
187 | } | |
188 | ||
189 | static int __init psci_pd_init_topology(struct device_node *np, bool add) | |
190 | { | |
191 | struct device_node *node; | |
192 | struct of_phandle_args child, parent; | |
193 | int ret; | |
194 | ||
195 | for_each_child_of_node(np, node) { | |
196 | if (of_parse_phandle_with_args(node, "power-domains", | |
197 | "#power-domain-cells", 0, &parent)) | |
198 | continue; | |
199 | ||
200 | child.np = node; | |
201 | child.args_count = 0; | |
202 | ||
203 | ret = add ? of_genpd_add_subdomain(&parent, &child) : | |
204 | of_genpd_remove_subdomain(&parent, &child); | |
205 | of_node_put(parent.np); | |
206 | if (ret) { | |
207 | of_node_put(node); | |
208 | return ret; | |
209 | } | |
210 | } | |
211 | ||
212 | return 0; | |
213 | } | |
214 | ||
215 | static int __init psci_pd_add_topology(struct device_node *np) | |
216 | { | |
217 | return psci_pd_init_topology(np, true); | |
218 | } | |
219 | ||
220 | static void __init psci_pd_remove_topology(struct device_node *np) | |
221 | { | |
222 | psci_pd_init_topology(np, false); | |
223 | } | |
224 | ||
225 | static const struct of_device_id psci_of_match[] __initconst = { | |
226 | { .compatible = "arm,psci-1.0" }, | |
227 | {} | |
228 | }; | |
229 | ||
230 | static int __init psci_idle_init_domains(void) | |
231 | { | |
232 | struct device_node *np = of_find_matching_node(NULL, psci_of_match); | |
233 | struct device_node *node; | |
234 | int ret = 0, pd_count = 0; | |
235 | ||
236 | if (!np) | |
237 | return -ENODEV; | |
238 | ||
239 | /* Currently limit the hierarchical topology to be used in OSI mode. */ | |
240 | if (!psci_has_osi_support()) | |
241 | goto out; | |
242 | ||
243 | /* | |
244 | * Parse child nodes for the "#power-domain-cells" property and | |
245 | * initialize a genpd/genpd-of-provider pair when it's found. | |
246 | */ | |
247 | for_each_child_of_node(np, node) { | |
248 | if (!of_find_property(node, "#power-domain-cells", NULL)) | |
249 | continue; | |
250 | ||
251 | ret = psci_pd_init(node); | |
252 | if (ret) | |
253 | goto put_node; | |
254 | ||
255 | pd_count++; | |
256 | } | |
257 | ||
258 | /* Bail out if not using the hierarchical CPU topology. */ | |
259 | if (!pd_count) | |
260 | goto out; | |
261 | ||
262 | /* Link genpd masters/subdomains to model the CPU topology. */ | |
263 | ret = psci_pd_add_topology(np); | |
264 | if (ret) | |
265 | goto remove_pd; | |
266 | ||
267 | /* Try to enable OSI mode. */ | |
268 | ret = psci_set_osi_mode(); | |
269 | if (ret) { | |
270 | pr_warn("failed to enable OSI mode: %d\n", ret); | |
271 | psci_pd_remove_topology(np); | |
272 | goto remove_pd; | |
273 | } | |
274 | ||
275 | osi_mode_enabled = true; | |
276 | of_node_put(np); | |
277 | pr_info("Initialized CPU PM domain topology\n"); | |
278 | return pd_count; | |
279 | ||
280 | put_node: | |
281 | of_node_put(node); | |
282 | remove_pd: | |
283 | if (pd_count) | |
284 | psci_pd_remove(); | |
285 | pr_err("failed to create CPU PM domains ret=%d\n", ret); | |
286 | out: | |
287 | of_node_put(np); | |
288 | return ret; | |
289 | } | |
290 | subsys_initcall(psci_idle_init_domains); | |
291 | ||
a5e0454c UH |
292 | struct device __init *psci_dt_attach_cpu(int cpu) |
293 | { | |
294 | struct device *dev; | |
295 | ||
a65a397f UH |
296 | if (!osi_mode_enabled) |
297 | return NULL; | |
298 | ||
a5e0454c UH |
299 | dev = dev_pm_domain_attach_by_name(get_cpu_device(cpu), "psci"); |
300 | if (IS_ERR_OR_NULL(dev)) | |
301 | return dev; | |
302 | ||
303 | pm_runtime_irq_safe(dev); | |
304 | if (cpu_online(cpu)) | |
305 | pm_runtime_get_sync(dev); | |
306 | ||
307 | return dev; | |
308 | } |