From 0000791312ecc9ef0de7ed9646a57f7a64547e2b Mon Sep 17 00:00:00 2001
From: Sebastian Huber <sebastian.huber@embedded-brains.de>
Date: Thu, 28 Apr 2016 10:27:51 +0200
Subject: bsp/qoriq: Add and use qoriq_reset_qman_and_bman()

---
 c/src/lib/libbsp/powerpc/qoriq/include/qoriq.h      | 11 +++++++++++
 c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c |  2 ++
 2 files changed, 13 insertions(+)

diff --git a/c/src/lib/libbsp/powerpc/qoriq/include/qoriq.h b/c/src/lib/libbsp/powerpc/qoriq/include/qoriq.h
index 755cc65e61..28228cb7bc 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/include/qoriq.h
+++ b/c/src/lib/libbsp/powerpc/qoriq/include/qoriq.h
@@ -529,6 +529,17 @@ extern uint8_t qoriq_bman_portal[2][16777216];
 extern uint8_t qoriq_qman_portal[2][16777216];
 #endif
 
+static inline void qoriq_reset_qman_and_bman(void)
+{
+#if QORIQ_CHIP_IS_T_VARIANT(QORIQ_CHIP_VARIANT)
+  qoriq.dcfg.qmbm_warmrst = 0x3;
+
+  while ((qoriq.dcfg.qmbm_warmrst & 0x3) != 0) {
+    /* Wait for reset done */
+  }
+#endif
+}
+
 #ifdef __cplusplus
 }
 #endif /* __cplusplus */
diff --git a/c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c b/c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c
index 7df5135b55..4159050e5d 100644
--- a/c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c
+++ b/c/src/lib/libbsp/powerpc/qoriq/startup/bsprestart.c
@@ -34,6 +34,8 @@ static RTEMS_NO_RETURN void do_restart(void *addr)
   void (*restart)(uintptr_t);
   uintptr_t fdt;
 
+  qoriq_reset_qman_and_bman();
+
   restart = addr;
 
   fdt = (uintptr_t) bsp_fdt_get();
-- 
cgit v1.2.3