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