diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index fab64cf52cc5f9f789927d660eb2c6202fa0ed0e..bb0880ffa23c3f08b55189439a078d6b2726099e 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
@@ -226,6 +243,58 @@ func tsp_cpu_suspend_entry
bl tsp_cpu_suspend_main
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
diff --git a/bl32/tsp/tsp-fvp.mk b/bl32/tsp/tsp-fvp.mk
index 5d8a0e3449bcfba570761831d0d2880bba922eaa..b1d0afef1beb96116b4e4ca9ab3b9fa0de21b653 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 07bd9c649cbb700d8340eadc3d146a2e482354d5..297556bb894856b82d018806deb5e728a74a7f8b 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 0000000000000000000000000000000000000000..d5d02c3047137c0c44b01689d0349c2e1078f232
--- /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 a7c7386580c267b3e85c2d936f12c23290611713..dff67d5acefd8709f1b9fbf1c5135cc9df477982 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -58,7 +58,7 @@ static tsp_args_t tsp_smc_args[PLATFORM_CORE_COUNT];
/*******************************************************************************
* 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 @@ static const entry_info_t tsp_entry_info = {
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 385d09c21e04f041bc05e9b7089c1e8a988d5872..3aa3e8c19da3b74992650dc9d8108e1d31039885 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 @@ typedef struct entry_info {
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 @@ typedef struct tsp_args {
/* 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 @@ CASSERT(TSP_ARGS_SIZE == sizeof(tsp_args_t), assert_sp_args_size_mismatch);
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_handler(void);
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 8406d313cf8cdf2ae9f3abc54a211c4c467ecfb0..772e972fa003c9107e7a045152b0e28c4c135bf2 100644
--- a/plat/fvp/bl32_plat_setup.c
+++ b/plat/fvp/bl32_plat_setup.c
@@ -73,6 +73,9 @@ void bl32_early_platform_setup(void)
* messages from TSP
*/
console_init(PL011_UART1_BASE);
+
+ /* Initialize the platform config for future decision making */
+ platform_config_setup();
}
/*******************************************************************************