plat/bcm/sp_min/src/opteed_main.c

   1 /*
   2 <:copyright-BRCM:2019:DUAL/GPL:standard
   3
   4    Copyright (c) 2019 Broadcom
   5    All Rights Reserved
   6
   7 This program is free software; you can redistribute it and/or modify
   8 it under the terms of the GNU General Public License, version 2, as published by
   9 the Free Software Foundation (the "GPL").
  10
  11 This program is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  14 GNU General Public License for more details.
  15
  16
  17 A copy of the GPL is available at http://www.broadcom.com/licenses/GPLv2.php, or by
  18 writing to the Free Software Foundation, Inc., 59 Temple Place - Suite 330,
  19 Boston, MA 02111-1307, USA.
  20
  21 :>
  22 */
  23 #if defined(SPD_opteed)
  24 #include <arch_helpers.h>
  25 #include <assert.h>
  26 #include <bl_common.h>
  27 #include <debug.h>
  28 #include <errno.h>
  29 #include <platform.h>
  30 #include <runtime_svc.h>
  31 #include <stddef.h>
  32 #include <arch.h>
  33
  34 static volatile uint32_t optee_smc_entry = 0;
  35
  36 static uint32_t monitor_stack_pointer [PLATFORM_MAX_CPUS_PER_CLUSTER];
  37 static uint32_t optee_params_register [PLATFORM_MAX_CPUS_PER_CLUSTER][8];
  38
  39 extern void save_atf_sysreg(void);
  40 extern void restore_atf_sysreg(void);
  41
  42 __attribute__((naked)) void opteed_jump(uint32_t jump, uint32_t *monitor_stack, uint32_t *params)
  43 {
  44    /* Save all GP registers */
  45    asm("push {r0-r12, r14}");
  46
  47    /* Find OPTEE's entry address and place to save ATF's monitor stack*/
  48    asm volatile ("mov r10, %0" "\n\t"
  49                  "mov r11, %1" "\n\t"
  50                  "mov r12, %2" "\n\t"
  51                  : : "r" (params), "r" (monitor_stack), "r" (jump));
  52
  53    /* Save ATF's monitor's stack in cache memory */
  54    asm("str sp, [r11]");
  55    save_atf_sysreg();
  56
  57    /* Load OPTEE's parameters */
  58    asm("ldm r10, {r0-r7}");
  59
  60    /* Turn off MMU and Cache needed to initialize OPTEE */
  61    asm("mrc p15, 0, r8, c1, c0, 0");
  62    asm("bic r8, r8, #0x5"); // (SCTLR_M_BIT | SCTLR_C_BIT) = 0x5
  63    asm("mcr p15, 0, r8, c1, c0, 0");
  64
  65    /* Save ATF's monitor's stack in non-cache memory */
  66    asm("str sp, [r11]");
  67    save_atf_sysreg();
  68
  69    /* Jump to OPTEE */
  70    asm("cps #0x13"); // switch to secure MODE_SVC mode (0x13)
  71    asm("blx r12");
  72    asm("nop");
  73    /* Returned from OPTEE with OPTEE's smc entry point in r12. ** DO NOT corrupt r12 ** */
  74
  75    /* Load ATF's monitor stack from non-cached memory */
  76    asm("ldr r9,=monitor_stack_pointer");
  77    /* Point to the monitor stack pointer for this CPU */
  78    asm("mrc p15, 0, r10, c0, c0, 5");
  79    asm("and r10, r10, #0xff");
  80    asm("lsl r10, r10, #2");
  81    asm("add r9, r10, r9");
  82    asm("ldr sp, [r9]");
  83
  84    /* Restore ATF's system registers */
  85    restore_atf_sysreg();
  86
  87    /* Turn ON MMU */
  88    asm("mrc p15, 0, r8, c1, c0, 0");
  89    asm("ldr r9,=0x5"); // SCTLR_M_BIT | SCTLR_C_BIT
  90    asm("orr r8, r8, r9");
  91    asm("mcr p15, 0, r8, c1, c0, 0");
  92    asm("nop");
  93    asm("nop");
  94    /* Save optee's SMC entry in cached memory */
  95    asm("ldr r8,=optee_smc_entry");
  96    asm("str r12,[r8]");
  97    /* Save the OPTEE's return registers r0-r7  in cached memory  */
  98    asm("ldr r8,=optee_params_register");
  99    asm("lsl r10, r10, #3");
 100    asm("add r8, r10, r8");
 101    asm("stm r8, {r0-r7}");
 102    /* Restore all GP registers */
 103    asm("pop {r0-r12, r14}");
 104    asm("bx lr");
 105 }
 106
 107 /*******************************************************************************
 108  * OPTEE Dispatcher setup. The OPTEED finds out the OPTEE entrypoint and type
 109  * (aarch32/aarch64) if not already known and initialises the context for entry
 110  * into OPTEE for its initialization.
 111  ******************************************************************************/
 112 int32_t opteed_setup(void)
 113 {
 114    entry_point_info_t *optee_ep_info;
 115    uint32_t cpu_id = plat_my_core_pos();
 116    uint32_t *monitor_stack = &monitor_stack_pointer[cpu_id];
 117    uint32_t *optee_params = &optee_params_register[cpu_id][0];
 118    /*
 119     * Get information about the Secure Payload (BL32) image. Its
 120     * absence is a critical failure.  TODO: Add support to
 121     * conditionally include the SPD service
 122     */
 123
 124    optee_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
 125    if (!optee_ep_info) {
 126       WARN("No OPTEE provided by BL2 boot loader, Booting device"
 127            " without OPTEE initialization. SMC`s destined for OPTEE"
 128            " will return SMC_UNK\n");
 129       return 1;
 130    }
 131
 132    /*
 133     * If there's no valid entry point for SP, we return a non-zero value
 134     * signalling failure initializing the service. We bail out without
 135     * registering any handlers
 136     */
 137    if (!optee_ep_info->pc)
 138      return 1;
 139
 140    opteed_jump(optee_ep_info->pc, monitor_stack, optee_params);
 141
 142    return 0;
 143 }
 144
 145
 146
 147 /*******************************************************************************
 148  * This function is responsible for handling all SMCs in the Trusted OS/App
 149  * range from the non-secure state as defined in the SMC Calling Convention
 150  * Document. It is also responsible for communicating with the Secure
 151  * payload to delegate work and return results back to the non-secure
 152  * state. Lastly it will also return any information that OPTEE needs to do
 153  * the work assigned to it.
 154  ******************************************************************************/
 155  uintptr_t opteed_smc_handler(uint32_t smc_fid,
 156                               u_register_t x1,
 157                               u_register_t x2,
 158                               u_register_t x3,
 159                               u_register_t x4,
 160                               void *cookie,
 161                               void *handle,
 162                               u_register_t flags)
 163 {
 164    uint32_t cpu_id = plat_my_core_pos();
 165    uint32_t *monitor_stack = &monitor_stack_pointer[cpu_id];
 166    uint32_t *optee_params = &optee_params_register[cpu_id][0];
 167    cookie = cookie;
 168    handle = handle;
 169    flags = flags;
 170
 171    /* Extract r0-r7 from saved context and pass to OPTEE */
 172    optee_params[0] = ((smc_ctx_t *)handle)->r0; // smc_fid;
 173    optee_params[1] = ((smc_ctx_t *)handle)->r1; // x1;
 174    optee_params[2] = ((smc_ctx_t *)handle)->r2; // x2;
 175    optee_params[3] = ((smc_ctx_t *)handle)->r3; // x3;
 176    optee_params[4] = ((smc_ctx_t *)handle)->r4; // x4;
 177    optee_params[5] = ((smc_ctx_t *)handle)->r5;
 178    optee_params[6] = ((smc_ctx_t *)handle)->r6;
 179    optee_params[7] = ((smc_ctx_t *)handle)->r7;
 180
 181    assert(optee_params[0] == smc_fid);
 182
 183    opteed_jump(optee_smc_entry, monitor_stack, optee_params);
 184
 185    /* Return the response from OPTEE */
 186    SMC_RET4(handle, optee_params[0], optee_params[1], optee_params[2], optee_params[3]);
 187 }
 188
 189 /* Define an OPTEED runtime service descriptor for fast SMC calls */
 190 DECLARE_RT_SVC(
 191                opteed_fast,
 192
 193                OEN_TOS_START,
 194                OEN_TOS_END,
 195                SMC_TYPE_FAST,
 196                opteed_setup,
 197                opteed_smc_handler
 198                );
 199
 200 /* Define an OPTEED runtime service descriptor for yielding SMC calls */
 201 DECLARE_RT_SVC(
 202                opteed_std,
 203
 204                OEN_TOS_START,
 205                OEN_TOS_END,
 206                SMC_TYPE_YIELD,
 207                NULL,
 208                opteed_smc_handler
 209 );
 210 #endif