Support ARM GICv3 mode

In ARM GICv3 mode, the interrupts are used as below for optee_os.
* FIQ - Foreign interrupts not handled by optee_os. This includes
the non-secure interrupts that should be handled by the REE and the
secure interrupts assigned to the monitor (aarch32 Monitor mode or
aarch64 EL3).
* IRQ - Native interrupts for optee_os.

And optee_os should use the system register interface to access the GICC
registers in GICv3 mode.

A new build flag `CFG_ARM_GICV3=y` should be set to support GICv3 mode.

Signed-off-by: David Wang <david.wang@arm.com>
Reviewed-by: Jens Wiklander <jens.wiklander@linaro.org>

7 files changed, 110 insertions, 10 deletions

diff --git a/core/arch/arm/include/arm32.h b/core/arch/arm/include/arm32.h
index 822ff95f..099471d5 100644
--- a/core/arch/arm/include/arm32.h
+++ b/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*/
diff --git a/core/arch/arm/include/arm64.h b/core/arch/arm/include/arm64.h
index 148b7615..f3790f8d 100644
--- a/core/arch/arm/include/arm64.h
+++ b/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*/
diff --git a/core/arch/arm/include/kernel/thread.h b/core/arch/arm/include/kernel/thread.h
index 831b5d68..5e340d3d 100644
--- a/core/arch/arm/include/kernel/thread.h
+++ b/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))
diff --git a/core/arch/arm/kernel/thread_a32.S b/core/arch/arm/kernel/thread_a32.S
index 1d2b0e70..d13b9bbe 100644
--- a/core/arch/arm/kernel/thread_a32.S
+++ b/core/arch/arm/kernel/thread_a32.S
@@ -507,14 +507,22 @@ END_FUNC thread_rpc
 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
 
diff --git a/core/arch/arm/kernel/thread_a64.S b/core/arch/arm/kernel/thread_a64.S
index b77ef7c1..bd27e4e5 100644
--- a/core/arch/arm/kernel/thread_a64.S
+++ b/core/arch/arm/kernel/thread_a64.S
@@ -826,9 +826,17 @@ END_FUNC el0_sync_abort
 .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
diff --git a/core/drivers/gic.c b/core/drivers/gic.c
index 459129cf..a202eabc 100644
--- a/core/drivers/gic.c
+++ b/core/drivers/gic.c
@@ -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.
  *
@@ -26,6 +26,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
+#include <arm.h>
 #include <assert.h>
 #include <drivers/gic.h>
 #include <kernel/interrupt.h>
@@ -103,7 +104,7 @@ static const struct itr_ops gic_ops = {
 	.set_affinity = gic_op_set_affinity,
 };
 
-static size_t probe_max_it(vaddr_t gicc_base, vaddr_t gicd_base)
+static size_t probe_max_it(vaddr_t gicc_base __maybe_unused, vaddr_t gicd_base)
 {
 	int i;
 	uint32_t old_ctlr;
@@ -114,8 +115,13 @@ static size_t probe_max_it(vaddr_t gicc_base, vaddr_t gicd_base)
 	/*
 	 * Probe which interrupt number is the largest.
 	 */
+#if defined(CFG_ARM_GICV3)
+	old_ctlr = read_icc_ctlr();
+	write_icc_ctlr(0);
+#else
 	old_ctlr = read32(gicc_base + GICC_CTLR);
 	write32(0, gicc_base + GICC_CTLR);
+#endif
 	for (i = max_regs; i >= 0; i--) {
 		uint32_t old_reg;
 		uint32_t reg;
@@ -133,13 +139,21 @@ static size_t probe_max_it(vaddr_t gicc_base, vaddr_t gicd_base)
 		}
 	}
 out:
+#if defined(CFG_ARM_GICV3)
+	write_icc_ctlr(old_ctlr);
+#else
 	write32(old_ctlr, gicc_base + GICC_CTLR);
+#endif
 	return ret;
 }
 
 void gic_cpu_init(struct gic_data *gd)
 {
+#if defined(CFG_ARM_GICV3)
+	assert(gd->gicd_base);
+#else
 	assert(gd->gicd_base && gd->gicc_base);
+#endif
 
 	/* per-CPU interrupts config:
 	 * ID0-ID7(SGI)   for Non-secure interrupts
@@ -151,14 +165,21 @@ void gic_cpu_init(struct gic_data *gd)
 	/* Set the priority mask to permit Non-secure interrupts, and to
 	 * allow the Non-secure world to adjust the priority mask itself
 	 */
+#if defined(CFG_ARM_GICV3)
+	write_icc_pmr(0x80);
+	write_icc_ctlr(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 |
+		       GICC_CTLR_FIQEN);
+#else
 	write32(0x80, gd->gicc_base + GICC_PMR);
 
 	/* Enable GIC */
 	write32(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 | GICC_CTLR_FIQEN,
 		gd->gicc_base + GICC_CTLR);
+#endif
 }
 
-void gic_init(struct gic_data *gd, vaddr_t gicc_base, vaddr_t gicd_base)
+void gic_init(struct gic_data *gd, vaddr_t gicc_base __maybe_unused,
+	      vaddr_t gicd_base)
 {
 	size_t n;
 
@@ -187,16 +208,22 @@ void gic_init(struct gic_data *gd, vaddr_t gicc_base, vaddr_t gicd_base)
 	/* Set the priority mask to permit Non-secure interrupts, and to
 	 * allow the Non-secure world to adjust the priority mask itself
 	 */
+#if defined(CFG_ARM_GICV3)
+	write_icc_pmr(0x80);
+	write_icc_ctlr(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 |
+		       GICC_CTLR_FIQEN);
+#else
 	write32(0x80, gd->gicc_base + GICC_PMR);
 
 	/* Enable GIC */
 	write32(GICC_CTLR_ENABLEGRP0 | GICC_CTLR_ENABLEGRP1 | GICC_CTLR_FIQEN,
 		gd->gicc_base + GICC_CTLR);
+#endif
 	write32(GICD_CTLR_ENABLEGRP0 | GICD_CTLR_ENABLEGRP1,
 		gd->gicd_base + GICD_CTLR);
 }
 
-void gic_init_base_addr(struct gic_data *gd, vaddr_t gicc_base,
+void gic_init_base_addr(struct gic_data *gd, vaddr_t gicc_base __maybe_unused,
 			vaddr_t gicd_base)
 {
 	gd->gicc_base = gicc_base;
@@ -318,14 +345,22 @@ static void gic_it_raise_sgi(struct gic_data *gd, size_t it,
 	write32(mask, gd->gicd_base + GICD_SGIR);
 }
 
-static uint32_t gic_read_iar(struct gic_data *gd)
+static uint32_t gic_read_iar(struct gic_data *gd __maybe_unused)
 {
+#if defined(CFG_ARM_GICV3)
+	return read_icc_iar0();
+#else
 	return read32(gd->gicc_base + GICC_IAR);
+#endif
 }
 
-static void gic_write_eoir(struct gic_data *gd, uint32_t eoir)
+static void gic_write_eoir(struct gic_data *gd __maybe_unused, uint32_t eoir)
 {
+#if defined(CFG_ARM_GICV3)
+	write_icc_eoir0(eoir);
+#else
 	write32(eoir, gd->gicc_base + GICC_EOIR);
+#endif
 }
 
 static bool gic_it_is_enabled(struct gic_data *gd, size_t it)
@@ -359,7 +394,11 @@ void gic_dump_state(struct gic_data *gd)
 {
 	int i;
 
+#if defined(CFG_ARM_GICV3)
+	DMSG("GICC_CTLR: 0x%x", read_icc_ctlr());
+#else
 	DMSG("GICC_CTLR: 0x%x", read32(gd->gicc_base + GICC_CTLR));
+#endif
 	DMSG("GICD_CTLR: 0x%x", read32(gd->gicd_base + GICD_CTLR));
 
 	for (i = 0; i < (int)gd->max_it; i++) {
diff --git a/documentation/interrupt_handling.md b/documentation/interrupt_handling.md
index 80633579..95aef8e1 100644
--- a/documentation/interrupt_handling.md
+++ b/documentation/interrupt_handling.md
@@ -28,10 +28,6 @@ Two types of interrupt are defined in optee_os:
 For ARM GICv2 mode, native interrupt is sent as FIQ and foreign interrupt is
 sent as IRQ.
 
-For ARM GICv3 mode, foreign interrupt is sent as FIQ which could be handled
-by either secure world (EL3 in AArch64) or normal world. This mode is not
-supported yet.
-
 Since IRQs are received using the state vector the actual vector used
 depends on the current state of the CPU. If the NS (non-secure) bit in SCR
 (Secure Control Register) is set then either HVBAR or VBAR (non-secure) is
@@ -41,6 +37,12 @@ receive IRQ that are supposed to be handled by normal world. When secure
 world receives an IRQ it has to be forwarded to normal world for
 processing.
 
+For ARM GICv3 mode, foreign interrupt is sent as FIQ which could be handled
+by either secure world (aarch32 Monitor mode or aarch64 EL3) or normal world.
+ARM GICv3 mode can be enabled by setting `CFG_ARM_GICV3=y`.
+
+The following descriptions take ARM GICv2 mode as the example.
+
 # The monitor
 The monitor manages all entry and exit of secure world. To enter secure
 world from normal world the monitor saves the state of normal world