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

include/lib/aarch64/arch_helpers.h

@@ -202,6 +202,10 @@ extern unsigned long read_cptr_el3(void);
 extern unsigned long read_cpacr(void);
 extern unsigned long read_cpuectlr(void);
 extern unsigned int read_cntfrq_el0(void);
+extern unsigned int read_cntps_ctl_el1(void);
+extern unsigned int read_cntps_tval_el1(void);
+extern unsigned long read_cntps_cval_el1(void);
+extern unsigned long read_cntpct_el0(void);
 extern unsigned long read_cnthctl_el2(void);
 
 extern unsigned long read_tpidr_el3(void);
@@ -210,6 +214,9 @@ extern void write_scr(unsigned long);
 extern void write_hcr(unsigned long);
 extern void write_cpacr(unsigned long);
 extern void write_cntfrq_el0(unsigned int);
+extern void write_cntps_ctl_el1(unsigned int);
+extern void write_cntps_tval_el1(unsigned int);
+extern void write_cntps_cval_el1(unsigned long);
 extern void write_cnthctl_el2(unsigned long);
 
 extern void write_vbar_el1(unsigned long);

lib/aarch64/sysreg_helpers.S

@@ -142,6 +142,15 @@
 	.globl	read_cntfrq_el0
 	.globl	write_cntfrq_el0
 
+	.globl	read_cntps_ctl_el1
+	.globl	write_cntps_ctl_el1
+
+	.globl	read_cntps_cval_el1
+	.globl	write_cntps_cval_el1
+
+	.globl	read_cntps_tval_el1
+	.globl	write_cntps_tval_el1
+
 	.globl	read_scr
 	.globl	write_scr
 
@@ -151,6 +160,7 @@
 	.globl	read_midr
 	.globl	read_mpidr
 
+	.globl	read_cntpct_el0
 	.globl	read_current_el
 	.globl	read_id_pfr1_el1
 	.globl	read_id_aa64pfr0_el1
@@ -672,6 +682,33 @@ func write_cntfrq_el0
 	msr	cntfrq_el0, x0
 	ret
 
+func read_cntps_ctl_el1
+	mrs	x0, cntps_ctl_el1
+	ret
+
+func write_cntps_ctl_el1
+	msr	cntps_ctl_el1, x0
+	ret
+
+func read_cntps_cval_el1
+	mrs	x0, cntps_cval_el1
+	ret
+
+func write_cntps_cval_el1
+	msr	cntps_cval_el1, x0
+	ret
+
+func read_cntps_tval_el1
+	mrs	x0, cntps_tval_el1
+	ret
+
+func write_cntps_tval_el1
+	msr	cntps_tval_el1, x0
+	ret
+
+func read_cntpct_el0
+	mrs	x0, cntpct_el0
+	ret
 
 func read_cpuectlr
 	mrs	x0, CPUECTLR_EL1

plat/fvp/bl32_plat_setup.c

@@ -73,6 +73,9 @@ void bl32_early_platform_setup(void)
 	 * messages from TSP
 	 */
 	console_init(PL011_UART1_BASE);
+
+	/* Initialize the platform config for future decision making */
+	platform_config_setup();
 }
 
 /*******************************************************************************

plat/fvp/plat_gic.c

@@ -29,18 +29,15 @@
  */
 
 #include <arch_helpers.h>
+#include <assert.h>
+#include <bl_common.h>
 #include <debug.h>
 #include <gic_v2.h>
 #include <gic_v3.h>
+#include <interrupt_mgmt.h>
 #include <platform.h>
 #include <stdint.h>
 
-
-/*******************************************************************************
- * TODO: Revisit if priorities are being set such that no non-secure interrupt
- * can have a higher priority than a secure one as recommended in the GICv2 spec
- ******************************************************************************/
-
 /*******************************************************************************
  * This function does some minimal GICv3 configuration. The Firmware itself does
  * not fully support GICv3 at this time and relies on GICv2 emulation as
@@ -284,3 +281,126 @@ void gic_setup(void)
 	gic_cpuif_setup(gicc_base);
 	gic_distif_setup(gicd_base);
 }
+
+/*******************************************************************************
+ * An ARM processor signals interrupt exceptions through the IRQ and FIQ pins.
+ * The interrupt controller knows which pin/line it uses to signal a type of
+ * interrupt. The platform knows which interrupt controller type is being used
+ * in a particular security state e.g. with an ARM GIC, normal world could use
+ * the GICv2 features while the secure world could use GICv3 features and vice
+ * versa.
+ * This function is exported by the platform to let the interrupt management
+ * framework determine for a type of interrupt and security state, which line
+ * should be used in the SCR_EL3 to control its routing to EL3. The interrupt
+ * line is represented as the bit position of the IRQ or FIQ bit in the SCR_EL3.
+ ******************************************************************************/
+uint32_t plat_interrupt_type_to_line(uint32_t type, uint32_t security_state)
+{
+	uint32_t gicc_base = platform_get_cfgvar(CONFIG_GICC_ADDR);
+
+	assert(type == INTR_TYPE_S_EL1 ||
+	       type == INTR_TYPE_EL3 ||
+	       type == INTR_TYPE_NS);
+
+	assert(security_state == NON_SECURE || security_state == SECURE);
+
+	/*
+	 * We ignore the security state parameter under the assumption that
+	 * both normal and secure worlds are using ARM GICv2. This parameter
+	 * will be used when the secure world starts using GICv3.
+	 */
+#if FVP_GIC_ARCH == 2
+	return gicv2_interrupt_type_to_line(gicc_base, type);
+#else
+#error "Invalid GIC architecture version specified for FVP port"
+#endif
+}
+
+#if FVP_GIC_ARCH == 2
+/*******************************************************************************
+ * This function returns the type of the highest priority pending interrupt at
+ * the GIC cpu interface. INTR_TYPE_INVAL is returned when there is no
+ * interrupt pending.
+ ******************************************************************************/
+uint32_t ic_get_pending_interrupt_type()
+{
+	uint32_t id, gicc_base;
+
+	gicc_base = platform_get_cfgvar(CONFIG_GICC_ADDR);
+	id = gicc_read_hppir(gicc_base);
+
+	/* Assume that all secure interrupts are S-EL1 interrupts */
+	if (id < 1022)
+		return INTR_TYPE_S_EL1;
+
+	if (id == GIC_SPURIOUS_INTERRUPT)
+		return INTR_TYPE_INVAL;
+
+	return INTR_TYPE_NS;
+}
+
+/*******************************************************************************
+ * This function returns the id of the highest priority pending interrupt at
+ * the GIC cpu interface. INTR_ID_UNAVAILABLE is returned when there is no
+ * interrupt pending.
+ ******************************************************************************/
+uint32_t ic_get_pending_interrupt_id()
+{
+	uint32_t id, gicc_base;
+
+	gicc_base = platform_get_cfgvar(CONFIG_GICC_ADDR);
+	id = gicc_read_hppir(gicc_base);
+
+	if (id < 1022)
+		return id;
+
+	if (id == 1023)
+		return INTR_ID_UNAVAILABLE;
+
+	/*
+	 * Find out which non-secure interrupt it is under the assumption that
+	 * the GICC_CTLR.AckCtl bit is 0.
+	 */
+	return gicc_read_ahppir(gicc_base);
+}
+
+/*******************************************************************************
+ * This functions reads the GIC cpu interface Interrupt Acknowledge register
+ * to start handling the pending interrupt. It returns the contents of the IAR.
+ ******************************************************************************/
+uint32_t ic_acknowledge_interrupt()
+{
+	return gicc_read_IAR(platform_get_cfgvar(CONFIG_GICC_ADDR));
+}
+
+/*******************************************************************************
+ * This functions writes the GIC cpu interface End Of Interrupt register with
+ * the passed value to finish handling the active interrupt
+ ******************************************************************************/
+void ic_end_of_interrupt(uint32_t id)
+{
+	gicc_write_EOIR(platform_get_cfgvar(CONFIG_GICC_ADDR), id);
+	return;
+}
+
+/*******************************************************************************
+ * This function returns the type of the interrupt id depending upon the group
+ * this interrupt has been configured under by the interrupt controller i.e.
+ * group0 or group1.
+ ******************************************************************************/
+uint32_t ic_get_interrupt_type(uint32_t id)
+{
+	uint32_t group;
+
+	group = gicd_get_igroupr(platform_get_cfgvar(CONFIG_GICD_ADDR), id);
+
+	/* Assume that all secure interrupts are S-EL1 interrupts */
+	if (group == GRP0)
+		return INTR_TYPE_S_EL1;
+	else
+		return INTR_TYPE_NS;
+}
+
+#else
+#error "Invalid GIC architecture version specified for FVP port"
+#endif

plat/fvp/platform.h

@@ -452,13 +452,20 @@ extern void plat_get_entry_point_info(unsigned long target_security,
 
 extern void fvp_cci_setup(void);
 
-/* Declarations for fvp_gic.c */
+/* Declarations for plat_gic.c */
+extern uint32_t ic_get_pending_interrupt_id(void);
+extern uint32_t ic_get_pending_interrupt_type(void);
+extern uint32_t ic_acknowledge_interrupt(void);
+extern uint32_t ic_get_interrupt_type(uint32_t id);
+extern void ic_end_of_interrupt(uint32_t id);
 extern void gic_cpuif_deactivate(unsigned int);
 extern void gic_cpuif_setup(unsigned int);
 extern void gic_pcpu_distif_setup(unsigned int);
 extern void gic_setup(void);
+extern uint32_t plat_interrupt_type_to_line(uint32_t type,
+					    uint32_t security_state);
 
-/* Declarations for fvp_topology.c */
+/* Declarations for plat_topology.c */
 extern int plat_setup_topology(void);
 extern int plat_get_max_afflvl(void);
 extern unsigned int plat_get_aff_count(unsigned int, unsigned long);

plat/fvp/platform.mk

@@ -86,3 +86,8 @@ ifeq (${RESET_TO_BL31}, 1)
 	BL31_SOURCES		+=	drivers/arm/tzc400/tzc400.c		\
 					plat/fvp/plat_security.c
 endif
+
+# Flag used by the FVP port to determine the version of ARM GIC architecture
+# to use for interrupt management in EL3.
+FVP_GIC_ARCH		:=	2
+$(eval $(call add_define,FVP_GIC_ARCH))

services/spd/tspd/tspd_common.c

@@ -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;

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;
 }
 
@@ -183,6 +267,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);
 

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);
 }
 
 /*******************************************************************************

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.
+ * '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.
+ * '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;

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