Merge pull request #102 from achingupta:ag/tf-issues#104-v2

5 files changed, 227 insertions, 28 deletions

diff --git a/services/spd/tspd/tspd_common.c b/services/spd/tspd/tspd_common.c
index d3fe5ddf..2ca6a56a 100644
--- a/services/spd/tspd/tspd_common.c
+++ b/services/spd/tspd/tspd_common.c
@@ -42,9 +42,9 @@
  * programming an entry into the secure payload.
  ******************************************************************************/
 int32_t tspd_init_secure_context(uint64_t entrypoint,
-				uint32_t rw,
-				uint64_t mpidr,
-				tsp_context_t *tsp_ctx)
+				 uint32_t rw,
+				 uint64_t mpidr,
+				 tsp_context_t *tsp_ctx)
 {
 	uint32_t scr, sctlr;
 	el1_sys_regs_t *el1_state;
@@ -65,10 +65,14 @@ int32_t tspd_init_secure_context(uint64_t entrypoint,
 	 */
 	memset(tsp_ctx, 0, sizeof(*tsp_ctx));
 
-	/* Set the right security state and register width for the SP */
+	/*
+	 * Set the right security state, register width and enable access to
+	 * the secure physical timer for the SP.
+	 */
 	scr = read_scr();
 	scr &= ~SCR_NS_BIT;
 	scr &= ~SCR_RW_BIT;
+	scr |= SCR_ST_BIT;
 	if (rw == TSP_AARCH64)
 		scr |= SCR_RW_BIT;
 
@@ -85,7 +89,14 @@ int32_t tspd_init_secure_context(uint64_t entrypoint,
 	write_ctx_reg(el1_state, CTX_SCTLR_EL1, sctlr);
 
 	/* Set this context as ready to be initialised i.e OFF */
-	tsp_ctx->state = TSP_STATE_OFF;
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_OFF);
+
+	/*
+	 * This context has not been used yet. It will become valid
+	 * when the TSP is interrupted and wants the TSPD to preserve
+	 * the context.
+	 */
+	clr_std_smc_active_flag(tsp_ctx->state);
 
 	/* Associate this context with the cpu specified */
 	tsp_ctx->mpidr = mpidr;
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index 21ff7ffe..74e2af0f 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -43,6 +43,9 @@
 #include <bl_common.h>
 #include <bl31.h>
 #include <context_mgmt.h>
+#include <debug.h>
+#include <errno.h>
+#include <platform.h>
 #include <runtime_svc.h>
 #include <stddef.h>
 #include <tsp.h>
@@ -68,6 +71,75 @@ DEFINE_SVC_UUID(tsp_uuid,
 
 int32_t tspd_init(void);
 
+/*******************************************************************************
+ * This function is the handler registered for S-EL1 interrupts by the TSPD. It
+ * validates the interrupt and upon success arranges entry into the TSP at
+ * 'tsp_fiq_entry()' for handling the interrupt.
+ ******************************************************************************/
+static uint64_t tspd_sel1_interrupt_handler(uint32_t id,
+					    uint32_t flags,
+					    void *handle,
+					    void *cookie)
+{
+	uint32_t linear_id;
+	uint64_t mpidr;
+	tsp_context_t *tsp_ctx;
+
+	/* Check the security state when the exception was generated */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+
+#if IMF_READ_INTERRUPT_ID
+	/* Check the security status of the interrupt */
+	assert(ic_get_interrupt_group(id) == SECURE);
+#endif
+
+	/* Sanity check the pointer to this cpu's context */
+	mpidr = read_mpidr();
+	assert(handle == cm_get_context(mpidr, NON_SECURE));
+
+	/* Save the non-secure context before entering the TSP */
+	cm_el1_sysregs_context_save(NON_SECURE);
+
+	/* Get a reference to this cpu's TSP context */
+	linear_id = platform_get_core_pos(mpidr);
+	tsp_ctx = &tspd_sp_context[linear_id];
+	assert(&tsp_ctx->cpu_ctx == cm_get_context(mpidr, SECURE));
+
+	/*
+	 * Determine if the TSP was previously preempted. Its last known
+	 * context has to be preserved in this case.
+	 * The TSP should return control to the TSPD after handling this
+	 * FIQ. Preserve essential EL3 context to allow entry into the
+	 * TSP at the FIQ entry point using the 'cpu_context' structure.
+	 * There is no need to save the secure system register context
+	 * since the TSP is supposed to preserve it during S-EL1 interrupt
+	 * handling.
+	 */
+	if (get_std_smc_active_flag(tsp_ctx->state)) {
+		tsp_ctx->saved_spsr_el3 = SMC_GET_EL3(&tsp_ctx->cpu_ctx,
+						      CTX_SPSR_EL3);
+		tsp_ctx->saved_elr_el3 = SMC_GET_EL3(&tsp_ctx->cpu_ctx,
+						     CTX_ELR_EL3);
+	}
+
+	SMC_SET_EL3(&tsp_ctx->cpu_ctx,
+		    CTX_SPSR_EL3,
+		    SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS));
+	SMC_SET_EL3(&tsp_ctx->cpu_ctx,
+		    CTX_ELR_EL3,
+		    (uint64_t) tsp_entry_info->fiq_entry);
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+
+	/*
+	 * Tell the TSP that it has to handle an FIQ synchronously. Also the
+	 * instruction in normal world where the interrupt was generated is
+	 * passed for debugging purposes. It is safe to retrieve this address
+	 * from ELR_EL3 as the secure context will not take effect until
+	 * el3_exit().
+	 */
+	SMC_RET2(&tsp_ctx->cpu_ctx, TSP_HANDLE_FIQ_AND_RETURN, read_elr_el3());
+}
 
 /*******************************************************************************
  * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type
@@ -131,7 +203,7 @@ int32_t tspd_setup(void)
 int32_t tspd_init(void)
 {
 	uint64_t mpidr = read_mpidr();
-	uint32_t linear_id = platform_get_core_pos(mpidr);
+	uint32_t linear_id = platform_get_core_pos(mpidr), flags;
 	uint64_t rc;
 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
 
@@ -142,7 +214,7 @@ int32_t tspd_init(void)
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
 	assert(rc != 0);
 	if (rc) {
-		tsp_ctx->state = TSP_STATE_ON;
+		set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
 
 		/*
 		 * TSP has been successfully initialized. Register power
@@ -151,6 +223,18 @@ int32_t tspd_init(void)
 		psci_register_spd_pm_hook(&tspd_pm);
 	}
 
+	/*
+	 * Register an interrupt handler for S-EL1 interrupts when generated
+	 * during code executing in the non-secure state.
+	 */
+	flags = 0;
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+					     tspd_sel1_interrupt_handler,
+					     flags);
+	if (rc)
+		panic();
+
 	return rc;
 }
 
@@ -184,6 +268,73 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
 	switch (smc_fid) {
 
 	/*
+	 * This function ID is used only by the TSP to indicate that it has
+	 * finished handling a S-EL1 FIQ interrupt. Execution should resume
+	 * in the normal world.
+	 */
+	case TSP_HANDLED_S_EL1_FIQ:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		assert(handle == cm_get_context(mpidr, SECURE));
+
+		/*
+		 * Restore the relevant EL3 state which saved to service
+		 * this SMC.
+		 */
+		if (get_std_smc_active_flag(tsp_ctx->state)) {
+			SMC_SET_EL3(&tsp_ctx->cpu_ctx,
+				    CTX_SPSR_EL3,
+				    tsp_ctx->saved_spsr_el3);
+			SMC_SET_EL3(&tsp_ctx->cpu_ctx,
+				    CTX_ELR_EL3,
+				    tsp_ctx->saved_elr_el3);
+		}
+
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
+		assert(ns_cpu_context);
+
+		/*
+		 * Restore non-secure state. There is no need to save the
+		 * secure system register context since the TSP was supposed
+		 * to preserve it during S-EL1 interrupt handling.
+		 */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+
+		SMC_RET0((uint64_t) ns_cpu_context);
+
+
+	/*
+	 * This function ID is used only by the TSP to indicate that it was
+	 * interrupted due to a EL3 FIQ interrupt. Execution should resume
+	 * in the normal world.
+	 */
+	case TSP_EL3_FIQ:
+		if (ns)
+			SMC_RET1(handle, SMC_UNK);
+
+		assert(handle == cm_get_context(mpidr, SECURE));
+
+		/* Assert that standard SMC execution has been preempted */
+		assert(get_std_smc_active_flag(tsp_ctx->state));
+
+		/* Save the secure system register state */
+		cm_el1_sysregs_context_save(SECURE);
+
+		/* Get a reference to the non-secure context */
+		ns_cpu_context = cm_get_context(mpidr, NON_SECURE);
+		assert(ns_cpu_context);
+
+		/* Restore non-secure state */
+		cm_el1_sysregs_context_restore(NON_SECURE);
+		cm_set_next_eret_context(NON_SECURE);
+
+		SMC_RET1(ns_cpu_context, TSP_EL3_FIQ);
+
+
+	/*
 	 * This function ID is used only by the SP to indicate it has
 	 * finished initialising itself after a cold boot
 	 */
@@ -282,7 +433,7 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
 			assert(&tsp_ctx->cpu_ctx == cm_get_context(mpidr, SECURE));
 			set_aapcs_args7(&tsp_ctx->cpu_ctx, smc_fid, x1, x2, 0, 0,
 					0, 0, 0);
-			cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->fast_smc_entry);
+			cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->fast_smc_entry);
 			cm_el1_sysregs_context_restore(SECURE);
 			cm_set_next_eret_context(SECURE);
 
diff --git a/services/spd/tspd/tspd_pm.c b/services/spd/tspd/tspd_pm.c
index 2447d9e8..d99aa222 100644
--- a/services/spd/tspd/tspd_pm.c
+++ b/services/spd/tspd/tspd_pm.c
@@ -56,10 +56,10 @@ static int32_t tspd_cpu_off_handler(uint64_t cookie)
 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
 
 	assert(tsp_entry_info);
-	assert(tsp_ctx->state == TSP_STATE_ON);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
 
 	/* Program the entry point and enter the TSP */
-	cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->cpu_off_entry);
+	cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->cpu_off_entry);
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
 
 	/*
@@ -73,7 +73,7 @@ static int32_t tspd_cpu_off_handler(uint64_t cookie)
 	 * Reset TSP's context for a fresh start when this cpu is turned on
 	 * subsequently.
 	 */
-	 tsp_ctx->state = TSP_STATE_OFF;
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_OFF);
 
 	 return 0;
 }
@@ -90,13 +90,13 @@ static void tspd_cpu_suspend_handler(uint64_t power_state)
 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
 
 	assert(tsp_entry_info);
-	assert(tsp_ctx->state == TSP_STATE_ON);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
 
 	/* Program the entry point, power_state parameter and enter the TSP */
 	write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx),
 		      CTX_GPREG_X0,
 		      power_state);
-	cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->cpu_suspend_entry);
+	cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->cpu_suspend_entry);
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
 
 	/*
@@ -107,7 +107,7 @@ static void tspd_cpu_suspend_handler(uint64_t power_state)
 		panic();
 
 	/* Update its context to reflect the state the TSP is in */
-	tsp_ctx->state = TSP_STATE_SUSPEND;
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_SUSPEND);
 }
 
 /*******************************************************************************
@@ -124,7 +124,7 @@ static void tspd_cpu_on_finish_handler(uint64_t cookie)
 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
 
 	assert(tsp_entry_info);
-	assert(tsp_ctx->state == TSP_STATE_OFF);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_OFF);
 
 	/* Initialise this cpu's secure context */
 	tspd_init_secure_context((uint64_t) tsp_entry_info->cpu_on_entry,
@@ -143,7 +143,7 @@ static void tspd_cpu_on_finish_handler(uint64_t cookie)
 		panic();
 
 	/* Update its context to reflect the state the SP is in */
-	tsp_ctx->state = TSP_STATE_ON;
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
 }
 
 /*******************************************************************************
@@ -159,13 +159,13 @@ static void tspd_cpu_suspend_finish_handler(uint64_t suspend_level)
 	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
 
 	assert(tsp_entry_info);
-	assert(tsp_ctx->state == TSP_STATE_SUSPEND);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_SUSPEND);
 
 	/* Program the entry point, suspend_level and enter the SP */
 	write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx),
 		      CTX_GPREG_X0,
 		      suspend_level);
-	cm_set_el3_elr(SECURE, (uint64_t) tsp_entry_info->cpu_resume_entry);
+	cm_set_elr_el3(SECURE, (uint64_t) tsp_entry_info->cpu_resume_entry);
 	rc = tspd_synchronous_sp_entry(tsp_ctx);
 
 	/*
@@ -176,7 +176,7 @@ static void tspd_cpu_suspend_finish_handler(uint64_t suspend_level)
 		panic();
 
 	/* Update its context to reflect the state the SP is in */
-	tsp_ctx->state = TSP_STATE_ON;
+	set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
 }
 
 /*******************************************************************************
diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h
index 81484e1c..b9cf496d 100644
--- a/services/spd/tspd/tspd_private.h
+++ b/services/spd/tspd/tspd_private.h
@@ -33,15 +33,47 @@
 
 #include <arch.h>
 #include <context.h>
+#include <interrupt_mgmt.h>
 #include <platform.h>
 #include <psci.h>
 
 /*******************************************************************************
  * Secure Payload PM state information e.g. SP is suspended, uninitialised etc
+ * and macros to access the state information in the per-cpu 'state' flags
  ******************************************************************************/
-#define TSP_STATE_OFF		0
-#define TSP_STATE_ON		1
-#define TSP_STATE_SUSPEND	2
+#define TSP_PSTATE_OFF		0
+#define TSP_PSTATE_ON		1
+#define TSP_PSTATE_SUSPEND	2
+#define TSP_PSTATE_SHIFT	0
+#define TSP_PSTATE_MASK	0x3
+#define get_tsp_pstate(state)	((state >> TSP_PSTATE_SHIFT) & TSP_PSTATE_MASK)
+#define clr_tsp_pstate(state)	(state &= ~(TSP_PSTATE_MASK \
+					    << TSP_PSTATE_SHIFT))
+#define set_tsp_pstate(st, pst)	do {					       \
+					clr_tsp_pstate(st);		       \
+					st |= (pst & TSP_PSTATE_MASK) <<       \
+						TSP_PSTATE_SHIFT;	       \
+				} while (0);
+
+
+/*
+ * This flag is used by the TSPD to determine if the TSP is servicing a standard
+ * SMC request prior to programming the next entry into the TSP e.g. if TSP
+ * execution is preempted by a non-secure interrupt and handed control to the
+ * normal world. If another request which is distinct from what the TSP was
+ * previously doing arrives, then this flag will be help the TSPD to either
+ * reject the new request or service it while ensuring that the previous context
+ * is not corrupted.
+ */
+#define STD_SMC_ACTIVE_FLAG_SHIFT	2
+#define STD_SMC_ACTIVE_FLAG_MASK	1
+#define get_std_smc_active_flag(state)	((state >> STD_SMC_ACTIVE_FLAG_SHIFT) \
+					 & STD_SMC_ACTIVE_FLAG_MASK)
+#define set_std_smc_active_flag(state)	(state |=                             \
+					 1 << STD_SMC_ACTIVE_FLAG_SHIFT)
+#define clr_std_smc_active_flag(state)	(state &=                             \
+					 ~(STD_SMC_ACTIVE_FLAG_MASK           \
+					   << STD_SMC_ACTIVE_FLAG_SHIFT))
 
 /*******************************************************************************
  * Secure Payload execution state information i.e. aarch32 or aarch64
@@ -106,13 +138,19 @@ CASSERT(TSPD_C_RT_CTX_SIZE == sizeof(c_rt_regs_t),	\
 
 /*******************************************************************************
  * Structure which helps the SPD to maintain the per-cpu state of the SP.
- * 'state'    - collection of flags to track SP state e.g. on/off
- * 'mpidr'    - mpidr to associate a context with a cpu
- * 'c_rt_ctx' - stack address to restore C runtime context from after returning
- *              from a synchronous entry into the SP.
- * 'cpu_ctx'  - space to maintain SP architectural state
+ * 'saved_spsr_el3' - temporary copy to allow FIQ handling when the TSP has been
+ *                    preempted.
+ * 'saved_elr_el3'  - temporary copy to allow FIQ handling when the TSP has been
+ *                    preempted.
+ * 'state'          - collection of flags to track SP state e.g. on/off
+ * 'mpidr'          - mpidr to associate a context with a cpu
+ * 'c_rt_ctx'       - stack address to restore C runtime context from after
+ *                    returning from a synchronous entry into the SP.
+ * 'cpu_ctx'        - space to maintain SP architectural state
  ******************************************************************************/
 typedef struct tsp_context {
+	uint64_t saved_elr_el3;
+	uint32_t saved_spsr_el3;
 	uint32_t state;
 	uint64_t mpidr;
 	uint64_t c_rt_ctx;
diff --git a/services/std_svc/psci/psci_entry.S b/services/std_svc/psci/psci_entry.S
index 3d0181a8..bc8d9004 100644
--- a/services/std_svc/psci/psci_entry.S
+++ b/services/std_svc/psci/psci_entry.S
@@ -30,7 +30,6 @@
 
 #include <arch.h>
 #include <asm_macros.S>
-#include <cm_macros.S>
 #include <psci.h>
 
 	.globl	psci_aff_on_finish_entry