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