[PATCH 3/3] bsp/leon3: Add bsp_reset() for SMP configuration

[Sebastian Huber]

---
 c/src/lib/libbsp/sparc/leon3/Makefile.am        |    1 +
 c/src/lib/libbsp/sparc/leon3/startup/bspreset.c |   62 +++++++++++++++++++++++
 c/src/lib/libbsp/sparc/shared/start/start.S     |    2 +
 3 files changed, 65 insertions(+), 0 deletions(-)
 create mode 100644 c/src/lib/libbsp/sparc/leon3/startup/bspreset.c

diff --git a/c/src/lib/libbsp/sparc/leon3/Makefile.am b/c/src/lib/libbsp/sparc/leon3/Makefile.am
index 64f0577..5134b34 100644
--- a/c/src/lib/libbsp/sparc/leon3/Makefile.am
+++ b/c/src/lib/libbsp/sparc/leon3/Makefile.am
@@ -38,6 +38,7 @@ libbsp_a_SOURCES += ../../shared/bspclean.c ../../shared/bsplibc.c \
     ../../sparc/shared/startup/bspgetworkarea.c ../../shared/sbrk.c startup/setvec.c \
     startup/spurious.c startup/bspidle.S startup/bspdelay.c \
     ../../shared/bspinit.c ../../sparc/shared/startup/early_malloc.c
+libbsp_a_SOURCES += startup/bspreset.c
 libbsp_a_SOURCES += startup/cpucounter.c
 
 # ISR Handler
diff --git a/c/src/lib/libbsp/sparc/leon3/startup/bspreset.c b/c/src/lib/libbsp/sparc/leon3/startup/bspreset.c
new file mode 100644
index 0000000..7c9af80
--- /dev/null
+++ b/c/src/lib/libbsp/sparc/leon3/startup/bspreset.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2014 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <rtems at embedded-brains.de>
+ *
+ * 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.
+ */
+
+#include <bsp.h>
+#include <bsp/bootcard.h>
+#include <leon.h>
+
+#ifdef RTEMS_SMP
+
+void bsp_reset(void)
+{
+  uint32_t self_cpu = rtems_smp_get_current_processor();
+
+  if (self_cpu == 0) {
+    volatile struct irqmp_regs *irqmp = LEON3_IrqCtrl_Regs;
+
+    if (irqmp != NULL) {
+      /*
+       * Value was choosen to get something in the magnitude of 1ms on a 200MHz
+       * processor.
+       */
+      uint32_t max_wait = 1234567;
+
+      uint32_t cpu_count = leon3_get_cpu_count(irqmp);
+      uint32_t halt_mask = 0;
+      uint32_t i;
+
+      for (i = 0; i < cpu_count; ++i) {
+        if (i != self_cpu) {
+          halt_mask |= UINT32_C(1) << i;
+        }
+      }
+
+      /* Wait some time for secondary processors to halt */
+      i = 0;
+      while ((irqmp->mpstat & halt_mask) != halt_mask && i < max_wait) {
+        ++i;
+      }
+    }
+
+    __asm__ volatile (
+      "mov 1, %g1\n"
+      "ta 0\n"
+      "nop"
+    );
+  }
+
+  leon3_power_down_loop();
+}
+
+#endif /* RTEMS_SMP */
diff --git a/c/src/lib/libbsp/sparc/shared/start/start.S b/c/src/lib/libbsp/sparc/shared/start/start.S
index d0eb512..0b74efc 100644
--- a/c/src/lib/libbsp/sparc/shared/start/start.S
+++ b/c/src/lib/libbsp/sparc/shared/start/start.S
@@ -362,6 +362,7 @@ zerobss:
         sub     %sp, 0x60, %sp          ! room for boot_card to save args
         nop
 
+#if !defined(START_LEON3_ENABLE_SMP)
         PUBLIC(BSP_fatal_return)
         PUBLIC(bsp_reset)
 SYM(BSP_fatal_return):
@@ -369,5 +370,6 @@ SYM(bsp_reset):
 	mov	1, %g1
         ta      0                       ! Halt if _main returns ...
         nop
+#endif
 
 /* end of file */
-- 
1.7.7