tlk: 6/19 update
[3rdparty/ote_partner/tlk.git] / arch / arm / arm / cache-ops.S
index 43b812e..de0b53f 100644 (file)
@@ -130,26 +130,15 @@ FUNCTION(arch_disable_cache)
        beq     .Licache_disable
        mrc     p15, 0, r0, c1, c0, 0           @ system control register
        tst     r0, #(1<<2)                     @ is the dcache already disabled?
-       beq     .Ldcache_already_disabled
+       beq     .Licache_disable
 
+       dmb
        bic     r0, #(1<<2)
        mcr     p15, 0, r0, c1, c0, 0           @ disable dcache
 
-#if ARM_CPU_CORTEX_A9
        @ flush and invalidate the dcache
        @ NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
        bl      flush_invalidate_cache_v7
-#endif
-
-       b       .Ldcache_disable_L2
-
-.Ldcache_already_disabled:
-#if ARM_CPU_CORTEX_A9
-       @ make sure all of the caches are invalidated
-       @ NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
-       bl      invalidate_cache_v7
-#endif
-.Ldcache_disable_L2:
 
 #if ARM_WITH_L2
        mrc     p15, 0, r0, c1, c0, 1           @ auxiliary control register
@@ -160,16 +149,17 @@ FUNCTION(arch_disable_cache)
 .Licache_disable:
        tst     r7, #ICACHE
        beq     .Ldone_disable
-
        mrc     p15, 0, r0, c1, c0, 0           @ system control register
+       tst     r0, #(1<<12)                    @ is the icache already disabled?
+       beq     .Ldone_disable
+
        bic     r0, #(1<<12)
        mcr     p15, 0, r0, c1, c0, 0           @ disable icache
+       mov     r0, #0
+       mcr     p15, 0, r0, c7, c5, 0           @ invalidate icache to PoU
 
 .Ldone_disable:
-#if ARM_CPU_CORTEX_A9
        mov     r0, #0
-       mcr     p15, 0, r0, c7, c5, 0           @ invalidate icache to PoU
-#endif
        msr     cpsr, r12
        ldmfd   sp!, {r4-r11, pc}
 
@@ -279,6 +269,9 @@ finished:
        ldmfd   sp!, {r0-r7, r9-r11}
        mov     r10, #0                 @ swith back to cache level 0
        mcr     p15, 2, r10, c0, c0, 0  @ select current cache level in cssr
+       dsb
+       isb
+       mov     pc, lr
 iflush:
        mcr     p15, 0, r10, c7, c10, 4 @ DSB
        mcr     p15, 0, r10, c7, c5, 0  @ invalidate I+BTB
@@ -321,7 +314,7 @@ invalidate_cache_v7:
 1003:
        orr     r11, r10, r9, lsl r5            @ factor way and cache number into r11
        orr     r11, r11, r7, lsl r2            @ factor index number into r11
-       mcr     p15, 0, r11, c7, c14, 2         @ clean/flush by set/way
+       mcr     p15, 0, r11, c7, c6, 2          @ invalidate by set/way
        subs    r9, r9, #1                      @ decrement the way
        bge     1003b
        subs    r7, r7, #1                      @ decrement the index