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