Refine the interrupt handlers

The handlers of native and foreign interrupts are hardcoded in FIQ and
IRQ handlers.
This patch generalizes these handlers in macros.
For ARM GICv2 mode, FIQ handler calls native interrupt handler and IRQ
handler calls foreign interrupt handler.

Signed-off-by: David Wang <david.wang@arm.com>
Tested-by: David Wang <david.wang@arm.com> (juno arm32 and arm64)
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>

core/arch/arm/kernel/thread_a32.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, Linaro Limited
+ * Copyright (c) 2016-2017, Linaro Limited
  * Copyright (c) 2014, STMicroelectronics International N.V.
  * All rights reserved.
  *
@@ -243,6 +243,53 @@ UNWIND(	.cantunwind)
 UNWIND(	.fnend)
 END_FUNC thread_resume
 
+/*
+ * Disables IRQ and FIQ and saves state of thread in fiq mode which has
+ * the banked r8-r12 registers, returns original CPSR.
+ */
+LOCAL_FUNC thread_save_state_fiq , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	mov	r9, lr
+
+	/*
+	 * Uses stack for temporary storage, while storing needed
+	 * context in the thread context struct.
+	 */
+
+	mrs	r8, cpsr
+
+	cpsid	aif			/* Disable Async abort, IRQ and FIQ */
+
+	push	{r4-r7}
+	push	{r0-r3}
+
+	mrs	r6, cpsr		/* Save current CPSR */
+
+	bl	thread_get_ctx_regs
+
+	pop	{r1-r4}			/* r0-r3 pushed above */
+	stm	r0!, {r1-r4}
+	pop	{r1-r4}			/* r4-r7 pushed above */
+	stm	r0!, {r1-r4}
+
+	cps     #CPSR_MODE_SYS
+	stm	r0!, {r8-r12}
+	stm     r0!, {sp, lr}
+
+	cps     #CPSR_MODE_SVC
+	mrs     r1, spsr
+	stm     r0!, {r1, sp, lr}
+
+	/* back to fiq mode */
+	orr	r6, r6, #ARM32_CPSR_FIA	/* Disable Async abort, IRQ and FIQ */
+	msr	cpsr, r6		/* Restore mode */
+
+	mov	r0, r8			/* Return original CPSR */
+	bx	r9
+UNWIND(	.fnend)
+END_FUNC thread_save_state_fiq
+
 /*
  * Disables IRQ and FIQ and saves state of thread, returns original
  * CPSR.
@@ -379,42 +426,59 @@ UNWIND(	.save	{r0})
 UNWIND(	.fnend)
 END_FUNC thread_rpc
 
-LOCAL_FUNC thread_fiq_handler , :
-UNWIND(	.fnstart)
-UNWIND(	.cantunwind)
-	/* FIQ has a +4 offset for lr compared to preferred return address */
+/* The handler of native interrupt. */
+.macro	native_intr_handler mode:req
+	/*
+	 * FIQ and IRQ have a +4 offset for lr compared to preferred return
+	 * address
+	 */
 	sub     lr, lr, #4
+
 	/*
-	 * We're saving {r0-r3} and the banked fiq registers {r8-r12}. The
-	 * banked fiq registers need to be saved because the secure monitor
-	 * doesn't save those. The treatment of the banked fiq registers is
-	 * somewhat analogous to the lazy save of VFP registers.
+	 * We're saving {r0-r3}. The banked fiq registers {r8-r12} need to be
+	 * saved if the native interrupt is sent as FIQ because the secure
+	 * monitor doesn't save those. The treatment of the banked fiq
+	 * registers is somewhat analogous to the lazy save of VFP registers.
 	 */
+	.ifc	\mode\(),fiq
 	push	{r0-r3, r8-r12, lr}
+	.else
+	push	{r0-r3, lr}
+	.endif
 	bl	thread_check_canaries
 	ldr	lr, =thread_nintr_handler_ptr
 	ldr	lr, [lr]
 	blx	lr
+	.ifc	\mode\(),fiq
 	pop	{r0-r3, r8-r12, lr}
+	.else
+	pop	{r0-r3, lr}
+	.endif
 	movs	pc, lr
-UNWIND(	.fnend)
-END_FUNC thread_fiq_handler
+.endm
 
-LOCAL_FUNC thread_irq_handler , :
-UNWIND(	.fnstart)
-UNWIND(	.cantunwind)
+/* The handler of foreign interrupt. */
+.macro foreign_intr_handler mode:req
+	.ifc	\mode\(),irq
 	/*
+	 * Disable FIQ if the foreign interrupt is sent as IRQ.
 	 * IRQ mode is set up to use tmp stack so FIQ has to be
 	 * disabled before touching the stack. We can also assign
 	 * SVC sp from IRQ sp to get SVC mode into the state we
 	 * need when doing the SMC below.
+	 * If it is sent as FIQ, the IRQ has already been masked by hardware
 	 */
-	cpsid	f			/* Disable FIQ also */
+	cpsid	f
+	.endif
 	sub	lr, lr, #4
 	push	{lr}
 	push	{r12}
 
+	.ifc	\mode\(),fiq
+	bl	thread_save_state_fiq
+	.else
 	bl	thread_save_state
+	.endif
 
 	mov	r0, #THREAD_FLAGS_EXIT_ON_FOREIGN_INTR
 	mrs	r1, spsr
@@ -438,6 +502,19 @@ UNWIND(	.cantunwind)
 	/* r4 is already filled in above */
 	smc	#0
 	b	.	/* SMC should not return */
+.endm
+
+LOCAL_FUNC thread_fiq_handler , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	native_intr_handler	fiq
+UNWIND(	.fnend)
+END_FUNC thread_fiq_handler
+
+LOCAL_FUNC thread_irq_handler , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+	foreign_intr_handler	irq
 UNWIND(	.fnend)
 END_FUNC thread_irq_handler
 

core/arch/arm/kernel/thread_a64.S

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, Linaro Limited
+ * Copyright (c) 2015-2017, Linaro Limited
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
@@ -680,14 +680,19 @@ LOCAL_FUNC el0_sync_abort , :
 	eret
 END_FUNC el0_sync_abort
 
-LOCAL_FUNC elx_irq , :
+/* The handler of foreign interrupt. */
+.macro foreign_intr_handler mode:req
 	/*
 	 * Update core local flags
 	 */
 	ldr	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 	lsl	w1, w1, #THREAD_CLF_SAVED_SHIFT
 	orr	w1, w1, #THREAD_CLF_TMP
+	.ifc	\mode\(),fiq
+	orr	w1, w1, #THREAD_CLF_FIQ
+	.else
 	orr	w1, w1, #THREAD_CLF_IRQ
+	.endif
 	str	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 
 	/* get pointer to current thread context in x0 */
@@ -740,30 +745,35 @@ LOCAL_FUNC elx_irq , :
 	/* w4 is already filled in above */
 	smc	#0
 	b	.	/* SMC should not return */
-END_FUNC elx_irq
+.endm
 
 /*
  * This struct is never used from C it's only here to visualize the
  * layout.
  *
- * struct elx_fiq_rec {
+ * struct elx_nintr_rec {
  * 	uint64_t x[19 - 4]; x4..x18
  * 	uint64_t lr;
  * 	uint64_t sp_el0;
  * };
  */
-#define ELX_FIQ_REC_X(x)		(8 * ((x) - 4))
-#define ELX_FIQ_REC_LR			(8 + ELX_FIQ_REC_X(19))
-#define ELX_FIQ_REC_SP_EL0		(8 + ELX_FIQ_REC_LR)
-#define ELX_FIQ_REC_SIZE		(8 + ELX_FIQ_REC_SP_EL0)
+#define ELX_NINTR_REC_X(x)		(8 * ((x) - 4))
+#define ELX_NINTR_REC_LR		(8 + ELX_NINTR_REC_X(19))
+#define ELX_NINTR_REC_SP_EL0		(8 + ELX_NINTR_REC_LR)
+#define ELX_NINTR_REC_SIZE		(8 + ELX_NINTR_REC_SP_EL0)
 
-LOCAL_FUNC elx_fiq , :
+/* The handler of native interrupt. */
+.macro native_intr_handler mode:req
 	/*
 	 * Update core local flags
 	 */
 	ldr	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 	lsl	w1, w1, #THREAD_CLF_SAVED_SHIFT
+	.ifc	\mode\(),fiq
 	orr	w1, w1, #THREAD_CLF_FIQ
+	.else
+	orr	w1, w1, #THREAD_CLF_IRQ
+	.endif
 	orr	w1, w1, #THREAD_CLF_TMP
 	str	w1, [sp, #THREAD_CORE_LOCAL_FLAGS]
 
@@ -779,12 +789,12 @@ LOCAL_FUNC elx_fiq , :
 	 * Save registers on stack that can be corrupted by a call to
 	 * a C function
 	 */
-	/* Make room for struct elx_fiq_rec */
-	sub	sp, sp, #ELX_FIQ_REC_SIZE
+	/* Make room for struct elx_nintr_rec */
+	sub	sp, sp, #ELX_NINTR_REC_SIZE
 	/* Store x4..x18 */
-	store_xregs sp, ELX_FIQ_REC_X(4), 4, 18
+	store_xregs sp, ELX_NINTR_REC_X(4), 4, 18
 	/* Store lr and original sp_el0 */
-	stp	x30, x2, [sp, #ELX_FIQ_REC_LR]
+	stp	x30, x2, [sp, #ELX_NINTR_REC_LR]
 
 	bl	thread_check_canaries
 	adr	x16, thread_nintr_handler_ptr
@@ -795,9 +805,9 @@ LOCAL_FUNC elx_fiq , :
 	 * Restore registers
 	 */
 	/* Restore x4..x18 */
-	load_xregs sp, ELX_FIQ_REC_X(4), 4, 18
+	load_xregs sp, ELX_NINTR_REC_X(4), 4, 18
 	/* Load  lr and original sp_el0 */
-	ldp	x30, x2, [sp, #ELX_FIQ_REC_LR]
+	ldp	x30, x2, [sp, #ELX_NINTR_REC_LR]
 	/* Restore SP_El0 */
 	mov	sp, x2
 	/* Switch back to SP_EL1 */
@@ -813,4 +823,12 @@ LOCAL_FUNC elx_fiq , :
 
 	/* Return from exception */
 	eret
+.endm
+
+LOCAL_FUNC elx_irq , :
+	foreign_intr_handler	irq
+END_FUNC elx_irq
+
+LOCAL_FUNC elx_fiq , :
+	native_intr_handler	fiq
 END_FUNC elx_fiq