Commit | Line | Data |
---|---|---|
e790f1de WD |
1 | /* |
2 | * This program is free software; you can redistribute it and/or modify | |
3 | * it under the terms of the GNU General Public License version 2 as | |
4 | * published by the Free Software Foundation. | |
5 | * | |
6 | * This program is distributed in the hope that it will be useful, | |
7 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
8 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
9 | * GNU General Public License for more details. | |
10 | * | |
11 | * Copyright (C) 2013 ARM Limited | |
12 | * | |
13 | * Author: Will Deacon <will.deacon@arm.com> | |
14 | */ | |
15 | ||
16 | #define pr_fmt(fmt) "psci: " fmt | |
17 | ||
18 | #include <linux/init.h> | |
19 | #include <linux/of.h> | |
00ef54bb | 20 | #include <linux/smp.h> |
e790f1de WD |
21 | |
22 | #include <asm/compiler.h> | |
cd1aebf5 | 23 | #include <asm/cpu_ops.h> |
e790f1de WD |
24 | #include <asm/errno.h> |
25 | #include <asm/psci.h> | |
00ef54bb | 26 | #include <asm/smp_plat.h> |
e790f1de | 27 | |
00ef54bb MR |
28 | #define PSCI_POWER_STATE_TYPE_STANDBY 0 |
29 | #define PSCI_POWER_STATE_TYPE_POWER_DOWN 1 | |
30 | ||
31 | struct psci_power_state { | |
32 | u16 id; | |
33 | u8 type; | |
34 | u8 affinity_level; | |
35 | }; | |
36 | ||
37 | struct psci_operations { | |
38 | int (*cpu_suspend)(struct psci_power_state state, | |
39 | unsigned long entry_point); | |
40 | int (*cpu_off)(struct psci_power_state state); | |
41 | int (*cpu_on)(unsigned long cpuid, unsigned long entry_point); | |
42 | int (*migrate)(unsigned long cpuid); | |
43 | }; | |
44 | ||
45 | static struct psci_operations psci_ops; | |
e790f1de WD |
46 | |
47 | static int (*invoke_psci_fn)(u64, u64, u64, u64); | |
48 | ||
49 | enum psci_function { | |
50 | PSCI_FN_CPU_SUSPEND, | |
51 | PSCI_FN_CPU_ON, | |
52 | PSCI_FN_CPU_OFF, | |
53 | PSCI_FN_MIGRATE, | |
54 | PSCI_FN_MAX, | |
55 | }; | |
56 | ||
57 | static u32 psci_function_id[PSCI_FN_MAX]; | |
58 | ||
59 | #define PSCI_RET_SUCCESS 0 | |
60 | #define PSCI_RET_EOPNOTSUPP -1 | |
61 | #define PSCI_RET_EINVAL -2 | |
62 | #define PSCI_RET_EPERM -3 | |
63 | ||
64 | static int psci_to_linux_errno(int errno) | |
65 | { | |
66 | switch (errno) { | |
67 | case PSCI_RET_SUCCESS: | |
68 | return 0; | |
69 | case PSCI_RET_EOPNOTSUPP: | |
70 | return -EOPNOTSUPP; | |
71 | case PSCI_RET_EINVAL: | |
72 | return -EINVAL; | |
73 | case PSCI_RET_EPERM: | |
74 | return -EPERM; | |
75 | }; | |
76 | ||
77 | return -EINVAL; | |
78 | } | |
79 | ||
80 | #define PSCI_POWER_STATE_ID_MASK 0xffff | |
81 | #define PSCI_POWER_STATE_ID_SHIFT 0 | |
82 | #define PSCI_POWER_STATE_TYPE_MASK 0x1 | |
83 | #define PSCI_POWER_STATE_TYPE_SHIFT 16 | |
84 | #define PSCI_POWER_STATE_AFFL_MASK 0x3 | |
85 | #define PSCI_POWER_STATE_AFFL_SHIFT 24 | |
86 | ||
87 | static u32 psci_power_state_pack(struct psci_power_state state) | |
88 | { | |
89 | return ((state.id & PSCI_POWER_STATE_ID_MASK) | |
90 | << PSCI_POWER_STATE_ID_SHIFT) | | |
91 | ((state.type & PSCI_POWER_STATE_TYPE_MASK) | |
92 | << PSCI_POWER_STATE_TYPE_SHIFT) | | |
93 | ((state.affinity_level & PSCI_POWER_STATE_AFFL_MASK) | |
94 | << PSCI_POWER_STATE_AFFL_SHIFT); | |
95 | } | |
96 | ||
97 | /* | |
98 | * The following two functions are invoked via the invoke_psci_fn pointer | |
99 | * and will not be inlined, allowing us to piggyback on the AAPCS. | |
100 | */ | |
101 | static noinline int __invoke_psci_fn_hvc(u64 function_id, u64 arg0, u64 arg1, | |
102 | u64 arg2) | |
103 | { | |
104 | asm volatile( | |
105 | __asmeq("%0", "x0") | |
106 | __asmeq("%1", "x1") | |
107 | __asmeq("%2", "x2") | |
108 | __asmeq("%3", "x3") | |
109 | "hvc #0\n" | |
110 | : "+r" (function_id) | |
111 | : "r" (arg0), "r" (arg1), "r" (arg2)); | |
112 | ||
113 | return function_id; | |
114 | } | |
115 | ||
116 | static noinline int __invoke_psci_fn_smc(u64 function_id, u64 arg0, u64 arg1, | |
117 | u64 arg2) | |
118 | { | |
119 | asm volatile( | |
120 | __asmeq("%0", "x0") | |
121 | __asmeq("%1", "x1") | |
122 | __asmeq("%2", "x2") | |
123 | __asmeq("%3", "x3") | |
124 | "smc #0\n" | |
125 | : "+r" (function_id) | |
126 | : "r" (arg0), "r" (arg1), "r" (arg2)); | |
127 | ||
128 | return function_id; | |
129 | } | |
130 | ||
131 | static int psci_cpu_suspend(struct psci_power_state state, | |
132 | unsigned long entry_point) | |
133 | { | |
134 | int err; | |
135 | u32 fn, power_state; | |
136 | ||
137 | fn = psci_function_id[PSCI_FN_CPU_SUSPEND]; | |
138 | power_state = psci_power_state_pack(state); | |
139 | err = invoke_psci_fn(fn, power_state, entry_point, 0); | |
140 | return psci_to_linux_errno(err); | |
141 | } | |
142 | ||
143 | static int psci_cpu_off(struct psci_power_state state) | |
144 | { | |
145 | int err; | |
146 | u32 fn, power_state; | |
147 | ||
148 | fn = psci_function_id[PSCI_FN_CPU_OFF]; | |
149 | power_state = psci_power_state_pack(state); | |
150 | err = invoke_psci_fn(fn, power_state, 0, 0); | |
151 | return psci_to_linux_errno(err); | |
152 | } | |
153 | ||
154 | static int psci_cpu_on(unsigned long cpuid, unsigned long entry_point) | |
155 | { | |
156 | int err; | |
157 | u32 fn; | |
158 | ||
159 | fn = psci_function_id[PSCI_FN_CPU_ON]; | |
160 | err = invoke_psci_fn(fn, cpuid, entry_point, 0); | |
161 | return psci_to_linux_errno(err); | |
162 | } | |
163 | ||
164 | static int psci_migrate(unsigned long cpuid) | |
165 | { | |
166 | int err; | |
167 | u32 fn; | |
168 | ||
169 | fn = psci_function_id[PSCI_FN_MIGRATE]; | |
170 | err = invoke_psci_fn(fn, cpuid, 0, 0); | |
171 | return psci_to_linux_errno(err); | |
172 | } | |
173 | ||
174 | static const struct of_device_id psci_of_match[] __initconst = { | |
175 | { .compatible = "arm,psci", }, | |
176 | {}, | |
177 | }; | |
178 | ||
179 | int __init psci_init(void) | |
180 | { | |
181 | struct device_node *np; | |
182 | const char *method; | |
183 | u32 id; | |
184 | int err = 0; | |
185 | ||
186 | np = of_find_matching_node(NULL, psci_of_match); | |
187 | if (!np) | |
188 | return -ENODEV; | |
189 | ||
190 | pr_info("probing function IDs from device-tree\n"); | |
191 | ||
192 | if (of_property_read_string(np, "method", &method)) { | |
193 | pr_warning("missing \"method\" property\n"); | |
194 | err = -ENXIO; | |
195 | goto out_put_node; | |
196 | } | |
197 | ||
198 | if (!strcmp("hvc", method)) { | |
199 | invoke_psci_fn = __invoke_psci_fn_hvc; | |
200 | } else if (!strcmp("smc", method)) { | |
201 | invoke_psci_fn = __invoke_psci_fn_smc; | |
202 | } else { | |
203 | pr_warning("invalid \"method\" property: %s\n", method); | |
204 | err = -EINVAL; | |
205 | goto out_put_node; | |
206 | } | |
207 | ||
208 | if (!of_property_read_u32(np, "cpu_suspend", &id)) { | |
209 | psci_function_id[PSCI_FN_CPU_SUSPEND] = id; | |
210 | psci_ops.cpu_suspend = psci_cpu_suspend; | |
211 | } | |
212 | ||
213 | if (!of_property_read_u32(np, "cpu_off", &id)) { | |
214 | psci_function_id[PSCI_FN_CPU_OFF] = id; | |
215 | psci_ops.cpu_off = psci_cpu_off; | |
216 | } | |
217 | ||
218 | if (!of_property_read_u32(np, "cpu_on", &id)) { | |
219 | psci_function_id[PSCI_FN_CPU_ON] = id; | |
220 | psci_ops.cpu_on = psci_cpu_on; | |
221 | } | |
222 | ||
223 | if (!of_property_read_u32(np, "migrate", &id)) { | |
224 | psci_function_id[PSCI_FN_MIGRATE] = id; | |
225 | psci_ops.migrate = psci_migrate; | |
226 | } | |
227 | ||
228 | out_put_node: | |
229 | of_node_put(np); | |
230 | return err; | |
231 | } | |
00ef54bb MR |
232 | |
233 | #ifdef CONFIG_SMP | |
234 | ||
cd1aebf5 | 235 | static int __init cpu_psci_cpu_init(struct device_node *dn, unsigned int cpu) |
00ef54bb MR |
236 | { |
237 | return 0; | |
238 | } | |
239 | ||
cd1aebf5 | 240 | static int __init cpu_psci_cpu_prepare(unsigned int cpu) |
00ef54bb | 241 | { |
00ef54bb MR |
242 | if (!psci_ops.cpu_on) { |
243 | pr_err("no cpu_on method, not booting CPU%d\n", cpu); | |
244 | return -ENODEV; | |
245 | } | |
246 | ||
00ef54bb MR |
247 | return 0; |
248 | } | |
249 | ||
652af899 MR |
250 | static int cpu_psci_cpu_boot(unsigned int cpu) |
251 | { | |
252 | int err = psci_ops.cpu_on(cpu_logical_map(cpu), __pa(secondary_entry)); | |
253 | if (err) | |
254 | pr_err("psci: failed to boot CPU%d (%d)\n", cpu, err); | |
255 | ||
256 | return err; | |
257 | } | |
258 | ||
831ccf79 MR |
259 | #ifdef CONFIG_HOTPLUG_CPU |
260 | static int cpu_psci_cpu_disable(unsigned int cpu) | |
261 | { | |
262 | /* Fail early if we don't have CPU_OFF support */ | |
263 | if (!psci_ops.cpu_off) | |
264 | return -EOPNOTSUPP; | |
265 | return 0; | |
266 | } | |
267 | ||
268 | static void cpu_psci_cpu_die(unsigned int cpu) | |
269 | { | |
270 | int ret; | |
271 | /* | |
272 | * There are no known implementations of PSCI actually using the | |
273 | * power state field, pass a sensible default for now. | |
274 | */ | |
275 | struct psci_power_state state = { | |
276 | .type = PSCI_POWER_STATE_TYPE_POWER_DOWN, | |
277 | }; | |
278 | ||
279 | ret = psci_ops.cpu_off(state); | |
280 | ||
281 | pr_crit("psci: unable to power off CPU%u (%d)\n", cpu, ret); | |
282 | } | |
283 | #endif | |
284 | ||
cd1aebf5 | 285 | const struct cpu_operations cpu_psci_ops = { |
00ef54bb | 286 | .name = "psci", |
cd1aebf5 MR |
287 | .cpu_init = cpu_psci_cpu_init, |
288 | .cpu_prepare = cpu_psci_cpu_prepare, | |
652af899 | 289 | .cpu_boot = cpu_psci_cpu_boot, |
831ccf79 MR |
290 | #ifdef CONFIG_HOTPLUG_CPU |
291 | .cpu_disable = cpu_psci_cpu_disable, | |
292 | .cpu_die = cpu_psci_cpu_die, | |
293 | #endif | |
00ef54bb MR |
294 | }; |
295 | ||
296 | #endif |