diff --git a/include/bl31/runtime_svc.h b/include/bl31/runtime_svc.h index 6d70896..0f510f7 100644 --- 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 0baa7dd..80c1405 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 06e21c4..e6b0492 100644 --- a/services/spd/tspd/tspd_main.c +++ b/services/spd/tspd/tspd_main.c @@ -43,6 +43,9 @@ #include #include #include +#include +#include +#include #include #include #include @@ -68,6 +71,75 @@ int32_t tspd_init(meminfo_t *bl32_meminfo); +/******************************************************************************* + * 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, + make_spsr(MODE_EL1, MODE_SP_ELX, TSP_AARCH64)); + 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 @@ -105,9 +177,9 @@ * for the time being. */ rc = tspd_init_secure_context(image_info->entrypoint, - TSP_AARCH64, - mpidr, - &tspd_sp_context[linear_id]); + TSP_AARCH64, + mpidr, + &tspd_sp_context[linear_id]); assert(rc == 0); /* @@ -132,7 +204,7 @@ int32_t tspd_init(meminfo_t *bl32_meminfo) { 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]; @@ -164,6 +236,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; } @@ -197,6 +281,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 #include +#include #include #include @@ -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;