Merge pull request #862 from vwadekar/spd-trusty-tlkd-changes

SPD changes for Trusty and TLKD

5 files changed, 132 insertions, 35 deletions

diff --git a/include/lib/aarch64/smcc_helpers.h b/include/lib/aarch64/smcc_helpers.h
index 6e633839..7fae7d8d 100644
--- a/include/lib/aarch64/smcc_helpers.h
+++ b/include/lib/aarch64/smcc_helpers.h
@@ -56,6 +56,22 @@
 	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X3, (_x3));	\
 	SMC_RET3(_h, (_x0), (_x1), (_x2));			\
 }
+#define SMC_RET5(_h, _x0, _x1, _x2, _x3, _x4)	{		\
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X4, (_x4));	\
+	SMC_RET4(_h, (_x0), (_x1), (_x2), (_x3));		\
+}
+#define SMC_RET6(_h, _x0, _x1, _x2, _x3, _x4, _x5)	{	\
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X5, (_x5));	\
+	SMC_RET5(_h, (_x0), (_x1), (_x2), (_x3), (_x4));	\
+}
+#define SMC_RET7(_h, _x0, _x1, _x2, _x3, _x4, _x5, _x6)	{	\
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X6, (_x6));	\
+	SMC_RET6(_h, (_x0), (_x1), (_x2), (_x3), (_x4), (_x5));	\
+}
+#define SMC_RET8(_h, _x0, _x1, _x2, _x3, _x4, _x5, _x6, _x7) {	\
+	write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X7, (_x7));	\
+	SMC_RET7(_h, (_x0), (_x1), (_x2), (_x3), (_x4), (_x5), (_x6));	\
+}
 
 /*
  * Convenience macros to access general purpose registers using handle provided
diff --git a/services/spd/tlkd/tlkd_main.c b/services/spd/tlkd/tlkd_main.c
index 0fddcb27..5ebe4fdd 100644
--- a/services/spd/tlkd/tlkd_main.c
+++ b/services/spd/tlkd/tlkd_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -58,6 +58,11 @@ extern const spd_pm_ops_t tlkd_pm_ops;
  ******************************************************************************/
 tlk_context_t tlk_ctx;
 
+/*******************************************************************************
+ * CPU number on which TLK booted up
+ ******************************************************************************/
+static int boot_cpu;
+
 /* TLK UID: RFC-4122 compliant UUID (version-5, sha-1) */
 DEFINE_SVC_UUID(tlk_uuid,
 		0xbd11e9c9, 0x2bba, 0x52ee, 0xb1, 0x72,
@@ -133,6 +138,12 @@ int32_t tlkd_init(void)
 	cm_init_my_context(tlk_entry_point);
 
 	/*
+	 * TLK runs only on a single CPU. Store the value of the boot
+	 * CPU for sanity checking later.
+	 */
+	boot_cpu = plat_my_core_pos();
+
+	/*
 	 * Arrange for an entry into the test secure payload.
 	 */
 	return tlkd_synchronous_sp_entry(&tlk_ctx);
@@ -163,8 +174,8 @@ uint64_t tlkd_smc_handler(uint32_t smc_fid,
 	/* Passing a NULL context is a critical programming error */
 	assert(handle);
 
-	/* These SMCs are only supported by CPU0 */
-	if ((read_mpidr() & MPIDR_CPU_MASK) != 0)
+	/* These SMCs are only supported by a single CPU */
+	if (boot_cpu != plat_my_core_pos())
 		SMC_RET1(handle, SMC_UNK);
 
 	/* Determine which security state this SMC originated from */
diff --git a/services/spd/trusty/smcall.h b/services/spd/trusty/smcall.h
index a1d91e5a..2abcee62 100644
--- a/services/spd/trusty/smcall.h
+++ b/services/spd/trusty/smcall.h
@@ -94,5 +94,6 @@
 
 #define SMC_SC_VDEV_RESET	  SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 23)
 #define SMC_SC_VDEV_KICK_VQ	  SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 24)
+#define SMC_SC_SET_ROT_PARAMS	  SMC_STDCALL_NR(SMC_ENTITY_TRUSTED_OS, 65535)
 
 #endif /* __LIB_SM_SMCALL_H */
diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index b21ce715..9c9de910 100644
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2017, ARM Limited and Contributors. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -28,7 +28,8 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <assert.h>
+#include <arch_helpers.h>
+#include <assert.h> /* for context_mgmt.h */
 #include <bl_common.h>
 #include <bl31.h>
 #include <context_mgmt.h>
@@ -41,8 +42,15 @@
 #include "smcall.h"
 #include "sm_err.h"
 
+/* 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;
 };
 
 struct trusty_cpu_ctx {
@@ -65,31 +73,60 @@ struct args {
 	uint64_t	r1;
 	uint64_t	r2;
 	uint64_t	r3;
+	uint64_t	r4;
+	uint64_t	r5;
+	uint64_t	r6;
+	uint64_t	r7;
 };
 
 struct trusty_cpu_ctx trusty_cpu_ctx[PLATFORM_CORE_COUNT];
 
 struct args trusty_init_context_stack(void **sp, void *new_stack);
-struct args trusty_context_switch_helper(void **sp, uint64_t r0, uint64_t r1,
-					 uint64_t r2, uint64_t r3);
+struct args trusty_context_switch_helper(void **sp, void *smc_params);
+
+static uint32_t current_vmid;
 
 static struct trusty_cpu_ctx *get_trusty_ctx(void)
 {
 	return &trusty_cpu_ctx[plat_my_core_pos()];
 }
 
+static uint32_t is_hypervisor_mode(void)
+{
+	uint64_t hcr = read_hcr();
+
+	return !!(hcr & HYP_ENABLE_FLAG);
+}
+
 static struct args trusty_context_switch(uint32_t security_state, uint64_t r0,
 					 uint64_t r1, uint64_t r2, uint64_t r3)
 {
 	struct args ret;
 	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
+	struct trusty_cpu_ctx *ctx_smc;
 
 	assert(ctx->saved_security_state != security_state);
 
+	ret.r7 = 0;
+	if (is_hypervisor_mode()) {
+		/* According to the ARM DEN0028A spec, VMID is stored in x7 */
+		ctx_smc = cm_get_context(NON_SECURE);
+		assert(ctx_smc);
+		ret.r7 = SMC_GET_GP(ctx_smc, CTX_GPREG_X7);
+	}
+	/* r4, r5, r6 reserved for future use. */
+	ret.r6 = 0;
+	ret.r5 = 0;
+	ret.r4 = 0;
+	ret.r3 = r3;
+	ret.r2 = r2;
+	ret.r1 = r1;
+	ret.r0 = r0;
+
 	cm_el1_sysregs_context_save(security_state);
 
 	ctx->saved_security_state = security_state;
-	ret = trusty_context_switch_helper(&ctx->saved_sp, r0, r1, r2, r3);
+	ret = trusty_context_switch_helper(&ctx->saved_sp, &ret);
 
 	assert(ctx->saved_security_state == !security_state);
 
@@ -200,11 +237,25 @@ static uint64_t trusty_smc_handler(uint32_t smc_fid,
 			 uint64_t flags)
 {
 	struct args ret;
+	uint32_t vmid = 0;
+	entry_point_info_t *ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+
+	/*
+	 * Return success for SET_ROT_PARAMS if Trusty is not present, as
+	 * Verified Boot is not even supported and returning success here
+	 * would not compromise the boot process.
+	 */
+	if (!ep_info && (smc_fid == SMC_SC_SET_ROT_PARAMS)) {
+		SMC_RET1(handle, 0);
+	} else if (!ep_info) {
+		SMC_RET1(handle, SMC_UNK);
+	}
 
 	if (is_caller_secure(flags)) {
 		if (smc_fid == SMC_SC_NS_RETURN) {
 			ret = trusty_context_switch(SECURE, x1, 0, 0, 0);
-			SMC_RET4(handle, ret.r0, ret.r1, ret.r2, ret.r3);
+			SMC_RET8(handle, ret.r0, ret.r1, ret.r2, ret.r3,
+				 ret.r4, ret.r5, ret.r6, ret.r7);
 		}
 		INFO("%s (0x%x, 0x%lx, 0x%lx, 0x%lx, 0x%lx, %p, %p, 0x%lx) \
 		     cpu %d, unknown smc\n",
@@ -220,8 +271,21 @@ static uint64_t trusty_smc_handler(uint32_t smc_fid,
 		case SMC_FC_FIQ_EXIT:
 			return trusty_fiq_exit(handle, x1, x2, x3);
 		default:
+			if (is_hypervisor_mode())
+				vmid = SMC_GET_GP(handle, CTX_GPREG_X7);
+
+			if ((current_vmid != 0) && (current_vmid != vmid)) {
+				/* This message will cause SMC mechanism
+				 * abnormal in multi-guest environment.
+				 * Change it to WARN in case you need it.
+				 */
+				VERBOSE("Previous SMC not finished.\n");
+				SMC_RET1(handle, SM_ERR_BUSY);
+			}
+			current_vmid = vmid;
 			ret = trusty_context_switch(NON_SECURE, smc_fid, x1,
 				x2, x3);
+			current_vmid = 0;
 			SMC_RET1(handle, ret.r0);
 		}
 	}
@@ -231,6 +295,7 @@ static int32_t trusty_init(void)
 {
 	void el3_exit(void);
 	entry_point_info_t *ep_info;
+	struct args zero_args = {0};
 	struct trusty_cpu_ctx *ctx = get_trusty_ctx();
 	uint32_t cpu = plat_my_core_pos();
 	int reg_width = GET_RW(read_ctx_reg(get_el3state_ctx(&ctx->cpu_ctx),
@@ -262,9 +327,9 @@ static int32_t trusty_init(void)
 	cm_set_next_eret_context(SECURE);
 
 	ctx->saved_security_state = ~0; /* initial saved state is invalid */
-	trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack);
+	trusty_init_context_stack(&ctx->saved_sp, &ctx->secure_stack.end);
 
-	trusty_context_switch_helper(&ctx->saved_sp, 0, 0, 0, 0);
+	trusty_context_switch_helper(&ctx->saved_sp, &zero_args);
 
 	cm_el1_sysregs_context_restore(NON_SECURE);
 	cm_set_next_eret_context(NON_SECURE);
@@ -332,43 +397,35 @@ static const spd_pm_ops_t trusty_pm = {
 static int32_t trusty_setup(void)
 {
 	entry_point_info_t *ep_info;
-	uint32_t instr;
 	uint32_t flags;
 	int ret;
-	int aarch32 = 0;
 
+	/* Get trusty's entry point info */
 	ep_info = bl31_plat_get_next_image_ep_info(SECURE);
 	if (!ep_info) {
 		INFO("Trusty image missing.\n");
 		return -1;
 	}
 
-	instr = *(uint32_t *)ep_info->pc;
-
-	if (instr >> 24 == 0xea) {
-		INFO("trusty: Found 32 bit image\n");
-		aarch32 = 1;
-	} else if (instr >> 8 == 0xd53810) {
-		INFO("trusty: Found 64 bit image\n");
-	} else {
-		INFO("trusty: Found unknown image, 0x%x\n", instr);
-	}
-
+	/* Trusty runs in AARCH64 mode */
 	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);
+	ep_info->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
+
+	/*
+	 * 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;
 
+	/* register init handler */
 	bl31_register_bl32_init(trusty_init);
 
+	/* register power management hooks */
 	psci_register_spd_pm_hook(&trusty_pm);
 
+	/* register interrupt handler */
 	flags = 0;
 	set_interrupt_rm_flag(flags, NON_SECURE);
 	ret = register_interrupt_type_handler(INTR_TYPE_S_EL1,
diff --git a/services/spd/trusty/trusty_helpers.S b/services/spd/trusty/trusty_helpers.S
index 9bbb044a..7e459ac8 100644
--- a/services/spd/trusty/trusty_helpers.S
+++ b/services/spd/trusty/trusty_helpers.S
@@ -60,8 +60,20 @@ func trusty_context_switch_helper
 	pop	x21, x22
 	pop	x19, x20
 	pop	x8, xzr
-	stp	x1, x2, [x8]
-	stp	x3, x4, [x8, #16]
+
+        ldr     x2, [x1]
+        ldr     x3, [x1, #0x08]
+        ldr     x4, [x1, #0x10]
+        ldr     x5, [x1, #0x18]
+        ldr     x6, [x1, #0x20]
+        ldr     x7, [x1, #0x28]
+        ldr     x10, [x1, #0x30]
+        ldr     x11, [x1, #0x38]
+
+        stp     x2, x3, [x8]
+        stp     x4, x5, [x8, #16]
+        stp     x6, x7, [x8, #32]
+        stp     x10, x11, [x8, #48]
 
 	ret
 endfunc trusty_context_switch_helper