From f64f7a2f29ec39a1b68d28a82805f124f80dbb50 Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Tue, 21 Nov 2017 15:13:46 +0100
Subject: bsp/qoriq: Implement bsp_reset()

---
 c/src/lib/libbsp/powerpc/qoriq/startup/bspreset.c | 42 ++++++++++++++++++++---
 1 file changed, 37 insertions(+), 5 deletions(-)

diff --git a/c/src/lib/libbsp/powerpc/qoriq/startup/bspreset.c b/c/src/lib/libbsp/powerpc/qoriq/startup/bspreset.c
index 5488823cb5..545aa28ee3 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/startup/bspreset.c
+++ b/c/src/lib/libbsp/powerpc/qoriq/startup/bspreset.c
@@ -7,10 +7,10 @@
  */
 
 /*
- * Copyright (c) 2010 embedded brains GmbH.  All rights reserved.
+ * Copyright (c) 2010, 2017 embedded brains GmbH.  All rights reserved.
  *
  *  embedded brains GmbH
- *  Obere Lagerstr. 30
+ *  Dornierstr. 4
  *  82178 Puchheim
  *  Germany
  *  <rtems@embedded-brains.de>
@@ -20,13 +20,45 @@
  * http://www.rtems.org/license/LICENSE.
  */
 
-#include <stdbool.h>
-
 #include <bsp/bootcard.h>
+#include <bsp/fdt.h>
+#include <bsp/qoriq.h>
+
+#include <libfdt.h>
+
+static int find_rstcr_node(const void *fdt, int node)
+{
+  return fdt_node_offset_by_prop_value(fdt, node, "fsl,has-rstcr", NULL, 0);
+}
 
 void bsp_reset(void)
 {
+  rtems_interrupt_level level;
+  const char *fdt;
+
+  rtems_interrupt_local_disable(level);
+  (void) level;
+
+  fdt = bsp_fdt_get();
+
+  /* If we do not find a RSTCR, then loop forever */
   while (true) {
-    /* Do nothing */
+    int node;
+
+    node = -1;
+
+    while ((node = find_rstcr_node(fdt, node)) >= 0) {
+      const uint32_t *reg;
+      int len;
+
+      reg = fdt_getprop(fdt, node, "reg", &len);
+      if (reg != NULL && len >= 4) {
+        volatile uint32_t *rstcr;
+
+        rstcr = (volatile uint32_t *)
+          ((uintptr_t) &qoriq + fdt32_to_cpu(reg[0]) + 0xb0);
+        *rstcr = 0x2;
+      }
+    }
   }
 }
-- 
cgit v1.2.3