3 files changed, 29 insertions, 16 deletions

diff --git a/firmware/export/system.h b/firmware/export/system.h
index f9a074349b..f554ac7cf1 100644
--- a/firmware/export/system.h
+++ b/firmware/export/system.h
@@ -253,6 +253,10 @@ static inline void cpu_boost_unlock(void)
     #define MIN_STACK_ALIGN 8
 #endif
 
+#ifdef CPU_MIPS
+    #define HAVE_CPU_CACHE_ALIGN
+#endif
+
 /* Define this if target has support for generating backtraces */
 #ifdef CPU_ARM
     #define HAVE_RB_BACKTRACE
diff --git a/firmware/target/mips/ingenic_jz47xx/system-target.h b/firmware/target/mips/ingenic_jz47xx/system-target.h
index 9dc1a5c8c8..862ec403d2 100644
--- a/firmware/target/mips/ingenic_jz47xx/system-target.h
+++ b/firmware/target/mips/ingenic_jz47xx/system-target.h
@@ -29,7 +29,7 @@
 #include "mipsregs.h"
 
 #define CACHE_SIZE       16*1024
-#define CACHE_LINE_SIZE  32
+#define CACHEALIGN_BITS  5
 #include "mmu-mips.h"
 
 #define CFG_UART_BASE    UART1_BASE      /* Base of the UART channel */
diff --git a/firmware/target/mips/mmu-mips.c b/firmware/target/mips/mmu-mips.c
index eb7004952e..f4ffbfa6ee 100644
--- a/firmware/target/mips/mmu-mips.c
+++ b/firmware/target/mips/mmu-mips.c
@@ -192,10 +192,11 @@ void commit_discard_dcache(void)
  */
 void commit_discard_dcache_range(const void *base, unsigned int size)
 {
-    register char *s;
+    char *ptr = CACHEALIGN_DOWN((char*)base);
+    char *end = CACHEALIGN_UP((char*)base + size);
 
-    for (s=(char *)base; s<(char *)base+size; s+=CACHEALIGN_SIZE)
-        __CACHE_OP(DCHitWBInv, s);
+    for(; ptr != end; ptr += CACHEALIGN_SIZE)
+        __CACHE_OP(DCHitWBInv, ptr);
 
     SYNC_WB();
 }
@@ -204,10 +205,11 @@ void commit_discard_dcache_range(const void *base, unsigned int size)
  */
 void commit_dcache_range(const void *base, unsigned int size)
 {
-    register char *s;
+    char *ptr = CACHEALIGN_DOWN((char*)base);
+    char *end = CACHEALIGN_UP((char*)base + size);
 
-    for (s=(char *)base; s<(char *)base+size; s+=CACHEALIGN_SIZE)
-        __CACHE_OP(DCHitWB, s);
+    for(; ptr != end; ptr += CACHEALIGN_SIZE)
+        __CACHE_OP(DCHitWB, ptr);
 
     SYNC_WB();
 }
@@ -217,17 +219,24 @@ void commit_dcache_range(const void *base, unsigned int size)
  */
 void discard_dcache_range(const void *base, unsigned int size)
 {
-    register char *s;
+    char *ptr = CACHEALIGN_DOWN((char*)base);
+    char *end = CACHEALIGN_UP((char*)base + size);
 
-    if (((int)base & CACHEALIGN_SIZE - 1) ||
-        (((int)base + size) & CACHEALIGN_SIZE - 1)) {
-            /* Overlapping sections, so we need to write back instead */
-            commit_discard_dcache_range(base, size);
-            return;
-    };
+    if(ptr != base) {
+        /* Start of region not cache aligned */
+        __CACHE_OP(DCHitWBInv, ptr);
+        ptr += CACHEALIGN_SIZE;
+    }
+
+    if(base+size != end) {
+        /* End of region not cache aligned */
+        end -= CACHEALIGN_SIZE;
+        __CACHE_OP(DCHitWBInv, end);
+    }
 
-    for (s=(char *)base; s<(char *)base+size; s+=CACHEALIGN_SIZE)
-        __CACHE_OP(DCHitInv, s);
+    /* Interior of region is safe to discard */
+    for(; ptr != end; ptr += CACHEALIGN_SIZE)
+        __CACHE_OP(DCHitInv, ptr);
 
     SYNC_WB();
 }