rk27xx: Fix commit_discard_idcache()

This version resembles how OF handle cache invalidates.
This seems to fix mysterious data aborts on plugin/codec loading
after introducing frequency scaling.

Credit goes to mortalis for pinpointing the reason of aborts.

Change-Id: I3477b3f65d593d7b43c36a0b06d863f71f000812

1 files changed, 17 insertions, 7 deletions

diff --git a/firmware/target/arm/rk27xx/system-rk27xx.c b/firmware/target/arm/rk27xx/system-rk27xx.c
index 8c75deef07..27373207bd 100644
--- a/firmware/target/arm/rk27xx/system-rk27xx.c
+++ b/firmware/target/arm/rk27xx/system-rk27xx.c
@@ -200,19 +200,29 @@ void udelay(unsigned usecs)
     );
 }
 
-void commit_discard_idcache(void)
+/* Invalidating both cache lines from single function
+ * gives sometimes strange data aborts.
+ * This version resembles how OF invalidates cache.
+ * noinline attribute is to guarantee that future
+ * gcc change will not decide to inline this call (although
+ * current arm-eabi version from our toolchain doesn't do that
+ */
+static void __attribute__((noinline)) cache_invalidate_way(int way)
 {
-    /* invalidate cache way 0 */
-    CACHEOP = 0x02;
+    /* Issue invalidata way command to the cache controler */
+    CACHEOP = ((way<<31)|0x2);
 
     /* wait for invalidate process to complete */
     while (CACHEOP & 0x03);
+}
 
-    /* invalidate cache way 1 */
-    CACHEOP = 0x80000002;
+void commit_discard_idcache(void)
+{
+    /* invalidate cache way 0 */
+    cache_invalidate_way(0);
 
-    /* wait for invalidate process to complete */
-    while (CACHEOP & 0x03);   
+    /* invalidate cache way 1 */
+    cache_invalidate_way(1);
 }
 void commit_discard_dcache (void) __attribute__((alias("commit_discard_idcache")));