Rewrite GIC drivers in C

Signed-off-by: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
[Mark: fold GICv3 and GICv2 patches for bisectability]
Signed-off-by: Mark Rutland <mark.rutland@arm.com>

8 files changed, 225 insertions, 147 deletions

diff --git a/Makefile.am b/Makefile.am
index ec7ed8b..0918b4e 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -90,8 +90,8 @@ CPPFLAGS	+= $(INITRD_FLAGS)
 CFLAGS		+= -Iinclude/ -I$(ARCH_SRC)/include/
 CFLAGS		+= -Wall -fomit-frame-pointer
 
-OFILES		+= boot_common.o ns.o
-OFILES		+= $(addprefix $(ARCH_SRC),boot.o stack.o cache.o $(GIC) mmu.o $(BOOTMETHOD) utils.o)
+OFILES		+= boot_common.o ns.o $(GIC)
+OFILES		+= $(addprefix $(ARCH_SRC),boot.o stack.o cache.o mmu.o $(BOOTMETHOD) utils.o)
 
 all: $(IMAGE)
 
diff --git a/arch/aarch64/gic-v3.S b/arch/aarch64/gic-v3.S
deleted file mode 100644
index 0d02838..0000000
--- a/arch/aarch64/gic-v3.S
+++ /dev/null
@@ -1,98 +0,0 @@
-/*
- * arch/aarch64/gic-v3.S - Secure gicv3 initialisation for stand-alone Linux booting
- *
- * Copyright (C) 2013 ARM Limited. All rights reserved.
- *
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE.txt file.
- */
-
-#include "common.S"
-
-	.text
-
-	.global gic_secure_init
-
-gic_secure_init:
-	/*
-	 * If GICv3 is not available, skip initialisation. The OS will probably
-	 * fail with a warning, but this should be easier to debug than a
-	 * failure within the boot wrapper.
-	 */
-	mrs	x0, id_aa64pfr0_el1
-	ubfx	x0, x0, #24, #4
-	cmp	x0, #1
-	b.ne	skip_gicv3
-
-	/*
-	 * Only the primary CPU setups the (re)distributors.
-	 */
-	cpuid	x0, x1
-	b.ne	setup_cpu_if			// secondary CPU
-
-	ldr	x1, =GIC_DIST_BASE		// GICD_CTLR
-	mov	w0, #7				// EnableGrp0 | EnableGrp1ns | EnableGrp1s
-	orr	w0, w0, #(3 << 4)		// ARE_S | ARE_NS
-	str	w0, [x1]
-
-	ldr	x2, =GIC_RDIST_BASE
-
-	mvn	w5, wzr
-
-next_rdist:
-	movn	w6, #(1 << 1)			// ProcessorSleep
-	ldr	w4, [x2, #0x014]		// GICR_WAKER
-	and	w4, w4, w6			// Clear ProcessorSleep
-	str	w4, [x2, #0x014]		// GICR_WAKER
-	dsb	st
-	isb
-
-1:	ldr	w4, [x2, #0x014]		// GICR_WAKER
-	ands	wzr, w4, #(1 << 2)		// Test ChildrenAsleep
-	b.ne	1b
-
-	add	x3, x2, #(1 << 16)		// SGI_base
-
-	str	w5, [x3, #0x80]			// GICR_IGROUP0
-	str	wzr, [x3, #0xD00]		// GICR_IGRPMOD0
-
-	ldr	w4, [x2, #8]			// GICR_TYPER
-	add	x3, x3, #(1 << 16)		// Next redist
-	tbz	w4, #1, 2f			// if VLPIS is set,
-	add	x3, x3, #(2 << 16)		// it is two page further away
-2:	mov	x2, x3
-	tbz	w4, #4, next_rdist
-
-	ldr	w2, [x1, #4]			// GICD_TYPER
-	and	w2, w2, #0x1f                   // ITLinesNumber
-	cbz	w2, setup_cpu_if
-
-	add	x3, x1, #0x84			// GICD_IGROUP1
-	add	x4, x1, #0xD04			// GICD_IGRPMOD1
-
-1:	str     w5, [x3], #4
-	str	wzr, [x4], #4
-	sub	w2, w2, #1
-	cbnz	w2, 1b
-
-setup_cpu_if:
-
-#define ICC_SRE_EL2	S3_4_C12_C9_5
-#define ICC_SRE_EL3	S3_6_C12_C12_5
-#define ICC_CTLR_EL1	S3_0_C12_C12_4
-#define ICC_CTLR_EL3	S3_6_C12_C12_4
-#define ICC_PMR_EL1	S3_0_C4_C6_0
-
-	// Enable SRE at EL3 and ICC_SRE_EL2 access
-	mov	x0, #((1 << 3) | (1 << 0))	// Enable | SRE
-	mrs	x1, ICC_SRE_EL3
-	orr	x1, x1, x0
-	msr	ICC_SRE_EL3, x1
-	isb
-
-	// Configure CPU interface
-	msr	ICC_CTLR_EL3, xzr
-	isb
-
-skip_gicv3:
-	ret
diff --git a/arch/aarch64/gic.S b/arch/aarch64/gic.S
deleted file mode 100644
index bf3b8ea..0000000
--- a/arch/aarch64/gic.S
+++ /dev/null
@@ -1,47 +0,0 @@
-/*
- * arch/aarch64/gic.S - Secure gic initialisation for stand-alone Linux booting
- *
- * Copyright (C) 2013 ARM Limited. All rights reserved.
- *
- * Use of this source code is governed by a BSD-style license that can be
- * found in the LICENSE.txt file.
- */
-
-#include "common.S"
-
-	.text
-
-	.global gic_secure_init
-
-gic_secure_init:
-	/*
-	 * Check for the primary CPU to avoid a race on the distributor
-	 * registers.
-	 */
-	cpuid	x0, x1
-	b.ne	1f				// secondary CPU
-
-	ldr	x1, =GIC_DIST_BASE		// GICD_CTLR
-	mov	w0, #3				// EnableGrp0 | EnableGrp1
-	str	w0, [x1]
-
-1:	ldr	x1, =GIC_DIST_BASE + 0x80	// GICD_IGROUPR
-	mov	w0, #~0				// Grp1 interrupts
-	str	w0, [x1]
-	b.ne	2f				// Only local interrupts for secondary CPUs
-	ldr     x2, =GIC_DIST_BASE + 0x04       // GICD_TYPER
-	ldr     w3, [x2]
-	ands    w3, w3, #0x1f                   // ITLinesNumber
-	b.eq    2f
-1:	str     w0, [x1, #4]!
-	subs    w3, w3, #1
-	b.ne    1b
-
-2:	ldr	x1, =GIC_CPU_BASE		// GICC_CTLR
-	mov	w0, #3				// EnableGrp0 | EnableGrp1
-	str	w0, [x1]
-
-	mov	w0, #1 << 7			// allow NS access to GICC_PMR
-	str	w0, [x1, #4]			// GICC_PMR
-
-	ret
diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index 249ce09..c5315f2 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -30,6 +30,8 @@
 
 #ifndef __ASSEMBLY__
 
+#include <stdint.h>
+
 #define sevl()		asm volatile ("sevl\n" : : : "memory")
 
 static inline unsigned long read_mpidr(void)
@@ -40,6 +42,18 @@ static inline unsigned long read_mpidr(void)
 	return mpidr & MPIDR_ID_BITS;
 }
 
+static inline uint64_t read_id_aa64pfr0(void)
+{
+	uint64_t val;
+
+	asm volatile ("mrs	%0, id_aa64pfr0_el1\n" : "=r" (val));
+	return val;
+}
+
+static inline int has_gicv3_sysreg(void)
+{
+	return !!((read_id_aa64pfr0() >> 24) & 0xf);
+}
 
 #endif /* !__ASSEMBLY__ */
 
diff --git a/arch/aarch64/include/asm/gic-v3.h b/arch/aarch64/include/asm/gic-v3.h
new file mode 100644
index 0000000..e743c02
--- /dev/null
+++ b/arch/aarch64/include/asm/gic-v3.h
@@ -0,0 +1,35 @@
+/*
+ * arch/aarch64/include/asm/gic-v3.h
+ *
+ * Copyright (C) 2015 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+#ifndef __ASM_AARCH64_GICV3_H
+#define __ASM_AARCH64_GICV3_H
+
+#define ICC_SRE_EL2	"S3_4_C12_C9_5"
+#define ICC_SRE_EL3	"S3_6_C12_C12_5"
+#define ICC_CTLR_EL1	"S3_0_C12_C12_4"
+#define ICC_CTLR_EL3	"S3_6_C12_C12_4"
+#define ICC_PMR_EL1	"S3_0_C4_C6_0"
+
+static inline uint32_t gic_read_icc_sre(void)
+{
+	uint32_t val;
+	asm volatile ("mrs %0, " ICC_SRE_EL3 : "=r" (val));
+	return val;
+}
+
+static inline void gic_write_icc_sre(uint32_t val)
+{
+	asm volatile ("msr " ICC_SRE_EL3 ", %0" : : "r" (val));
+}
+
+static inline void gic_write_icc_ctlr(uint32_t val)
+{
+	asm volatile ("msr " ICC_CTLR_EL3 ", %0" : : "r" (val));
+}
+
+#endif
diff --git a/gic-v3.c b/gic-v3.c
new file mode 100644
index 0000000..476f703
--- /dev/null
+++ b/gic-v3.c
@@ -0,0 +1,116 @@
+/*
+ * gic-v3.c
+ *
+ * Copyright (C) 2015 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+
+#include <stdint.h>
+
+#include <cpu.h>
+#include <asm/gic-v3.h>
+#include <asm/io.h>
+
+#define GICD_CTLR			0x0
+#define GICD_TYPER			0x4
+#define GICD_IGROUP0			0x80
+#define GICD_IGRPMOD0			0xd00
+
+#define GICD_CTLR_EnableGrp0		(1 << 0)
+#define GICD_CTLR_EnableGrp1ns		(1 << 1)
+#define GICD_CTLR_EnableGrp1s		(1 << 2)
+#define GICD_CTLR_ARE_S			(1 << 4)
+#define GICD_CTLR_ARE_NS		(1 << 5)
+#define GICD_TYPER_ITLineNumber		0x1f
+
+#define GICR_WAKER			0x14
+
+#define GICR_TYPER			0x8
+#define GICR_IGROUP0			0x80
+#define GICR_IGRPMOD0			0xD00
+
+#define GICR_WAKER_ProcessorSleep	(1 << 1)
+#define GICR_WAKER_ChildrenAsleep	(1 << 2)
+
+#define GICR_TYPER_VLPIS		(1 << 1)
+#define GICR_TYPER_Last			(1 << 4)
+
+#define ICC_SRE_SRE			(1 << 0)
+#define ICC_SRE_Enable			(1 << 3)
+
+void gic_secure_init_primary(void)
+{
+	unsigned int i;
+	void *gicr_ptr = (void *)GIC_RDIST_BASE;
+	void *gicd_base = (void *)GIC_DIST_BASE;
+	uint32_t typer;
+
+	raw_writel(GICD_CTLR_EnableGrp0 | GICD_CTLR_EnableGrp1ns
+		| GICD_CTLR_EnableGrp1s | GICD_CTLR_ARE_S | GICD_CTLR_ARE_NS,
+		gicd_base + GICD_CTLR);
+
+	do {
+		/*
+		 * Wake up redistributor: kick ProcessorSleep and wait for
+		 * ChildrenAsleep to be 0.
+		 */
+		uint32_t waker = raw_readl(gicr_ptr + GICR_WAKER);
+		waker &= ~GICR_WAKER_ProcessorSleep;
+		raw_writel(waker, gicr_ptr + GICR_WAKER);
+		dsb(st);
+		isb();
+		do {
+			waker = raw_readl(gicr_ptr + GICR_WAKER);
+		} while (waker & GICR_WAKER_ChildrenAsleep);
+
+		/*
+		 * GICR_TYPER is 64-bit, but we do not need the upper half that
+		 * contains CPU affinity.
+		 */
+		typer = raw_readl(gicr_ptr + GICR_TYPER);
+
+		gicr_ptr += 0x10000; /* Go to SGI_Base */
+		raw_writel(~0x0, gicr_ptr + GICR_IGROUP0);
+		raw_writel(0x0, gicr_ptr + GICR_IGRPMOD0);
+
+		/* Next redist */
+		gicr_ptr += 0x10000;
+		if (typer & GICR_TYPER_VLPIS)
+			gicr_ptr += 0x20000;
+
+	} while (!(typer & GICR_TYPER_Last));
+
+	typer = raw_readl(gicd_base + GICD_TYPER);
+	for (i = 1; i < (typer & GICD_TYPER_ITLineNumber); i++) {
+		raw_writel(~0x0, gicd_base + GICD_IGROUP0 + i * 4);
+		raw_writel(0x0, gicd_base + GICD_IGRPMOD0 + i * 4);
+	}
+}
+
+void gic_secure_init(void)
+{
+	uint32_t cpu = read_mpidr();
+
+	uint32_t sre;
+
+	/*
+	 * If GICv3 is not available, skip initialisation. The OS will probably
+	 * fail with a warning, but this should be easier to debug than a
+	 * failure within the boot wrapper.
+	 */
+	if (!has_gicv3_sysreg())
+		return;
+
+	if (cpu == 0)
+		gic_secure_init_primary();
+
+	sre = gic_read_icc_sre();
+	sre |= ICC_SRE_Enable | ICC_SRE_SRE;
+	gic_write_icc_sre(sre);
+	isb();
+
+	gic_write_icc_ctlr(0);
+	isb();
+}
diff --git a/gic.c b/gic.c
new file mode 100644
index 0000000..a84779e
--- /dev/null
+++ b/gic.c
@@ -0,0 +1,57 @@
+/*
+ * gic.c - Secure gic initialisation for stand-alone Linux booting
+ *
+ * Copyright (C) 2015 ARM Limited. All rights reserved.
+ *
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE.txt file.
+ */
+
+#include <stdint.h>
+
+#include <cpu.h>
+#include <asm/gic-v3.h>
+#include <asm/io.h>
+
+#define GICD_CTLR			0x0
+#define GICD_TYPER			0x4
+#define GICD_IGROUPRn			0x80
+
+#define GICD_CTLR_EnableGrp0		(1 << 0)
+#define GICD_CTLR_EnableGrp1		(1 << 1)
+#define GICD_TYPER_ITLineNumber		0x1f
+
+#define GICC_CTLR			0x0
+#define GICC_PMR			0x4
+
+#define GICC_CTLR_EnableGrp0		(1 << 0)
+#define GICC_CTLR_EnableGrp1		(1 << 1)
+
+void gic_secure_init(void)
+{
+	unsigned int i;
+
+	uint32_t cpu = read_mpidr();
+	void *gicd_base = (void *)GIC_DIST_BASE;
+	void *gicc_base = (void *)GIC_CPU_BASE;
+
+	/* Set local interrupts to Group 1 (those fields are banked) */
+	raw_writel(~0, gicd_base + GICD_IGROUPRn);
+
+	if (cpu == 0) {
+		uint32_t typer = raw_readl(gicd_base + GICD_TYPER);
+
+		/* Set SPIs to Group 1 */
+		for (i = 1; i < (typer & GICD_TYPER_ITLineNumber); i++)
+			raw_writel(~0, gicd_base + GICD_IGROUPRn + i * 4);
+
+		raw_writel(GICD_CTLR_EnableGrp0 | GICD_CTLR_EnableGrp1,
+			   gicd_base + GICD_CTLR);
+	}
+
+	raw_writel(GICC_CTLR_EnableGrp0 | GICC_CTLR_EnableGrp1,
+		   gicc_base + GICC_CTLR);
+
+	/* Allow NS access to GICC_PMR */
+	raw_writel(1 << 7, gicc_base + GICC_PMR);
+}
diff --git a/include/cpu.h b/include/cpu.h
index f0872e4..41e1ded 100644
--- a/include/cpu.h
+++ b/include/cpu.h
@@ -15,6 +15,7 @@
 
 #ifndef __ASSEMBLY__
 
+#define isb()		asm volatile ("isb\n" : : : "memory")
 #define dsb(arg)	asm volatile ("dsb " #arg "\n" : : : "memory")
 #define sev()		asm volatile ("sev\n" : : : "memory")
 #define wfe()		asm volatile ("wfe\n" : : : "memory")