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 
10 #define pr_fmt(fmt) "CPUidle PSCI: " fmt
11 
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>
17 #include <linux/psci.h>
18 #include <linux/slab.h>
19 #include <linux/string.h>
20 
21 #include "cpuidle-psci.h"
22 
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 
292 struct device __init *psci_dt_attach_cpu(int cpu)
293 {
294 	struct device *dev;
295 
296 	if (!osi_mode_enabled)
297 		return NULL;
298 
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 }
309