From d62dfc7d991597ed4a6b6ccf1b9280c04ffd0c22 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Tue, 24 Nov 2015 08:33:52 +0100
Subject: bsp/altera-cyclone-v: Add fatal extension handler

---
 c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am  |  2 +-
 .../libbsp/arm/altera-cyclone-v/startup/bspclean.c | 43 ++++++++++++++++++++++
 .../libbsp/arm/altera-cyclone-v/startup/bspreset.c | 18 +--------
 3 files changed, 45 insertions(+), 18 deletions(-)
 create mode 100644 c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspclean.c

diff --git a/c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am b/c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am
index 6afe6814aa..095e0dfa42 100644
--- a/c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am
+++ b/c/src/lib/libbsp/arm/altera-cyclone-v/Makefile.am
@@ -151,7 +151,6 @@ libbsp_a_SOURCES += hwlib/src/hwmgr/alt_reset_manager.c
 
 # Shared
 libbsp_a_SOURCES += ../../shared/bootcard.c
-libbsp_a_SOURCES += ../../shared/bspclean.c
 libbsp_a_SOURCES += ../../shared/bspgetworkarea.c
 libbsp_a_SOURCES += ../../shared/bsplibc.c
 libbsp_a_SOURCES += ../../shared/bsppost.c
@@ -167,6 +166,7 @@ libbsp_a_SOURCES += ../shared/arm-cp15-set-exception-handler.c
 libbsp_a_SOURCES += ../shared/arm-cp15-set-ttb-entries.c
 
 # Startup
+libbsp_a_SOURCES += startup/bspclean.c
 libbsp_a_SOURCES += startup/bspreset.c
 libbsp_a_SOURCES += startup/bspstart.c
 libbsp_a_SOURCES += startup/bspstarthooks.c
diff --git a/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspclean.c b/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspclean.c
new file mode 100644
index 0000000000..5504ed65c3
--- /dev/null
+++ b/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspclean.c
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2015 embedded brains GmbH.  All rights reserved.
+ *
+ *  embedded brains GmbH
+ *  Dornierstr. 4
+ *  82178 Puchheim
+ *  Germany
+ *  <info@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.org/license/LICENSE.
+ */
+
+#include <bsp.h>
+#include <bsp/bootcard.h>
+#include <rtems/bspIo.h>
+#include <rtems/score/smpimpl.h>
+
+void bsp_fatal_extension(
+  rtems_fatal_source src,
+  bool is_internal,
+  rtems_fatal_code code
+)
+{
+#ifdef RTEMS_SMP
+  if (src == RTEMS_FATAL_SOURCE_SMP && code == SMP_FATAL_SHUTDOWN_RESPONSE) {
+    while (true) {
+      _ARM_Wait_for_event();
+    }
+  }
+#endif
+
+#if BSP_PRINT_EXCEPTION_CONTEXT
+  if (src == RTEMS_FATAL_SOURCE_EXCEPTION) {
+    rtems_exception_frame_print((const rtems_exception_frame *) code);
+  }
+#endif
+
+#if BSP_RESET_BOARD_AT_EXIT
+  bsp_reset();
+#endif
+}
diff --git a/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspreset.c b/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspreset.c
index 3b7f10a822..c4af106fed 100644
--- a/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspreset.c
+++ b/c/src/lib/libbsp/arm/altera-cyclone-v/startup/bspreset.c
@@ -14,24 +14,8 @@
 
 #include <bsp/bootcard.h>
 #include <bsp/alt_reset_manager.h>
-#include "socal/alt_rstmgr.h"
-#include "socal/hps.h"
 
 void bsp_reset(void)
 {
-  uint32_t           self_cpu  = rtems_get_current_processor();
-  volatile uint32_t *mpumodrst = ALT_RSTMGR_MPUMODRST_ADDR;
-  
-  if( self_cpu == 0 ) {
-    /* Reset CPU1 */
-    (*mpumodrst) |= ALT_RSTMGR_MPUMODRST_CPU1_SET_MSK;
-    
-    /* Simply call the reset method from alteras HWLIB */
-    (void) alt_reset_cold_reset();
-  } else {
-    /* Keep CPU1 waiting until it gets reset by CPU0 */
-    while ( true ) {
-      __asm__ volatile ("wfi");
-    }
-  }
+  alt_reset_cold_reset();
 }
-- 
cgit v1.2.3