Rewrite boot methods in C

This patch brings the boot-wrapper into the twenty-first century, by
introducing it to the C language. This change, along with the next few
patches, will greatly simplify the AArch32 port and overall maintenance.

The assembly side of PSCI now only contains basic dispatchers, for both
boot and SMC entries.

Initial boot flow:
	asm	_start			boot.S
		start_(no_)el3		psci.S or spin.S
	C	first_spin		boot_common.c
		spin			boot_common.c
	asm	jump_kernel if CPU0	boot.S

CPU_ON flow:
	asm	smc_entry		psci.S
	C	psci_cpu_on		psci.c
	asm	smc_exit		psci.S

CPU_OFF flow:
	asm	smc_entry		psci.S
	C	psci_cpu_off		psci.c
		spin			boot_common.c

Signed-off-by: Jean-Philippe Brucker <jean-philippe.brucker@arm.com>
Signed-off-by: Mark Rutland <mark.rutland@arm.com>

13 files changed, 376 insertions, 190 deletions

diff --git a/Makefile.am b/Makefile.am
index 92d146e..929df7a 100644
--- a/Makefile.am
+++ b/Makefile.am
@@ -23,10 +23,13 @@ DEFINES		+= -DUART_BASE=$(UART_BASE)
 DEFINES		+= -DSTACK_SIZE=256
 
 OFILES		=
+
+CFLAGS		+= -mgeneral-regs-only -mstrict-align
 ARCH_SRC	:= arch/aarch64/
 
 if PSCI
 BOOTMETHOD	:= psci.o
+OFILES		+= psci.o
 PSCI_NODE	:= psci {				\
 			compatible = \"arm,psci\";	\
 			method = \"smc\";		\
@@ -85,7 +88,9 @@ endif
 
 CPPFLAGS	+= $(INITRD_FLAGS)
 CFLAGS		+= -Iinclude/ -I$(ARCH_SRC)/include/
+CFLAGS		+= -Wall -fomit-frame-pointer
 
+OFILES		+= boot_common.o
 OFILES		+= $(addprefix $(ARCH_SRC),boot.o stack.o cache.o $(GIC) mmu.o ns.o $(BOOTMETHOD) utils.o)
 
 all: $(IMAGE)
@@ -98,6 +103,9 @@ $(IMAGE): $(OFILES) model.lds fdt.dtb $(KERNEL_IMAGE) $(FILESYSTEM)
 %.o: %.S Makefile
 	$(CC) $(CPPFLAGS) -D__ASSEMBLY__ $(CFLAGS) $(DEFINES) -c -o $@ $<
 
+%.o: %.c Makefile
+	$(CC) $(CPPFLAGS) $(CFLAGS) $(DEFINES) -c -o $@ $<
+
 model.lds: $(LD_SCRIPT) Makefile
 	$(CPP) $(CPPFLAGS) -ansi -DPHYS_OFFSET=$(PHYS_OFFSET) -DMBOX_OFFSET=$(MBOX_OFFSET) -DKERNEL_OFFSET=$(KERNEL_OFFSET) -DFDT_OFFSET=$(FDT_OFFSET) -DFS_OFFSET=$(FS_OFFSET) -DKERNEL=$(KERNEL_IMAGE) -DFILESYSTEM=$(FILESYSTEM) -P -C -o $@ $<
 
diff --git a/arch/aarch64/boot.S b/arch/aarch64/boot.S
index f5ba572..67d9294 100644
--- a/arch/aarch64/boot.S
+++ b/arch/aarch64/boot.S
@@ -12,6 +12,8 @@
 	.section .init
 
 	.globl	_start
+	.globl jump_kernel
+
 _start:
 	cpuid	x0, x1
 	bl	find_logical_id
@@ -24,9 +26,16 @@ _start:
 	 */
 	mrs	x0, CurrentEL
 	cmp	x0, #CURRENTEL_EL3
-	b.ne	start_no_el3			// skip EL3 initialisation
+	b.eq	1f
+
+	mov	w0, #1
+	ldr	x1, =flag_no_el3
+	str	w0, [x1]
+
+	bl	setup_stack
+	b	start_no_el3
 
-	mov	x0, #0x30			// RES1
+1:	mov	x0, #0x30			// RES1
 	orr	x0, x0, #(1 << 0)		// Non-secure EL1
 	orr	x0, x0, #(1 << 8)		// HVC enable
 	orr	x0, x0, #(1 << 10)		// 64-bit EL2
@@ -43,4 +52,53 @@ _start:
 
 err_invalid_id:
 	b	.
+
+	/*
+	 * Drop to the kernel
+	 * x0:		entry address
+	 * x1-x4:	arguments
+	 */
+jump_kernel:
+	mov	x19, x0
+	mov	x20, x1
+	mov	x21, x2
+	mov	x22, x3
+	mov	x23, x4
+
+	ldr	x0, =SCTLR_EL2_RESET
+	msr	sctlr_el2, x0
+
+	cpuid	x0, x1
+	bl	find_logical_id
+	bl	setup_stack		// Reset stack pointer
+
+	ldr	w0, flag_no_el3
+	cmp	w0, #0			// Prepare Z flag
+
+	b.ne	el2_trampoline		// No EL3
+
+	mov	x4, #SPSR_KERNEL
+	adr	x5, el2_trampoline
+	drop_el	x4, x5
+
+el2_trampoline:
+	bl	flush_caches
+
+	cpuid	x0, x1
+	b.ne	1f
+	bl	ns_init_system
+
+	/* Load kernel parameters */
+1:	mov	x0, x20
+	mov	x1, x21
+	mov	x2, x22
+	mov	x3, x23
+
+	br	x19
+
 	.ltorg
+
+	.data
+	.align 3
+flag_no_el3:
+	.long 0
diff --git a/arch/aarch64/include/asm/cpu.h b/arch/aarch64/include/asm/cpu.h
index df17164..12f7a08 100644
--- a/arch/aarch64/include/asm/cpu.h
+++ b/arch/aarch64/include/asm/cpu.h
@@ -13,6 +13,8 @@
 
 #ifndef __ASSEMBLY__
 
+#define sevl()		asm volatile ("sevl\n" : : : "memory")
+
 static inline unsigned long read_mpidr(void)
 {
 	unsigned long mpidr;
diff --git a/arch/aarch64/include/asm/psci.h b/arch/aarch64/include/asm/psci.h
new file mode 100644
index 0000000..ddab76c
--- /dev/null
+++ b/arch/aarch64/include/asm/psci.h
@@ -0,0 +1,40 @@
+/*
+ * arch/aarch64/include/asm/psci.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_PSCI_H
+#define __ASM_AARCH64_PSCI_H
+
+#ifndef __ASSEMBLY__
+
+static inline int psci_store_address(unsigned long address,
+				     unsigned long *branch_entry)
+{
+	unsigned long ret;
+
+	asm volatile (
+	"1:\n"
+	"ldxr	%0, [%2]\n"
+	"subs	%0, %0, %3\n"
+	"b.ne	2f\n"
+	"stxr	%w0, %1, [%2]\n"
+	"cbnz	%w0, 1b\n"
+	"2:\n"
+	: "=&r" (ret)
+	: "r" (address), "r" (branch_entry), "J" (PSCI_ADDR_INVALID)
+	: "cc");
+
+	if (ret != 0)
+		/* ret value comes from subs */
+		return PSCI_RET_ALREADY_ON;
+
+	return PSCI_RET_SUCCESS;
+}
+
+#endif /* !__ASSEMBLY__ */
+
+#endif
diff --git a/arch/aarch64/psci.S b/arch/aarch64/psci.S
index bfd54f1..b8d07e5 100644
--- a/arch/aarch64/psci.S
+++ b/arch/aarch64/psci.S
@@ -6,21 +6,9 @@
  * Use of this source code is governed by a BSD-style license that can be
  * found in the LICENSE.txt file.
  */
-#include "common.S"
-
-#define PSCI_CPU_OFF		0x84000002
-#define PSCI_CPU_ON		0xc4000003
-
-#define PSCI_RET_SUCCESS	0
-#define PSCI_RET_NOT_IMPL	(-1)
-#define PSCI_RET_INVALID	(-2)
-#define PSCI_RET_DENIED		(-3)
+#include <psci.h>
 
-#ifndef CPU_IDS
-#error No CPU MPIDRs provided.
-#endif
-
-#define ADDR_INVALID		(-1)
+#include "common.S"
 
 	.macro	ventry	label
 	.align	7
@@ -44,23 +32,17 @@ vector:
 	ventry	err_exception
 
 	// lower EL, AArch64
-	ventry	psci_call64
+	ventry	smc_entry64
 	ventry	err_exception
 	ventry	err_exception
 	ventry	err_exception
 
 	// lower EL, AArch32
-	ventry	psci_call32
+	ventry	smc_entry32
 	ventry	err_exception
 	ventry	err_exception
 	ventry	err_exception
 
-	.data
-branch_table:
-	.rept (NR_CPUS)
-	.quad ADDR_INVALID
-	.endr
-
 	.text
 
 	.globl start_no_el3
@@ -69,68 +51,51 @@ branch_table:
 err_exception:
 	b err_exception
 
-psci_call32:
-	mov	w0, PSCI_RET_NOT_IMPL
-	eret
-
-psci_call64:
-	ldr	x7, =PSCI_CPU_OFF
+	.macro branch_if val, addr
+	ldr	x7, =\val
 	cmp	x0, x7
-	b.eq	psci_cpu_off
-
-	ldr	x7, =PSCI_CPU_ON
-	cmp	x0, x7
-	b.eq	psci_cpu_on
+	adr	x7, \addr
+	b.eq	do_call
+	.endm
 
-	mov	x0, PSCI_RET_NOT_IMPL
+smc_entry32:
+	mov	w0, PSCI_RET_NOT_SUPPORTED
 	eret
 
-/*
- * x1 - optional power state parameter, ignored here
- */
-psci_cpu_off:
-	cpuid	x0, x1
-	bl	find_logical_id
-	adr	x1, branch_table
-	mov	x2, #ADDR_INVALID
-	str	x2, [x1, x0, lsl #3]
-
-	b	spin
-
-/*
- * x1 - target cpu
- * x2 - address
- */
-psci_cpu_on:
-	mov	x15, x30
-	mov	x14, x2
+smc_entry64:
+	/* SMC entry uses 112 bytes of stack */
+	stp	x18, x19, [sp, #-16]!
+	stp	x20, x21, [sp, #-16]!
+	stp	x22, x23, [sp, #-16]!
+	stp	x24, x25, [sp, #-16]!
+	stp	x26, x27, [sp, #-16]!
+	stp	x28, x29, [sp, #-16]!
+	// Keep sp aligned to 16 bytes
+	stp	x30, xzr, [sp, #-16]!
+
+	/* If function ID matches, do_call with procedure address in x7 */
+	branch_if PSCI_CPU_ON_64,	psci_cpu_on
+	branch_if PSCI_CPU_OFF,		psci_cpu_off
+
+	/* Otherwise, return error in x0/w0 */
+	mov	x0, PSCI_RET_NOT_SUPPORTED
+	b	smc_exit
+
+do_call:
 	mov	x0, x1
-
-	bl	find_logical_id
-	cmp	x0, #-1
-	b.eq	1f
-
-	adr	x3, branch_table
-	add	x3, x3, x0, lsl #3
-
-	ldr	x4, =ADDR_INVALID
-
-	ldxr	x5, [x3]
-	cmp	x4, x5
-	b.ne	1f
-
-	stxr	w4, x14, [x3]
-	cbnz	w4, 1f
-
-	dsb	ishst
-	sev
-
-	mov	x0, #PSCI_RET_SUCCESS
-	mov	x30, x15
-	eret
-
-1:	mov	x0, #PSCI_RET_DENIED
-	mov	x30, x15
+	mov	x1, x2
+	mov	x2, x3
+
+	blr	x7
+
+smc_exit:
+	ldp	x30, xzr, [sp], #16
+	ldp	x28, x29, [sp], #16
+	ldp	x26, x27, [sp], #16
+	ldp	x24, x25, [sp], #16
+	ldp	x22, x23, [sp], #16
+	ldp	x20, x21, [sp], #16
+	ldp	x18, x19, [sp], #16
 	eret
 
 
@@ -142,41 +107,7 @@ start_el3:
 	/* only boot the primary cpu (entry 0 in the table) */
 	cpuid	x0, x1
 	bl	find_logical_id
-	cbnz	x0, spin
-
-	adr	x2, branch_table
-	adr	x1, start_cpu0
-	str	x1, [x2]
-	sevl
-	b	spin
-
-/*
- * Poll the release table, waiting for a valid address to appear.
- * When a valid address appears, branch to it.
- */
-spin:
-	cpuid	x0, x1
-	bl	find_logical_id
-	cmp	x0, #-1
-	b.eq	spin_dead
-
-	adr	x1, branch_table
-	mov	x3, #ADDR_INVALID
-
-	add	x1, x1, x0, lsl #3
-
-1:	wfe
-	ldr	x2, [x1]
-	cmp	x2, x3
-	b.eq	1b
-
-	ldr	x0, =SCTLR_EL2_RESET
-	msr	sctlr_el2, x0
-
-	mov	x3, #SPSR_KERNEL
-	adr	x4, el2_trampoline
-	mov	x0, x2
-	drop_el	x3, x4
+	b	psci_first_spin
 
 /*
  * This PSCI implementation requires EL3. Without EL3 we'll only boot the
@@ -185,29 +116,7 @@ spin:
 start_no_el3:
 	cpuid	x0, x1
 	bl	find_logical_id
-	cbz	x0, start_cpu0
+	cbz	x0, psci_first_spin
 spin_dead:
 	wfe
 	b	spin_dead
-
-
-/*
- * Clean and invalidate the caches at EL2 to simplify EL3's cache usage.
- */
-el2_trampoline:
-	mov	x15, x0
-	bl	flush_caches
-	br	x15
-
-start_cpu0:
-	/*
-	 * Kernel parameters
-	 */
-	mov	x0, xzr
-	mov	x1, xzr
-	mov	x2, xzr
-	mov	x3, xzr
-
-	bl	ns_init_system
-	ldr	x0, =dtb
-	b	kernel
diff --git a/arch/aarch64/spin.S b/arch/aarch64/spin.S
index 4997bac..72603cf 100644
--- a/arch/aarch64/spin.S
+++ b/arch/aarch64/spin.S
@@ -15,41 +15,23 @@
 	.globl start_el3
 
 start_el3:
-	/*
-	 * Prepare the switch to the EL2_SP1 mode from EL3
-	 */
-	ldr	x0, =SCTLR_EL2_RESET
-	msr	sctlr_el2, x0
-	ldr	x0, =start_no_el3		// Return after mode switch
-	mov	x1, #SPSR_KERNEL
-	drop_el	x1, x0
-
 start_no_el3:
-	/*
-	 * Kernel parameters
-	 */
-	mov	x0, xzr
-	mov	x1, xzr
-	mov	x2, xzr
-	mov	x3, xzr
-
-	cpuid	x4, x5
-	b.eq	2f
-
-	/*
-	 * Secondary CPUs
-	 */
-1:	wfe
-	ldr	x4, mbox
-	cbz	x4, 1b
-	br	x4				// branch to the given address
+	cpuid	x0, x1
+	bl	find_logical_id
 
-2:
 	/*
-	 * Primary CPU
+	 * Primary CPU (x0 = 0) jumps to kernel, the other ones wait for an
+	 * address to appear in mbox
 	 */
-	bl	ns_init_system
-	ldr	x0, =dtb			// device tree blob
-	b	kernel
+	adr	x3, mbox
+	adr	x4, kernel_address
+	cmp	x0, #0
+	csel	x1, x3, x4, ne
+	mov	x2, #0
+	bl	first_spin
+
+kernel_address:
+	.align 3
+	.long 0
 
 	.ltorg
diff --git a/arch/aarch64/utils.S b/arch/aarch64/utils.S
index cca0c11..ae22ea7 100644
--- a/arch/aarch64/utils.S
+++ b/arch/aarch64/utils.S
@@ -7,28 +7,11 @@
  * found in the LICENSE.txt file.
  */
 
+#include <cpu.h>
+
 	.globl find_logical_id
 	.globl setup_vector
 
-#include "common.S"
-
-	.data
-
-	/*
-	 * Array of the CPU ID (MPIDR & MPIDR_ID_BITS) of each CPU in the system.
-	 * The index into the array is used as a logical id, and an index into
-	 * the branch table. The branch table is automatically padded to the
-	 * same size as the id table.
-	 *
-	 * The first CPU in the table is considered to be the primary CPU, and
-	 * is the only CPU to immediately branch off to the kernel.
-	 */
-	.align 3
-id_table:
-	.quad CPU_IDS
-__id_end:
-	.quad MPIDR_INVALID
-
 	.text
 
 /*
diff --git a/boot_common.c b/boot_common.c
new file mode 100644
index 0000000..983c5d9
--- /dev/null
+++ b/boot_common.c
@@ -0,0 +1,68 @@
+/*
+ * boot_common.c - common spin function for all boot methods
+ *
+ * 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 <cpu.h>
+#include <spin.h>
+
+extern unsigned long kernel;
+extern unsigned long dtb;
+
+void __noreturn jump_kernel(unsigned long address,
+			    unsigned long a0,
+			    unsigned long a1,
+			    unsigned long a2,
+			    unsigned long a3);
+
+const unsigned long id_table[] = { CPU_IDS };
+
+/**
+ * Wait for an address to appear in mbox, and jump to it.
+ *
+ * @mbox: location to watch
+ * @invalid: value of an invalid address, 0 or -1 depending on the boot method
+ * @is_entry: when true, pass boot parameters to the kernel, instead of 0
+ */
+void __noreturn spin(unsigned long *mbox, unsigned long invalid, int is_entry)
+{
+	unsigned long addr = invalid;
+
+	while (addr == invalid) {
+		wfe();
+		addr = *mbox;
+	}
+
+	if (is_entry)
+		jump_kernel(addr, (unsigned long)&dtb, 0, 0, 0);
+
+	jump_kernel(addr, 0, 0, 0, 0);
+
+	unreachable();
+}
+
+/**
+ * Primary CPU finishes platform initialisation and jumps to the kernel.
+ * Secondaries are parked, waiting for their mbox to contain a valid address.
+ *
+ * @cpu: logical CPU number
+ * @mbox: location to watch
+ * @invalid: value of an invalid address, 0 or -1 depending on the boot method
+ */
+void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
+			   unsigned long invalid)
+{
+	if (cpu == 0) {
+		*mbox = (unsigned long)&kernel;
+		sevl();
+		spin(mbox, invalid, 1);
+	} else {
+		*mbox = invalid;
+		spin(mbox, invalid, 0);
+	}
+
+	unreachable();
+}
diff --git a/include/compiler.h b/include/compiler.h
new file mode 100644
index 0000000..f9ee2f3
--- /dev/null
+++ b/include/compiler.h
@@ -0,0 +1,18 @@
+/*
+ * include/compiler.h - common compiler defines
+ *
+ * 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.
+ *
+ * Note: we only support GCC.
+ */
+#ifndef __COMPILER_H
+#define __COMPILER_H
+
+#define unreachable()	__builtin_unreachable()
+
+#define __noreturn	__attribute__((noreturn))
+
+#endif
diff --git a/include/cpu.h b/include/cpu.h
index 0fb85be..f0872e4 100644
--- a/include/cpu.h
+++ b/include/cpu.h
@@ -13,4 +13,13 @@
 
 #define MPIDR_INVALID	(-1)
 
+#ifndef __ASSEMBLY__
+
+#define dsb(arg)	asm volatile ("dsb " #arg "\n" : : : "memory")
+#define sev()		asm volatile ("sev\n" : : : "memory")
+#define wfe()		asm volatile ("wfe\n" : : : "memory")
+
+unsigned int find_logical_id(unsigned long mpidr);
+
+#endif /* !__ASSEMBLY__ */
 #endif
diff --git a/include/psci.h b/include/psci.h
new file mode 100644
index 0000000..a2caa59
--- /dev/null
+++ b/include/psci.h
@@ -0,0 +1,29 @@
+/*
+ * include/psci.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 __PSCI_H
+#define __PSCI_H
+
+#define PSCI_CPU_OFF			0x84000002
+#define PSCI_CPU_ON_64			0xc4000003
+
+#define PSCI_RET_SUCCESS		0
+#define PSCI_RET_NOT_SUPPORTED		(-1)
+#define PSCI_RET_INVALID_PARAMETERS	(-2)
+#define PSCI_RET_DENIED			(-3)
+#define PSCI_RET_ALREADY_ON		(-4)
+#define PSCI_RET_ON_PENDING		(-5)
+#define PSCI_RET_INTERNAL_FAILURE	(-6)
+#define PSCI_RET_NOT_PRESENT		(-7)
+#define PSCI_RET_DISABLED		(-8)
+
+#define PSCI_ADDR_INVALID		(-1)
+
+#include <asm/psci.h>
+
+#endif
diff --git a/include/spin.h b/include/spin.h
new file mode 100644
index 0000000..15bbe27
--- /dev/null
+++ b/include/spin.h
@@ -0,0 +1,19 @@
+/*
+ * include/spin.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 __SPIN_H
+#define __SPIN_H
+
+#include <compiler.h>
+
+void __noreturn spin(unsigned long *mbox, unsigned long invalid, int is_entry);
+
+void __noreturn first_spin(unsigned int cpu, unsigned long *mbox,
+			   unsigned long invalid_addr);
+
+#endif
diff --git a/psci.c b/psci.c
new file mode 100644
index 0000000..6c47577
--- /dev/null
+++ b/psci.c
@@ -0,0 +1,61 @@
+/*
+ * psci.c - basic PSCI implementation
+ *
+ * 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 <psci.h>
+#include <spin.h>
+
+#ifndef CPU_IDS
+#error "No MPIDRs provided"
+#endif
+
+static unsigned long branch_table[NR_CPUS];
+
+int psci_cpu_on(unsigned long target_mpidr, unsigned long address)
+{
+	int ret;
+	unsigned int cpu = find_logical_id(target_mpidr);
+
+	if (cpu == MPIDR_INVALID)
+		return PSCI_RET_INVALID_PARAMETERS;
+
+	ret = psci_store_address(address, branch_table + cpu);
+
+	dsb(ishst);
+	sev();
+
+	return ret;
+}
+
+int psci_cpu_off(void)
+{
+	unsigned long mpidr = read_mpidr();
+	unsigned int cpu = find_logical_id(mpidr);
+
+	if (cpu == MPIDR_INVALID)
+		return PSCI_RET_DENIED;
+
+	branch_table[cpu] = PSCI_ADDR_INVALID;
+
+	spin(branch_table + cpu, PSCI_ADDR_INVALID, 0);
+
+	unreachable();
+}
+
+void __noreturn psci_first_spin(unsigned int cpu)
+{
+	if (cpu == MPIDR_INVALID)
+		while (1);
+
+	first_spin(cpu, branch_table + cpu, PSCI_ADDR_INVALID);
+
+	unreachable();
+}