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 <bl_common.h> 10 #include <bl31.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 "smcall.h" 19 #include "sm_err.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 cm_el1_sysregs_context_save(security_state); 103 104 ctx->saved_security_state = security_state; 105 ret = trusty_context_switch_helper(&ctx->saved_sp, &ret); 106 107 assert(ctx->saved_security_state == !security_state); 108 109 cm_el1_sysregs_context_restore(security_state); 110 cm_set_next_eret_context(security_state); 111 112 return ret; 113 } 114 115 static uint64_t trusty_fiq_handler(uint32_t id, 116 uint32_t flags, 117 void *handle, 118 void *cookie) 119 { 120 struct args ret; 121 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 122 123 assert(!is_caller_secure(flags)); 124 125 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_ENTER, 0, 0, 0); 126 if (ret.r0) { 127 SMC_RET0(handle); 128 } 129 130 if (ctx->fiq_handler_active) { 131 INFO("%s: fiq handler already active\n", __func__); 132 SMC_RET0(handle); 133 } 134 135 ctx->fiq_handler_active = 1; 136 memcpy(&ctx->fiq_gpregs, get_gpregs_ctx(handle), sizeof(ctx->fiq_gpregs)); 137 ctx->fiq_pc = SMC_GET_EL3(handle, CTX_ELR_EL3); 138 ctx->fiq_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3); 139 ctx->fiq_sp_el1 = read_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1); 140 141 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp); 142 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_handler_pc, ctx->fiq_handler_cpsr); 143 144 SMC_RET0(handle); 145 } 146 147 static uint64_t trusty_set_fiq_handler(void *handle, uint64_t cpu, 148 uint64_t handler, uint64_t stack) 149 { 150 struct trusty_cpu_ctx *ctx; 151 152 if (cpu >= PLATFORM_CORE_COUNT) { 153 ERROR("%s: cpu %ld >= %d\n", __func__, cpu, PLATFORM_CORE_COUNT); 154 return SM_ERR_INVALID_PARAMETERS; 155 } 156 157 ctx = &trusty_cpu_ctx[cpu]; 158 ctx->fiq_handler_pc = handler; 159 ctx->fiq_handler_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3); 160 ctx->fiq_handler_sp = stack; 161 162 SMC_RET1(handle, 0); 163 } 164 165 static uint64_t trusty_get_fiq_regs(void *handle) 166 { 167 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 168 uint64_t sp_el0 = read_ctx_reg(&ctx->fiq_gpregs, CTX_GPREG_SP_EL0); 169 170 SMC_RET4(handle, ctx->fiq_pc, ctx->fiq_cpsr, sp_el0, ctx->fiq_sp_el1); 171 } 172 173 static uint64_t trusty_fiq_exit(void *handle, uint64_t x1, uint64_t x2, uint64_t x3) 174 { 175 struct args ret; 176 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 177 178 if (!ctx->fiq_handler_active) { 179 NOTICE("%s: fiq handler not active\n", __func__); 180 SMC_RET1(handle, SM_ERR_INVALID_PARAMETERS); 181 } 182 183 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_EXIT, 0, 0, 0); 184 if (ret.r0 != 1) { 185 INFO("%s(%p) SMC_FC_FIQ_EXIT returned unexpected value, %ld\n", 186 __func__, handle, ret.r0); 187 } 188 189 /* 190 * Restore register state to state recorded on fiq entry. 191 * 192 * x0, sp_el1, pc and cpsr need to be restored because el1 cannot 193 * restore them. 194 * 195 * x1-x4 and x8-x17 need to be restored here because smc_handler64 196 * corrupts them (el1 code also restored them). 197 */ 198 memcpy(get_gpregs_ctx(handle), &ctx->fiq_gpregs, sizeof(ctx->fiq_gpregs)); 199 ctx->fiq_handler_active = 0; 200 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1); 201 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_pc, ctx->fiq_cpsr); 202 203 SMC_RET0(handle); 204 } 205 206 static uint64_t trusty_smc_handler(uint32_t smc_fid, 207 uint64_t x1, 208 uint64_t x2, 209 uint64_t x3, 210 uint64_t x4, 211 void *cookie, 212 void *handle, 213 uint64_t flags) 214 { 215 struct args ret; 216 uint32_t vmid = 0; 217 entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE); 218 219 /* 220 * Return success for SET_ROT_PARAMS if Trusty is not present, as 221 * Verified Boot is not even supported and returning success here 222 * would not compromise the boot process. 223 */ 224 if (!ep_info && (smc_fid == SMC_SC_SET_ROT_PARAMS)) { 225 SMC_RET1(handle, 0); 226 } else if (!ep_info) { 227 SMC_RET1(handle, SMC_UNK); 228 } 229 230 if (is_caller_secure(flags)) { 231 if (smc_fid == SMC_SC_NS_RETURN) { 232 ret = trusty_context_switch(SECURE, x1, 0, 0, 0); 233 SMC_RET8(handle, ret.r0, ret.r1, ret.r2, ret.r3, 234 ret.r4, ret.r5, ret.r6, ret.r7); 235 } 236 INFO("%s (0x%x, 0x%lx, 0x%lx, 0x%lx, 0x%lx, %p, %p, 0x%lx) \ 237 cpu %d, unknown smc\n", 238 __func__, smc_fid, x1, x2, x3, x4, cookie, handle, flags, 239 plat_my_core_pos()); 240 SMC_RET1(handle, SMC_UNK); 241 } else { 242 switch (smc_fid) { 243 case SMC_FC64_SET_FIQ_HANDLER: 244 return trusty_set_fiq_handler(handle, x1, x2, x3); 245 case SMC_FC64_GET_FIQ_REGS: 246 return trusty_get_fiq_regs(handle); 247 case SMC_FC_FIQ_EXIT: 248 return trusty_fiq_exit(handle, x1, x2, x3); 249 default: 250 if (is_hypervisor_mode()) 251 vmid = SMC_GET_GP(handle, CTX_GPREG_X7); 252 253 if ((current_vmid != 0) && (current_vmid != vmid)) { 254 /* This message will cause SMC mechanism 255 * abnormal in multi-guest environment. 256 * Change it to WARN in case you need it. 257 */ 258 VERBOSE("Previous SMC not finished.\n"); 259 SMC_RET1(handle, SM_ERR_BUSY); 260 } 261 current_vmid = vmid; 262 ret = trusty_context_switch(NON_SECURE, smc_fid, x1, 263 x2, x3); 264 current_vmid = 0; 265 SMC_RET1(handle, ret.r0); 266 } 267 } 268 } 269 270 static int32_t trusty_init(void) 271 { 272 void el3_exit(void); 273 entry_point_info_t *ep_info; 274 struct args zero_args = {0}; 275 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 276 uint32_t cpu = plat_my_core_pos(); 277 int reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx), 278 CTX_SPSR_EL3)); 279 280 /* 281 * Get information about the Trusty image. Its absence is a critical 282 * failure. 283 */ 284 ep_info = bl31_plat_get_next_image_ep_info(SECURE); 285 assert(ep_info); 286 287 cm_el1_sysregs_context_save(NON_SECURE); 288 289 cm_set_context(&ctx->cpu_ctx, SECURE); 290 cm_init_my_context(ep_info); 291 292 /* 293 * Adjust secondary cpu entry point for 32 bit images to the 294 * end of exeption vectors 295 */ 296 if ((cpu != 0) && (reg_width == MODE_RW_32)) { 297 INFO("trusty: cpu %d, adjust entry point to 0x%lx\n", 298 cpu, ep_info->pc + (1U << 5)); 299 cm_set_elr_el3(SECURE, ep_info->pc + (1U << 5)); 300 } 301 302 cm_el1_sysregs_context_restore(SECURE); 303 cm_set_next_eret_context(SECURE); 304 305 ctx->saved_security_state = ~0; /* initial saved state is invalid */ 306 trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end); 307 308 trusty_context_switch_helper(&ctx->saved_sp, &zero_args); 309 310 cm_el1_sysregs_context_restore(NON_SECURE); 311 cm_set_next_eret_context(NON_SECURE); 312 313 return 0; 314 } 315 316 static void trusty_cpu_suspend(void) 317 { 318 struct args ret; 319 320 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, 0, 0, 0); 321 if (ret.r0 != 0) { 322 INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %ld\n", 323 __func__, plat_my_core_pos(), ret.r0); 324 } 325 } 326 327 static void trusty_cpu_resume(void) 328 { 329 struct args ret; 330 331 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, 0, 0, 0); 332 if (ret.r0 != 0) { 333 INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %ld\n", 334 __func__, plat_my_core_pos(), ret.r0); 335 } 336 } 337 338 static int32_t trusty_cpu_off_handler(uint64_t unused) 339 { 340 trusty_cpu_suspend(); 341 342 return 0; 343 } 344 345 static void trusty_cpu_on_finish_handler(uint64_t unused) 346 { 347 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 348 349 if (!ctx->saved_sp) { 350 trusty_init(); 351 } else { 352 trusty_cpu_resume(); 353 } 354 } 355 356 static void trusty_cpu_suspend_handler(uint64_t unused) 357 { 358 trusty_cpu_suspend(); 359 } 360 361 static void trusty_cpu_suspend_finish_handler(uint64_t unused) 362 { 363 trusty_cpu_resume(); 364 } 365 366 static const spd_pm_ops_t trusty_pm = { 367 .svc_off = trusty_cpu_off_handler, 368 .svc_suspend = trusty_cpu_suspend_handler, 369 .svc_on_finish = trusty_cpu_on_finish_handler, 370 .svc_suspend_finish = trusty_cpu_suspend_finish_handler, 371 }; 372 373 static int32_t trusty_setup(void) 374 { 375 entry_point_info_t *ep_info; 376 uint32_t flags; 377 int ret; 378 379 /* Get trusty's entry point info */ 380 ep_info = bl31_plat_get_next_image_ep_info(SECURE); 381 if (!ep_info) { 382 INFO("Trusty image missing.\n"); 383 return -1; 384 } 385 386 /* Trusty runs in AARCH64 mode */ 387 SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE); 388 ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); 389 390 /* 391 * arg0 = TZDRAM aperture available for BL32 392 * arg1 = BL32 boot params 393 * arg2 = BL32 boot params length 394 */ 395 ep_info->args.arg1 = ep_info->args.arg2; 396 ep_info->args.arg2 = TRUSTY_PARAMS_LEN_BYTES; 397 398 /* register init handler */ 399 bl31_register_bl32_init(trusty_init); 400 401 /* register power management hooks */ 402 psci_register_spd_pm_hook(&trusty_pm); 403 404 /* register interrupt handler */ 405 flags = 0; 406 set_interrupt_rm_flag(flags, NON_SECURE); 407 ret = register_interrupt_type_handler(INTR_TYPE_S_EL1, 408 trusty_fiq_handler, 409 flags); 410 if (ret) 411 ERROR("trusty: failed to register fiq handler, ret = %d\n", ret); 412 413 return 0; 414 } 415 416 /* Define a SPD runtime service descriptor for fast SMC calls */ 417 DECLARE_RT_SVC( 418 trusty_fast, 419 420 OEN_TOS_START, 421 SMC_ENTITY_SECURE_MONITOR, 422 SMC_TYPE_FAST, 423 trusty_setup, 424 trusty_smc_handler 425 ); 426 427 /* Define a SPD runtime service descriptor for standard SMC calls */ 428 DECLARE_RT_SVC( 429 trusty_std, 430 431 OEN_TAP_START, 432 SMC_ENTITY_SECURE_MONITOR, 433 SMC_TYPE_STD, 434 NULL, 435 trusty_smc_handler 436 ); 437