services/spd/trusty/trusty.c

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