Add support for synchronous FIQ handling in TSP
This patch adds support in the TSP for handling S-EL1 interrupts
handed over by the TSPD. It includes GIC support in its platform port,
updates various statistics related to FIQ handling, exports an entry
point that the TSPD can use to hand over interrupts and defines the
handover protocol w.r.t what context is the TSP expected to preserve
and the state in which the entry point is invoked by the TSPD.
Change-Id: I93b22e5a8133400e4da366f5fc862f871038df39
diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index fab64cf..bb0880f 100644
--- a/bl32/tsp/aarch64/tsp_entrypoint.S
+++ b/bl32/tsp/aarch64/tsp_entrypoint.S
@@ -39,6 +39,7 @@
.globl tsp_cpu_suspend_entry
.globl tsp_cpu_resume_entry
.globl tsp_fast_smc_entry
+ .globl tsp_fiq_entry
/* ---------------------------------------------
* Populate the params in x0-x7 from the pointer
@@ -53,6 +54,22 @@
smc #0
.endm
+ .macro save_eret_context reg1 reg2
+ mrs \reg1, elr_el1
+ mrs \reg2, spsr_el1
+ stp \reg1, \reg2, [sp, #-0x10]!
+ stp x30, x18, [sp, #-0x10]!
+ .endm
+
+ .macro restore_eret_context reg1 reg2
+ ldp x30, x18, [sp], #0x10
+ ldp \reg1, \reg2, [sp], #0x10
+ msr elr_el1, \reg1
+ msr spsr_el1, \reg2
+ .endm
+
+ .section .text, "ax"
+ .align 3
func tsp_entrypoint
@@ -227,6 +244,58 @@
restore_args_call_smc
/*---------------------------------------------
+ * This entrypoint is used by the TSPD to pass
+ * control for handling a pending S-EL1 FIQ.
+ * 'x0' contains a magic number which indicates
+ * this. TSPD expects control to be handed back
+ * at the end of FIQ processing. This is done
+ * through an SMC. The handover agreement is:
+ *
+ * 1. PSTATE.DAIF are set upon entry. 'x1' has
+ * the ELR_EL3 from the non-secure state.
+ * 2. TSP has to preserve the callee saved
+ * general purpose registers, SP_EL1/EL0 and
+ * LR.
+ * 3. TSP has to preserve the system and vfp
+ * registers (if applicable).
+ * 4. TSP can use 'x0-x18' to enable its C
+ * runtime.
+ * 5. TSP returns to TSPD using an SMC with
+ * 'x0' = TSP_HANDLED_S_EL1_FIQ
+ * ---------------------------------------------
+ */
+func tsp_fiq_entry
+#if DEBUG
+ mov x2, #(TSP_HANDLE_FIQ_AND_RETURN & ~0xffff)
+ movk x2, #(TSP_HANDLE_FIQ_AND_RETURN & 0xffff)
+ cmp x0, x2
+ b.ne tsp_fiq_entry_panic
+#endif
+ /*---------------------------------------------
+ * Save any previous context needed to perform
+ * an exception return from S-EL1 e.g. context
+ * from a previous IRQ. Update statistics and
+ * handle the FIQ before returning to the TSPD.
+ * IRQ/FIQs are not enabled since that will
+ * complicate the implementation. Execution
+ * will be transferred back to the normal world
+ * in any case. A non-zero return value from the
+ * fiq handler is an error.
+ * ---------------------------------------------
+ */
+ save_eret_context x2 x3
+ bl tsp_update_sync_fiq_stats
+ bl tsp_fiq_handler
+ cbnz x0, tsp_fiq_entry_panic
+ restore_eret_context x2 x3
+ mov x0, #(TSP_HANDLED_S_EL1_FIQ & ~0xffff)
+ movk x0, #(TSP_HANDLED_S_EL1_FIQ & 0xffff)
+ smc #0
+
+tsp_fiq_entry_panic:
+ b tsp_fiq_entry_panic
+
+ /*---------------------------------------------
* This entrypoint is used by the TSPD when this
* cpu resumes execution after an earlier
* CPU_SUSPEND psci call to ask the TSP to
diff --git a/bl32/tsp/tsp-fvp.mk b/bl32/tsp/tsp-fvp.mk
index 5d8a0e3..b1d0afe 100644
--- a/bl32/tsp/tsp-fvp.mk
+++ b/bl32/tsp/tsp-fvp.mk
@@ -29,7 +29,9 @@
#
# TSP source files specific to FVP platform
-BL32_SOURCES += plat/common/aarch64/platform_mp_stack.S \
- plat/fvp/bl32_plat_setup.c \
+BL32_SOURCES += drivers/arm/gic/gic_v2.c \
+ plat/common/aarch64/platform_mp_stack.S \
plat/fvp/aarch64/plat_common.c \
- plat/fvp/aarch64/plat_helpers.S
+ plat/fvp/aarch64/plat_helpers.S \
+ plat/fvp/bl32_plat_setup.c \
+ plat/fvp/plat_gic.c
diff --git a/bl32/tsp/tsp.mk b/bl32/tsp/tsp.mk
index 07bd9c6..297556b 100644
--- a/bl32/tsp/tsp.mk
+++ b/bl32/tsp/tsp.mk
@@ -31,9 +31,10 @@
BL32_SOURCES += bl32/tsp/tsp_main.c \
bl32/tsp/aarch64/tsp_entrypoint.S \
bl32/tsp/aarch64/tsp_request.S \
+ bl32/tsp/tsp_interrupt.c \
+ bl32/tsp/tsp_timer.c \
common/aarch64/early_exceptions.S \
- lib/locks/exclusive/spinlock.S \
- bl32/tsp/tsp_timer.c
+ lib/locks/exclusive/spinlock.S
BL32_LINKERFILE := bl32/tsp/tsp.ld.S
diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c
new file mode 100644
index 0000000..d5d02c3
--- /dev/null
+++ b/bl32/tsp/tsp_interrupt.c
@@ -0,0 +1,109 @@
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <arch_helpers.h>
+#include <assert.h>
+#include <debug.h>
+#include <gic_v2.h>
+#include <tsp.h>
+#include <platform.h>
+
+/*******************************************************************************
+ * This function updates the TSP statistics for FIQs handled synchronously i.e
+ * the ones that have been handed over by the TSPD. It also keeps count of the
+ * number of times control was passed back to the TSPD after handling an FIQ.
+ * In the future it will be possible that the TSPD hands over an FIQ to the TSP
+ * but does not expect it to return execution. This statistic will be useful to
+ * distinguish between these two models of synchronous FIQ handling.
+ * The 'elr_el3' parameter contains the address of the instruction in normal
+ * world where this FIQ was generated.
+ ******************************************************************************/
+void tsp_update_sync_fiq_stats(uint32_t type, uint64_t elr_el3)
+{
+ uint64_t mpidr = read_mpidr();
+ uint32_t linear_id = platform_get_core_pos(mpidr);
+
+ tsp_stats[linear_id].sync_fiq_count++;
+ if (type == TSP_HANDLE_FIQ_AND_RETURN)
+ tsp_stats[linear_id].sync_fiq_ret_count++;
+
+ spin_lock(&console_lock);
+ printf("TSP: cpu 0x%x sync fiq request from 0x%llx \n\r",
+ mpidr, elr_el3);
+ INFO("cpu 0x%x: %d sync fiq requests, %d sync fiq returns\n",
+ mpidr,
+ tsp_stats[linear_id].sync_fiq_count,
+ tsp_stats[linear_id].sync_fiq_ret_count);
+ spin_unlock(&console_lock);
+}
+
+/*******************************************************************************
+ * TSP FIQ handler called as a part of both synchronous and asynchronous
+ * handling of FIQ interrupts. It returns 0 upon successfully handling a S-EL1
+ * FIQ and treats all other FIQs as EL3 interrupts. It assumes that the GIC
+ * architecture version in v2.0 and the secure physical timer interrupt is the
+ * only S-EL1 interrupt that it needs to handle.
+ ******************************************************************************/
+int32_t tsp_fiq_handler()
+{
+ uint64_t mpidr = read_mpidr();
+ uint32_t linear_id = platform_get_core_pos(mpidr), id;
+
+ /*
+ * Get the highest priority pending interrupt id and see if it is the
+ * secure physical generic timer interrupt in which case, handle it.
+ * Otherwise throw this interrupt at the EL3 firmware.
+ */
+ id = ic_get_pending_interrupt_id();
+
+ /* TSP can only handle the secure physical timer interrupt */
+ if (id != IRQ_SEC_PHY_TIMER)
+ return TSP_EL3_FIQ;
+
+ /*
+ * Handle the interrupt. Also sanity check if it has been preempted by
+ * another secure interrupt through an assertion.
+ */
+ id = ic_acknowledge_interrupt();
+ assert(id == IRQ_SEC_PHY_TIMER);
+ tsp_generic_timer_handler();
+ ic_end_of_interrupt(id);
+
+ /* Update the statistics and print some messages */
+ tsp_stats[linear_id].fiq_count++;
+ spin_lock(&console_lock);
+ printf("TSP: cpu 0x%x handled fiq %d \n\r",
+ mpidr, id);
+ INFO("cpu 0x%x: %d fiq requests \n",
+ mpidr, tsp_stats[linear_id].fiq_count);
+ spin_unlock(&console_lock);
+
+ return 0;
+}
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index a7c7386..dff67d5 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -58,7 +58,7 @@
/*******************************************************************************
* Per cpu data structure to keep track of TSP activity
******************************************************************************/
-static work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
+work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
/*******************************************************************************
* Single reference to the various entry points exported by the test secure
@@ -71,6 +71,7 @@
tsp_cpu_off_entry,
tsp_cpu_resume_entry,
tsp_cpu_suspend_entry,
+ tsp_fiq_entry,
};
diff --git a/include/bl32/payloads/tsp.h b/include/bl32/payloads/tsp.h
index 385d09c..3aa3e8c 100644
--- a/include/bl32/payloads/tsp.h
+++ b/include/bl32/payloads/tsp.h
@@ -42,7 +42,16 @@
#define TSP_RESUME_DONE 0xf2000004
#define TSP_WORK_DONE 0xf2000005
-/* SMC function ID that TSP uses to request service from secure montior */
+/*
+ * Function identifiers to handle FIQs through the synchronous handling model.
+ * If the TSP was previously interrupted then control has to be returned to
+ * the TSPD after handling the interrupt else execution can remain in the TSP.
+ */
+#define TSP_HANDLED_S_EL1_FIQ 0xf2000006
+#define TSP_EL3_FIQ 0xf2000007
+#define TSP_HANDLE_FIQ_AND_RETURN 0x2004
+
+/* SMC function ID that TSP uses to request service from secure monitor */
#define TSP_GET_ARGS 0xf2001000
/* Function IDs for various TSP services */
@@ -86,16 +95,17 @@
#include <cassert.h>
#include <platform.h> /* For CACHE_WRITEBACK_GRANULE */
+#include <spinlock.h>
#include <stdint.h>
typedef void (*tsp_generic_fptr_t)(uint64_t arg0,
- uint64_t arg1,
- uint64_t arg2,
- uint64_t arg3,
- uint64_t arg4,
- uint64_t arg5,
- uint64_t arg6,
- uint64_t arg7);
+ uint64_t arg1,
+ uint64_t arg2,
+ uint64_t arg3,
+ uint64_t arg4,
+ uint64_t arg5,
+ uint64_t arg6,
+ uint64_t arg7);
typedef struct entry_info {
tsp_generic_fptr_t fast_smc_entry;
@@ -103,9 +113,13 @@
tsp_generic_fptr_t cpu_off_entry;
tsp_generic_fptr_t cpu_resume_entry;
tsp_generic_fptr_t cpu_suspend_entry;
+ tsp_generic_fptr_t fiq_entry;
} entry_info_t;
typedef struct work_statistics {
+ uint32_t fiq_count; /* Number of FIQs on this cpu */
+ uint32_t sync_fiq_count; /* Number of sync. fiqs on this cpu */
+ uint32_t sync_fiq_ret_count; /* Number of fiq returns on this cpu */
uint32_t smc_count; /* Number of returns on this cpu */
uint32_t eret_count; /* Number of entries on this cpu */
uint32_t cpu_on_count; /* Number of cpu on requests */
@@ -120,7 +134,7 @@
/* Macros to access members of the above structure using their offsets */
#define read_sp_arg(args, offset) ((args)->_regs[offset >> 3])
-#define write_sp_arg(args, offset, val)(((args)->_regs[offset >> 3]) \
+#define write_sp_arg(args, offset, val) (((args)->_regs[offset >> 3]) \
= val)
/*
@@ -131,6 +145,14 @@
extern void tsp_get_magic(uint64_t args[4]);
+extern void tsp_fiq_entry(uint64_t arg0,
+ uint64_t arg1,
+ uint64_t arg2,
+ uint64_t arg3,
+ uint64_t arg4,
+ uint64_t arg5,
+ uint64_t arg6,
+ uint64_t arg7);
extern void tsp_fast_smc_entry(uint64_t arg0,
uint64_t arg1,
uint64_t arg2,
@@ -203,6 +225,13 @@
extern void tsp_generic_timer_stop(void);
extern void tsp_generic_timer_save(void);
extern void tsp_generic_timer_restore(void);
+
+/* FIQ management functions */
+extern void tsp_update_sync_fiq_stats(uint32_t type, uint64_t elr_el3);
+
+/* Data structure to keep track of TSP statistics */
+extern spinlock_t console_lock;
+extern work_statistics_t tsp_stats[PLATFORM_CORE_COUNT];
#endif /* __ASSEMBLY__ */
#endif /* __BL2_H__ */
diff --git a/plat/fvp/bl32_plat_setup.c b/plat/fvp/bl32_plat_setup.c
index 8406d31..772e972 100644
--- a/plat/fvp/bl32_plat_setup.c
+++ b/plat/fvp/bl32_plat_setup.c
@@ -73,6 +73,9 @@
* messages from TSP
*/
console_init(PL011_UART1_BASE);
+
+ /* Initialize the platform config for future decision making */
+ platform_config_setup();
}
/*******************************************************************************