[PATCH 20/32] leon: allow SMP boot from any CPU

[Daniel Hellstrom]

From: Martin Aberg <maberg at gaisler.com>

---
 c/src/lib/libbsp/sparc/leon3/startup/bspstart.c |  5 +++++
 c/src/lib/libbsp/sparc/shared/start/start.S     | 14 +++++++++-----
 2 files changed, 14 insertions(+), 5 deletions(-)

diff --git a/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c b/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
index 80c2bc0..6353ca3 100644
--- a/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
+++ b/c/src/lib/libbsp/sparc/leon3/startup/bspstart.c
@@ -36,6 +36,11 @@ int CPU_SPARC_HAS_SNOOPING;
 /* Index of CPU, in an AMP system CPU-index may be non-zero */
 uint32_t LEON3_Cpu_Index = 0;
 
+#if defined(RTEMS_SMP)
+/* Index of the boot CPU. Set by the first CPU at boot to its CPU ID. */
+int LEON3_Boot_Cpu = -1;
+#endif
+
 /*
  * set_snooping
  *
diff --git a/c/src/lib/libbsp/sparc/shared/start/start.S b/c/src/lib/libbsp/sparc/shared/start/start.S
index 529a3c7..6a9faaf 100644
--- a/c/src/lib/libbsp/sparc/shared/start/start.S
+++ b/c/src/lib/libbsp/sparc/shared/start/start.S
@@ -281,13 +281,16 @@ SYM(hard_reset):
 	rd	%asr17, %o0		! get CPU identifier
 	srl	%o0, LEON3_ASR17_PROCESSOR_INDEX_SHIFT, %o0
 
-	cmp	%o0, 0
-	beq	cpu0
-	 nop
-
 	sll	%o0, PER_CPU_CONTROL_SIZE_LOG2, %l0
 	add	%g6, %l0, %g6
 
+	/* If LEON3_Boot_Cpu < 0 then assign us as boot CPU and continue. */
+	set	SYM(LEON3_Boot_Cpu), %o1
+	ld	[%o1], %o2
+	tst	%o2
+	bneg	.Lbootcpu
+	 nop
+
 	ld	[%g6 + PER_CPU_INTERRUPT_STACK_HIGH], %sp ! set stack pointer
 	sub	%sp, 4, %sp		! stack starts at end of area - 4
 	andn	%sp, 0x0f, %sp		! align stack on 16-byte boundary
@@ -295,7 +298,8 @@ SYM(hard_reset):
 
 	call	SYM(bsp_start_on_secondary_processor) ! does not return
 	 sub	%sp, SPARC_MINIMUM_STACK_FRAME_SIZE, %sp
-cpu0:
+.Lbootcpu:
+	st	%o0, [%o1]
 #endif
 
 	set	(SYM(rdb_start)), %g5	! End of RAM
-- 
2.7.4