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/include/bl31/runtime_svc.h b/include/bl31/runtime_svc.h
index 6d7089678c1984ea4415b7fe76aa9b3e9764886a..0f510f76064d9ccfaae2a760a77bd6271ad7a9bc 100644 (file)
--- a/include/bl31/runtime_svc.h
+++ b/include/bl31/runtime_svc.h
@@ -135,9 +135,12 @@
 typedef int32_t (*rt_svc_init_t)(void);
 
 /* Convenience macros to return from SMC handler */
+#define SMC_RET0(_h)   { \
+       return (uint64_t) (_h);         \
+}
 #define SMC_RET1(_h, _x0)      { \
        write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X0, (_x0)); \
-       return _x0; \
+       SMC_RET0(_h);                                           \
 }
 #define SMC_RET2(_h, _x0, _x1) { \
        write_ctx_reg(get_gpregs_ctx(_h), CTX_GPREG_X1, (_x1)); \

diff --git a/services/spd/tspd/tspd_common.c b/services/spd/tspd/tspd_common.c
index d975ec4f9f6a41e6329f02d99c4e0409dcf5f0a5..2ca6a56af2b7fb3a7485c76c892b459e23c52374 100644 (file)
--- 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 6909cf65e10ac46e584a3fd752241915170481f6..74e2af0fe3952f6aa37aa8cf1b32449008bee459 100644 (file)
--- 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 @@ 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];
 
@@ -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

diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h
index bb0afcd960901a316fc7db5f8f8d3fd97c19efad..b9cf496d47f09f77e827992d0209aeec0fb5626f 100644 (file)
--- 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 @@ 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.
- * '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;