1 /* 2 * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved. 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions are met: 6 * 7 * Redistributions of source code must retain the above copyright notice, this 8 * list of conditions and the following disclaimer. 9 * 10 * Redistributions in binary form must reproduce the above copyright notice, 11 * this list of conditions and the following disclaimer in the documentation 12 * and/or other materials provided with the distribution. 13 * 14 * Neither the name of ARM nor the names of its contributors may be used 15 * to endorse or promote products derived from this software without specific 16 * prior written permission. 17 * 18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 19 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 20 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 21 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 22 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 23 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 24 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 25 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 26 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 27 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 28 * POSSIBILITY OF SUCH DAMAGE. 29 */ 30 31 #include <arch_helpers.h> 32 #include <assert.h> /* for context_mgmt.h */ 33 #include <bl_common.h> 34 #include <bl31.h> 35 #include <context_mgmt.h> 36 #include <debug.h> 37 #include <interrupt_mgmt.h> 38 #include <platform.h> 39 #include <runtime_svc.h> 40 #include <string.h> 41 42 #include "smcall.h" 43 #include "sm_err.h" 44 45 /* macro to check if Hypervisor is enabled in the HCR_EL2 register */ 46 #define HYP_ENABLE_FLAG 0x286001 47 48 /* length of Trusty's input parameters (in bytes) */ 49 #define TRUSTY_PARAMS_LEN_BYTES (4096*2) 50 51 struct trusty_stack { 52 uint8_t space[PLATFORM_STACK_SIZE] __aligned(16); 53 uint32_t end; 54 }; 55 56 struct trusty_cpu_ctx { 57 cpu_context_t cpu_ctx; 58 void *saved_sp; 59 uint32_t saved_security_state; 60 int fiq_handler_active; 61 uint64_t fiq_handler_pc; 62 uint64_t fiq_handler_cpsr; 63 uint64_t fiq_handler_sp; 64 uint64_t fiq_pc; 65 uint64_t fiq_cpsr; 66 uint64_t fiq_sp_el1; 67 gp_regs_t fiq_gpregs; 68 struct trusty_stack secure_stack; 69 }; 70 71 struct args { 72 uint64_t r0; 73 uint64_t r1; 74 uint64_t r2; 75 uint64_t r3; 76 uint64_t r4; 77 uint64_t r5; 78 uint64_t r6; 79 uint64_t r7; 80 }; 81 82 struct trusty_cpu_ctx trusty_cpu_ctx[PLATFORM_CORE_COUNT]; 83 84 struct args trusty_init_context_stack(void **sp, void *new_stack); 85 struct args trusty_context_switch_helper(void **sp, void *smc_params); 86 87 static uint32_t current_vmid; 88 89 static struct trusty_cpu_ctx *get_trusty_ctx(void) 90 { 91 return &trusty_cpu_ctx[plat_my_core_pos()]; 92 } 93 94 static uint32_t is_hypervisor_mode(void) 95 { 96 uint64_t hcr = read_hcr(); 97 98 return !!(hcr & HYP_ENABLE_FLAG); 99 } 100 101 static struct args trusty_context_switch(uint32_t security_state, uint64_t r0, 102 uint64_t r1, uint64_t r2, uint64_t r3) 103 { 104 struct args ret; 105 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 106 struct trusty_cpu_ctx *ctx_smc; 107 108 assert(ctx->saved_security_state != security_state); 109 110 ret.r7 = 0; 111 if (is_hypervisor_mode()) { 112 /* According to the ARM DEN0028A spec, VMID is stored in x7 */ 113 ctx_smc = cm_get_context(NON_SECURE); 114 assert(ctx_smc); 115 ret.r7 = SMC_GET_GP(ctx_smc, CTX_GPREG_X7); 116 } 117 /* r4, r5, r6 reserved for future use. */ 118 ret.r6 = 0; 119 ret.r5 = 0; 120 ret.r4 = 0; 121 ret.r3 = r3; 122 ret.r2 = r2; 123 ret.r1 = r1; 124 ret.r0 = r0; 125 126 cm_el1_sysregs_context_save(security_state); 127 128 ctx->saved_security_state = security_state; 129 ret = trusty_context_switch_helper(&ctx->saved_sp, &ret); 130 131 assert(ctx->saved_security_state == !security_state); 132 133 cm_el1_sysregs_context_restore(security_state); 134 cm_set_next_eret_context(security_state); 135 136 return ret; 137 } 138 139 static uint64_t trusty_fiq_handler(uint32_t id, 140 uint32_t flags, 141 void *handle, 142 void *cookie) 143 { 144 struct args ret; 145 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 146 147 assert(!is_caller_secure(flags)); 148 149 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_ENTER, 0, 0, 0); 150 if (ret.r0) { 151 SMC_RET0(handle); 152 } 153 154 if (ctx->fiq_handler_active) { 155 INFO("%s: fiq handler already active\n", __func__); 156 SMC_RET0(handle); 157 } 158 159 ctx->fiq_handler_active = 1; 160 memcpy(&ctx->fiq_gpregs, get_gpregs_ctx(handle), sizeof(ctx->fiq_gpregs)); 161 ctx->fiq_pc = SMC_GET_EL3(handle, CTX_ELR_EL3); 162 ctx->fiq_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3); 163 ctx->fiq_sp_el1 = read_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1); 164 165 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp); 166 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_handler_pc, ctx->fiq_handler_cpsr); 167 168 SMC_RET0(handle); 169 } 170 171 static uint64_t trusty_set_fiq_handler(void *handle, uint64_t cpu, 172 uint64_t handler, uint64_t stack) 173 { 174 struct trusty_cpu_ctx *ctx; 175 176 if (cpu >= PLATFORM_CORE_COUNT) { 177 ERROR("%s: cpu %ld >= %d\n", __func__, cpu, PLATFORM_CORE_COUNT); 178 return SM_ERR_INVALID_PARAMETERS; 179 } 180 181 ctx = &trusty_cpu_ctx[cpu]; 182 ctx->fiq_handler_pc = handler; 183 ctx->fiq_handler_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3); 184 ctx->fiq_handler_sp = stack; 185 186 SMC_RET1(handle, 0); 187 } 188 189 static uint64_t trusty_get_fiq_regs(void *handle) 190 { 191 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 192 uint64_t sp_el0 = read_ctx_reg(&ctx->fiq_gpregs, CTX_GPREG_SP_EL0); 193 194 SMC_RET4(handle, ctx->fiq_pc, ctx->fiq_cpsr, sp_el0, ctx->fiq_sp_el1); 195 } 196 197 static uint64_t trusty_fiq_exit(void *handle, uint64_t x1, uint64_t x2, uint64_t x3) 198 { 199 struct args ret; 200 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 201 202 if (!ctx->fiq_handler_active) { 203 NOTICE("%s: fiq handler not active\n", __func__); 204 SMC_RET1(handle, SM_ERR_INVALID_PARAMETERS); 205 } 206 207 ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_EXIT, 0, 0, 0); 208 if (ret.r0 != 1) { 209 INFO("%s(%p) SMC_FC_FIQ_EXIT returned unexpected value, %ld\n", 210 __func__, handle, ret.r0); 211 } 212 213 /* 214 * Restore register state to state recorded on fiq entry. 215 * 216 * x0, sp_el1, pc and cpsr need to be restored because el1 cannot 217 * restore them. 218 * 219 * x1-x4 and x8-x17 need to be restored here because smc_handler64 220 * corrupts them (el1 code also restored them). 221 */ 222 memcpy(get_gpregs_ctx(handle), &ctx->fiq_gpregs, sizeof(ctx->fiq_gpregs)); 223 ctx->fiq_handler_active = 0; 224 write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1); 225 cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_pc, ctx->fiq_cpsr); 226 227 SMC_RET0(handle); 228 } 229 230 static uint64_t trusty_smc_handler(uint32_t smc_fid, 231 uint64_t x1, 232 uint64_t x2, 233 uint64_t x3, 234 uint64_t x4, 235 void *cookie, 236 void *handle, 237 uint64_t flags) 238 { 239 struct args ret; 240 uint32_t vmid = 0; 241 entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE); 242 243 /* 244 * Return success for SET_ROT_PARAMS if Trusty is not present, as 245 * Verified Boot is not even supported and returning success here 246 * would not compromise the boot process. 247 */ 248 if (!ep_info && (smc_fid == SMC_SC_SET_ROT_PARAMS)) { 249 SMC_RET1(handle, 0); 250 } else if (!ep_info) { 251 SMC_RET1(handle, SMC_UNK); 252 } 253 254 if (is_caller_secure(flags)) { 255 if (smc_fid == SMC_SC_NS_RETURN) { 256 ret = trusty_context_switch(SECURE, x1, 0, 0, 0); 257 SMC_RET8(handle, ret.r0, ret.r1, ret.r2, ret.r3, 258 ret.r4, ret.r5, ret.r6, ret.r7); 259 } 260 INFO("%s (0x%x, 0x%lx, 0x%lx, 0x%lx, 0x%lx, %p, %p, 0x%lx) \ 261 cpu %d, unknown smc\n", 262 __func__, smc_fid, x1, x2, x3, x4, cookie, handle, flags, 263 plat_my_core_pos()); 264 SMC_RET1(handle, SMC_UNK); 265 } else { 266 switch (smc_fid) { 267 case SMC_FC64_SET_FIQ_HANDLER: 268 return trusty_set_fiq_handler(handle, x1, x2, x3); 269 case SMC_FC64_GET_FIQ_REGS: 270 return trusty_get_fiq_regs(handle); 271 case SMC_FC_FIQ_EXIT: 272 return trusty_fiq_exit(handle, x1, x2, x3); 273 default: 274 if (is_hypervisor_mode()) 275 vmid = SMC_GET_GP(handle, CTX_GPREG_X7); 276 277 if ((current_vmid != 0) && (current_vmid != vmid)) { 278 /* This message will cause SMC mechanism 279 * abnormal in multi-guest environment. 280 * Change it to WARN in case you need it. 281 */ 282 VERBOSE("Previous SMC not finished.\n"); 283 SMC_RET1(handle, SM_ERR_BUSY); 284 } 285 current_vmid = vmid; 286 ret = trusty_context_switch(NON_SECURE, smc_fid, x1, 287 x2, x3); 288 current_vmid = 0; 289 SMC_RET1(handle, ret.r0); 290 } 291 } 292 } 293 294 static int32_t trusty_init(void) 295 { 296 void el3_exit(void); 297 entry_point_info_t *ep_info; 298 struct args zero_args = {0}; 299 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 300 uint32_t cpu = plat_my_core_pos(); 301 int reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx), 302 CTX_SPSR_EL3)); 303 304 /* 305 * Get information about the Trusty image. Its absence is a critical 306 * failure. 307 */ 308 ep_info = bl31_plat_get_next_image_ep_info(SECURE); 309 assert(ep_info); 310 311 cm_el1_sysregs_context_save(NON_SECURE); 312 313 cm_set_context(&ctx->cpu_ctx, SECURE); 314 cm_init_my_context(ep_info); 315 316 /* 317 * Adjust secondary cpu entry point for 32 bit images to the 318 * end of exeption vectors 319 */ 320 if ((cpu != 0) && (reg_width == MODE_RW_32)) { 321 INFO("trusty: cpu %d, adjust entry point to 0x%lx\n", 322 cpu, ep_info->pc + (1U << 5)); 323 cm_set_elr_el3(SECURE, ep_info->pc + (1U << 5)); 324 } 325 326 cm_el1_sysregs_context_restore(SECURE); 327 cm_set_next_eret_context(SECURE); 328 329 ctx->saved_security_state = ~0; /* initial saved state is invalid */ 330 trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end); 331 332 trusty_context_switch_helper(&ctx->saved_sp, &zero_args); 333 334 cm_el1_sysregs_context_restore(NON_SECURE); 335 cm_set_next_eret_context(NON_SECURE); 336 337 return 0; 338 } 339 340 static void trusty_cpu_suspend(void) 341 { 342 struct args ret; 343 344 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, 0, 0, 0); 345 if (ret.r0 != 0) { 346 INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %ld\n", 347 __func__, plat_my_core_pos(), ret.r0); 348 } 349 } 350 351 static void trusty_cpu_resume(void) 352 { 353 struct args ret; 354 355 ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, 0, 0, 0); 356 if (ret.r0 != 0) { 357 INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %ld\n", 358 __func__, plat_my_core_pos(), ret.r0); 359 } 360 } 361 362 static int32_t trusty_cpu_off_handler(uint64_t unused) 363 { 364 trusty_cpu_suspend(); 365 366 return 0; 367 } 368 369 static void trusty_cpu_on_finish_handler(uint64_t unused) 370 { 371 struct trusty_cpu_ctx *ctx = get_trusty_ctx(); 372 373 if (!ctx->saved_sp) { 374 trusty_init(); 375 } else { 376 trusty_cpu_resume(); 377 } 378 } 379 380 static void trusty_cpu_suspend_handler(uint64_t unused) 381 { 382 trusty_cpu_suspend(); 383 } 384 385 static void trusty_cpu_suspend_finish_handler(uint64_t unused) 386 { 387 trusty_cpu_resume(); 388 } 389 390 static const spd_pm_ops_t trusty_pm = { 391 .svc_off = trusty_cpu_off_handler, 392 .svc_suspend = trusty_cpu_suspend_handler, 393 .svc_on_finish = trusty_cpu_on_finish_handler, 394 .svc_suspend_finish = trusty_cpu_suspend_finish_handler, 395 }; 396 397 static int32_t trusty_setup(void) 398 { 399 entry_point_info_t *ep_info; 400 uint32_t flags; 401 int ret; 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 /* Trusty runs in AARCH64 mode */ 411 SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE); 412 ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); 413 414 /* 415 * arg0 = TZDRAM aperture available for BL32 416 * arg1 = BL32 boot params 417 * arg2 = BL32 boot params length 418 */ 419 ep_info->args.arg1 = ep_info->args.arg2; 420 ep_info->args.arg2 = TRUSTY_PARAMS_LEN_BYTES; 421 422 /* register init handler */ 423 bl31_register_bl32_init(trusty_init); 424 425 /* register power management hooks */ 426 psci_register_spd_pm_hook(&trusty_pm); 427 428 /* register interrupt handler */ 429 flags = 0; 430 set_interrupt_rm_flag(flags, NON_SECURE); 431 ret = register_interrupt_type_handler(INTR_TYPE_S_EL1, 432 trusty_fiq_handler, 433 flags); 434 if (ret) 435 ERROR("trusty: failed to register fiq handler, ret = %d\n", ret); 436 437 return 0; 438 } 439 440 /* Define a SPD runtime service descriptor for fast SMC calls */ 441 DECLARE_RT_SVC( 442 trusty_fast, 443 444 OEN_TOS_START, 445 SMC_ENTITY_SECURE_MONITOR, 446 SMC_TYPE_FAST, 447 trusty_setup, 448 trusty_smc_handler 449 ); 450 451 /* Define a SPD runtime service descriptor for standard SMC calls */ 452 DECLARE_RT_SVC( 453 trusty_std, 454 455 OEN_TAP_START, 456 SMC_ENTITY_SECURE_MONITOR, 457 SMC_TYPE_STD, 458 NULL, 459 trusty_smc_handler 460 ); 461