1 /*
2 * Copyright (c) 2022, MediaTek Inc. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <assert.h>
8 #include <errno.h>
9 #if MTK_SIP_KERNEL_BOOT_ENABLE
10 #include <cold_boot.h>
11 #endif
12 #include <common/debug.h>
13 #include <common/runtime_svc.h>
14 #include <lib/mtk_init/mtk_init.h>
15 #include <mtk_sip_svc.h>
16
17 #define SMC_HANDLER_DEBUG(...) VERBOSE(__VA_ARGS__)
18 #define SMC_HANDLER_DEBUG_NOT_IMP_MSG "%s[0x%x] smc handler not implemented\n"
19 #define SMC_HANDLER_DEBUG_START_MSG "%s[0x%x] smc handler start, smc desc. index:%d\n"
20 #define SMC_HANDLER_DEBUG_END_MSG "%s[0x%x] smc handler end\n"
21
22 /*
23 * These macros below are used to identify SIP calls from Kernel,
24 * Hypervisor, or 2ndBootloader
25 */
26 #define SIP_FID_ORI_MASK (0xc000)
27 #define SIP_FID_ORI_SHIFT (14)
28 #define SIP_FID_KERNEL (0x0)
29 #define SIP_FID_KERNEL_VIA_GZ (0x1)
30 #define SIP_FID_GZ (0x2)
31
32 #define GET_SMC_ORI(_fid) (((_fid) & SIP_FID_ORI_MASK) >> SIP_FID_ORI_SHIFT)
33 #define GET_SMC_ORI_NUM(_fid) ((_fid) & ~(SIP_FID_ORI_MASK))
34
35 #define is_from_nsel2(_ori) (_ori == SIP_FID_GZ)
36 #define is_from_bl33(_ori) \
37 ((_ori != SIP_FID_GZ) && (is_el1_2nd_bootloader() == 1))
38 #define is_from_nsel1(_ori) \
39 (((_ori == SIP_FID_KERNEL) || \
40 (_ori == SIP_FID_KERNEL_VIA_GZ)) && \
41 (is_el1_2nd_bootloader() == 0))
42
43 #define is_smc_forbidden(_ori) (_ori == SIP_FID_KERNEL_VIA_GZ)
44
45 #define MASK_32_BIT (0xffffffffU)
46 #define SMC_ID_EXPAND_AS_SMC_OPERATION(_smc_id, _smc_num) \
47 case _smc_id##_AARCH32: \
48 { \
49 x1 = x1 & MASK_32_BIT; \
50 x2 = x2 & MASK_32_BIT; \
51 x3 = x3 & MASK_32_BIT; \
52 x4 = x4 & MASK_32_BIT; \
53 } \
54 case _smc_id##_AARCH64: \
55 { \
56 if (_smc_id##_descriptor_index < 0) { \
57 SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_NOT_IMP_MSG, #_smc_id, smc_id); \
58 break; \
59 } \
60 if (_smc_id##_descriptor_index >= smc_id_descriptor_max) { \
61 SMC_HANDLER_DEBUG("smc descriptor index[%d] exceed max[%d]\n", \
62 _smc_id##_descriptor_index, smc_id_descriptor_max); \
63 break; \
64 } \
65 SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_START_MSG, #_smc_id, smc_id, \
66 _smc_id##_descriptor_index); \
67 ret = smc_handler_pool[_smc_id##_descriptor_index].smc_handler(x1,\
68 x2, x3, x4, handle, &smc_ret); \
69 SMC_HANDLER_DEBUG(SMC_HANDLER_DEBUG_END_MSG, #_smc_id, smc_id); \
70 break; \
71 }
72
73 #define SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX(_smc_id, _smc_num) \
74 short _smc_id##_descriptor_index __section("mtk_plat_ro") = -1;
75
76 MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
77 MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_DESCRIPTOR_INDEX);
78
79 IMPORT_SYM(uintptr_t, __MTK_SMC_POOL_START__, MTK_SMC_POOL_START);
80 IMPORT_SYM(uintptr_t, __MTK_SMC_POOL_END_UNALIGNED__, MTK_SMC_POOL_END_UNALIGNED);
81
82 static const struct smc_descriptor *smc_handler_pool;
83 static short smc_id_descriptor_max;
84
85 #if !MTK_SIP_KERNEL_BOOT_ENABLE
86 /*
87 * If there is no SMC request needs to be served in 2nd bootloader,
88 * disable the service path inherently.
89 */
is_el1_2nd_bootloader(void)90 bool is_el1_2nd_bootloader(void)
91 {
92 return false;
93 }
94 #endif
95
print_smc_descriptor(const struct smc_descriptor pool[])96 static void print_smc_descriptor(const struct smc_descriptor pool[])
97 {
98 const struct smc_descriptor *p_smc_desc;
99
100 INFO("print smc descriptor pool\n");
101 for (p_smc_desc = &pool[0];
102 (char *)p_smc_desc < (char *)MTK_SMC_POOL_END_UNALIGNED;
103 p_smc_desc++) {
104 INFO("descriptor name:%s\n", p_smc_desc->smc_name);
105 INFO("descriptor index:%d\n", *p_smc_desc->smc_descriptor_index);
106 INFO("smc id 32:0x%x, smc id 64:0x%x\n",
107 p_smc_desc->smc_id_aarch32, p_smc_desc->smc_id_aarch64);
108 }
109 }
110
mtk_smc_handler_init(void)111 static int mtk_smc_handler_init(void)
112 {
113 const struct smc_descriptor *iter;
114 short index_cnt;
115 int ret = 0;
116
117 smc_handler_pool = (const struct smc_descriptor *)MTK_SMC_POOL_START;
118 /* Designate descriptor index point to smc_handler_pool */
119 for (index_cnt = 0, iter = &smc_handler_pool[0];
120 (char *)iter < (char *)MTK_SMC_POOL_END_UNALIGNED;
121 iter++, index_cnt++) {
122 if (index_cnt < 0) {
123 SMC_HANDLER_DEBUG("smc handler pool index overflow!\n");
124 ret = -EPERM;
125 assert(0);
126 break;
127 }
128 *(iter->smc_descriptor_index) = index_cnt;
129 }
130 smc_id_descriptor_max = index_cnt;
131 print_smc_descriptor(smc_handler_pool);
132 return ret;
133 }
134 MTK_EARLY_PLAT_INIT(mtk_smc_handler_init);
135
136 /* This function handles Mediatek defined SiP Calls from Bootloader */
mtk_smc_handler_bl33(uint32_t smc_id,u_register_t x1,u_register_t x2,u_register_t x3,u_register_t x4,void * cookie,void * handle,u_register_t flags)137 static uintptr_t mtk_smc_handler_bl33(uint32_t smc_id,
138 u_register_t x1,
139 u_register_t x2,
140 u_register_t x3,
141 u_register_t x4,
142 void *cookie,
143 void *handle,
144 u_register_t flags)
145 {
146 uintptr_t ret = MTK_SIP_E_SUCCESS;
147 struct smccc_res smc_ret = {0};
148
149 switch (smc_id) {
150 MTK_SIP_SMC_FROM_BL33_TABLE(SMC_ID_EXPAND_AS_SMC_OPERATION);
151 default:
152 INFO("BL33 SMC ID:0x%x not supported\n", smc_id);
153 ret = SMC_UNK;
154 break;
155 }
156 SMC_RET4(handle, ret, smc_ret.a1, smc_ret.a2, smc_ret.a3);
157 }
158
159 /* This function handles Mediatek defined SiP Calls from Kernel */
mtk_smc_handler_nsel1(uint32_t smc_id,u_register_t x1,u_register_t x2,u_register_t x3,u_register_t x4,void * cookie,void * handle,u_register_t flags)160 static uintptr_t mtk_smc_handler_nsel1(uint32_t smc_id,
161 u_register_t x1,
162 u_register_t x2,
163 u_register_t x3,
164 u_register_t x4,
165 void *cookie,
166 void *handle,
167 u_register_t flags)
168 {
169 uintptr_t ret = MTK_SIP_E_SUCCESS;
170 struct smccc_res smc_ret = {0};
171
172 switch (smc_id) {
173 MTK_SIP_SMC_FROM_NS_EL1_TABLE(SMC_ID_EXPAND_AS_SMC_OPERATION);
174 default:
175 INFO("NSEL1 SMC ID:0x%x not supported\n", smc_id);
176 ret = SMC_UNK;
177 break;
178 }
179 SMC_RET4(handle, ret, smc_ret.a1, smc_ret.a2, smc_ret.a3);
180 }
181
mtk_smc_handler(uint32_t smc_id,u_register_t x1,u_register_t x2,u_register_t x3,u_register_t x4,void * cookie,void * handle,u_register_t flags)182 static uintptr_t mtk_smc_handler(uint32_t smc_id,
183 u_register_t x1,
184 u_register_t x2,
185 u_register_t x3,
186 u_register_t x4,
187 void *cookie,
188 void *handle,
189 u_register_t flags)
190 {
191 uintptr_t ret = SMC_UNK;
192 uint32_t ns;
193 uint32_t smc_ori;
194 uint32_t smc_num;
195
196 /* Get SMC Originator bit 14.15 */
197 smc_ori = GET_SMC_ORI(smc_id);
198 /* Get SMC Number. Clean bit 14.15 */
199 smc_num = GET_SMC_ORI_NUM(smc_id);
200
201 /* Determine which security state this SMC originated from */
202 ns = is_caller_non_secure(flags);
203
204 if (ns && is_smc_forbidden(smc_ori)) {
205 ERROR("%s: Forbidden SMC call (0x%x)\n", __func__, smc_id);
206 SMC_RET1(handle, ret);
207 }
208
209 if (!ns) {
210 /* SiP SMC service secure world's call */
211 INFO("Secure SMC ID:0x%x not supported\n", smc_id);
212 SMC_RET1(handle, ret);
213 }
214 if (is_from_bl33(smc_ori)) {
215 /* SiP SMC service secure bootloader's call */
216 return mtk_smc_handler_bl33(smc_num, x1, x2, x3, x4,
217 cookie, handle, flags);
218 } else if (is_from_nsel1(smc_ori)) {
219 /* SiP SMC service kernel's call */
220 return mtk_smc_handler_nsel1(smc_num, x1, x2, x3, x4,
221 cookie, handle, flags);
222 }
223 INFO("SMC ID:0x%x not supported\n", smc_id);
224 SMC_RET1(handle, ret);
225 }
226
227 /* Define a runtime service descriptor for fast SMC calls */
228 DECLARE_RT_SVC(
229 mtk_smc_handler,
230 OEN_SIP_START,
231 OEN_SIP_END,
232 SMC_TYPE_FAST,
233 NULL,
234 mtk_smc_handler
235 );
236