TSP: Allow preemption of synchronous S-EL1 interrupt handling

Earlier the TSP only ever expected to be preempted during Standard SMC
processing. If a S-EL1 interrupt triggered while in the normal world, it
will routed to S-EL1 `synchronously` for handling. The `synchronous` S-EL1
interrupt handler `tsp_sel1_intr_entry` used to panic if this S-EL1 interrupt
was preempted by another higher priority pending interrupt which should be
handled in EL3 e.g. Group0 interrupt in GICv3.

With this patch, the `tsp_sel1_intr_entry` now expects `TSP_PREEMPTED` as the
return code from the `tsp_common_int_handler` in addition to 0 (interrupt
successfully handled) and in both cases it issues an SMC with id
`TSP_HANDLED_S_EL1_INTR`. The TSPD switches the context and returns back
to normal world. In case a higher priority EL3 interrupt was pending, the
execution will be routed to EL3 where interrupt will be handled. On return
back to normal world, the pending S-EL1 interrupt which was preempted will
get routed to S-EL1 to be handled `synchronously` via `tsp_sel1_intr_entry`.

Change-Id: I2087c7fedb37746fbd9200cdda9b6dba93e16201

2 files changed, 40 insertions, 20 deletions

diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index d183dff9..531ab9bf 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -327,11 +327,13 @@ endfunc tsp_cpu_suspend_entry
 
 	/*-------------------------------------------------
 	 * This entrypoint is used by the TSPD to pass
-	 * control for handling a pending S-EL1 Interrupt.
-	 * 'x0' contains a magic number which indicates
-	 * this. TSPD expects control to be handed back
-	 * at the end of interrupt processing. This is
-	 * done through an SMC. The handover agreement is:
+	 * control for `synchronously` handling a S-EL1
+	 * Interrupt which was triggered while executing
+	 * in normal world. 'x0' contains a magic number
+	 * which indicates this. TSPD expects control to
+	 * be handed back at the end of interrupt
+	 * processing. This is done through an SMC.
+	 * The handover agreement is:
 	 *
 	 * 1. PSTATE.DAIF are set upon entry. 'x1' has
 	 *    the ELR_EL3 from the non-secure state.
@@ -348,8 +350,7 @@ endfunc tsp_cpu_suspend_entry
 	 */
 func	tsp_sel1_intr_entry
 #if DEBUG
-	mov	x2, #(TSP_HANDLE_SEL1_INTR_AND_RETURN & ~0xffff)
-	movk	x2, #(TSP_HANDLE_SEL1_INTR_AND_RETURN &  0xffff)
+	mov_imm	x2, TSP_HANDLE_SEL1_INTR_AND_RETURN
 	cmp	x0, x2
 	b.ne	tsp_sel1_int_entry_panic
 #endif
@@ -362,19 +363,33 @@ func	tsp_sel1_intr_entry
 	 * IRQ/FIQs are not enabled since that will
 	 * complicate the implementation. Execution
 	 * will be transferred back to the normal world
-	 * in any case. A non-zero return value from the
-	 * interrupt handler is an error.
+	 * in any case. The handler can return 0
+	 * if the interrupt was handled or TSP_PREEMPTED
+	 * if the expected interrupt was preempted
+	 * by an interrupt that should be handled in EL3
+	 * e.g. Group 0 interrupt in GICv3. In both
+	 * the cases switch to EL3 using SMC with id
+	 * TSP_HANDLED_S_EL1_INTR. Any other return value
+	 * from the handler will result in panic.
 	 * ------------------------------------------------
 	 */
 	save_eret_context x2 x3
 	bl	tsp_update_sync_sel1_intr_stats
 	bl	tsp_common_int_handler
-	cbnz	x0, tsp_sel1_int_entry_panic
+	/* Check if the S-EL1 interrupt has been handled */
+	cbnz	x0, tsp_sel1_intr_check_preemption
+	b	tsp_sel1_intr_return
+tsp_sel1_intr_check_preemption:
+	/* Check if the S-EL1 interrupt has been preempted */
+	mov_imm	x1, TSP_PREEMPTED
+	cmp	x0, x1
+	b.ne	tsp_sel1_int_entry_panic
+tsp_sel1_intr_return:
+	mov_imm	x0, TSP_HANDLED_S_EL1_INTR
 	restore_eret_context x2 x3
-	mov	x0, #(TSP_HANDLED_S_EL1_INTR & ~0xffff)
-	movk	x0, #(TSP_HANDLED_S_EL1_INTR &  0xffff)
 	smc	#0
 
+	/* Should never reach here */
 tsp_sel1_int_entry_panic:
 	b	tsp_sel1_int_entry_panic
 endfunc tsp_sel1_intr_entry
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index 4c4861d7..4b894258 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -89,18 +89,22 @@ uint64_t tspd_handle_sp_preemption(void *handle)
 	assert(ns_cpu_context);
 
 	/*
-	 * Restore non-secure state. The secure system
-	 * register context will be saved when required.
+	 * To allow Secure EL1 interrupt handler to re-enter TSP while TSP
+	 * is preempted, the secure system register context which will get
+	 * overwritten must be additionally saved. This is currently done
+	 * by the TSPD S-EL1 interrupt handler.
+	 */
+
+	/*
+	 * Restore non-secure state.
 	 */
 	cm_el1_sysregs_context_restore(NON_SECURE);
 	cm_set_next_eret_context(NON_SECURE);
 
 	/*
-	 * We need to restore non secure context according to
-	 * the SEL1 context which got preempted and currently
-	 * TSP can only be preempted when a STD SMC is ongoing.
-	 * Return SMC_PREEMPTED in x0 and restore non secure
-	 * context.
+	 * The TSP was preempted during STD SMC execution.
+	 * Return back to the normal world with SMC_PREEMPTED as error
+	 * code in x0.
 	 */
 	SMC_RET1(ns_cpu_context, SMC_PREEMPTED);
 }
@@ -327,7 +331,8 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
 
 	/*
 	 * This function ID is used only by the TSP to indicate that it has
-	 * finished handling a S-EL1 interrupt. Execution should resume
+	 * finished handling a S-EL1 interrupt or was preempted by a higher
+	 * priority pending EL3 interrupt. Execution should resume
 	 * in the normal world.
 	 */
 	case TSP_HANDLED_S_EL1_INTR: