3 files changed, 53 insertions, 24 deletions

diff --git a/arch/arm/mm/cache-v6.S b/arch/arm/mm/cache-v6.S
index 99fa688dfadd..c96fa1b3f49f 100644
--- a/arch/arm/mm/cache-v6.S
+++ b/arch/arm/mm/cache-v6.S
@@ -203,6 +203,10 @@ ENTRY(v6_flush_kern_dcache_area)
  *      - end     - virtual end address of region
  */
 v6_dma_inv_range:
+#ifdef CONFIG_DMA_CACHE_RWFO
+        ldrb    r2, [r0]                        @ read for ownership
+        strb    r2, [r0]                        @ write for ownership
+#endif
         tst     r0, #D_CACHE_LINE_SIZE - 1
         bic     r0, r0, #D_CACHE_LINE_SIZE - 1
 #ifdef HARVARD_CACHE
@@ -211,6 +215,10 @@ v6_dma_inv_range:
         mcrne   p15, 0, r0, c7, c11, 1          @ clean unified line
 #endif
         tst     r1, #D_CACHE_LINE_SIZE - 1
+#ifdef CONFIG_DMA_CACHE_RWFO
+        ldrneb  r2, [r1, #-1]                   @ read for ownership
+        strneb  r2, [r1, #-1]                   @ write for ownership
+#endif
         bic     r1, r1, #D_CACHE_LINE_SIZE - 1
 #ifdef HARVARD_CACHE
         mcrne   p15, 0, r1, c7, c14, 1          @ clean & invalidate D line
@@ -218,10 +226,6 @@ v6_dma_inv_range:
         mcrne   p15, 0, r1, c7, c15, 1          @ clean & invalidate unified line
 #endif
 1:
-#ifdef CONFIG_DMA_CACHE_RWFO
-        ldr     r2, [r0]                        @ read for ownership
-        str     r2, [r0]                        @ write for ownership
-#endif
 #ifdef HARVARD_CACHE
         mcr     p15, 0, r0, c7, c6, 1           @ invalidate D line
 #else
@@ -229,6 +233,10 @@ v6_dma_inv_range:
 #endif
         add     r0, r0, #D_CACHE_LINE_SIZE
         cmp     r0, r1
+#ifdef CONFIG_DMA_CACHE_RWFO
+        ldrlo   r2, [r0]                        @ read for ownership
+        strlo   r2, [r0]                        @ write for ownership
+#endif
         blo     1b
         mov     r0, #0
         mcr     p15, 0, r0, c7, c10, 4          @ drain write buffer
@@ -263,12 +271,12 @@ v6_dma_clean_range:
  *      - end     - virtual end address of region
  */
 ENTRY(v6_dma_flush_range)
-        bic     r0, r0, #D_CACHE_LINE_SIZE - 1
-1:
 #ifdef CONFIG_DMA_CACHE_RWFO
-        ldr     r2, [r0]                        @ read for ownership
-        str     r2, [r0]                        @ write for ownership
+        ldrb    r2, [r0]                @ read for ownership
+        strb    r2, [r0]                @ write for ownership
 #endif
+        bic     r0, r0, #D_CACHE_LINE_SIZE - 1
+1:
 #ifdef HARVARD_CACHE
         mcr     p15, 0, r0, c7, c14, 1          @ clean & invalidate D line
 #else
@@ -276,6 +284,10 @@ ENTRY(v6_dma_flush_range)
 #endif
         add     r0, r0, #D_CACHE_LINE_SIZE
         cmp     r0, r1
+#ifdef CONFIG_DMA_CACHE_RWFO
+        ldrlob  r2, [r0]                        @ read for ownership
+        strlob  r2, [r0]                        @ write for ownership
+#endif
         blo     1b
         mov     r0, #0
         mcr     p15, 0, r0, c7, c10, 4          @ drain write buffer
diff --git a/arch/arm/mm/cache-v7.S b/arch/arm/mm/cache-v7.S
index a3ebf7a4f49b..6136e68ce953 100644
--- a/arch/arm/mm/cache-v7.S
+++ b/arch/arm/mm/cache-v7.S
@@ -173,15 +173,22 @@ ENTRY(v7_coherent_user_range)
  UNWIND(.fnstart                )
         dcache_line_size r2, r3
         sub     r3, r2, #1
-        bic     r0, r0, r3
+        bic     r12, r0, r3
 1:
- USER(  mcr     p15, 0, r0, c7, c11, 1  )       @ clean D line to the point of unification
+ USER(  mcr     p15, 0, r12, c7, c11, 1 )       @ clean D line to the point of unification
+        add     r12, r12, r2
+        cmp     r12, r1
+        blo     1b
         dsb
- USER(  mcr     p15, 0, r0, c7, c5, 1   )       @ invalidate I line
-        add     r0, r0, r2
+        icache_line_size r2, r3
+        sub     r3, r2, #1
+        bic     r12, r0, r3
 2:
-        cmp     r0, r1
-        blo     1b
+ USER(  mcr     p15, 0, r12, c7, c5, 1  )       @ invalidate I line
+        add     r12, r12, r2
+        cmp     r12, r1
+        blo     2b
+3:
         mov     r0, #0
         ALT_SMP(mcr     p15, 0, r0, c7, c1, 6)  @ invalidate BTB Inner Shareable
         ALT_UP(mcr      p15, 0, r0, c7, c5, 6)  @ invalidate BTB
@@ -194,10 +201,10 @@ ENTRY(v7_coherent_user_range)
  * isn't mapped, just try the next page.
  */
 9001:
-        mov     r0, r0, lsr #12
-        mov     r0, r0, lsl #12
-        add     r0, r0, #4096
-        b       2b
+        mov     r12, r12, lsr #12
+        mov     r12, r12, lsl #12
+        add     r12, r12, #4096
+        b       3b
  UNWIND(.fnend          )
 ENDPROC(v7_coherent_kern_range)
 ENDPROC(v7_coherent_user_range)
diff --git a/arch/arm/mm/proc-macros.S b/arch/arm/mm/proc-macros.S
index 7d63beaf9745..b795afd0a2c6 100644
--- a/arch/arm/mm/proc-macros.S
+++ b/arch/arm/mm/proc-macros.S
@@ -61,17 +61,27 @@
         .endm
 
 /*
- * cache_line_size - get the cache line size from the CSIDR register
- * (available on ARMv7+). It assumes that the CSSR register was configured
- * to access the L1 data cache CSIDR.
+ * dcache_line_size - get the minimum D-cache line size from the CTR register
+ * on ARMv7.
  */
         .macro  dcache_line_size, reg, tmp
-        mrc     p15, 1, \tmp, c0, c0, 0         @ read CSIDR
-        and     \tmp, \tmp, #7                  @ cache line size encoding
-        mov     \reg, #16                       @ size offset
+        mrc     p15, 0, \tmp, c0, c0, 1         @ read ctr
+        lsr     \tmp, \tmp, #16
+        and     \tmp, \tmp, #0xf                @ cache line size encoding
+        mov     \reg, #4                        @ bytes per word
         mov     \reg, \reg, lsl \tmp            @ actual cache line size
         .endm
 
+/*
+ * icache_line_size - get the minimum I-cache line size from the CTR register
+ * on ARMv7.
+ */
+        .macro  icache_line_size, reg, tmp
+        mrc     p15, 0, \tmp, c0, c0, 1         @ read ctr
+        and     \tmp, \tmp, #0xf                @ cache line size encoding
+        mov     \reg, #4                        @ bytes per word
+        mov     \reg, \reg, lsl \tmp            @ actual cache line size
+        .endm
 
 /*
  * Sanity check the PTE configuration for the code below - which makes