Add S-EL1 interrupt handling support in the TSPD
This patch adds support in the TSPD for registering a handler for
S-EL1 interrupts. This handler ferries the interrupts generated in the
non-secure state to the TSP at 'tsp_fiq_entry'. Support has been added
to the smc handler to resume execution in the non-secure state once
interrupt handling has been completed by the TSP.
There is also support for resuming execution in the normal world if
the TSP receives a EL3 interrupt. This code is currently unused.
Change-Id: I816732595a2635e299572965179f11aa0bf93b69
diff --git a/services/spd/tspd/tspd_common.c b/services/spd/tspd/tspd_common.c
index d975ec4..2ca6a56 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;
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index 6909cf6..74e2af0 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 @@
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_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];
@@ -151,6 +223,18 @@
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 @@
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
*/
diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h
index bb0afcd..b9cf496 100644
--- a/services/spd/tspd/tspd_private.h
+++ b/services/spd/tspd/tspd_private.h
@@ -33,6 +33,7 @@
#include <arch.h>
#include <context.h>
+#include <interrupt_mgmt.h>
#include <platform.h>
#include <psci.h>
@@ -137,13 +138,19 @@
/*******************************************************************************
* 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;