Rework the TSPD setup code

There is no mechanism which allows the TSPD to specify what SPSR to
use when entering BL3-2 instead of BL3-3. This patch divides the
responsibility between tspd_setup() and tspd_init() for initializing
the TSPD and TSP to support the alternate BL3-2 initialization flow
where BL3-1 handsover control to BL3-2 instead of BL3-3.
SPSR generated by TSPD for TSP is preserved due the new division of
labour which fixes #174.

This patch also moves the cpu_context initialization code from
tspd_setup() to tspd_init() immediately before entering the TSP.
Instead tspd_setup() updates the BL3-2 entrypoint info structure
with the state required for initializing the TSP later.

Fixes  ARM-software/TF-issues#174

Change-Id: Ida0a8a48d466c71d5b07b8c7f2af169b73f96940

4 files changed, 52 insertions, 35 deletions

diff --git a/services/spd/tspd/tspd_common.c b/services/spd/tspd/tspd_common.c
index 1b9609f..322413c 100644
--- a/services/spd/tspd/tspd_common.c
+++ b/services/spd/tspd/tspd_common.c
@@ -36,20 +36,21 @@
 #include "tspd_private.h"
 
 /*******************************************************************************
- * Given a secure payload entrypoint, register width, cpu id & pointer to a
- * context data structure, this function will create a secure context ready for
- * programming an entry into the secure payload.
+ * Given a secure payload entrypoint info pointer, entry point PC, register
+ * width, cpu id & pointer to a context data structure, this function will
+ * initialize tsp context and entry point info for the secure payload
  ******************************************************************************/
-int32_t tspd_init_secure_context(uint64_t entrypoint,
-				 uint32_t rw,
-				 uint64_t mpidr,
-				 tsp_context_t *tsp_ctx)
+void tspd_init_tsp_ep_state(struct entry_point_info *tsp_entry_point,
+				uint32_t rw,
+				uint64_t pc,
+				tsp_context_t *tsp_ctx)
 {
-	entry_point_info_t ep;
 	uint32_t ep_attr;
 
 	/* Passing a NULL context is a critical programming error */
 	assert(tsp_ctx);
+	assert(tsp_entry_point);
+	assert(pc);
 
 	/*
 	 * We support AArch64 TSP for now.
@@ -58,25 +59,24 @@ int32_t tspd_init_secure_context(uint64_t entrypoint,
 	assert(rw == TSP_AARCH64);
 
 	/* Associate this context with the cpu specified */
-	tsp_ctx->mpidr = mpidr;
+	tsp_ctx->mpidr = read_mpidr_el1();
 	tsp_ctx->state = 0;
 	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_OFF);
 	clr_std_smc_active_flag(tsp_ctx->state);
 
-	cm_set_context_by_mpidr(mpidr, &tsp_ctx->cpu_ctx, SECURE);
+	cm_set_context(&tsp_ctx->cpu_ctx, SECURE);
 
 	/* initialise an entrypoint to set up the CPU context */
 	ep_attr = SECURE | EP_ST_ENABLE;
 	if (read_sctlr_el3() & SCTLR_EE_BIT)
 		ep_attr |= EP_EE_BIG;
-	SET_PARAM_HEAD(&ep, PARAM_EP, VERSION_1, ep_attr);
-	ep.pc = entrypoint;
-	ep.spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
-	memset(&ep.args, 0, sizeof(ep.args));
+	SET_PARAM_HEAD(tsp_entry_point, PARAM_EP, VERSION_1, ep_attr);
 
-	cm_init_context(mpidr, &ep);
-
-	return 0;
+	tsp_entry_point->pc = pc;
+	tsp_entry_point->spsr = SPSR_64(MODE_EL1,
+					MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS);
+	memset(&tsp_entry_point->args, 0, sizeof(tsp_entry_point->args));
 }
 
 /*******************************************************************************
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index f1dbe68..b2c3615 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -144,8 +144,7 @@ static uint64_t tspd_sel1_interrupt_handler(uint32_t id,
  ******************************************************************************/
 int32_t tspd_setup(void)
 {
-	entry_point_info_t *image_info;
-	int32_t rc;
+	entry_point_info_t *tsp_ep_info;
 	uint64_t mpidr = read_mpidr();
 	uint32_t linear_id;
 
@@ -156,15 +155,20 @@ int32_t tspd_setup(void)
 	 * absence is a critical failure.  TODO: Add support to
 	 * conditionally include the SPD service
 	 */
-	image_info = bl31_plat_get_next_image_ep_info(SECURE);
-	assert(image_info);
+	tsp_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (!tsp_ep_info) {
+		WARN("No TSP provided by BL2 boot loader, Booting device"
+			" without TSP initialization. SMC`s destined for TSP"
+			" will return SMC_UNK\n");
+		return 1;
+	}
 
 	/*
 	 * If there's no valid entry point for SP, we return a non-zero value
 	 * signalling failure initializing the service. We bail out without
 	 * registering any handlers
 	 */
-	if (!image_info->pc)
+	if (!tsp_ep_info->pc)
 		return 1;
 
 	/*
@@ -172,11 +176,10 @@ int32_t tspd_setup(void)
 	 * state i.e whether AArch32 or AArch64. Assuming it's AArch64
 	 * for the time being.
 	 */
-	rc = tspd_init_secure_context(image_info->pc,
-				     TSP_AARCH64,
-				     mpidr,
-				     &tspd_sp_context[linear_id]);
-	assert(rc == 0);
+	tspd_init_tsp_ep_state(tsp_ep_info,
+				TSP_AARCH64,
+				tsp_ep_info->pc,
+				&tspd_sp_context[linear_id]);
 
 	/*
 	 * All TSPD initialization done. Now register our init function with
@@ -184,7 +187,7 @@ int32_t tspd_setup(void)
 	 */
 	bl31_register_bl32_init(&tspd_init);
 
-	return rc;
+	return 0;
 }
 
 /*******************************************************************************
@@ -202,6 +205,16 @@ int32_t tspd_init(void)
 	uint32_t linear_id = platform_get_core_pos(mpidr), flags;
 	uint64_t rc;
 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+	entry_point_info_t *tsp_entry_point;
+
+	/*
+	 * Get information about the Secure Payload (BL32) image. Its
+	 * absence is a critical failure.
+	 */
+	tsp_entry_point = bl31_plat_get_next_image_ep_info(SECURE);
+	assert(tsp_entry_point);
+
+	cm_init_context(mpidr, tsp_entry_point);
 
 	/*
 	 * Arrange for an entry into the test secure payload. We expect an array
diff --git a/services/spd/tspd/tspd_pm.c b/services/spd/tspd/tspd_pm.c
index ec4989d..e9e037a 100644
--- a/services/spd/tspd/tspd_pm.c
+++ b/services/spd/tspd/tspd_pm.c
@@ -123,16 +123,19 @@ static void tspd_cpu_on_finish_handler(uint64_t cookie)
 	uint64_t mpidr = read_mpidr();
 	uint32_t linear_id = platform_get_core_pos(mpidr);
 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+	entry_point_info_t tsp_on_entrypoint;
 
 	assert(tsp_vectors);
 	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_OFF);
 
-	/* Initialise this cpu's secure context */
-	tspd_init_secure_context((uint64_t) &tsp_vectors->cpu_on_entry,
+	tspd_init_tsp_ep_state(&tsp_on_entrypoint,
 				TSP_AARCH64,
-				mpidr,
+				(uint64_t) &tsp_vectors->cpu_on_entry,
 				tsp_ctx);
 
+	/* Initialise this cpu's secure context */
+	cm_init_context(mpidr, &tsp_on_entrypoint);
+
 	/* Enter the TSP */
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
 
diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h
index 5d7bf4b..4d48dbd 100644
--- a/services/spd/tspd/tspd_private.h
+++ b/services/spd/tspd/tspd_private.h
@@ -192,10 +192,11 @@ uint64_t tspd_enter_sp(uint64_t *c_rt_ctx);
 void __dead2 tspd_exit_sp(uint64_t c_rt_ctx, uint64_t ret);
 uint64_t tspd_synchronous_sp_entry(tsp_context_t *tsp_ctx);
 void __dead2 tspd_synchronous_sp_exit(tsp_context_t *tsp_ctx, uint64_t ret);
-int32_t tspd_init_secure_context(uint64_t entrypoint,
-					uint32_t rw,
-					uint64_t mpidr,
-					tsp_context_t *tsp_ctx);
+void tspd_init_tsp_ep_state(struct entry_point_info *tsp_ep,
+				uint32_t rw,
+				uint64_t pc,
+				tsp_context_t *tsp_ctx);
+
 extern tsp_context_t tspd_sp_context[TSPD_CORE_COUNT];
 extern struct tsp_vectors *tsp_vectors;
 #endif /*__ASSEMBLY__*/