[rtems commit] bsp/gen83xx: Invalidate caches only if disabled

[Sebastian Huber]

Module:    rtems
Branch:    master
Commit:    bfde536ec46b58091dbdd9efd4bb0e8c2fd46549
Changeset: http://git.rtems.org/rtems/commit/?id=bfde536ec46b58091dbdd9efd4bb0e8c2fd46549

Author:    Sebastian Huber <sebastian.huber at embedded-brains.de>
Date:      Wed Jan  9 11:12:48 2013 +0100

bsp/gen83xx: Invalidate caches only if disabled

---

 c/src/lib/libbsp/powerpc/gen83xx/startup/cpuinit.c |   13 ++++++++++---
 1 files changed, 10 insertions(+), 3 deletions(-)

diff --git a/c/src/lib/libbsp/powerpc/gen83xx/startup/cpuinit.c b/c/src/lib/libbsp/powerpc/gen83xx/startup/cpuinit.c
index 0cffc24..ab409ca 100644
--- a/c/src/lib/libbsp/powerpc/gen83xx/startup/cpuinit.c
+++ b/c/src/lib/libbsp/powerpc/gen83xx/startup/cpuinit.c
@@ -141,14 +141,21 @@ void cpu_init( void)
 {
   BAT dbat, ibat;
   uint32_t msr;
+  uint32_t hid0;
 
   /* Clear MMU and segment registers */
   clear_mmu_regs();
 
   /* Clear caches */
-  PPC_CLEAR_SPECIAL_PURPOSE_REGISTER_BITS( HID0, HID0_ILOCK | HID0_DLOCK | HID0_ICE | HID0_DCE);
-  PPC_SET_SPECIAL_PURPOSE_REGISTER_BITS( HID0, HID0_ICFI | HID0_DCI);
-  PPC_CLEAR_SPECIAL_PURPOSE_REGISTER_BITS( HID0, HID0_ICFI | HID0_DCI);
+  hid0 = PPC_SPECIAL_PURPOSE_REGISTER(HID0);
+  if ((hid0 & (HID0_ICE | HID0_DCE)) == 0) {
+    hid0 &= ~(HID0_ILOCK | HID0_DLOCK | HID0_ICE | HID0_DCE);
+    PPC_SET_SPECIAL_PURPOSE_REGISTER(HID0, hid0);
+    hid0 |= HID0_ICFI | HID0_DCI;
+    PPC_SET_SPECIAL_PURPOSE_REGISTER(HID0, hid0);
+    hid0 &= ~(HID0_ICFI | HID0_DCI);
+    PPC_SET_SPECIAL_PURPOSE_REGISTER(HID0, hid0);
+  }
 
   /*
    * Set up IBAT registers in MMU