[rtems commit] bsp/leon3: Fix SMP initialization

[Sebastian Huber]

Module:    rtems
Branch:    master
Commit:    3d770018d90a57b4afad930ee1953be674c48be1
Changeset: http://git.rtems.org/rtems/commit/?id=3d770018d90a57b4afad930ee1953be674c48be1

Author:    Sebastian Huber <sebastian.huber at embedded-brains.de>
Date:      Wed Feb  5 15:15:51 2014 +0100

bsp/leon3: Fix SMP initialization

Avoid usage of the same stack area by multiple secondary processors at
the same time.

Avoid magic delay loops.

---

 c/src/lib/libbsp/sparc/leon3/include/leon.h  |    7 ++++
 c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c |   31 ++---------------
 c/src/lib/libbsp/sparc/shared/start/start.S  |   47 ++++++++++++-------------
 3 files changed, 34 insertions(+), 51 deletions(-)

diff --git a/c/src/lib/libbsp/sparc/leon3/include/leon.h b/c/src/lib/libbsp/sparc/leon3/include/leon.h
index 694622f..82e2c6d 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/leon.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/leon.h
@@ -310,6 +310,13 @@ void apbuart_outbyte_polled(
  */
 int apbuart_inbyte_nonblocking(struct apbuart_regs *regs);
 
+/**
+ * @brief Initializes a secondary processor.
+ *
+ * @param[in] cpu The processor executing this function.
+ */
+void leon3_secondary_cpu_initialize(uint32_t cpu);
+
 #endif /* !ASM */
 
 #ifdef __cplusplus
diff --git a/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c b/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
index 0a87dad..e62ddac 100644
--- a/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
+++ b/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
@@ -13,8 +13,8 @@
  *  http://www.rtems.com/license/LICENSE.
  */
 
-#include <rtems.h>
 #include <bsp.h>
+#include <leon.h>
 #include <rtems/bspIo.h>
 #include <rtems/bspsmp.h>
 #include <stdlib.h>
@@ -40,10 +40,8 @@ static rtems_isr bsp_ap_ipi_isr(
   rtems_smp_process_interrupt();
 }
 
-static void leon3_secondary_cpu_initialize(void)
+void leon3_secondary_cpu_initialize(uint32_t cpu)
 {
-  uint32_t cpu = rtems_smp_get_current_processor();
-
   sparc_leon3_set_cctrl( 0x80000F );
   LEON_Unmask_interrupt(LEON3_MP_IRQ);
   LEON3_IrqCtrl_Regs->mask[cpu] |= 1 << LEON3_MP_IRQ;
@@ -51,13 +49,6 @@ static void leon3_secondary_cpu_initialize(void)
   rtems_smp_secondary_cpu_initialize();
 }
 
-/*
- *  Used to pass information to start.S when bringing secondary CPUs
- *  out of reset.
- */
-void *bsp_ap_stack;
-void *bsp_ap_entry;
-
 static void bsp_smp_delay( int );
 
 uint32_t bsp_smp_initialize( uint32_t configured_cpu_count )
@@ -87,28 +78,14 @@ uint32_t bsp_smp_initialize( uint32_t configured_cpu_count )
     set_vector(bsp_ap_ipi_isr, LEON_TRAP_TYPE(LEON3_MP_IRQ), 1);
   }
 
-  for ( cpu=1 ; cpu < found_cpus ; cpu++ ) {
-    const Per_CPU_Control *per_cpu = _Per_CPU_Get_by_index( cpu );
-
+  for ( cpu = 1 ; cpu < found_cpus ; ++cpu ) {
     #if defined(RTEMS_DEBUG)
       printk( "Waking CPU %d\n", cpu );
     #endif
 
-    bsp_ap_stack = per_cpu->interrupt_stack_high -
-                      CPU_MINIMUM_STACK_FRAME_SIZE;
-    bsp_ap_entry = leon3_secondary_cpu_initialize;
-
     LEON3_IrqCtrl_Regs->mpstat = 1 << cpu;
-    bsp_smp_delay( 1000000 );
-    #if defined(RTEMS_DEBUG)
-      printk(
-        "CPU %d is %s\n",
-        cpu,
-        per_cpu->state == PER_CPU_STATE_READY_TO_BEGIN_MULTITASKING ?
-          "online" : "offline"
-      );
-    #endif
   }
+
   return found_cpus;
 }
 
diff --git a/c/src/lib/libbsp/sparc/shared/start/start.S b/c/src/lib/libbsp/sparc/shared/start/start.S
index e6884ee..756a06e 100644
--- a/c/src/lib/libbsp/sparc/shared/start/start.S
+++ b/c/src/lib/libbsp/sparc/shared/start/start.S
@@ -16,11 +16,11 @@
  */
 
 #include <rtems/asm.h>
-#include <rtems/system.h>
+#include <rtems/score/percpu.h>
 #include <bspopts.h>
 
 #if defined(RTEMS_SMP) && defined(BSP_LEON3_SMP)
-  #define ENABLE_SMP
+  #define START_LEON3_ENABLE_SMP
 #endif
 
 /*
@@ -32,15 +32,6 @@
   nop; \
   nop;
 
-#if defined(ENABLE_SMP)
-/*
- * Variables to contain information used with bring a secondary core
- * out of reset.
- */
-  	.global bsp_ap_stack
-  	.global bsp_ap_entry
-#endif
-
 /*
  *  Software trap. Treat as BAD_TRAP for the time being...
  */
@@ -231,20 +222,28 @@ SYM(hard_reset):
         nop
         nop
 
-#if defined(ENABLE_SMP)
-	rd	%asr17, %g1
-	srl	%g1, 28, %g1
-	and	%g1, 0xff, %g1			! extract cpu id
-	cmp	%g1, 0
+#if defined(START_LEON3_ENABLE_SMP)
+	rd	%asr17, %o0		! get CPU identifier
+	srl	%o0, 28, %o0		! CPU index is upper 4 bits so shift
+
+	cmp	%o0, 0
 	beq	cpu0
-	nop
-	set	SYM(bsp_ap_stack), %g1		! set the stack pointer
-	ld	[%g1], %sp
-	mov	%sp, %fp
-	set	SYM(bsp_ap_entry), %g1		! where to start
-	ld	[%g1], %g1
-	call	%g1
-	nop
+	 nop
+
+	sethi	%hi(_Per_CPU_Information), %o1 ! get per-CPU control
+	add	%o1, %lo(_Per_CPU_Information), %o1
+	sll	%o0, PER_CPU_CONTROL_SIZE_LOG2, %o2
+	add	%o1, %o2, %o1
+
+	ld	[%o1 + 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
+	mov	%sp, %fp		! set frame pointer
+
+	call	SYM(leon3_secondary_cpu_initialize) ! does not return
+	 sub	%sp, CPU_MINIMUM_STACK_FRAME_SIZE, %sp
+	ba	SYM(bsp_reset)		! just in case
+	 nop
 cpu0:
 #endif