1 /*
2 * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <arch_helpers.h>
8 #include <assert.h> /* for context_mgmt.h */
9 #include <bl31.h>
10 #include <bl_common.h>
11 #include <context_mgmt.h>
12 #include <debug.h>
13 #include <interrupt_mgmt.h>
14 #include <platform.h>
15 #include <runtime_svc.h>
16 #include <string.h>
17
18 #include "sm_err.h"
19 #include "smcall.h"
20
21 /* macro to check if Hypervisor is enabled in the HCR_EL2 register */
22 #define HYP_ENABLE_FLAG 0x286001
23
24 /* length of Trusty's input parameters (in bytes) */
25 #define TRUSTY_PARAMS_LEN_BYTES (4096*2)
26
27 struct trusty_stack {
28 uint8_t space[PLATFORM_STACK_SIZE] __aligned(16);
29 uint32_t end;
30 };
31
32 struct trusty_cpu_ctx {
33 cpu_context_t cpu_ctx;
34 void *saved_sp;
35 uint32_t saved_security_state;
36 int fiq_handler_active;
37 uint64_t fiq_handler_pc;
38 uint64_t fiq_handler_cpsr;
39 uint64_t fiq_handler_sp;
40 uint64_t fiq_pc;
41 uint64_t fiq_cpsr;
42 uint64_t fiq_sp_el1;
43 gp_regs_t fiq_gpregs;
44 struct trusty_stack secure_stack;
45 };
46
47 struct args {
48 uint64_t r0;
49 uint64_t r1;
50 uint64_t r2;
51 uint64_t r3;
52 uint64_t r4;
53 uint64_t r5;
54 uint64_t r6;
55 uint64_t r7;
56 };
57
58 struct trusty_cpu_ctx trusty_cpu_ctx[PLATFORM_CORE_COUNT];
59
60 struct args trusty_init_context_stack(void **sp, void *new_stack);
61 struct args trusty_context_switch_helper(void **sp, void *smc_params);
62
63 static uint32_t current_vmid;
64
get_trusty_ctx(void)65 static struct trusty_cpu_ctx *get_trusty_ctx(void)
66 {
67 return &trusty_cpu_ctx[plat_my_core_pos()];
68 }
69
is_hypervisor_mode(void)70 static uint32_t is_hypervisor_mode(void)
71 {
72 uint64_t hcr = read_hcr();
73
74 return !!(hcr & HYP_ENABLE_FLAG);
75 }
76
trusty_context_switch(uint32_t security_state,uint64_t r0,uint64_t r1,uint64_t r2,uint64_t r3)77 static struct args trusty_context_switch(uint32_t security_state, uint64_t r0,
78 uint64_t r1, uint64_t r2, uint64_t r3)
79 {
80 struct args ret;
81 struct trusty_cpu_ctx *ctx = get_trusty_ctx();
82 struct trusty_cpu_ctx *ctx_smc;
83
84 assert(ctx->saved_security_state != security_state);
85
86 ret.r7 = 0;
87 if (is_hypervisor_mode()) {
88 /* According to the ARM DEN0028A spec, VMID is stored in x7 */
89 ctx_smc = cm_get_context(NON_SECURE);
90 assert(ctx_smc);
91 ret.r7 = SMC_GET_GP(ctx_smc, CTX_GPREG_X7);
92 }
93 /* r4, r5, r6 reserved for future use. */
94 ret.r6 = 0;
95 ret.r5 = 0;
96 ret.r4 = 0;
97 ret.r3 = r3;
98 ret.r2 = r2;
99 ret.r1 = r1;
100 ret.r0 = r0;
101
102 /*
103 * To avoid the additional overhead in PSCI flow, skip FP context
104 * saving/restoring in case of CPU suspend and resume, asssuming that
105 * when it's needed the PSCI caller has preserved FP context before
106 * going here.
107 */
108 #if CTX_INCLUDE_FPREGS
109 if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
110 fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state)));
111 #endif
112 cm_el1_sysregs_context_save(security_state);
113
114 ctx->saved_security_state = security_state;
115 ret = trusty_context_switch_helper(&ctx->saved_sp, &ret);
116
117 assert(ctx->saved_security_state == !security_state);
118
119 cm_el1_sysregs_context_restore(security_state);
120 #if CTX_INCLUDE_FPREGS
121 if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
122 fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state)));
123 #endif
124
125 cm_set_next_eret_context(security_state);
126
127 return ret;
128 }
129
trusty_fiq_handler(uint32_t id,uint32_t flags,void * handle,void * cookie)130 static uint64_t trusty_fiq_handler(uint32_t id,
131 uint32_t flags,
132 void *handle,
133 void *cookie)
134 {
135 struct args ret;
136 struct trusty_cpu_ctx *ctx = get_trusty_ctx();
137
138 assert(!is_caller_secure(flags));
139
140 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_ENTER, 0, 0, 0);
141 if (ret.r0) {
142 SMC_RET0(handle);
143 }
144
145 if (ctx->fiq_handler_active) {
146 INFO("%s: fiq handler already active\n", __func__);
147 SMC_RET0(handle);
148 }
149
150 ctx->fiq_handler_active = 1;
151 memcpy(&ctx->fiq_gpregs, get_gpregs_ctx(handle), sizeof(ctx->fiq_gpregs));
152 ctx->fiq_pc = SMC_GET_EL3(handle, CTX_ELR_EL3);
153 ctx->fiq_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3);
154 ctx->fiq_sp_el1 = read_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1);
155
156 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp);
157 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_handler_pc, ctx->fiq_handler_cpsr);
158
159 SMC_RET0(handle);
160 }
161
trusty_set_fiq_handler(void * handle,uint64_t cpu,uint64_t handler,uint64_t stack)162 static uint64_t trusty_set_fiq_handler(void *handle, uint64_t cpu,
163 uint64_t handler, uint64_t stack)
164 {
165 struct trusty_cpu_ctx *ctx;
166
167 if (cpu >= PLATFORM_CORE_COUNT) {
168 ERROR("%s: cpu %ld >= %d\n", __func__, cpu, PLATFORM_CORE_COUNT);
169 return SM_ERR_INVALID_PARAMETERS;
170 }
171
172 ctx = &trusty_cpu_ctx[cpu];
173 ctx->fiq_handler_pc = handler;
174 ctx->fiq_handler_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3);
175 ctx->fiq_handler_sp = stack;
176
177 SMC_RET1(handle, 0);
178 }
179
trusty_get_fiq_regs(void * handle)180 static uint64_t trusty_get_fiq_regs(void *handle)
181 {
182 struct trusty_cpu_ctx *ctx = get_trusty_ctx();
183 uint64_t sp_el0 = read_ctx_reg(&ctx->fiq_gpregs, CTX_GPREG_SP_EL0);
184
185 SMC_RET4(handle, ctx->fiq_pc, ctx->fiq_cpsr, sp_el0, ctx->fiq_sp_el1);
186 }
187
trusty_fiq_exit(void * handle,uint64_t x1,uint64_t x2,uint64_t x3)188 static uint64_t trusty_fiq_exit(void *handle, uint64_t x1, uint64_t x2, uint64_t x3)
189 {
190 struct args ret;
191 struct trusty_cpu_ctx *ctx = get_trusty_ctx();
192
193 if (!ctx->fiq_handler_active) {
194 NOTICE("%s: fiq handler not active\n", __func__);
195 SMC_RET1(handle, SM_ERR_INVALID_PARAMETERS);
196 }
197
198 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_EXIT, 0, 0, 0);
199 if (ret.r0 != 1) {
200 INFO("%s(%p) SMC_FC_FIQ_EXIT returned unexpected value, %ld\n",
201 __func__, handle, ret.r0);
202 }
203
204 /*
205 * Restore register state to state recorded on fiq entry.
206 *
207 * x0, sp_el1, pc and cpsr need to be restored because el1 cannot
208 * restore them.
209 *
210 * x1-x4 and x8-x17 need to be restored here because smc_handler64
211 * corrupts them (el1 code also restored them).
212 */
213 memcpy(get_gpregs_ctx(handle), &ctx->fiq_gpregs, sizeof(ctx->fiq_gpregs));
214 ctx->fiq_handler_active = 0;
215 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1);
216 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_pc, ctx->fiq_cpsr);
217
218 SMC_RET0(handle);
219 }
220
trusty_smc_handler(uint32_t smc_fid,uint64_t x1,uint64_t x2,uint64_t x3,uint64_t x4,void * cookie,void * handle,uint64_t flags)221 static uint64_t trusty_smc_handler(uint32_t smc_fid,
222 uint64_t x1,
223 uint64_t x2,
224 uint64_t x3,
225 uint64_t x4,
226 void *cookie,
227 void *handle,
228 uint64_t flags)
229 {
230 struct args ret;
231 uint32_t vmid = 0;
232 entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE);
233
234 /*
235 * Return success for SET_ROT_PARAMS if Trusty is not present, as
236 * Verified Boot is not even supported and returning success here
237 * would not compromise the boot process.
238 */
239 if (!ep_info && (smc_fid == SMC_YC_SET_ROT_PARAMS)) {
240 SMC_RET1(handle, 0);
241 } else if (!ep_info) {
242 SMC_RET1(handle, SMC_UNK);
243 }
244
245 if (is_caller_secure(flags)) {
246 if (smc_fid == SMC_YC_NS_RETURN) {
247 ret = trusty_context_switch(SECURE, x1, 0, 0, 0);
248 SMC_RET8(handle, ret.r0, ret.r1, ret.r2, ret.r3,
249 ret.r4, ret.r5, ret.r6, ret.r7);
250 }
251 INFO("%s (0x%x, 0x%lx, 0x%lx, 0x%lx, 0x%lx, %p, %p, 0x%lx) \
252 cpu %d, unknown smc\n",
253 __func__, smc_fid, x1, x2, x3, x4, cookie, handle, flags,
254 plat_my_core_pos());
255 SMC_RET1(handle, SMC_UNK);
256 } else {
257 switch (smc_fid) {
258 case SMC_FC64_SET_FIQ_HANDLER:
259 return trusty_set_fiq_handler(handle, x1, x2, x3);
260 case SMC_FC64_GET_FIQ_REGS:
261 return trusty_get_fiq_regs(handle);
262 case SMC_FC_FIQ_EXIT:
263 return trusty_fiq_exit(handle, x1, x2, x3);
264 default:
265 if (is_hypervisor_mode())
266 vmid = SMC_GET_GP(handle, CTX_GPREG_X7);
267
268 if ((current_vmid != 0) && (current_vmid != vmid)) {
269 /* This message will cause SMC mechanism
270 * abnormal in multi-guest environment.
271 * Change it to WARN in case you need it.
272 */
273 VERBOSE("Previous SMC not finished.\n");
274 SMC_RET1(handle, SM_ERR_BUSY);
275 }
276 current_vmid = vmid;
277 ret = trusty_context_switch(NON_SECURE, smc_fid, x1,
278 x2, x3);
279 current_vmid = 0;
280 SMC_RET1(handle, ret.r0);
281 }
282 }
283 }
284
trusty_init(void)285 static int32_t trusty_init(void)
286 {
287 void el3_exit(void);
288 entry_point_info_t *ep_info;
289 struct args zero_args = {0};
290 struct trusty_cpu_ctx *ctx = get_trusty_ctx();
291 uint32_t cpu = plat_my_core_pos();
292 int reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx),
293 CTX_SPSR_EL3));
294
295 /*
296 * Get information about the Trusty image. Its absence is a critical
297 * failure.
298 */
299 ep_info = bl31_plat_get_next_image_ep_info(SECURE);
300 assert(ep_info);
301
302 cm_el1_sysregs_context_save(NON_SECURE);
303
304 cm_set_context(&ctx->cpu_ctx, SECURE);
305 cm_init_my_context(ep_info);
306
307 /*
308 * Adjust secondary cpu entry point for 32 bit images to the
309 * end of exeption vectors
310 */
311 if ((cpu != 0) && (reg_width == MODE_RW_32)) {
312 INFO("trusty: cpu %d, adjust entry point to 0x%lx\n",
313 cpu, ep_info->pc + (1U << 5));
314 cm_set_elr_el3(SECURE, ep_info->pc + (1U << 5));
315 }
316
317 cm_el1_sysregs_context_restore(SECURE);
318 cm_set_next_eret_context(SECURE);
319
320 ctx->saved_security_state = ~0; /* initial saved state is invalid */
321 trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end);
322
323 trusty_context_switch_helper(&ctx->saved_sp, &zero_args);
324
325 cm_el1_sysregs_context_restore(NON_SECURE);
326 cm_set_next_eret_context(NON_SECURE);
327
328 return 0;
329 }
330
trusty_cpu_suspend(void)331 static void trusty_cpu_suspend(void)
332 {
333 struct args ret;
334
335 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, 0, 0, 0);
336 if (ret.r0 != 0) {
337 INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %ld\n",
338 __func__, plat_my_core_pos(), ret.r0);
339 }
340 }
341
trusty_cpu_resume(void)342 static void trusty_cpu_resume(void)
343 {
344 struct args ret;
345
346 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, 0, 0, 0);
347 if (ret.r0 != 0) {
348 INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %ld\n",
349 __func__, plat_my_core_pos(), ret.r0);
350 }
351 }
352
trusty_cpu_off_handler(uint64_t unused)353 static int32_t trusty_cpu_off_handler(uint64_t unused)
354 {
355 trusty_cpu_suspend();
356
357 return 0;
358 }
359
trusty_cpu_on_finish_handler(uint64_t unused)360 static void trusty_cpu_on_finish_handler(uint64_t unused)
361 {
362 struct trusty_cpu_ctx *ctx = get_trusty_ctx();
363
364 if (!ctx->saved_sp) {
365 trusty_init();
366 } else {
367 trusty_cpu_resume();
368 }
369 }
370
trusty_cpu_suspend_handler(uint64_t unused)371 static void trusty_cpu_suspend_handler(uint64_t unused)
372 {
373 trusty_cpu_suspend();
374 }
375
trusty_cpu_suspend_finish_handler(uint64_t unused)376 static void trusty_cpu_suspend_finish_handler(uint64_t unused)
377 {
378 trusty_cpu_resume();
379 }
380
381 static const spd_pm_ops_t trusty_pm = {
382 .svc_off = trusty_cpu_off_handler,
383 .svc_suspend = trusty_cpu_suspend_handler,
384 .svc_on_finish = trusty_cpu_on_finish_handler,
385 .svc_suspend_finish = trusty_cpu_suspend_finish_handler,
386 };
387
trusty_setup(void)388 static int32_t trusty_setup(void)
389 {
390 entry_point_info_t *ep_info;
391 uint32_t flags;
392 int ret;
393
394 /* Get trusty's entry point info */
395 ep_info = bl31_plat_get_next_image_ep_info(SECURE);
396 if (!ep_info) {
397 INFO("Trusty image missing.\n");
398 return -1;
399 }
400
401 /* Trusty runs in AARCH64 mode */
402 SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE);
403 ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
404
405 /*
406 * arg0 = TZDRAM aperture available for BL32
407 * arg1 = BL32 boot params
408 * arg2 = BL32 boot params length
409 */
410 ep_info->args.arg1 = ep_info->args.arg2;
411 ep_info->args.arg2 = TRUSTY_PARAMS_LEN_BYTES;
412
413 /* register init handler */
414 bl31_register_bl32_init(trusty_init);
415
416 /* register power management hooks */
417 psci_register_spd_pm_hook(&trusty_pm);
418
419 /* register interrupt handler */
420 flags = 0;
421 set_interrupt_rm_flag(flags, NON_SECURE);
422 ret = register_interrupt_type_handler(INTR_TYPE_S_EL1,
423 trusty_fiq_handler,
424 flags);
425 if (ret)
426 ERROR("trusty: failed to register fiq handler, ret = %d\n", ret);
427
428 return 0;
429 }
430
431 /* Define a SPD runtime service descriptor for fast SMC calls */
432 DECLARE_RT_SVC(
433 trusty_fast,
434
435 OEN_TOS_START,
436 SMC_ENTITY_SECURE_MONITOR,
437 SMC_TYPE_FAST,
438 trusty_setup,
439 trusty_smc_handler
440 );
441
442 /* Define a SPD runtime service descriptor for yielding SMC calls */
443 DECLARE_RT_SVC(
444 trusty_std,
445
446 OEN_TAP_START,
447 SMC_ENTITY_SECURE_MONITOR,
448 SMC_TYPE_YIELD,
449 NULL,
450 trusty_smc_handler
451 );
452