2011-06-28 Joel Sherrill <joel.sherrill@oarcorp.com>

		Jennifer Averett <jennifer.averett@OARcorp.com>

	PR 1801/bsps
	* Makefile.am, include/leon.h: Add SMP support to LEON3 BSP.
	* smp/getcpuid.c, smp/smp_leon3.c: New files.

5 files changed, 211 insertions, 2 deletions

diff --git a/c/src/lib/libbsp/sparc/leon3/ChangeLog b/c/src/lib/libbsp/sparc/leon3/ChangeLog
index acb0331f99..b6ff8b1763 100644
--- a/c/src/lib/libbsp/sparc/leon3/ChangeLog
+++ b/c/src/lib/libbsp/sparc/leon3/ChangeLog
@@ -1,3 +1,10 @@
+2011-06-28	Joel Sherrill <joel.sherrill@oarcorp.com>
+		Jennifer Averett <jennifer.averett@OARcorp.com>
+
+	PR 1801/bsps
+	* Makefile.am, include/leon.h: Add SMP support to LEON3 BSP.
+	* smp/getcpuid.c, smp/smp_leon3.c: New files.
+
 2011-06-19	Ralf Corsépius <ralf.corsepius@rtems.org>
 
 	* Makefile.am: Fix broken path to clockdrv_shell.h.
diff --git a/c/src/lib/libbsp/sparc/leon3/Makefile.am b/c/src/lib/libbsp/sparc/leon3/Makefile.am
index 7a73536e8b..5edbe9db8d 100644
--- a/c/src/lib/libbsp/sparc/leon3/Makefile.am
+++ b/c/src/lib/libbsp/sparc/leon3/Makefile.am
@@ -91,8 +91,8 @@ libbsp_a_SOURCES += ../../sparc/shared/i2c/i2cmst.c
 libbsp_a_SOURCES += timer/timer.c
 
 if HAS_SMP
-libbsp_a_SOURCES += ../../shared/smp/getcpuid.c ../../shared/smp/smp_stub.c \
-    ../../shared/smp/bspsmp_wait_for.c
+libbsp_a_SOURCES += smp/getcpuid.c
+libbsp_a_SOURCES += smp/smp_leon3.c
 endif
 
 if HAS_NETWORKING
diff --git a/c/src/lib/libbsp/sparc/leon3/include/leon.h b/c/src/lib/libbsp/sparc/leon3/include/leon.h
index ee5ae30af8..b953e59b40 100644
--- a/c/src/lib/libbsp/sparc/leon3/include/leon.h
+++ b/c/src/lib/libbsp/sparc/leon3/include/leon.h
@@ -222,6 +222,10 @@ extern int LEON3_Cpu_Index;
 
 #define LEON3_MP_IRQ    14        /* Irq used by shared memory driver */
 
+#define LEON3_IRQMPSTATUS_CPUNR     28
+#define LEON3_IRQMPSTATUS_BROADCAST 27
+
+
 #ifndef ASM
 
 /*
diff --git a/c/src/lib/libbsp/sparc/leon3/smp/getcpuid.c b/c/src/lib/libbsp/sparc/leon3/smp/getcpuid.c
new file mode 100644
index 0000000000..8bde93f8d9
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/leon3/smp/getcpuid.c
@@ -0,0 +1,18 @@
+/*
+ *  Extracted from smp-imps.c
+ *
+ *  <Insert copyright here : it must be BSD-like so anyone can use it>
+ *
+ *  Author:  Erich Boleyn  <erich@uruk.org>   http://www.uruk.org/~erich/
+ *
+ *  $Id$
+ */
+
+int bsp_smp_processor_id(void)
+{
+  unsigned int id;
+  __asm__ __volatile__( "rd     %%asr17,%0\n\t" : "=r" (id) : );
+
+  return ((id >> 28) & 0xff);
+}
+
diff --git a/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c b/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
new file mode 100644
index 0000000000..702f77b96f
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/leon3/smp/smp_leon3.c
@@ -0,0 +1,180 @@
+/**
+ *  @file
+ *
+ *  LEON3 SMP BSP Support
+ */
+
+/*
+ *  COPYRIGHT (c) 1989-2011.
+ *  On-Line Applications Research Corporation (OAR).
+ *
+ *  The license and distribution terms for this file may be
+ *  found in the file LICENSE in this distribution or at
+ *  http://www.rtems.com/license/LICENSE.
+ *
+ *  $Id$
+ */
+
+#include <rtems.h>
+#include <bsp.h>
+#include <rtems/bspIo.h>
+#include <rtems/bspsmp.h>
+#include <stdlib.h>
+
+#define RTEMS_DEBUG
+
+static inline void sparc_leon3_set_cctrl( unsigned int val )
+{
+  __asm__ volatile( "sta %0, [%%g0] 2" : : "r" (val) );
+}
+
+static inline unsigned int sparc_leon3_get_cctrl( void )
+{
+  unsigned int v = 0;
+  __asm__ volatile( "lda [%%g0] 2, %0" : "=r" (v) : "0" (v) );
+  return v;
+}
+
+rtems_isr bsp_ap_ipi_isr(
+  rtems_vector_number vector
+)
+{
+  LEON_Clear_interrupt(LEON3_MP_IRQ);
+  rtems_smp_process_interrupt();
+}
+
+void bsp_smp_secondary_cpu_initialize(int cpu)
+{
+  sparc_leon3_set_cctrl( 0x80000F );
+  LEON_Unmask_interrupt(LEON3_MP_IRQ);
+  LEON3_IrqCtrl_Regs->mask[cpu] |= 1 << LEON3_MP_IRQ;
+}
+
+/*
+ *  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 );
+
+int bsp_smp_initialize(
+  int maximum
+)
+{
+  int cpu;
+  int found_cpus = 0;
+
+  sparc_leon3_set_cctrl( 0x80000F );
+  found_cpus =
+    ((LEON3_IrqCtrl_Regs->mpstat >> LEON3_IRQMPSTATUS_CPUNR) & 0xf)  + 1;
+  #if defined(RTEMS_DEBUG)
+    printk( "Found %d CPUs\n", found_cpus );
+  #endif
+
+  if ( found_cpus > rtems_configuration_smp_maximum_processors ) {
+    printk(
+      "%d CPUs IS MORE THAN CONFIGURED -- ONLY USING %d\n",
+      found_cpus,
+      rtems_configuration_smp_maximum_processors
+    );
+    found_cpus = rtems_configuration_smp_maximum_processors;
+  }
+
+  if ( found_cpus == 1 )
+    return 1;
+
+  for ( cpu=1 ; cpu < found_cpus ; cpu++ ) {
+    
+    #if defined(RTEMS_DEBUG)
+      printk( "Waking CPU %d\n", cpu );
+    #endif
+
+    bsp_ap_stack = _Per_CPU_Information[cpu].interrupt_stack_high -
+                      CPU_MINIMUM_STACK_FRAME_SIZE;
+    bsp_ap_entry = rtems_smp_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_Information[cpu].state == RTEMS_BSP_SMP_CPU_INITIALIZED) ?
+	   "online" : "offline")
+      );
+    #endif
+  }
+
+  if ( found_cpus > 1 ) {
+    LEON_Unmask_interrupt(LEON3_MP_IRQ);
+    set_vector(bsp_ap_ipi_isr, LEON_TRAP_TYPE(LEON3_MP_IRQ), 1);
+  }
+  return found_cpus;
+}
+
+void bsp_smp_interrupt_cpu(
+  int cpu
+)
+{
+  /* send interrupt to destination CPU */
+  LEON3_IrqCtrl_Regs->force[cpu] = 1 << LEON3_MP_IRQ;
+}
+
+void bsp_smp_broadcast_interrupt(void)
+{
+  int dest_cpu;
+  int cpu;
+  int max_cpus;
+
+  cpu = bsp_smp_processor_id();
+  max_cpus = rtems_smp_get_number_of_processors();
+
+  for ( dest_cpu=0 ; dest_cpu < max_cpus ; dest_cpu++ ) {
+    if ( cpu == dest_cpu )
+      continue;
+    bsp_smp_interrupt_cpu( dest_cpu );
+    /* this is likely needed due to the ISR code not being SMP aware yet */
+    bsp_smp_delay( 100000 );
+  }
+}
+
+extern __inline__ void __delay(unsigned long loops)
+{
+   __asm__ __volatile__("cmp %0, 0\n\t"
+     "1: bne 1b\n\t"
+     "subcc %0, 1, %0\n" :
+     "=&r" (loops) :
+     "0" (loops) :
+     "cc"
+  );
+}
+
+/*
+ *  Kill time without depending on the timer being present or programmed.
+ *
+ *  This is not very sophisticated.
+ */
+void bsp_smp_delay( int max )
+{
+   __delay( max );
+}
+
+void bsp_smp_wait_for(
+  volatile unsigned int *address,
+  unsigned int           desired,
+  int                    maximum_usecs
+)
+{
+  int iterations;
+  volatile unsigned int *p = address;
+
+  for (iterations=0 ;  iterations < maximum_usecs ; iterations++ ) {
+    if ( *p == desired )
+      break;
+    bsp_smp_delay( 5000 );
+  }
+}
+
+