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

5 files changed, 188 insertions, 6 deletions

diff --git a/bl32/tsp/aarch64/tsp_entrypoint.S b/bl32/tsp/aarch64/tsp_entrypoint.S
index fab64cf5..bb0880ff 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 @@ func tsp_cpu_suspend_entry
 	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 5d8a0e34..b1d0afef 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 07bd9c64..297556bb 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 00000000..d5d02c30
--- /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 a7c73865..dff67d5a 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,
 };