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