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 65 static struct trusty_cpu_ctx *get_trusty_ctx(void) 66 { 67 return &trusty_cpu_ctx[plat_my_core_pos()]; 68 } 69 70 static uint32_t is_hypervisor_mode(void) 71 { 72 uint64_t hcr = read_hcr(); 73 74 return !!(hcr & HYP_ENABLE_FLAG); 75 } 76 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 (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME) 109 fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state))); 110 cm_el1_sysregs_context_save(security_state); 111 112 ctx->saved_security_state = security_state; 113 ret = trusty_context_switch_helper(&ctx->saved_sp, &ret); 114 115 assert(ctx->saved_security_state == !security_state); 116 117 cm_el1_sysregs_context_restore(security_state); 118 if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME) 119 fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state))); 120 121 cm_set_next_eret_context(security_state); 122 123 return ret; 124 } 125 126 static uint64_t trusty_fiq_handler(uint32_t id, 127 uint32_t flags, 128 void *handle, 129 void *cookie) 130 { 131 struct args ret; 132 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 133 134 assert(!is_caller_secure(flags)); 135 136 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_ENTER, 0, 0, 0); 137 if (ret.r0) { 138 SMC_RET0(handle); 139 } 140 141 if (ctx->fiq_handler_active) { 142 INFO("%s: fiq handler already active\n", __func__); 143 SMC_RET0(handle); 144 } 145 146 ctx->fiq_handler_active = 1; 147 memcpy(&ctx->fiq_gpregs, get_gpregs_ctx(handle), sizeof(ctx->fiq_gpregs)); 148 ctx->fiq_pc = SMC_GET_EL3(handle, CTX_ELR_EL3); 149 ctx->fiq_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3); 150 ctx->fiq_sp_el1 = read_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1); 151 152 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp); 153 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_handler_pc, ctx->fiq_handler_cpsr); 154 155 SMC_RET0(handle); 156 } 157 158 static uint64_t trusty_set_fiq_handler(void *handle, uint64_t cpu, 159 uint64_t handler, uint64_t stack) 160 { 161 struct trusty_cpu_ctx *ctx; 162 163 if (cpu >= PLATFORM_CORE_COUNT) { 164 ERROR("%s: cpu %ld >= %d\n", __func__, cpu, PLATFORM_CORE_COUNT); 165 return SM_ERR_INVALID_PARAMETERS; 166 } 167 168 ctx = &trusty_cpu_ctx[cpu]; 169 ctx->fiq_handler_pc = handler; 170 ctx->fiq_handler_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3); 171 ctx->fiq_handler_sp = stack; 172 173 SMC_RET1(handle, 0); 174 } 175 176 static uint64_t trusty_get_fiq_regs(void *handle) 177 { 178 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 179 uint64_t sp_el0 = read_ctx_reg(&ctx->fiq_gpregs, CTX_GPREG_SP_EL0); 180 181 SMC_RET4(handle, ctx->fiq_pc, ctx->fiq_cpsr, sp_el0, ctx->fiq_sp_el1); 182 } 183 184 static uint64_t trusty_fiq_exit(void *handle, uint64_t x1, uint64_t x2, uint64_t x3) 185 { 186 struct args ret; 187 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 188 189 if (!ctx->fiq_handler_active) { 190 NOTICE("%s: fiq handler not active\n", __func__); 191 SMC_RET1(handle, SM_ERR_INVALID_PARAMETERS); 192 } 193 194 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_EXIT, 0, 0, 0); 195 if (ret.r0 != 1) { 196 INFO("%s(%p) SMC_FC_FIQ_EXIT returned unexpected value, %ld\n", 197 __func__, handle, ret.r0); 198 } 199 200 /* 201 * Restore register state to state recorded on fiq entry. 202 * 203 * x0, sp_el1, pc and cpsr need to be restored because el1 cannot 204 * restore them. 205 * 206 * x1-x4 and x8-x17 need to be restored here because smc_handler64 207 * corrupts them (el1 code also restored them). 208 */ 209 memcpy(get_gpregs_ctx(handle), &ctx->fiq_gpregs, sizeof(ctx->fiq_gpregs)); 210 ctx->fiq_handler_active = 0; 211 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1); 212 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_pc, ctx->fiq_cpsr); 213 214 SMC_RET0(handle); 215 } 216 217 static uint64_t trusty_smc_handler(uint32_t smc_fid, 218 uint64_t x1, 219 uint64_t x2, 220 uint64_t x3, 221 uint64_t x4, 222 void *cookie, 223 void *handle, 224 uint64_t flags) 225 { 226 struct args ret; 227 uint32_t vmid = 0; 228 entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE); 229 230 /* 231 * Return success for SET_ROT_PARAMS if Trusty is not present, as 232 * Verified Boot is not even supported and returning success here 233 * would not compromise the boot process. 234 */ 235 if (!ep_info && (smc_fid == SMC_YC_SET_ROT_PARAMS)) { 236 SMC_RET1(handle, 0); 237 } else if (!ep_info) { 238 SMC_RET1(handle, SMC_UNK); 239 } 240 241 if (is_caller_secure(flags)) { 242 if (smc_fid == SMC_YC_NS_RETURN) { 243 ret = trusty_context_switch(SECURE, x1, 0, 0, 0); 244 SMC_RET8(handle, ret.r0, ret.r1, ret.r2, ret.r3, 245 ret.r4, ret.r5, ret.r6, ret.r7); 246 } 247 INFO("%s (0x%x, 0x%lx, 0x%lx, 0x%lx, 0x%lx, %p, %p, 0x%lx) \ 248 cpu %d, unknown smc\n", 249 __func__, smc_fid, x1, x2, x3, x4, cookie, handle, flags, 250 plat_my_core_pos()); 251 SMC_RET1(handle, SMC_UNK); 252 } else { 253 switch (smc_fid) { 254 case SMC_FC64_SET_FIQ_HANDLER: 255 return trusty_set_fiq_handler(handle, x1, x2, x3); 256 case SMC_FC64_GET_FIQ_REGS: 257 return trusty_get_fiq_regs(handle); 258 case SMC_FC_FIQ_EXIT: 259 return trusty_fiq_exit(handle, x1, x2, x3); 260 default: 261 if (is_hypervisor_mode()) 262 vmid = SMC_GET_GP(handle, CTX_GPREG_X7); 263 264 if ((current_vmid != 0) && (current_vmid != vmid)) { 265 /* This message will cause SMC mechanism 266 * abnormal in multi-guest environment. 267 * Change it to WARN in case you need it. 268 */ 269 VERBOSE("Previous SMC not finished.\n"); 270 SMC_RET1(handle, SM_ERR_BUSY); 271 } 272 current_vmid = vmid; 273 ret = trusty_context_switch(NON_SECURE, smc_fid, x1, 274 x2, x3); 275 current_vmid = 0; 276 SMC_RET1(handle, ret.r0); 277 } 278 } 279 } 280 281 static int32_t trusty_init(void) 282 { 283 void el3_exit(void); 284 entry_point_info_t *ep_info; 285 struct args zero_args = {0}; 286 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 287 uint32_t cpu = plat_my_core_pos(); 288 int reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx), 289 CTX_SPSR_EL3)); 290 291 /* 292 * Get information about the Trusty image. Its absence is a critical 293 * failure. 294 */ 295 ep_info = bl31_plat_get_next_image_ep_info(SECURE); 296 assert(ep_info); 297 298 fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE))); 299 cm_el1_sysregs_context_save(NON_SECURE); 300 301 cm_set_context(&ctx->cpu_ctx, SECURE); 302 cm_init_my_context(ep_info); 303 304 /* 305 * Adjust secondary cpu entry point for 32 bit images to the 306 * end of exeption vectors 307 */ 308 if ((cpu != 0) && (reg_width == MODE_RW_32)) { 309 INFO("trusty: cpu %d, adjust entry point to 0x%lx\n", 310 cpu, ep_info->pc + (1U << 5)); 311 cm_set_elr_el3(SECURE, ep_info->pc + (1U << 5)); 312 } 313 314 cm_el1_sysregs_context_restore(SECURE); 315 fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE))); 316 cm_set_next_eret_context(SECURE); 317 318 ctx->saved_security_state = ~0; /* initial saved state is invalid */ 319 trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end); 320 321 trusty_context_switch_helper(&ctx->saved_sp, &zero_args); 322 323 cm_el1_sysregs_context_restore(NON_SECURE); 324 fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE))); 325 cm_set_next_eret_context(NON_SECURE); 326 327 return 0; 328 } 329 330 static void trusty_cpu_suspend(uint32_t off) 331 { 332 struct args ret; 333 334 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, off, 0, 0); 335 if (ret.r0 != 0) { 336 INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %ld\n", 337 __func__, plat_my_core_pos(), ret.r0); 338 } 339 } 340 341 static void trusty_cpu_resume(uint32_t on) 342 { 343 struct args ret; 344 345 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, on, 0, 0); 346 if (ret.r0 != 0) { 347 INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %ld\n", 348 __func__, plat_my_core_pos(), ret.r0); 349 } 350 } 351 352 static int32_t trusty_cpu_off_handler(uint64_t unused) 353 { 354 trusty_cpu_suspend(1); 355 356 return 0; 357 } 358 359 static void trusty_cpu_on_finish_handler(uint64_t unused) 360 { 361 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 362 363 if (!ctx->saved_sp) { 364 trusty_init(); 365 } else { 366 trusty_cpu_resume(1); 367 } 368 } 369 370 static void trusty_cpu_suspend_handler(uint64_t unused) 371 { 372 trusty_cpu_suspend(0); 373 } 374 375 static void trusty_cpu_suspend_finish_handler(uint64_t unused) 376 { 377 trusty_cpu_resume(0); 378 } 379 380 static const spd_pm_ops_t trusty_pm = { 381 .svc_off = trusty_cpu_off_handler, 382 .svc_suspend = trusty_cpu_suspend_handler, 383 .svc_on_finish = trusty_cpu_on_finish_handler, 384 .svc_suspend_finish = trusty_cpu_suspend_finish_handler, 385 }; 386 387 void plat_trusty_set_boot_args(aapcs64_params_t *args); 388 389 #ifdef TSP_SEC_MEM_SIZE 390 #pragma weak plat_trusty_set_boot_args 391 void plat_trusty_set_boot_args(aapcs64_params_t *args) 392 { 393 args->arg0 = TSP_SEC_MEM_SIZE; 394 } 395 #endif 396 397 static int32_t trusty_setup(void) 398 { 399 entry_point_info_t *ep_info; 400 uint32_t instr; 401 uint32_t flags; 402 int ret; 403 int aarch32 = 0; 404 405 /* Get trusty's entry point info */ 406 ep_info = bl31_plat_get_next_image_ep_info(SECURE); 407 if (!ep_info) { 408 INFO("Trusty image missing.\n"); 409 return -1; 410 } 411 412 instr = *(uint32_t *)ep_info->pc; 413 414 if (instr >> 24 == 0xea) { 415 INFO("trusty: Found 32 bit image\n"); 416 aarch32 = 1; 417 } else if (instr >> 8 == 0xd53810 || instr >> 16 == 0x9400) { 418 INFO("trusty: Found 64 bit image\n"); 419 } else { 420 NOTICE("trusty: Found unknown image, 0x%x\n", instr); 421 } 422 423 SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE); 424 if (!aarch32) 425 ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, 426 DISABLE_ALL_EXCEPTIONS); 427 else 428 ep_info->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM, 429 SPSR_E_LITTLE, 430 DAIF_FIQ_BIT | 431 DAIF_IRQ_BIT | 432 DAIF_ABT_BIT); 433 memset(&ep_info->args, 0, sizeof(ep_info->args)); 434 plat_trusty_set_boot_args(&ep_info->args); 435 436 /* register init handler */ 437 bl31_register_bl32_init(trusty_init); 438 439 /* register power management hooks */ 440 psci_register_spd_pm_hook(&trusty_pm); 441 442 /* register interrupt handler */ 443 flags = 0; 444 set_interrupt_rm_flag(flags, NON_SECURE); 445 ret = register_interrupt_type_handler(INTR_TYPE_S_EL1, 446 trusty_fiq_handler, 447 flags); 448 if (ret) 449 ERROR("trusty: failed to register fiq handler, ret = %d\n", ret); 450 451 if (aarch32) { 452 entry_point_info_t *ns_ep_info; 453 uint32_t spsr; 454 455 ns_ep_info = bl31_plat_get_next_image_ep_info(NON_SECURE); 456 if (!ep_info) { 457 NOTICE("Trusty: non-secure image missing.\n"); 458 return -1; 459 } 460 spsr = ns_ep_info->spsr; 461 if (GET_RW(spsr) == MODE_RW_64 && GET_EL(spsr) == MODE_EL2) { 462 spsr &= ~(MODE_EL_MASK << MODE_EL_SHIFT); 463 spsr |= MODE_EL1 << MODE_EL_SHIFT; 464 } 465 if (GET_RW(spsr) == MODE_RW_32 && GET_M32(spsr) == MODE32_hyp) { 466 spsr &= ~(MODE32_MASK << MODE32_SHIFT); 467 spsr |= MODE32_svc << MODE32_SHIFT; 468 } 469 if (spsr != ns_ep_info->spsr) { 470 NOTICE("Trusty: Switch bl33 from EL2 to EL1 (spsr 0x%x -> 0x%x)\n", 471 ns_ep_info->spsr, spsr); 472 ns_ep_info->spsr = spsr; 473 } 474 } 475 476 return 0; 477 } 478 479 /* Define a SPD runtime service descriptor for fast SMC calls */ 480 DECLARE_RT_SVC( 481 trusty_fast, 482 483 OEN_TOS_START, 484 SMC_ENTITY_SECURE_MONITOR, 485 SMC_TYPE_FAST, 486 trusty_setup, 487 trusty_smc_handler 488 ); 489 490 /* Define a SPD runtime service descriptor for yielding SMC calls */ 491 DECLARE_RT_SVC( 492 trusty_std, 493 494 OEN_TAP_START, 495 SMC_ENTITY_SECURE_MONITOR, 496 SMC_TYPE_YIELD, 497 NULL, 498 trusty_smc_handler 499 ); 500