Merge pull request #1239 from arve-android/trusty-fixes
Trusty fixes
diff --git a/docs/spd/trusty-dispatcher.rst b/docs/spd/trusty-dispatcher.rst
index f1982ea..10b111d 100644
--- a/docs/spd/trusty-dispatcher.rst
+++ b/docs/spd/trusty-dispatcher.rst
@@ -8,6 +8,23 @@
Open Source Project (AOSP) webpage for Trusty hosted at
https://source.android.com/security/trusty
+Boot parameters
+===============
+
+Custom boot parameters can be passed to Trusty by providing a platform
+specific function:
+
+.. code:: c
+
+ void plat_trusty_set_boot_args(aapcs64_params_t *args)
+
+If this function is provided ``args->arg0`` must be set to the memory
+size allocated to trusty. If the platform does not provide this
+function, but defines ``TSP_SEC_MEM_SIZE``, a default implementation
+will pass the memory size from ``TSP_SEC_MEM_SIZE``. ``args->arg1``
+can be set to a platform specific parameter block, and ``args->arg2``
+should then be set to the size of that block.
+
Supported platforms
===================
diff --git a/plat/nvidia/tegra/common/tegra_bl31_setup.c b/plat/nvidia/tegra/common/tegra_bl31_setup.c
index d5d3d53..d89ad7b 100644
--- a/plat/nvidia/tegra/common/tegra_bl31_setup.c
+++ b/plat/nvidia/tegra/common/tegra_bl31_setup.c
@@ -24,6 +24,9 @@
#include <tegra_def.h>
#include <tegra_private.h>
+/* length of Trusty's input parameters (in bytes) */
+#define TRUSTY_PARAMS_LEN_BYTES (4096*2)
+
extern void zeromem16(void *mem, unsigned int length);
/*******************************************************************************
@@ -58,6 +61,8 @@
static plat_params_from_bl2_t plat_bl31_params_from_bl2 = {
.tzdram_size = (uint64_t)TZDRAM_SIZE
};
+static unsigned long bl32_mem_size;
+static unsigned long bl32_boot_params;
/*******************************************************************************
* This variable holds the non-secure image entry address
@@ -147,8 +152,11 @@
assert(from_bl2->bl33_ep_info);
bl33_image_ep_info = *from_bl2->bl33_ep_info;
- if (from_bl2->bl32_ep_info)
+ if (from_bl2->bl32_ep_info) {
bl32_image_ep_info = *from_bl2->bl32_ep_info;
+ bl32_mem_size = from_bl2->bl32_ep_info->args.arg0;
+ bl32_boot_params = from_bl2->bl32_ep_info->args.arg2;
+ }
/*
* Parse platform specific parameters - TZDRAM aperture base and size
@@ -233,6 +241,15 @@
INFO("BL3-1: Boot CPU: %s Processor [%lx]\n", (impl == DENVER_IMPL) ?
"Denver" : "ARM", read_mpidr());
}
+
+#ifdef SPD_trusty
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+ args->arg0 = bl32_mem_size;
+ args->arg1 = bl32_boot_params;
+ args->arg2 = TRUSTY_PARAMS_LEN_BYTES;
+}
+#endif
/*******************************************************************************
* Initialize the gic, configure the SCR.
diff --git a/services/spd/trusty/generic-arm64-smcall.c b/services/spd/trusty/generic-arm64-smcall.c
new file mode 100644
index 0000000..38da279
--- /dev/null
+++ b/services/spd/trusty/generic-arm64-smcall.c
@@ -0,0 +1,94 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <debug.h>
+#include <runtime_svc.h>
+
+#include "generic-arm64-smcall.h"
+
+int trusty_disable_serial_debug;
+
+struct dputc_state {
+ char linebuf[128];
+ unsigned l;
+};
+
+static struct dputc_state dputc_state[2];
+
+static void trusty_dputc(char ch, int secure)
+{
+ unsigned i;
+ struct dputc_state *s = &dputc_state[!secure];
+
+ if (trusty_disable_serial_debug)
+ return;
+
+ s->linebuf[s->l++] = ch;
+ if (s->l == sizeof(s->linebuf) || ch == '\n') {
+ if (secure)
+ printf("secure os: ");
+ else
+ printf("non-secure os: ");
+ for (i = 0; i < s->l; i++) {
+ putchar(s->linebuf[i]);
+ }
+ if (ch != '\n') {
+ printf(" <...>\n");
+ }
+ s->l = 0;
+ }
+}
+
+static uint64_t trusty_get_reg_base(uint32_t reg)
+{
+ switch (reg) {
+ case 0:
+ return PLAT_ARM_GICD_BASE;
+
+ case 1:
+ return PLAT_ARM_GICC_BASE;
+
+ default:
+ NOTICE("%s(0x%x) unknown reg\n", __func__, reg);
+ return SMC_UNK;
+ }
+}
+
+static uint64_t trusty_generic_platform_smc(uint32_t smc_fid,
+ uint64_t x1,
+ uint64_t x2,
+ uint64_t x3,
+ uint64_t x4,
+ void *cookie,
+ void *handle,
+ uint64_t flags)
+{
+ switch (smc_fid) {
+ case SMC_FC_DEBUG_PUTC:
+ trusty_dputc(x1, is_caller_secure(flags));
+ SMC_RET1(handle, 0);
+
+ case SMC_FC_GET_REG_BASE:
+ case SMC_FC64_GET_REG_BASE:
+ SMC_RET1(handle, trusty_get_reg_base(x1));
+
+ default:
+ NOTICE("%s(0x%x, 0x%lx) unknown smc\n", __func__, smc_fid, x1);
+ SMC_RET1(handle, SMC_UNK);
+ }
+}
+
+/* Define a SPD runtime service descriptor for fast SMC calls */
+DECLARE_RT_SVC(
+ trusty_fast,
+
+ SMC_ENTITY_PLATFORM_MONITOR,
+ SMC_ENTITY_PLATFORM_MONITOR,
+ SMC_TYPE_FAST,
+ NULL,
+ trusty_generic_platform_smc
+);
+
diff --git a/services/spd/trusty/generic-arm64-smcall.h b/services/spd/trusty/generic-arm64-smcall.h
new file mode 100644
index 0000000..06efc72
--- /dev/null
+++ b/services/spd/trusty/generic-arm64-smcall.h
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include "smcall.h"
+
+#define SMC_ENTITY_PLATFORM_MONITOR 61
+
+/*
+ * SMC calls implemented by EL3 monitor
+ */
+
+/*
+ * Write character in r1 to debug console
+ */
+#define SMC_FC_DEBUG_PUTC SMC_FASTCALL_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x0)
+
+/*
+ * Get register base address
+ * r1: SMC_GET_GIC_BASE_GICD or SMC_GET_GIC_BASE_GICC
+ */
+#define SMC_GET_GIC_BASE_GICD 0
+#define SMC_GET_GIC_BASE_GICC 1
+#define SMC_FC_GET_REG_BASE SMC_FASTCALL_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x1)
+#define SMC_FC64_GET_REG_BASE SMC_FASTCALL64_NR(SMC_ENTITY_PLATFORM_MONITOR, 0x1)
diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index ecbcfae..d6e5726 100644
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -13,6 +13,7 @@
#include <interrupt_mgmt.h>
#include <platform.h>
#include <runtime_svc.h>
+#include <stdbool.h>
#include <string.h>
#include "sm_err.h"
@@ -21,9 +22,6 @@
/* macro to check if Hypervisor is enabled in the HCR_EL2 register */
#define HYP_ENABLE_FLAG 0x286001
-/* length of Trusty's input parameters (in bytes) */
-#define TRUSTY_PARAMS_LEN_BYTES (4096*2)
-
struct trusty_stack {
uint8_t space[PLATFORM_STACK_SIZE] __aligned(16);
uint32_t end;
@@ -105,10 +103,8 @@
* when it's needed the PSCI caller has preserved FP context before
* going here.
*/
-#if CTX_INCLUDE_FPREGS
if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
fpregs_context_save(get_fpregs_ctx(cm_get_context(security_state)));
-#endif
cm_el1_sysregs_context_save(security_state);
ctx->saved_security_state = security_state;
@@ -117,10 +113,8 @@
assert(ctx->saved_security_state == !security_state);
cm_el1_sysregs_context_restore(security_state);
-#if CTX_INCLUDE_FPREGS
if (r0 != SMC_FC_CPU_SUSPEND && r0 != SMC_FC_CPU_RESUME)
fpregs_context_restore(get_fpregs_ctx(cm_get_context(security_state)));
-#endif
cm_set_next_eret_context(security_state);
@@ -299,6 +293,7 @@
ep_info = bl31_plat_get_next_image_ep_info(SECURE);
assert(ep_info);
+ fpregs_context_save(get_fpregs_ctx(cm_get_context(NON_SECURE)));
cm_el1_sysregs_context_save(NON_SECURE);
cm_set_context(&ctx->cpu_ctx, SECURE);
@@ -315,6 +310,7 @@
}
cm_el1_sysregs_context_restore(SECURE);
+ fpregs_context_restore(get_fpregs_ctx(cm_get_context(SECURE)));
cm_set_next_eret_context(SECURE);
ctx->saved_security_state = ~0; /* initial saved state is invalid */
@@ -323,27 +319,28 @@
trusty_context_switch_helper(&ctx->saved_sp, &zero_args);
cm_el1_sysregs_context_restore(NON_SECURE);
+ fpregs_context_restore(get_fpregs_ctx(cm_get_context(NON_SECURE)));
cm_set_next_eret_context(NON_SECURE);
return 0;
}
-static void trusty_cpu_suspend(void)
+static void trusty_cpu_suspend(uint32_t off)
{
struct args ret;
- ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, 0, 0, 0);
+ ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_SUSPEND, off, 0, 0);
if (ret.r0 != 0) {
INFO("%s: cpu %d, SMC_FC_CPU_SUSPEND returned unexpected value, %ld\n",
__func__, plat_my_core_pos(), ret.r0);
}
}
-static void trusty_cpu_resume(void)
+static void trusty_cpu_resume(uint32_t on)
{
struct args ret;
- ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, 0, 0, 0);
+ ret = trusty_context_switch(NON_SECURE, SMC_FC_CPU_RESUME, on, 0, 0);
if (ret.r0 != 0) {
INFO("%s: cpu %d, SMC_FC_CPU_RESUME returned unexpected value, %ld\n",
__func__, plat_my_core_pos(), ret.r0);
@@ -352,7 +349,7 @@
static int32_t trusty_cpu_off_handler(uint64_t unused)
{
- trusty_cpu_suspend();
+ trusty_cpu_suspend(1);
return 0;
}
@@ -364,18 +361,18 @@
if (!ctx->saved_sp) {
trusty_init();
} else {
- trusty_cpu_resume();
+ trusty_cpu_resume(1);
}
}
static void trusty_cpu_suspend_handler(uint64_t unused)
{
- trusty_cpu_suspend();
+ trusty_cpu_suspend(0);
}
static void trusty_cpu_suspend_finish_handler(uint64_t unused)
{
- trusty_cpu_resume();
+ trusty_cpu_resume(0);
}
static const spd_pm_ops_t trusty_pm = {
@@ -385,11 +382,23 @@
.svc_suspend_finish = trusty_cpu_suspend_finish_handler,
};
+void plat_trusty_set_boot_args(aapcs64_params_t *args);
+
+#ifdef TSP_SEC_MEM_SIZE
+#pragma weak plat_trusty_set_boot_args
+void plat_trusty_set_boot_args(aapcs64_params_t *args)
+{
+ args->arg0 = TSP_SEC_MEM_SIZE;
+}
+#endif
+
static int32_t trusty_setup(void)
{
entry_point_info_t *ep_info;
+ uint32_t instr;
uint32_t flags;
int ret;
+ bool aarch32 = false;
/* Get trusty's entry point info */
ep_info = bl31_plat_get_next_image_ep_info(SECURE);
@@ -398,17 +407,29 @@
return -1;
}
- /* Trusty runs in AARCH64 mode */
- SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE);
- ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+ instr = *(uint32_t *)ep_info->pc;
- /*
- * arg0 = TZDRAM aperture available for BL32
- * arg1 = BL32 boot params
- * arg2 = BL32 boot params length
- */
- ep_info->args.arg1 = ep_info->args.arg2;
- ep_info->args.arg2 = TRUSTY_PARAMS_LEN_BYTES;
+ if (instr >> 24 == 0xeaU) {
+ INFO("trusty: Found 32 bit image\n");
+ aarch32 = true;
+ } else if (instr >> 8 == 0xd53810U || instr >> 16 == 0x9400U) {
+ INFO("trusty: Found 64 bit image\n");
+ } else {
+ NOTICE("trusty: Found unknown image, 0x%x\n", instr);
+ }
+
+ SET_PARAM_HEAD(ep_info, PARAM_EP, VERSION_1, SECURE | EP_ST_ENABLE);
+ if (!aarch32)
+ ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
+ DISABLE_ALL_EXCEPTIONS);
+ else
+ ep_info->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
+ SPSR_E_LITTLE,
+ DAIF_FIQ_BIT |
+ DAIF_IRQ_BIT |
+ DAIF_ABT_BIT);
+ (void)memset(&ep_info->args, 0, sizeof(ep_info->args));
+ plat_trusty_set_boot_args(&ep_info->args);
/* register init handler */
bl31_register_bl32_init(trusty_init);
@@ -425,6 +446,31 @@
if (ret)
ERROR("trusty: failed to register fiq handler, ret = %d\n", ret);
+ if (aarch32) {
+ entry_point_info_t *ns_ep_info;
+ uint32_t spsr;
+
+ ns_ep_info = bl31_plat_get_next_image_ep_info(NON_SECURE);
+ if (!ep_info) {
+ NOTICE("Trusty: non-secure image missing.\n");
+ return -1;
+ }
+ spsr = ns_ep_info->spsr;
+ if (GET_RW(spsr) == MODE_RW_64 && GET_EL(spsr) == MODE_EL2) {
+ spsr &= ~(MODE_EL_MASK << MODE_EL_SHIFT);
+ spsr |= MODE_EL1 << MODE_EL_SHIFT;
+ }
+ if (GET_RW(spsr) == MODE_RW_32 && GET_M32(spsr) == MODE32_hyp) {
+ spsr &= ~(MODE32_MASK << MODE32_SHIFT);
+ spsr |= MODE32_svc << MODE32_SHIFT;
+ }
+ if (spsr != ns_ep_info->spsr) {
+ NOTICE("Trusty: Switch bl33 from EL2 to EL1 (spsr 0x%x -> 0x%x)\n",
+ ns_ep_info->spsr, spsr);
+ ns_ep_info->spsr = spsr;
+ }
+ }
+
return 0;
}
diff --git a/services/spd/trusty/trusty.mk b/services/spd/trusty/trusty.mk
index beca875..a571fa2 100644
--- a/services/spd/trusty/trusty.mk
+++ b/services/spd/trusty/trusty.mk
@@ -8,3 +8,11 @@
SPD_SOURCES := services/spd/trusty/trusty.c \
services/spd/trusty/trusty_helpers.S
+
+ifeq (${TRUSTY_SPD_WITH_GENERIC_SERVICES},1)
+SPD_SOURCES += services/spd/trusty/generic-arm64-smcall.c
+endif
+
+NEED_BL32 := yes
+
+CTX_INCLUDE_FPREGS := 1