psci.c
2.72 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
/*
* Copyright (c) 2006-2021, RT-Thread Development Team
*
* SPDX-License-Identifier: Apache-2.0
*
* Change Logs:
* Date Author Notes
* 2021-09-09 GuEe-GUI The first version
*/
#include <psci.h>
#include <smccc.h>
#include <armv8.h>
typedef uint64_t (*psci_call_handle)(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2);
static uint64_t psci_smc_call(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2)
{
return arm_smc_call(fn, arg0, arg1, arg2, 0, 0, 0, 0).x0;
}
static uint64_t psci_hvc_call(uint32_t fn, uint64_t arg0, uint64_t arg1, uint64_t arg2)
{
return arm_hvc_call(fn, arg0, arg1, arg2, 0, 0, 0, 0).x0;
}
static psci_call_handle psci_call = psci_smc_call;
static uint64_t shutdown_args[3] = {0, 0, 0};
static uint64_t reboot_args[3] = {0, 0, 0};
void arm_psci_init(uint64_t method, uint64_t *platform_shutdown_args, uint64_t *platform_reboot_args)
{
switch (method)
{
case PSCI_METHOD_SMC:
psci_call = psci_smc_call;
break;
case PSCI_METHOD_HVC:
if (rt_hw_get_current_el() < 2)
{
psci_call = psci_hvc_call;
}
break;
}
if (platform_shutdown_args != RT_NULL)
{
shutdown_args[0] = platform_shutdown_args[0];
shutdown_args[1] = platform_shutdown_args[1];
shutdown_args[2] = platform_shutdown_args[2];
}
if (platform_reboot_args != RT_NULL)
{
reboot_args[0] = platform_reboot_args[0];
reboot_args[1] = platform_reboot_args[1];
reboot_args[2] = platform_reboot_args[2];
}
}
uint32_t arm_psci_get_version()
{
return (uint32_t)psci_call(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
}
uint32_t arm_psci_get_affinity_info(uint64_t target_affinity, uint64_t lowest_affinity_level)
{
return (uint32_t)psci_call(PSCI_0_2_FN_AFFINITY_INFO, target_affinity, lowest_affinity_level, 0);
}
uint32_t arm_psci_get_feature(uint32_t psci_func_id)
{
return (uint32_t)psci_call(PSCI_1_0_FN_PSCI_FEATURES, psci_func_id, 0, 0);
}
uint32_t arm_psci_cpu_off(uint64_t state)
{
return (uint32_t)psci_call(PSCI_0_2_FN_CPU_OFF, state, 0, 0);
}
uint32_t arm_psci_cpu_on(uint64_t mpid, uint64_t entry)
{
/* [40:63] and [24:31] must be zero, other is aff3, aff2, aff1, aff0 */
mpid = mpid & 0xff00ffffff;
return (uint32_t)psci_call(PSCI_0_2_FN_CPU_ON, mpid, entry, 0);
}
uint32_t arm_psci_cpu_suspend(uint32_t power_state, uint64_t entry)
{
return (uint32_t)psci_call(PSCI_0_2_FN_CPU_SUSPEND, power_state, entry, 0);
}
void arm_psci_system_off()
{
psci_call(PSCI_0_2_FN_SYSTEM_OFF, shutdown_args[0], shutdown_args[1], shutdown_args[2]);
}
void arm_psci_system_reboot()
{
psci_call(PSCI_0_2_FN_SYSTEM_RESET, reboot_args[0], reboot_args[1], reboot_args[2]);
}