From 07713db65bae02b394cafef808b37b99cc2cd747 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Tue, 18 Feb 2014 12:46:08 +0100
Subject: bsp/leon3: Add bsp_reset() for SMP configuration

---
 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(+)
 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 b543f17e75..a66da9be82 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 0000000000..7392efe230
--- /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@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 = rtems_smp_get_processor_count();
+      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 df17a9bfe4..79ae02286d 100644
--- a/c/src/lib/libbsp/sparc/shared/start/start.S
+++ b/c/src/lib/libbsp/sparc/shared/start/start.S
@@ -360,6 +360,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):
@@ -367,5 +368,6 @@ SYM(bsp_reset):
 	mov	1, %g1
         ta      0                       ! Halt if _main returns ...
         nop
+#endif
 
 /* end of file */
-- 
cgit v1.2.3