1 /*
2 * Copyright 2020 Carlo Caione <ccaione@baylibre.com>
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #define DT_DRV_COMPAT arm_psci_0_2
8
9 #define LOG_LEVEL CONFIG_PM_CPU_OPS_LOG_LEVEL
10 #include <logging/log.h>
11 LOG_MODULE_REGISTER(psci);
12
13 #include <kernel.h>
14 #include <arch/cpu.h>
15
16 #include <soc.h>
17 #include <device.h>
18 #include <init.h>
19
20 #include <drivers/pm_cpu_ops.h>
21 #include "pm_cpu_ops_psci.h"
22
23 static struct psci psci_data;
24
psci_to_dev_err(int ret)25 static int psci_to_dev_err(int ret)
26 {
27 switch (ret) {
28 case PSCI_RET_SUCCESS:
29 return 0;
30 case PSCI_RET_NOT_SUPPORTED:
31 return -ENOTSUP;
32 case PSCI_RET_INVALID_PARAMS:
33 case PSCI_RET_INVALID_ADDRESS:
34 return -EINVAL;
35 case PSCI_RET_DENIED:
36 return -EPERM;
37 }
38
39 return -EINVAL;
40 }
41
pm_cpu_off(void)42 int pm_cpu_off(void)
43 {
44 int ret;
45
46 if (psci_data.conduit == SMCCC_CONDUIT_NONE)
47 return -EINVAL;
48
49 ret = psci_data.invoke_psci_fn(PSCI_0_2_FN_CPU_OFF, 0, 0, 0);
50
51 return psci_to_dev_err(ret);
52 }
53
pm_cpu_on(unsigned long cpuid,uintptr_t entry_point)54 int pm_cpu_on(unsigned long cpuid,
55 uintptr_t entry_point)
56 {
57 int ret;
58
59 if (psci_data.conduit == SMCCC_CONDUIT_NONE)
60 return -EINVAL;
61
62 ret = psci_data.invoke_psci_fn(PSCI_FN_NATIVE(0_2, CPU_ON), cpuid,
63 (unsigned long) entry_point, 0);
64
65 return psci_to_dev_err(ret);
66 }
67
__invoke_psci_fn_hvc(unsigned long function_id,unsigned long arg0,unsigned long arg1,unsigned long arg2)68 static unsigned long __invoke_psci_fn_hvc(unsigned long function_id,
69 unsigned long arg0,
70 unsigned long arg1,
71 unsigned long arg2)
72 {
73 struct arm_smccc_res res;
74
75 arm_smccc_hvc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res);
76 return res.a0;
77 }
78
__invoke_psci_fn_smc(unsigned long function_id,unsigned long arg0,unsigned long arg1,unsigned long arg2)79 static unsigned long __invoke_psci_fn_smc(unsigned long function_id,
80 unsigned long arg0,
81 unsigned long arg1,
82 unsigned long arg2)
83 {
84 struct arm_smccc_res res;
85
86 arm_smccc_smc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res);
87 return res.a0;
88 }
89
psci_get_version(void)90 static uint32_t psci_get_version(void)
91 {
92 return psci_data.invoke_psci_fn(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
93 }
94
set_conduit_method(void)95 static int set_conduit_method(void)
96 {
97 const char *method;
98
99 method = DT_PROP(DT_INST(0, DT_DRV_COMPAT), method);
100
101 if (!strcmp("hvc", method)) {
102 psci_data.conduit = SMCCC_CONDUIT_HVC;
103 psci_data.invoke_psci_fn = __invoke_psci_fn_hvc;
104 } else if (!strcmp("smc", method)) {
105 psci_data.conduit = SMCCC_CONDUIT_SMC;
106 psci_data.invoke_psci_fn = __invoke_psci_fn_smc;
107 } else {
108 LOG_ERR("Invalid conduit method");
109 return -EINVAL;
110 }
111
112 return 0;
113 }
114
psci_detect(void)115 static int psci_detect(void)
116 {
117 uint32_t ver = psci_get_version();
118
119 LOG_DBG("Detected PSCIv%d.%d",
120 PSCI_VERSION_MAJOR(ver),
121 PSCI_VERSION_MINOR(ver));
122
123 if (PSCI_VERSION_MAJOR(ver) == 0 && PSCI_VERSION_MINOR(ver) < 2) {
124 LOG_ERR("PSCI unsupported version");
125 return -ENOTSUP;
126 }
127
128 psci_data.ver = ver;
129
130 return 0;
131 }
132
psci_version(void)133 uint32_t psci_version(void)
134 {
135 return psci_data.ver;
136 }
137
psci_init(const struct device * dev)138 static int psci_init(const struct device *dev)
139 {
140 psci_data.conduit = SMCCC_CONDUIT_NONE;
141
142 if (set_conduit_method()) {
143 return -ENOTSUP;
144 }
145
146 return psci_detect();
147 }
148
149 DEVICE_DT_INST_DEFINE(0, psci_init, NULL,
150 &psci_data, NULL, PRE_KERNEL_1, CONFIG_KERNEL_INIT_PRIORITY_DEVICE,
151 NULL);
152