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