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