Support ARM GICv3 mode

core/arch/arm/include/arm32.h

@@ -601,6 +601,38 @@ static __always_inline uint32_t read_r7(void)
 	asm volatile ("mov %0, r7" : "=r" (val));
 	return val;
 }
+
+/* Register read/write functions for GICC registers by using system interface */
+static inline uint32_t read_icc_ctlr(void)
+{
+	uint32_t v;
+
+	asm volatile ("mrc p15,0,%0,c12,c12,4" : "=r" (v));
+	return v;
+}
+
+static inline void write_icc_ctlr(uint32_t v)
+{
+	asm volatile ("mcr p15,0,%0,c12,c12,4" : : "r" (v));
+}
+
+static inline void write_icc_pmr(uint32_t v)
+{
+	asm volatile ("mcr p15,0,%0,c4,c6,0" : : "r" (v));
+}
+
+static inline uint32_t read_icc_iar0(void)
+{
+	uint32_t v;
+
+	asm volatile ("mrc p15,0,%0,c12,c8,0" : "=r" (v));
+	return v;
+}
+
+static inline void write_icc_eoir0(uint32_t v)
+{
+	asm volatile ("mcr p15,0,%0,c12,c8,1" : : "r" (v));
+}
 #endif /*ASM*/
 
 #endif /*ARM32_H*/

core/arch/arm/include/arm64.h

@@ -304,6 +304,12 @@ DEFINE_U64_REG_WRITE_FUNC(mair_el1)
 
 DEFINE_REG_READ_FUNC_(cntpct, uint64_t, cntpct_el0)
 
+/* Register read/write functions for GICC registers by using system interface */
+DEFINE_REG_READ_FUNC_(icc_ctlr, uint32_t, S3_0_C12_C12_4)
+DEFINE_REG_WRITE_FUNC_(icc_ctlr, uint32_t, S3_0_C12_C12_4)
+DEFINE_REG_WRITE_FUNC_(icc_pmr, uint32_t, S3_0_C4_C6_0)
+DEFINE_REG_READ_FUNC_(icc_iar0, uint32_t, S3_0_c12_c8_0)
+DEFINE_REG_WRITE_FUNC_(icc_eoir0, uint32_t, S3_0_c12_c8_1)
 #endif /*ASM*/
 
 #endif /*ARM64_H*/

core/arch/arm/include/kernel/thread.h

@@ -306,8 +306,13 @@ void thread_restore_foreign_intr(void);
  * thread_*_exceptions() functions below.
  * These definitions are compatible with both ARM32 and ARM64.
  */
+#if defined(CFG_ARM_GICV3)
+#define THREAD_EXCP_FOREIGN_INTR	(ARM32_CPSR_F >> ARM32_CPSR_F_SHIFT)
+#define THREAD_EXCP_NATIVE_INTR		(ARM32_CPSR_I >> ARM32_CPSR_F_SHIFT)
+#else
 #define THREAD_EXCP_FOREIGN_INTR	(ARM32_CPSR_I >> ARM32_CPSR_F_SHIFT)
 #define THREAD_EXCP_NATIVE_INTR		(ARM32_CPSR_F >> ARM32_CPSR_F_SHIFT)
+#endif
 #define THREAD_EXCP_ALL			(THREAD_EXCP_FOREIGN_INTR	\
 					| THREAD_EXCP_NATIVE_INTR	\
 					| (ARM32_CPSR_A >> ARM32_CPSR_F_SHIFT))

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,27 @@ 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)
+#if defined(CFG_ARM_GICV3)
+	foreign_intr_handler	fiq
+#else
+	native_intr_handler	fiq
+#endif
+UNWIND(	.fnend)
+END_FUNC thread_fiq_handler
+
+LOCAL_FUNC thread_irq_handler , :
+UNWIND(	.fnstart)
+UNWIND(	.cantunwind)
+#if defined(CFG_ARM_GICV3)
+	native_intr_handler	irq
+#else
+	foreign_intr_handler	irq
+#endif
 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,20 @@ LOCAL_FUNC elx_fiq , :
 
 	/* Return from exception */
 	eret
+.endm
+
+LOCAL_FUNC elx_irq , :
+#if defined(CFG_ARM_GICV3)
+	native_intr_handler	irq
+#else
+	foreign_intr_handler	irq
+#endif
+END_FUNC elx_irq
+
+LOCAL_FUNC elx_fiq , :
+#if defined(CFG_ARM_GICV3)
+	foreign_intr_handler	fiq
+#else
+	native_intr_handler	fiq
+#endif
 END_FUNC elx_fiq