1 /* 2 * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved. 3 * 4 * SPDX-License-Identifier: BSD-3-Clause 5 */ 6 7 #include <assert.h> 8 #include <stdbool.h> 9 #include <string.h> 10 11 #include <arch_helpers.h> 12 #include <bl31/bl31.h> 13 #include <bl31/interrupt_mgmt.h> 14 #include <common/bl_common.h> 15 #include <common/debug.h> 16 #include <common/runtime_svc.h> 17 #include <lib/el3_runtime/context_mgmt.h> 18 #include <plat/common/platform.h> 19 20 #include "sm_err.h" 21 #include "smcall.h" 22 23 /* macro to check if Hypervisor is enabled in the HCR_EL2 register */ 24 #define HYP_ENABLE_FLAG 0x286001U 25 26 /* length of Trusty's input parameters (in bytes) */ 27 #define TRUSTY_PARAMS_LEN_BYTES (4096U * 2) 28 29 struct trusty_stack { 30 uint8_t space[PLATFORM_STACK_SIZE] __aligned(16); 31 uint32_t end; 32 }; 33 34 struct trusty_cpu_ctx { 35 cpu_context_t cpu_ctx; 36 void *saved_sp; 37 uint32_t saved_security_state; 38 int32_t fiq_handler_active; 39 uint64_t fiq_handler_pc; 40 uint64_t fiq_handler_cpsr; 41 uint64_t fiq_handler_sp; 42 uint64_t fiq_pc; 43 uint64_t fiq_cpsr; 44 uint64_t fiq_sp_el1; 45 gp_regs_t fiq_gpregs; 46 struct trusty_stack secure_stack; 47 }; 48 49 struct smc_args { 50 uint64_t r0; 51 uint64_t r1; 52 uint64_t r2; 53 uint64_t r3; 54 uint64_t r4; 55 uint64_t r5; 56 uint64_t r6; 57 uint64_t r7; 58 }; 59 60 static struct trusty_cpu_ctx trusty_cpu_ctx[PLATFORM_CORE_COUNT]; 61 62 struct smc_args trusty_init_context_stack(void **sp, void *new_stack); 63 struct smc_args trusty_context_switch_helper(void **sp, void *smc_params); 64 65 static uint32_t current_vmid; 66 67 static struct trusty_cpu_ctx *get_trusty_ctx(void) 68 { 69 return &trusty_cpu_ctx[plat_my_core_pos()]; 70 } 71 72 static bool is_hypervisor_mode(void) 73 { 74 uint64_t hcr = read_hcr(); 75 76 return ((hcr & HYP_ENABLE_FLAG) != 0U) ? true : false; 77 } 78 79 static struct smc_args trusty_context_switch(uint32_t security_state, uint64_t r0, 80 uint64_t r1, uint64_t r2, uint64_t r3) 81 { 82 struct smc_args args, ret_args; 83 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 84 struct trusty_cpu_ctx *ctx_smc; 85 86 assert(ctx->saved_security_state != security_state); 87 88 args.r7 = 0; 89 if (is_hypervisor_mode()) { 90 /* According to the ARM DEN0028A spec, VMID is stored in x7 */ 91 ctx_smc = cm_get_context(NON_SECURE); 92 assert(ctx_smc != NULL); 93 args.r7 = SMC_GET_GP(ctx_smc, CTX_GPREG_X7); 94 } 95 /* r4, r5, r6 reserved for future use. */ 96 args.r6 = 0; 97 args.r5 = 0; 98 args.r4 = 0; 99 args.r3 = r3; 100 args.r2 = r2; 101 args.r1 = r1; 102 args.r0 = r0; 103 104 /* 105 * To avoid the additional overhead in PSCI flow, skip FP context 106 * saving/restoring in case of CPU suspend and resume, assuming that 107 * when it's needed the PSCI caller has preserved FP context before 108 * going here. 109 */ 110 if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME) 111 fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state))); 112 cm_el1_sysregs_context_save(security_state); 113 114 ctx->saved_security_state = security_state; 115 ret_args = trusty_context_switch_helper(&ctx->saved_sp, &args); 116 117 assert(ctx->saved_security_state == ((security_state == 0U) ? 1U : 0U)); 118 119 cm_el1_sysregs_context_restore(security_state); 120 if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME) 121 fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state))); 122 123 cm_set_next_eret_context(security_state); 124 125 return ret_args; 126 } 127 128 static uint64_t trusty_fiq_handler(uint32_t id, 129 uint32_t flags, 130 void *handle, 131 void *cookie) 132 { 133 struct smc_args ret; 134 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 135 136 assert(!is_caller_secure(flags)); 137 138 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_ENTER, 0, 0, 0); 139 if (ret.r0 != 0U) { 140 SMC_RET0(handle); 141 } 142 143 if (ctx->fiq_handler_active != 0) { 144 INFO("%s: fiq handler already active\n", __func__); 145 SMC_RET0(handle); 146 } 147 148 ctx->fiq_handler_active = 1; 149 (void)memcpy(&ctx->fiq_gpregs, get_gpregs_ctx(handle), sizeof(ctx->fiq_gpregs)); 150 ctx->fiq_pc = SMC_GET_EL3(handle, CTX_ELR_EL3); 151 ctx->fiq_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3); 152 ctx->fiq_sp_el1 = read_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1); 153 154 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp); 155 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_handler_pc, (uint32_t)ctx->fiq_handler_cpsr); 156 157 SMC_RET0(handle); 158 } 159 160 static uint64_t trusty_set_fiq_handler(void *handle, uint64_t cpu, 161 uint64_t handler, uint64_t stack) 162 { 163 struct trusty_cpu_ctx *ctx; 164 165 if (cpu >= (uint64_t)PLATFORM_CORE_COUNT) { 166 ERROR("%s: cpu %lld >= %d\n", __func__, cpu, PLATFORM_CORE_COUNT); 167 return (uint64_t)SM_ERR_INVALID_PARAMETERS; 168 } 169 170 ctx = &trusty_cpu_ctx[cpu]; 171 ctx->fiq_handler_pc = handler; 172 ctx->fiq_handler_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3); 173 ctx->fiq_handler_sp = stack; 174 175 SMC_RET1(handle, 0); 176 } 177 178 static uint64_t trusty_get_fiq_regs(void *handle) 179 { 180 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 181 uint64_t sp_el0 = read_ctx_reg(&ctx->fiq_gpregs, CTX_GPREG_SP_EL0); 182 183 SMC_RET4(handle, ctx->fiq_pc, ctx->fiq_cpsr, sp_el0, ctx->fiq_sp_el1); 184 } 185 186 static uint64_t trusty_fiq_exit(void *handle, uint64_t x1, uint64_t x2, uint64_t x3) 187 { 188 struct smc_args ret; 189 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 190 191 if (ctx->fiq_handler_active == 0) { 192 NOTICE("%s: fiq handler not active\n", __func__); 193 SMC_RET1(handle, (uint64_t)SM_ERR_INVALID_PARAMETERS); 194 } 195 196 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_EXIT, 0, 0, 0); 197 if (ret.r0 != 1U) { 198 INFO("%s(%p) SMC_FC_FIQ_EXIT returned unexpected value, %lld\n", 199 __func__, handle, ret.r0); 200 } 201 202 /* 203 * Restore register state to state recorded on fiq entry. 204 * 205 * x0, sp_el1, pc and cpsr need to be restored because el1 cannot 206 * restore them. 207 * 208 * x1-x4 and x8-x17 need to be restored here because smc_handler64 209 * corrupts them (el1 code also restored them). 210 */ 211 (void)memcpy(get_gpregs_ctx(handle), &ctx->fiq_gpregs, sizeof(ctx->fiq_gpregs)); 212 ctx->fiq_handler_active = 0; 213 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1); 214 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_pc, (uint32_t)ctx->fiq_cpsr); 215 216 SMC_RET0(handle); 217 } 218 219 static uintptr_t trusty_smc_handler(uint32_t smc_fid, 220 u_register_t x1, 221 u_register_t x2, 222 u_register_t x3, 223 u_register_t x4, 224 void *cookie, 225 void *handle, 226 u_register_t flags) 227 { 228 struct smc_args ret; 229 uint32_t vmid = 0U; 230 entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE); 231 232 /* 233 * Return success for SET_ROT_PARAMS if Trusty is not present, as 234 * Verified Boot is not even supported and returning success here 235 * would not compromise the boot process. 236 */ 237 if ((ep_info == NULL) && (smc_fid == SMC_YC_SET_ROT_PARAMS)) { 238 SMC_RET1(handle, 0); 239 } else if (ep_info == NULL) { 240 SMC_RET1(handle, SMC_UNK); 241 } else { 242 ; /* do nothing */ 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 285 static int32_t trusty_init(void) 286 { 287 entry_point_info_t *ep_info; 288 struct smc_args zero_args = {0}; 289 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 290 uint32_t cpu = plat_my_core_pos(); 291 uint64_t reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx), 292 CTX_SPSR_EL3)); 293 294 /* 295 * Get information about the Trusty image. Its absence is a critical 296 * failure. 297 */ 298 ep_info = bl31_plat_get_next_image_ep_info(SECURE); 299 assert(ep_info != NULL); 300 301 fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE))); 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 exception vectors 310 */ 311 if ((cpu != 0U) && (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 fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE))); 319 cm_set_next_eret_context(SECURE); 320 321 ctx->saved_security_state = ~0U; /* initial saved state is invalid */ 322 (void)trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end); 323 324 (void)trusty_context_switch_helper(&ctx->saved_sp, &zero_args); 325 326 cm_el1_sysregs_context_restore(NON_SECURE); 327 fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE))); 328 cm_set_next_eret_context(NON_SECURE); 329 330 return 1; 331 } 332 333 static void trusty_cpu_suspend(uint32_t off) 334 { 335 struct smc_args ret; 336 337 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, off, 0, 0); 338 if (ret.r0 != 0U) { 339 INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %lld\n", 340 __func__, plat_my_core_pos(), ret.r0); 341 } 342 } 343 344 static void trusty_cpu_resume(uint32_t on) 345 { 346 struct smc_args ret; 347 348 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, on, 0, 0); 349 if (ret.r0 != 0U) { 350 INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %lld\n", 351 __func__, plat_my_core_pos(), ret.r0); 352 } 353 } 354 355 static int32_t trusty_cpu_off_handler(u_register_t unused) 356 { 357 trusty_cpu_suspend(1); 358 359 return 0; 360 } 361 362 static void trusty_cpu_on_finish_handler(u_register_t unused) 363 { 364 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 365 366 if (ctx->saved_sp == NULL) { 367 (void)trusty_init(); 368 } else { 369 trusty_cpu_resume(1); 370 } 371 } 372 373 static void trusty_cpu_suspend_handler(u_register_t unused) 374 { 375 trusty_cpu_suspend(0); 376 } 377 378 static void trusty_cpu_suspend_finish_handler(u_register_t unused) 379 { 380 trusty_cpu_resume(0); 381 } 382 383 static const spd_pm_ops_t trusty_pm = { 384 .svc_off = trusty_cpu_off_handler, 385 .svc_suspend = trusty_cpu_suspend_handler, 386 .svc_on_finish = trusty_cpu_on_finish_handler, 387 .svc_suspend_finish = trusty_cpu_suspend_finish_handler, 388 }; 389 390 void plat_trusty_set_boot_args(aapcs64_params_t *args); 391 392 #ifdef TSP_SEC_MEM_SIZE 393 #pragma weak plat_trusty_set_boot_args 394 void plat_trusty_set_boot_args(aapcs64_params_t *args) 395 { 396 args->arg0 = TSP_SEC_MEM_SIZE; 397 } 398 #endif 399 400 static int32_t trusty_setup(void) 401 { 402 entry_point_info_t *ep_info; 403 uint32_t instr; 404 uint32_t flags; 405 int32_t ret; 406 bool aarch32 = false; 407 408 /* Get trusty's entry point info */ 409 ep_info = bl31_plat_get_next_image_ep_info(SECURE); 410 if (ep_info == NULL) { 411 INFO("Trusty image missing.\n"); 412 return -1; 413 } 414 415 instr = *(uint32_t *)ep_info->pc; 416 417 if (instr >> 24 == 0xeaU) { 418 INFO("trusty: Found 32 bit image\n"); 419 aarch32 = true; 420 } else if (instr >> 8 == 0xd53810U || instr >> 16 == 0x9400U) { 421 INFO("trusty: Found 64 bit image\n"); 422 } else { 423 ERROR("trusty: Found unknown image, 0x%x\n", instr); 424 return -1; 425 } 426 427 SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE); 428 if (!aarch32) 429 ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, 430 DISABLE_ALL_EXCEPTIONS); 431 else 432 ep_info->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM, 433 SPSR_E_LITTLE, 434 DAIF_FIQ_BIT | 435 DAIF_IRQ_BIT | 436 DAIF_ABT_BIT); 437 (void)memset(&ep_info->args, 0, sizeof(ep_info->args)); 438 plat_trusty_set_boot_args(&ep_info->args); 439 440 /* register init handler */ 441 bl31_register_bl32_init(trusty_init); 442 443 /* register power management hooks */ 444 psci_register_spd_pm_hook(&trusty_pm); 445 446 /* register interrupt handler */ 447 flags = 0; 448 set_interrupt_rm_flag(flags, NON_SECURE); 449 ret = register_interrupt_type_handler(INTR_TYPE_S_EL1, 450 trusty_fiq_handler, 451 flags); 452 if (ret != 0) { 453 ERROR("trusty: failed to register fiq handler, ret = %d\n", ret); 454 } 455 456 if (aarch32) { 457 entry_point_info_t *ns_ep_info; 458 uint32_t spsr; 459 460 ns_ep_info = bl31_plat_get_next_image_ep_info(NON_SECURE); 461 if (ns_ep_info == NULL) { 462 NOTICE("Trusty: non-secure image missing.\n"); 463 return -1; 464 } 465 spsr = ns_ep_info->spsr; 466 if (GET_RW(spsr) == MODE_RW_64 && GET_EL(spsr) == MODE_EL2) { 467 spsr &= ~(MODE_EL_MASK << MODE_EL_SHIFT); 468 spsr |= MODE_EL1 << MODE_EL_SHIFT; 469 } 470 if (GET_RW(spsr) == MODE_RW_32 && GET_M32(spsr) == MODE32_hyp) { 471 spsr &= ~(MODE32_MASK << MODE32_SHIFT); 472 spsr |= MODE32_svc << MODE32_SHIFT; 473 } 474 if (spsr != ns_ep_info->spsr) { 475 NOTICE("Trusty: Switch bl33 from EL2 to EL1 (spsr 0x%x -> 0x%x)\n", 476 ns_ep_info->spsr, spsr); 477 ns_ep_info->spsr = spsr; 478 } 479 } 480 481 return 0; 482 } 483 484 /* Define a SPD runtime service descriptor for fast SMC calls */ 485 DECLARE_RT_SVC( 486 trusty_fast, 487 488 OEN_TOS_START, 489 SMC_ENTITY_SECURE_MONITOR, 490 SMC_TYPE_FAST, 491 trusty_setup, 492 trusty_smc_handler 493 ); 494 495 /* Define a SPD runtime service descriptor for yielding SMC calls */ 496 DECLARE_RT_SVC( 497 trusty_std, 498 499 OEN_TAP_START, 500 SMC_ENTITY_SECURE_MONITOR, 501 SMC_TYPE_YIELD, 502 NULL, 503 trusty_smc_handler 504 ); 505