arm64: provide entry points for BSP and non-BSP

It's helpful to differentiate the startup paths for
the BSP and the non-BSP. Therefore have c_entry
be an 2 element array of function pointers. The
non-BSP paths have an entry point one instruction after
stage/module entry.

BUG=chrome-os-partner:30785
BRANCH=None
TEST=Built and booted to kernel.

Change-Id: I40bb40462906f1b1eaf2db8584985095e8ac0bae
Signed-off-by: Patrick Georgi <pgeorgi@chromium.org>
Original-Commit-Id: ce10f954041b3fd581ad8a3d82dee567b68637fe
Original-Change-Id: Ia573b1095dca5f69e371bf1ddf6b6df72fa3b52e
Original-Signed-off-by: Aaron Durbin <adurbin@chromium.org>
Original-Reviewed-on: https://chromium-review.googlesource.com/218844
Original-Reviewed-by: Furquan Shaikh <furquan@chromium.org>
Reviewed-on: http://review.coreboot.org/9090
Tested-by: build bot (Jenkins)
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>

3 files changed, 51 insertions, 20 deletions

diff --git a/src/arch/arm64/c_entry.c b/src/arch/arm64/c_entry.c
index f4a9848ee1..5e3dfe871d 100644
--- a/src/arch/arm64/c_entry.c
+++ b/src/arch/arm64/c_entry.c
@@ -24,13 +24,6 @@
 #include <arch/stages.h>
 #include "cpu-internal.h"
 
-/*
- * This variable holds entry point for CPUs starting up. Before the other
- * CPUs are brought up this value will change to provide the secondary
- * code path.
- */
-void (*c_entry)(void) = &arm64_init;
-
 void __attribute__((weak)) arm64_soc_init(void)
 {
 	/* Default weak implementation does nothing. */
@@ -54,7 +47,7 @@ static void seed_stack(void)
 		*slot++ = 0xdeadbeefdeadbeefULL;
 }
 
-void arm64_init(void)
+static void arm64_init(void)
 {
 	cpu_set_bsp();
 	seed_stack();
@@ -71,11 +64,15 @@ static void secondary_cpu_start(void)
 	arch_secondary_cpu_init();
 }
 
+/*
+ * This variable holds entry point for CPUs starting up. The first
+ * element is the BSP path, and the second is the non-BSP path.
+ */
+void (*c_entry[2])(void) = { &arm64_init, &secondary_cpu_start };
+
 extern void arm64_cpu_startup(void);
+
 void *prepare_secondary_cpu_startup(void)
 {
-	c_entry = &secondary_cpu_start;
-	dcache_clean_invalidate_by_mva(c_entry, sizeof(c_entry));
-
-	return &arm64_cpu_startup;
+	return secondary_entry_point(&arm64_cpu_startup);
 }
diff --git a/src/arch/arm64/include/arch/stages.h b/src/arch/arm64/include/arch/stages.h
index 97714926eb..fd633e484f 100644
--- a/src/arch/arm64/include/arch/stages.h
+++ b/src/arch/arm64/include/arch/stages.h
@@ -20,18 +20,30 @@
 #ifndef __ARCH_STAGES_H
 #define __ARCH_STAGES_H
 
+#include <stdint.h>
+
 extern void main(void);
 
 void stage_entry(void);
 void stage_exit(void *);
 void jmp_to_elf_entry(void *entry, unsigned long buffer, unsigned long size);
 
-/* C entry point for all arm64 stages. */
-void arm64_init(void);
-
 /* This function is called upon initial entry of each stage. It is called prior
  * to main(). That means all of the common infrastructure will most likely not
  * be available to be used (such as console). */
 void arm64_soc_init(void);
 
+/*
+ * Stages and rmodules have 2 entry points: BSP and non-BSP. Provided
+ * a pointer the correct non-BSP entry point will be returned. The
+ * first instruction is for BSP and the 2nd is for non-BSP. Instructions
+ * are all 32-bit on arm64.
+ */
+static inline void *secondary_entry_point(void *e)
+{
+	uintptr_t nonbsp = (uintptr_t)e;
+
+	return (void *)(nonbsp + sizeof(uint32_t));
+}
+
 #endif
diff --git a/src/arch/arm64/stage_entry.S b/src/arch/arm64/stage_entry.S
index 6b8ca7b424..5a5ddabb51 100644
--- a/src/arch/arm64/stage_entry.S
+++ b/src/arch/arm64/stage_entry.S
@@ -105,6 +105,8 @@ ENTRY(arm64_c_environment)
 
 	/* Get entry point by dereferencing c_entry. */
 	ldr	x1, 1f
+	/* Retrieve entry in c_entry array using  x26 as the index. */
+	adds	x1, x1, x26, lsl #3
 	ldr	x1, [x1]
 	/* Move back the arguments from x25 to x0 */
 	mov     x0, x25
@@ -114,14 +116,28 @@ ENTRY(arm64_c_environment)
 	.quad	c_entry
 ENDPROC(arm64_c_environment)
 
+/* The first 2 instructions are for BSP and secondary CPUs,
+ * respectively. x26 holds the index into c_entry array. */
+.macro split_bsp_path
+	b	2000f
+	b	2001f
+	2000:
+	mov	x26, #0
+	b	2002f
+	2001:
+	mov	x26, #1
+	2002:
+.endm
+
 ENTRY(__rmodule_entry)
+	split_bsp_path
 	/* Save the arguments to secmon in x25 */
-	mov    x25, x0
-	b      arm64_c_environment
+	mov	x25, x0
+	b	arm64_c_environment
 ENDPROC(__rmodule_entry)
 
-CPU_RESET_ENTRY(arm64_cpu_startup)
-        read_current x0, sctlr
+ENTRY(_arm64_cpu_startup)
+	read_current x0, sctlr
 	bic	x0, x0, #(1 << 25)	/* Little Endian */
 	bic	x0, x0, #(1 << 19)	/* XN not enforced */
 	bic	x0, x0, #(1 << 12)	/* Disable Instruction Cache */
@@ -129,8 +145,14 @@ CPU_RESET_ENTRY(arm64_cpu_startup)
 	write_current sctlr, x0, x1
 	isb
 	b	arm64_c_environment
+ENDPROC(_arm64_cpu_startup)
+
+CPU_RESET_ENTRY(arm64_cpu_startup)
+	split_bsp_path
+	b	_arm64_cpu_startup
 ENDPROC(arm64_cpu_startup)
 
 ENTRY(stage_entry)
-	b	arm64_cpu_startup
+	split_bsp_path
+	b	_arm64_cpu_startup
 ENDPROC(stage_entry)