1 /*
2 * Copyright (c) 2006-2022, RT-Thread Development Team
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 *
6 * Change Logs:
7 * Date Author Notes
8 * 2021-09-09 GuEe-GUI The first version
9 * 2022-09-24 GuEe-GUI Add operations and fdt init support
10 */
11
12 #include <rtthread.h>
13
14 #define DBG_TAG "osi.psci"
15 #define DBG_LVL DBG_INFO
16 #include <rtdbg.h>
17
18 /* support cpu mpidr and smccc from libcpu */
19 #include <cpu.h>
20 #include <smccc.h>
21 #include <psci.h>
22 #include <drivers/ofw.h>
23 #include <drivers/platform.h>
24 #include <drivers/core/dm.h>
25
26 struct psci_ops
27 {
28 rt_uint32_t (*get_version)(void);
29 rt_uint32_t (*cpu_on)(int cpuid, rt_ubase_t entry_point);
30 rt_uint32_t (*cpu_off)(rt_uint32_t state);
31 rt_uint32_t (*cpu_suspend)(rt_uint32_t power_state, rt_ubase_t entry_point);
32 rt_uint32_t (*migrate)(int cpuid);
33 rt_uint32_t (*get_affinity_info)(rt_ubase_t target_affinity, rt_ubase_t lowest_affinity_level);
34 rt_uint32_t (*migrate_info_type)(void);
35 };
36
37 struct psci_0_1_func_ids
38 {
39 rt_uint32_t cpu_on;
40 rt_uint32_t cpu_off;
41 rt_uint32_t cpu_suspend;
42 rt_uint32_t migrate;
43 };
44
45 typedef rt_err_t (*psci_init_ofw_handle)(struct rt_ofw_node *np);
46 typedef rt_ubase_t (*psci_call_handle)(rt_uint32_t fn, rt_ubase_t arg0, rt_ubase_t arg1, rt_ubase_t arg2);
47
48 /* [40:63] and [24:31] must be zero, other is aff3 (64bit), aff2, aff1, aff0 */
49 #ifdef ARCH_CPU_64BIT
50 #define PSCI_FNC_ID(version_major, version_min, name) PSCI_##version_major##_##version_min##_FN64_##name
51 #define MPIDR_MASK 0xff00ffffff
52 #else
53 #define PSCI_FNC_ID(version_major, version_min, name) PSCI_##version_major##_##version_min##_FN_##name
54 #define MPIDR_MASK 0x00ffffff
55 #endif
56
57 static struct psci_ops _psci_ops = {};
58
59 static struct psci_0_1_func_ids psci_0_1_func_ids = {};
60 static psci_call_handle psci_call;
61
62 /* PSCI SMCCC */
psci_smc_call(rt_uint32_t fn,rt_ubase_t arg0,rt_ubase_t arg1,rt_ubase_t arg2)63 static rt_ubase_t psci_smc_call(rt_uint32_t fn, rt_ubase_t arg0, rt_ubase_t arg1, rt_ubase_t arg2)
64 {
65 struct arm_smccc_res_t res;
66
67 arm_smccc_smc(fn, arg0, arg1, arg2, 0, 0, 0, 0, &res, RT_NULL);
68
69 return res.a0;
70 }
71
psci_hvc_call(rt_uint32_t fn,rt_ubase_t arg0,rt_ubase_t arg1,rt_ubase_t arg2)72 static rt_ubase_t psci_hvc_call(rt_uint32_t fn, rt_ubase_t arg0, rt_ubase_t arg1, rt_ubase_t arg2)
73 {
74 struct arm_smccc_res_t res;
75
76 arm_smccc_hvc(fn, arg0, arg1, arg2, 0, 0, 0, 0, &res, RT_NULL);
77
78 return res.a0;
79 }
80
81 /* PSCI VERSION */
psci_0_1_get_version(void)82 static rt_uint32_t psci_0_1_get_version(void)
83 {
84 return PSCI_VERSION(0, 1);
85 }
86
psci_0_2_get_version(void)87 static rt_uint32_t psci_0_2_get_version(void)
88 {
89 return (rt_uint32_t)psci_call(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
90 }
91
92 /* PSCI FEATURES */
psci_get_features(rt_uint32_t psci_func_id)93 static rt_uint32_t psci_get_features(rt_uint32_t psci_func_id)
94 {
95 return (rt_uint32_t)psci_call(PSCI_1_0_FN_PSCI_FEATURES, psci_func_id, 0, 0);
96 }
97
98 /* PSCI CPU_ON */
psci_cpu_on(rt_uint32_t func_id,int cpuid,rt_ubase_t entry_point)99 static rt_uint32_t psci_cpu_on(rt_uint32_t func_id, int cpuid, rt_ubase_t entry_point)
100 {
101 rt_uint32_t ret = -PSCI_RET_INVALID_PARAMETERS;
102
103 if (cpuid < RT_CPUS_NR)
104 {
105 rt_ubase_t mpid = rt_cpu_mpidr_table[cpuid] & MPIDR_MASK;
106
107 ret = (rt_uint32_t)psci_call(func_id, mpid, entry_point, 0);
108 }
109
110 return ret;
111 }
112
psci_0_1_cpu_on(int cpuid,rt_ubase_t entry_point)113 static rt_uint32_t psci_0_1_cpu_on(int cpuid, rt_ubase_t entry_point)
114 {
115 return psci_cpu_on(psci_0_1_func_ids.cpu_on, cpuid, entry_point);
116 }
117
psci_0_2_cpu_on(int cpuid,rt_ubase_t entry_point)118 static rt_uint32_t psci_0_2_cpu_on(int cpuid, rt_ubase_t entry_point)
119 {
120 return psci_cpu_on(PSCI_FNC_ID(0, 2, CPU_ON), cpuid, entry_point);
121 }
122
123 /* PSCI CPU_OFF */
psci_cpu_off(rt_uint32_t func_id,rt_uint32_t state)124 static rt_uint32_t psci_cpu_off(rt_uint32_t func_id, rt_uint32_t state)
125 {
126 return (rt_uint32_t)psci_call(func_id, state, 0, 0);
127 }
128
psci_0_1_cpu_off(rt_uint32_t state)129 static rt_uint32_t psci_0_1_cpu_off(rt_uint32_t state)
130 {
131 return psci_cpu_off(psci_0_1_func_ids.cpu_off, state);
132 }
133
psci_0_2_cpu_off(rt_uint32_t state)134 static rt_uint32_t psci_0_2_cpu_off(rt_uint32_t state)
135 {
136 return psci_cpu_off(PSCI_0_2_FN_CPU_OFF, state);
137 }
138
139 /* PSCI CPU_SUSPEND */
psci_cpu_suspend(rt_uint32_t func_id,rt_uint32_t power_state,rt_ubase_t entry_point)140 static rt_uint32_t psci_cpu_suspend(rt_uint32_t func_id, rt_uint32_t power_state, rt_ubase_t entry_point)
141 {
142 return (rt_uint32_t)psci_call(func_id, power_state, entry_point, 0);
143 }
144
psci_0_1_cpu_suspend(rt_uint32_t power_state,rt_ubase_t entry_point)145 static rt_uint32_t psci_0_1_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point)
146 {
147 return psci_cpu_suspend(psci_0_1_func_ids.cpu_suspend, power_state, entry_point);
148 }
149
psci_0_2_cpu_suspend(rt_uint32_t power_state,rt_ubase_t entry_point)150 static rt_uint32_t psci_0_2_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point)
151 {
152 return psci_cpu_suspend(PSCI_FNC_ID(0, 2, CPU_SUSPEND), power_state, entry_point);
153 }
154
155 /* PSCI CPU_MIGRATE */
psci_migrate(rt_uint32_t func_id,int cpuid)156 static rt_uint32_t psci_migrate(rt_uint32_t func_id, int cpuid)
157 {
158 rt_uint32_t ret = -PSCI_RET_INVALID_PARAMETERS;
159
160 if (cpuid < RT_CPUS_NR)
161 {
162 rt_ubase_t mpid = rt_cpu_mpidr_table[cpuid] & MPIDR_MASK;
163
164 ret = (rt_uint32_t)psci_call(func_id, mpid, 0, 0);
165 }
166
167 return ret;
168 }
169
psci_0_1_migrate(int cpuid)170 static rt_uint32_t psci_0_1_migrate(int cpuid)
171 {
172 return psci_migrate(psci_0_1_func_ids.migrate, cpuid);
173 }
174
psci_0_2_migrate(int cpuid)175 static rt_uint32_t psci_0_2_migrate(int cpuid)
176 {
177 return psci_migrate(PSCI_FNC_ID(0, 2, MIGRATE), cpuid);
178 }
179
180 /* PSCI AFFINITY_INFO */
psci_affinity_info(rt_ubase_t target_affinity,rt_ubase_t lowest_affinity_level)181 static rt_uint32_t psci_affinity_info(rt_ubase_t target_affinity, rt_ubase_t lowest_affinity_level)
182 {
183 return (rt_uint32_t)psci_call(PSCI_FNC_ID(0, 2, AFFINITY_INFO), target_affinity, lowest_affinity_level, 0);
184 }
185
186 /* PSCI MIGRATE_INFO_TYPE */
psci_migrate_info_type(void)187 static rt_uint32_t psci_migrate_info_type(void)
188 {
189 return (rt_uint32_t)psci_call(PSCI_0_2_FN_MIGRATE_INFO_TYPE, 0, 0, 0);
190 }
191
192 /* PSCI SYSTEM_OFF */
psci_system_off(void)193 void psci_system_off(void)
194 {
195 psci_call(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0);
196 }
197
198 /* PSCI SYSTEM_RESET */
psci_system_reboot(void)199 void psci_system_reboot(void)
200 {
201 if (psci_get_features(PSCI_FNC_ID(1, 1, SYSTEM_RESET2)) != PSCI_RET_NOT_SUPPORTED)
202 {
203 /*
204 * reset_type[31] = 0 (architectural)
205 * reset_type[30:0] = 0 (SYSTEM_WARM_RESET)
206 * cookie = 0 (ignored by the implementation)
207 */
208 psci_call(PSCI_FNC_ID(1, 1, SYSTEM_RESET2), 0, 0, 0);
209 }
210 else
211 {
212 psci_call(PSCI_0_2_FN_SYSTEM_RESET, 0, 0, 0);
213 }
214
215 }
216
217 #define PSCI_CALL_FN_RET(fn, ...) \
218 ({ \
219 rt_uint32_t rc; \
220 rc = PSCI_RET_NOT_SUPPORTED; \
221 if (_psci_ops.fn) \
222 rc = _psci_ops.fn(__VA_ARGS__); \
223 rc; \
224 })
225
226 #define PSCI_CALL_FN(fn, ...) \
227 ({ \
228 if (_psci_ops.fn) \
229 _psci_ops.fn(__VA_ARGS__); \
230 })
231
rt_psci_get_version(void)232 rt_uint32_t rt_psci_get_version(void)
233 {
234 return PSCI_CALL_FN_RET(get_version);
235 }
236
rt_psci_cpu_on(int cpuid,rt_ubase_t entry_point)237 rt_uint32_t rt_psci_cpu_on(int cpuid, rt_ubase_t entry_point)
238 {
239 return PSCI_CALL_FN_RET(cpu_on, cpuid, entry_point);
240 }
241
rt_psci_cpu_off(rt_uint32_t state)242 rt_uint32_t rt_psci_cpu_off(rt_uint32_t state)
243 {
244 return PSCI_CALL_FN_RET(cpu_off, state);
245 }
246
rt_psci_cpu_suspend(rt_uint32_t power_state,rt_ubase_t entry_point)247 rt_uint32_t rt_psci_cpu_suspend(rt_uint32_t power_state, rt_ubase_t entry_point)
248 {
249 return PSCI_CALL_FN_RET(cpu_suspend, power_state, entry_point);
250 }
251
rt_psci_migrate(int cpuid)252 rt_uint32_t rt_psci_migrate(int cpuid)
253 {
254 return PSCI_CALL_FN_RET(migrate, cpuid);
255 }
256
rt_psci_get_affinity_info(rt_ubase_t target_affinity,rt_ubase_t lowest_affinity_level)257 rt_uint32_t rt_psci_get_affinity_info(rt_ubase_t target_affinity, rt_ubase_t lowest_affinity_level)
258 {
259 return PSCI_CALL_FN_RET(get_affinity_info, target_affinity, lowest_affinity_level);
260 }
261
rt_psci_migrate_info_type(void)262 rt_uint32_t rt_psci_migrate_info_type(void)
263 {
264 return PSCI_CALL_FN_RET(migrate_info_type);
265 }
266
267 #undef PSCI_CALL_FN_RET
268 #undef PSCI_CALL_FN
269
270 /* PSCI INIT */
psci_0_1_init(struct rt_ofw_node * np)271 static rt_err_t psci_0_1_init(struct rt_ofw_node *np)
272 {
273 rt_err_t err = RT_EOK;
274 rt_uint32_t func_id;
275
276 _psci_ops.get_version = psci_0_1_get_version;
277
278 if (!rt_ofw_prop_read_u32(np, "cpu_on", &func_id))
279 {
280 psci_0_1_func_ids.cpu_on = func_id;
281 _psci_ops.cpu_on = psci_0_1_cpu_on;
282 }
283
284 if (!rt_ofw_prop_read_u32(np, "cpu_off", &func_id))
285 {
286 psci_0_1_func_ids.cpu_off = func_id;
287 _psci_ops.cpu_off = psci_0_1_cpu_off;
288 }
289
290 if (!rt_ofw_prop_read_u32(np, "cpu_suspend", &func_id))
291 {
292 psci_0_1_func_ids.cpu_suspend = func_id;
293 _psci_ops.cpu_suspend = psci_0_1_cpu_suspend;
294 }
295
296 if (!rt_ofw_prop_read_u32(np, "migrate", &func_id))
297 {
298 psci_0_1_func_ids.migrate = func_id;
299 _psci_ops.migrate = psci_0_1_migrate;
300 }
301
302 return err;
303 }
304
psci_0_2_init(struct rt_ofw_node * np)305 static rt_err_t psci_0_2_init(struct rt_ofw_node *np)
306 {
307 rt_err_t err = RT_EOK;
308 rt_uint32_t version = psci_0_2_get_version();
309
310 if (version >= PSCI_VERSION(0, 2))
311 {
312 _psci_ops.get_version = psci_0_2_get_version;
313 _psci_ops.cpu_on = psci_0_2_cpu_on;
314 _psci_ops.cpu_off = psci_0_2_cpu_off;
315 _psci_ops.cpu_suspend = psci_0_2_cpu_suspend;
316 _psci_ops.migrate = psci_0_2_migrate;
317 _psci_ops.get_affinity_info = psci_affinity_info;
318 _psci_ops.migrate_info_type = psci_migrate_info_type;
319 }
320 else
321 {
322 LOG_E("PSCI version detected");
323 err = -RT_EINVAL;
324 }
325
326 return err;
327 }
328
psci_1_0_init(struct rt_ofw_node * np)329 static rt_err_t psci_1_0_init(struct rt_ofw_node *np)
330 {
331 rt_err_t err;
332
333 err = psci_0_2_init(np);
334
335 return err;
336 }
337
psci_ofw_init(struct rt_platform_device * pdev)338 static rt_err_t psci_ofw_init(struct rt_platform_device *pdev)
339 {
340 rt_err_t err = RT_EOK;
341 const char *method;
342 const struct rt_ofw_node_id *id = pdev->id;
343 struct rt_ofw_node *np = pdev->parent.ofw_node;
344
345 if (!rt_ofw_prop_read_string(np, "method", &method))
346 {
347 if (!rt_strcmp(method, "smc"))
348 {
349 psci_call = psci_smc_call;
350 }
351 else if (!rt_strcmp(method, "hvc"))
352 {
353 psci_call = psci_hvc_call;
354 }
355 else
356 {
357 LOG_E("Invalid \"method\" property: %s", method);
358 err = -RT_EINVAL;
359 }
360
361 if (!err)
362 {
363 psci_init_ofw_handle psci_init = (psci_init_ofw_handle)id->data;
364
365 err = psci_init(np);
366
367 if (!err)
368 {
369 rt_uint32_t version = rt_psci_get_version();
370
371 rt_ofw_data(np) = &_psci_ops;
372
373 RT_UNUSED(version);
374
375 LOG_I("Using PSCI v%d.%d Function IDs", PSCI_VERSION_MAJOR(version), PSCI_VERSION_MINOR(version));
376 }
377 }
378 }
379 else
380 {
381 err = -RT_ENOSYS;
382 }
383
384 return err;
385 }
386
psci_probe(struct rt_platform_device * pdev)387 static rt_err_t psci_probe(struct rt_platform_device *pdev)
388 {
389 rt_err_t err;
390
391 err = psci_ofw_init(pdev);
392
393 return err;
394 }
395
396 static const struct rt_ofw_node_id psci_ofw_ids[] =
397 {
398 { .compatible = "arm,psci", .data = psci_0_1_init },
399 { .compatible = "arm,psci-0.2", .data = psci_0_2_init },
400 { .compatible = "arm,psci-1.0", .data = psci_1_0_init },
401 { /* sentinel */ }
402 };
403
404 static struct rt_platform_driver psci_driver =
405 {
406 .name = "arm-psci",
407 .ids = psci_ofw_ids,
408
409 .probe = psci_probe,
410 };
411
psci_drv_register(void)412 static int psci_drv_register(void)
413 {
414 rt_platform_driver_register(&psci_driver);
415
416 return 0;
417 }
418 INIT_PLATFORM_EXPORT(psci_drv_register);
419