services/spd/trusty/trusty.c

   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 <bl31.h>
  10 #include <bl_common.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 <stdbool.h>
  17 #include <string.h>
  18
  19 #include "sm_err.h"
  20 #include "smcall.h"
  21
  22 /* macro to check if Hypervisor is enabled in the HCR_EL2 register */
  23 #define HYP_ENABLE_FLAG         0x286001
  24
  25 struct trusty_stack {
  26         uint8_t space[PLATFORM_STACK_SIZE] __aligned(16);
  27         uint32_t end;
  28 };
  29
  30 struct trusty_cpu_ctx {
  31         cpu_context_t   cpu_ctx;
  32         void            *saved_sp;
  33         uint32_t        saved_security_state;
  34         int             fiq_handler_active;
  35         uint64_t        fiq_handler_pc;
  36         uint64_t        fiq_handler_cpsr;
  37         uint64_t        fiq_handler_sp;
  38         uint64_t        fiq_pc;
  39         uint64_t        fiq_cpsr;
  40         uint64_t        fiq_sp_el1;
  41         gp_regs_t       fiq_gpregs;
  42         struct trusty_stack     secure_stack;
  43 };
  44
  45 struct args {
  46         uint64_t        r0;
  47         uint64_t        r1;
  48         uint64_t        r2;
  49         uint64_t        r3;
  50         uint64_t        r4;
  51         uint64_t        r5;
  52         uint64_t        r6;
  53         uint64_t        r7;
  54 };
  55
  56 struct trusty_cpu_ctx trusty_cpu_ctx[PLATFORM_CORE_COUNT];
  57
  58 struct args trusty_init_context_stack(void **sp, void *new_stack);
  59 struct args trusty_context_switch_helper(void **sp, void *smc_params);
  60
  61 static uint32_t current_vmid;
  62
  63 static struct trusty_cpu_ctx *get_trusty_ctx(void)
  64 {
  65         return &trusty_cpu_ctx[plat_my_core_pos()];
  66 }
  67
  68 static uint32_t is_hypervisor_mode(void)
  69 {
  70         uint64_t hcr = read_hcr();
  71
  72         return !!(hcr & HYP_ENABLE_FLAG);
  73 }
  74
  75 static struct args trusty_context_switch(uint32_t security_state, uint64_t r0,
  76                                          uint64_t r1, uint64_t r2, uint64_t r3)
  77 {
  78         struct args ret;
  79         struct trusty_cpu_ctx *ctx = get_trusty_ctx();
  80         struct trusty_cpu_ctx *ctx_smc;
  81
  82         assert(ctx->saved_security_state != security_state);
  83
  84         ret.r7 = 0;
  85         if (is_hypervisor_mode()) {
  86                 /* According to the ARM DEN0028A spec, VMID is stored in x7 */
  87                 ctx_smc = cm_get_context(NON_SECURE);
  88                 assert(ctx_smc);
  89                 ret.r7 = SMC_GET_GP(ctx_smc, CTX_GPREG_X7);
  90         }
  91         /* r4, r5, r6 reserved for future use. */
  92         ret.r6 = 0;
  93         ret.r5 = 0;
  94         ret.r4 = 0;
  95         ret.r3 = r3;
  96         ret.r2 = r2;
  97         ret.r1 = r1;
  98         ret.r0 = r0;
  99
 100         /*
 101          * To avoid the additional overhead in PSCI flow, skip FP context
 102          * saving/restoring in case of CPU suspend and resume, asssuming that
 103          * when it's needed the PSCI caller has preserved FP context before
 104          * going here.
 105          */
 106         if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
 107                 fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state)));
 108         cm_el1_sysregs_context_save(security_state);
 109
 110         ctx->saved_security_state = security_state;
 111         ret = trusty_context_switch_helper(&ctx->saved_sp, &ret);
 112
 113         assert(ctx->saved_security_state == !security_state);
 114
 115         cm_el1_sysregs_context_restore(security_state);
 116         if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
 117                 fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state)));
 118
 119         cm_set_next_eret_context(security_state);
 120
 121         return ret;
 122 }
 123
 124 static uint64_t trusty_fiq_handler(uint32_t id,
 125                                    uint32_t flags,
 126                                    void *handle,
 127                                    void *cookie)
 128 {
 129         struct args ret;
 130         struct trusty_cpu_ctx *ctx = get_trusty_ctx();
 131
 132         assert(!is_caller_secure(flags));
 133
 134         ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_ENTER, 0, 0, 0);
 135         if (ret.r0) {
 136                 SMC_RET0(handle);
 137         }
 138
 139         if (ctx->fiq_handler_active) {
 140                 INFO("%s: fiq handler already active\n", __func__);
 141                 SMC_RET0(handle);
 142         }
 143
 144         ctx->fiq_handler_active = 1;
 145         memcpy(&ctx->fiq_gpregs, get_gpregs_ctx(handle), sizeof(ctx->fiq_gpregs));
 146         ctx->fiq_pc = SMC_GET_EL3(handle, CTX_ELR_EL3);
 147         ctx->fiq_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3);
 148         ctx->fiq_sp_el1 = read_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1);
 149
 150         write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp);
 151         cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_handler_pc, ctx->fiq_handler_cpsr);
 152
 153         SMC_RET0(handle);
 154 }
 155
 156 static uint64_t trusty_set_fiq_handler(void *handle, uint64_t cpu,
 157                         uint64_t handler, uint64_t stack)
 158 {
 159         struct trusty_cpu_ctx *ctx;
 160
 161         if (cpu >= PLATFORM_CORE_COUNT) {
 162                 ERROR("%s: cpu %ld >= %d\n", __func__, cpu, PLATFORM_CORE_COUNT);
 163                 return SM_ERR_INVALID_PARAMETERS;
 164         }
 165
 166         ctx = &trusty_cpu_ctx[cpu];
 167         ctx->fiq_handler_pc = handler;
 168         ctx->fiq_handler_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3);
 169         ctx->fiq_handler_sp = stack;
 170
 171         SMC_RET1(handle, 0);
 172 }
 173
 174 static uint64_t trusty_get_fiq_regs(void *handle)
 175 {
 176         struct trusty_cpu_ctx *ctx = get_trusty_ctx();
 177         uint64_t sp_el0 = read_ctx_reg(&ctx->fiq_gpregs, CTX_GPREG_SP_EL0);
 178
 179         SMC_RET4(handle, ctx->fiq_pc, ctx->fiq_cpsr, sp_el0, ctx->fiq_sp_el1);
 180 }
 181
 182 static uint64_t trusty_fiq_exit(void *handle, uint64_t x1, uint64_t x2, uint64_t x3)
 183 {
 184         struct args ret;
 185         struct trusty_cpu_ctx *ctx = get_trusty_ctx();
 186
 187         if (!ctx->fiq_handler_active) {
 188                 NOTICE("%s: fiq handler not active\n", __func__);
 189                 SMC_RET1(handle, SM_ERR_INVALID_PARAMETERS);
 190         }
 191
 192         ret = trusty_context_switch(NON_SECURE, SMC_FC_FIQ_EXIT, 0, 0, 0);
 193         if (ret.r0 != 1) {
 194                 INFO("%s(%p) SMC_FC_FIQ_EXIT returned unexpected value, %ld\n",
 195                        __func__, handle, ret.r0);
 196         }
 197
 198         /*
 199          * Restore register state to state recorded on fiq entry.
 200          *
 201          * x0, sp_el1, pc and cpsr need to be restored because el1 cannot
 202          * restore them.
 203          *
 204          * x1-x4 and x8-x17 need to be restored here because smc_handler64
 205          * corrupts them (el1 code also restored them).
 206          */
 207         memcpy(get_gpregs_ctx(handle), &ctx->fiq_gpregs, sizeof(ctx->fiq_gpregs));
 208         ctx->fiq_handler_active = 0;
 209         write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1);
 210         cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_pc, ctx->fiq_cpsr);
 211
 212         SMC_RET0(handle);
 213 }
 214
 215 static uint64_t trusty_smc_handler(uint32_t smc_fid,
 216                          uint64_t x1,
 217                          uint64_t x2,
 218                          uint64_t x3,
 219                          uint64_t x4,
 220                          void *cookie,
 221                          void *handle,
 222                          uint64_t flags)
 223 {
 224         struct args ret;
 225         uint32_t vmid = 0;
 226         entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE);
 227
 228         /*
 229          * Return success for SET_ROT_PARAMS if Trusty is not present, as
 230          * Verified Boot is not even supported and returning success here
 231          * would not compromise the boot process.
 232          */
 233         if (!ep_info && (smc_fid == SMC_YC_SET_ROT_PARAMS)) {
 234                 SMC_RET1(handle, 0);
 235         } else if (!ep_info) {
 236                 SMC_RET1(handle, SMC_UNK);
 237         }
 238
 239         if (is_caller_secure(flags)) {
 240                 if (smc_fid == SMC_YC_NS_RETURN) {
 241                         ret = trusty_context_switch(SECURE, x1, 0, 0, 0);
 242                         SMC_RET8(handle, ret.r0, ret.r1, ret.r2, ret.r3,
 243                                  ret.r4, ret.r5, ret.r6, ret.r7);
 244                 }
 245                 INFO("%s (0x%x, 0x%lx, 0x%lx, 0x%lx, 0x%lx, %p, %p, 0x%lx) \
 246                      cpu %d, unknown smc\n",
 247                      __func__, smc_fid, x1, x2, x3, x4, cookie, handle, flags,
 248                      plat_my_core_pos());
 249                 SMC_RET1(handle, SMC_UNK);
 250         } else {
 251                 switch (smc_fid) {
 252                 case SMC_FC64_SET_FIQ_HANDLER:
 253                         return trusty_set_fiq_handler(handle, x1, x2, x3);
 254                 case SMC_FC64_GET_FIQ_REGS:
 255                         return trusty_get_fiq_regs(handle);
 256                 case SMC_FC_FIQ_EXIT:
 257                         return trusty_fiq_exit(handle, x1, x2, x3);
 258                 default:
 259                         if (is_hypervisor_mode())
 260                                 vmid = SMC_GET_GP(handle, CTX_GPREG_X7);
 261
 262                         if ((current_vmid != 0) && (current_vmid != vmid)) {
 263                                 /* This message will cause SMC mechanism
 264                                  * abnormal in multi-guest environment.
 265                                  * Change it to WARN in case you need it.
 266                                  */
 267                                 VERBOSE("Previous SMC not finished.\n");
 268                                 SMC_RET1(handle, SM_ERR_BUSY);
 269                         }
 270                         current_vmid = vmid;
 271                         ret = trusty_context_switch(NON_SECURE, smc_fid, x1,
 272                                 x2, x3);
 273                         current_vmid = 0;
 274                         SMC_RET1(handle, ret.r0);
 275                 }
 276         }
 277 }
 278
 279 static int32_t trusty_init(void)
 280 {
 281         void el3_exit(void);
 282         entry_point_info_t *ep_info;
 283         struct args zero_args = {0};
 284         struct trusty_cpu_ctx *ctx = get_trusty_ctx();
 285         uint32_t cpu = plat_my_core_pos();
 286         int reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx),
 287                                CTX_SPSR_EL3));
 288
 289         /*
 290          * Get information about the Trusty image. Its absence is a critical
 291          * failure.
 292          */
 293         ep_info = bl31_plat_get_next_image_ep_info(SECURE);
 294         assert(ep_info);
 295
 296         fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
 297         cm_el1_sysregs_context_save(NON_SECURE);
 298
 299         cm_set_context(&ctx->cpu_ctx, SECURE);
 300         cm_init_my_context(ep_info);
 301
 302         /*
 303          * Adjust secondary cpu entry point for 32 bit images to the
 304          * end of exeption vectors
 305          */
 306         if ((cpu != 0) && (reg_width == MODE_RW_32)) {
 307                 INFO("trusty: cpu %d, adjust entry point to 0x%lx\n",
 308                      cpu, ep_info->pc + (1U << 5));
 309                 cm_set_elr_el3(SECURE, ep_info->pc + (1U << 5));
 310         }
 311
 312         cm_el1_sysregs_context_restore(SECURE);
 313         fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
 314         cm_set_next_eret_context(SECURE);
 315
 316         ctx->saved_security_state = ~0; /* initial saved state is invalid */
 317         trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end);
 318
 319         trusty_context_switch_helper(&ctx->saved_sp, &zero_args);
 320
 321         cm_el1_sysregs_context_restore(NON_SECURE);
 322         fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
 323         cm_set_next_eret_context(NON_SECURE);
 324
 325         return 0;
 326 }
 327
 328 static void trusty_cpu_suspend(uint32_t off)
 329 {
 330         struct args ret;
 331
 332         ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, off, 0, 0);
 333         if (ret.r0 != 0) {
 334                 INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %ld\n",
 335                      __func__, plat_my_core_pos(), ret.r0);
 336         }
 337 }
 338
 339 static void trusty_cpu_resume(uint32_t on)
 340 {
 341         struct args ret;
 342
 343         ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, on, 0, 0);
 344         if (ret.r0 != 0) {
 345                 INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %ld\n",
 346                      __func__, plat_my_core_pos(), ret.r0);
 347         }
 348 }
 349
 350 static int32_t trusty_cpu_off_handler(uint64_t unused)
 351 {
 352         trusty_cpu_suspend(1);
 353
 354         return 0;
 355 }
 356
 357 static void trusty_cpu_on_finish_handler(uint64_t unused)
 358 {
 359         struct trusty_cpu_ctx *ctx = get_trusty_ctx();
 360
 361         if (!ctx->saved_sp) {
 362                 trusty_init();
 363         } else {
 364                 trusty_cpu_resume(1);
 365         }
 366 }
 367
 368 static void trusty_cpu_suspend_handler(uint64_t unused)
 369 {
 370         trusty_cpu_suspend(0);
 371 }
 372
 373 static void trusty_cpu_suspend_finish_handler(uint64_t unused)
 374 {
 375         trusty_cpu_resume(0);
 376 }
 377
 378 static const spd_pm_ops_t trusty_pm = {
 379         .svc_off = trusty_cpu_off_handler,
 380         .svc_suspend = trusty_cpu_suspend_handler,
 381         .svc_on_finish = trusty_cpu_on_finish_handler,
 382         .svc_suspend_finish = trusty_cpu_suspend_finish_handler,
 383 };
 384
 385 void plat_trusty_set_boot_args(aapcs64_params_t *args);
 386
 387 #ifdef TSP_SEC_MEM_SIZE
 388 #pragma weak plat_trusty_set_boot_args
 389 void plat_trusty_set_boot_args(aapcs64_params_t *args)
 390 {
 391         args->arg0 = TSP_SEC_MEM_SIZE;
 392 }
 393 #endif
 394
 395 static int32_t trusty_setup(void)
 396 {
 397         entry_point_info_t *ep_info;
 398         uint32_t instr;
 399         uint32_t flags;
 400         int ret;
 401         bool aarch32 = false;
 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         instr = *(uint32_t *)ep_info->pc;
 411
 412         if (instr >> 24 == 0xeaU) {
 413                 INFO("trusty: Found 32 bit image\n");
 414                 aarch32 = true;
 415         } else if (instr >> 8 == 0xd53810U || instr >> 16 == 0x9400U) {
 416                 INFO("trusty: Found 64 bit image\n");
 417         } else {
 418                 NOTICE("trusty: Found unknown image, 0x%x\n", instr);
 419         }
 420
 421         SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE);
 422         if (!aarch32)
 423                 ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
 424                                         DISABLE_ALL_EXCEPTIONS);
 425         else
 426                 ep_info->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
 427                                             SPSR_E_LITTLE,
 428                                             DAIF_FIQ_BIT |
 429                                             DAIF_IRQ_BIT |
 430                                             DAIF_ABT_BIT);
 431         (void)memset(&ep_info->args, 0, sizeof(ep_info->args));
 432         plat_trusty_set_boot_args(&ep_info->args);
 433
 434         /* register init handler */
 435         bl31_register_bl32_init(trusty_init);
 436
 437         /* register power management hooks */
 438         psci_register_spd_pm_hook(&trusty_pm);
 439
 440         /* register interrupt handler */
 441         flags = 0;
 442         set_interrupt_rm_flag(flags, NON_SECURE);
 443         ret = register_interrupt_type_handler(INTR_TYPE_S_EL1,
 444                                               trusty_fiq_handler,
 445                                               flags);
 446         if (ret)
 447                 ERROR("trusty: failed to register fiq handler, ret = %d\n", ret);
 448
 449         if (aarch32) {
 450                 entry_point_info_t *ns_ep_info;
 451                 uint32_t spsr;
 452
 453                 ns_ep_info = bl31_plat_get_next_image_ep_info(NON_SECURE);
 454                 if (ns_ep_info == NULL) {
 455                         NOTICE("Trusty: non-secure image missing.\n");
 456                         return -1;
 457                 }
 458                 spsr = ns_ep_info->spsr;
 459                 if (GET_RW(spsr) == MODE_RW_64 && GET_EL(spsr) == MODE_EL2) {
 460                         spsr &= ~(MODE_EL_MASK << MODE_EL_SHIFT);
 461                         spsr |= MODE_EL1 << MODE_EL_SHIFT;
 462                 }
 463                 if (GET_RW(spsr) == MODE_RW_32 && GET_M32(spsr) == MODE32_hyp) {
 464                         spsr &= ~(MODE32_MASK << MODE32_SHIFT);
 465                         spsr |= MODE32_svc << MODE32_SHIFT;
 466                 }
 467                 if (spsr != ns_ep_info->spsr) {
 468                         NOTICE("Trusty: Switch bl33 from EL2 to EL1 (spsr 0x%x -> 0x%x)\n",
 469                                ns_ep_info->spsr, spsr);
 470                         ns_ep_info->spsr = spsr;
 471                 }
 472         }
 473
 474         return 0;
 475 }
 476
 477 /* Define a SPD runtime service descriptor for fast SMC calls */
 478 DECLARE_RT_SVC(
 479         trusty_fast,
 480
 481         OEN_TOS_START,
 482         SMC_ENTITY_SECURE_MONITOR,
 483         SMC_TYPE_FAST,
 484         trusty_setup,
 485         trusty_smc_handler
 486 );
 487
 488 /* Define a SPD runtime service descriptor for yielding SMC calls */
 489 DECLARE_RT_SVC(
 490         trusty_std,
 491
 492         OEN_TAP_START,
 493         SMC_ENTITY_SECURE_MONITOR,
 494         SMC_TYPE_YIELD,
 495         NULL,
 496         trusty_smc_handler
 497 );