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
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index 54276f2..9999c43 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 @@
* ---------------------------------------------
*/
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 ccb4cdd..f84b5e0 100644
--- a/bl32/tsp/aarch64/tsp_exceptions.S
+++ b/bl32/tsp/aarch64/tsp_exceptions.S
@@ -120,7 +120,14 @@
.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 d5d02c3..5719c06 100644
--- a/bl32/tsp/tsp_interrupt.c
+++ b/bl32/tsp/tsp_interrupt.c
@@ -107,3 +107,18 @@
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 ec74db4..1c3f3b9 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -66,6 +66,7 @@
* 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 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 @@
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_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 @@
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);
}