Non-Secure Interrupt support during Standard SMC processing in TSP

Implements support for Non Secure Interrupts preempting the
Standard SMC call in EL1. Whenever an IRQ is trapped in the
Secure world we securely handover to the Normal world
to process the interrupt. The normal world then issues
"resume" smc call to resume the previous interrupted SMC call.
Fixes ARM-software/tf-issues#105

Change-Id: I72b760617dee27438754cdfc9fe9bcf4cc024858

4 files changed, 55 insertions, 13 deletions

diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 54276f2c..9999c432 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -39,8 +39,11 @@
 	.globl	tsp_cpu_suspend_entry
 	.globl	tsp_cpu_resume_entry
 	.globl	tsp_fast_smc_entry
+	.globl	tsp_std_smc_entry
 	.globl	tsp_fiq_entry
 
+
+
 	/* ---------------------------------------------
 	 * Populate the params in x0-x7 from the pointer
 	 * to the smc args structure in x0.
@@ -317,8 +320,22 @@ tsp_cpu_resume_panic:
 	 * ---------------------------------------------
 	 */
 func tsp_fast_smc_entry
-	bl	tsp_fast_smc_handler
+	bl	tsp_smc_handler
 	restore_args_call_smc
 tsp_fast_smc_entry_panic:
 	b	tsp_fast_smc_entry_panic
 
+	/*---------------------------------------------
+	 * This entrypoint is used by the TSPD to ask
+	 * the TSP to service a std smc request.
+	 * We will enable preemption during execution
+	 * of tsp_smc_handler.
+	 * ---------------------------------------------
+	 */
+func tsp_std_smc_entry
+	msr	daifclr, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
+	bl	tsp_smc_handler
+	msr	daifset, #DAIF_FIQ_BIT | DAIF_IRQ_BIT
+	restore_args_call_smc
+tsp_std_smc_entry_panic:
+	b	tsp_std_smc_entry_panic
diff --git a/bl32/tsp/aarch64/tsp_exceptions.S b/bl32/tsp/aarch64/tsp_exceptions.S
index ccb4cddd..f84b5e09 100644
--- a/bl32/tsp/aarch64/tsp_exceptions.S
+++ b/bl32/tsp/aarch64/tsp_exceptions.S
@@ -120,7 +120,14 @@ sync_exception_sp_elx:
 
 	.align	7
 irq_sp_elx:
-	b	irq_sp_elx
+	save_caller_regs_and_lr
+	/* We just update some statistics in the handler */
+	bl	tsp_irq_received
+	/* Hand over control to the normal world to handle the IRQ */
+	smc	#0
+	/* The resume std smc starts from here */
+	restore_caller_regs_and_lr
+	eret
 	check_vector_size irq_sp_elx
 
 	.align	7
diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c
index d5d02c30..5719c063 100644
--- a/bl32/tsp/tsp_interrupt.c
+++ b/bl32/tsp/tsp_interrupt.c
@@ -107,3 +107,18 @@ int32_t tsp_fiq_handler()
 
 	return 0;
 }
+
+int32_t tsp_irq_received()
+{
+	uint64_t mpidr = read_mpidr();
+	uint32_t linear_id = platform_get_core_pos(mpidr);
+
+	tsp_stats[linear_id].irq_count++;
+	spin_lock(&console_lock);
+	printf("TSP: cpu 0x%x received irq\n\r", mpidr);
+	INFO("cpu 0x%x: %d irq requests \n",
+	     mpidr, tsp_stats[linear_id].irq_count);
+	spin_unlock(&console_lock);
+
+	return TSP_PREEMPTED;
+}
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index ec74db42..1c3f3b98 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -66,6 +66,7 @@ work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
  * to change.
  ******************************************************************************/
 static const entry_info_t tsp_entry_info = {
+	tsp_std_smc_entry,
 	tsp_fast_smc_entry,
 	tsp_cpu_on_entry,
 	tsp_cpu_off_entry,
@@ -309,9 +310,9 @@ tsp_args_t *tsp_cpu_resume_main(uint64_t suspend_level,
  * TSP fast smc handler. The secure monitor jumps to this function by
  * doing the ERET after populating X0-X7 registers. The arguments are received
  * in the function arguments in order. Once the service is rendered, this
- * function returns to Secure Monitor by raising SMC
+ * function returns to Secure Monitor by raising SMC.
  ******************************************************************************/
-tsp_args_t *tsp_fast_smc_handler(uint64_t func,
+tsp_args_t *tsp_smc_handler(uint64_t func,
 			       uint64_t arg1,
 			       uint64_t arg2,
 			       uint64_t arg3,
@@ -324,18 +325,20 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
 	uint64_t service_args[2];
 	uint64_t mpidr = read_mpidr();
 	uint32_t linear_id = platform_get_core_pos(mpidr);
+	const char *smc_type;
 
 	/* Update this cpu's statistics */
 	tsp_stats[linear_id].smc_count++;
 	tsp_stats[linear_id].eret_count++;
 
-	printf("SP: cpu 0x%x received fast smc 0x%x\n", read_mpidr(), func);
+	smc_type = ((func >> 31) & 1) == 1 ? "fast" : "standard";
+
+	printf("SP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(), smc_type, func);
 	INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr,
 	     tsp_stats[linear_id].smc_count,
 	     tsp_stats[linear_id].eret_count);
 
 	/* Render secure services and obtain results here */
-
 	results[0] = arg1;
 	results[1] = arg2;
 
@@ -346,20 +349,20 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
 	tsp_get_magic(service_args);
 
 	/* Determine the function to perform based on the function ID */
-	switch (func) {
-	case TSP_FID_ADD:
+	switch (TSP_BARE_FID(func)) {
+	case TSP_ADD:
 		results[0] += service_args[0];
 		results[1] += service_args[1];
 		break;
-	case TSP_FID_SUB:
+	case TSP_SUB:
 		results[0] -= service_args[0];
 		results[1] -= service_args[1];
 		break;
-	case TSP_FID_MUL:
+	case TSP_MUL:
 		results[0] *= service_args[0];
 		results[1] *= service_args[1];
 		break;
-	case TSP_FID_DIV:
+	case TSP_DIV:
 		results[0] /= service_args[0] ? service_args[0] : 1;
 		results[1] /= service_args[1] ? service_args[1] : 1;
 		break;
@@ -367,9 +370,9 @@ tsp_args_t *tsp_fast_smc_handler(uint64_t func,
 		break;
 	}
 
-	return set_smc_args(func,
+	return set_smc_args(func, 0,
 			    results[0],
 			    results[1],
-			    0, 0, 0, 0, 0);
+			    0, 0, 0, 0);
 }