1 files changed, 61 insertions, 0 deletions
diff --git a/bsps/lm32/milkymist/start/bspclean.c b/bsps/lm32/milkymist/start/bspclean.c
new file mode 100644
index 0000000000..b010292d06
--- /dev/null
+++ b/bsps/lm32/milkymist/start/bspclean.c
@@ -0,0 +1,61 @@
+/*
+ *  Milkymist shutdown routine
+ */
+
+/*
+ *  COPYRIGHT (c) 2010 Sebastien Bourdeauducq
+ *
+ *  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 "../include/system_conf.h"
+
+static void reboot(void)
+{
+  /* Writing to CSR_SYSTEM_ID causes a system reset */
+  MM_WRITE(MM_SYSTEM_ID, 1);
+}
+
+static void icap_write(int val, unsigned int w)
+{
+  while (!(MM_READ(MM_ICAP) & ICAP_READY));
+  if (!val)
+    w |= ICAP_CE|ICAP_WRITE;
+  MM_WRITE(MM_ICAP, w);
+}
+
+static void reconf(void)
+{
+  icap_write(0, 0xffff); /* dummy word */
+  icap_write(0, 0xffff); /* dummy word */
+  icap_write(0, 0xffff); /* dummy word */
+  icap_write(0, 0xffff); /* dummy word */
+  icap_write(1, 0xaa99); /* sync word part 1 */
+  icap_write(1, 0x5566); /* sync word part 2 */
+  icap_write(1, 0x30a1); /* write to command register */
+  icap_write(1, 0x0000); /* null command */
+  icap_write(1, 0x30a1); /* write to command register */
+  icap_write(1, 0x000e); /* reboot command */
+  icap_write(1, 0x2000); /* NOP */
+  icap_write(1, 0x2000); /* NOP */
+  icap_write(1, 0x2000); /* NOP */
+  icap_write(1, 0x2000); /* NOP */
+  icap_write(0, 0x1111); /* NULL */
+  icap_write(0, 0xffff); /* dummy word */
+}
+
+void bsp_fatal_extension(
+  rtems_fatal_source source,
+  bool always_set_to_false,
+  rtems_fatal_code error
+)
+{
+  if (source == RTEMS_FATAL_SOURCE_EXIT && error)
+    reconf();
+  else
+    reboot();
+}