msm: Update arm cortex cache enable/disable
enable/disable caches even if already enabled/disabled.
Also, disable caches before reenabling them.
Change-Id: I1a9668402187d2254955256d6471d4980a4a2945
diff --git a/arch/arm/cache-ops.S b/arch/arm/cache-ops.S
index 8a545dc..6fc7d19 100644
--- a/arch/arm/cache-ops.S
+++ b/arch/arm/cache-ops.S
@@ -130,8 +130,6 @@
tst r7, #DCACHE
beq .Licache_disable
mrc p15, 0, r0, c1, c0, 0 // cr1
- tst r0, #(1<<2) // is the dcache already disabled?
- beq .Ldcache_already_disabled
bic r0, #(1<<2)
mcr p15, 0, r0, c1, c0, 0 // disable dcache
@@ -140,13 +138,6 @@
// NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
bl flush_invalidate_cache_v7
- b .Ldcache_disable_L2
-
-.Ldcache_already_disabled:
- // 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
-
.Ldcache_disable_L2:
#if ARM_WITH_L2
@@ -180,13 +171,10 @@
mrs r12, cpsr // save the old interrupt state
.word 0xf10c01c0 /* cpsid iaf */ // interrupts disabled
-
+
.Ldcache_enable:
tst r7, #DCACHE
beq .Licache_enable
- mrc p15, 0, r0, c1, c0, 0 // cr1
- tst r0, #(1<<2) // is the dcache already enabled?
- bne .Licache_enable
// invalidate L1 and L2
// NOTE: trashes a bunch of registers, can't be spilling stuff to the stack
@@ -200,6 +188,10 @@
#endif
mrc p15, 0, r0, c1, c0, 0 // cr1
+ bic r0, #(1<<2)
+ mcr p15, 0, r0, c1, c0, 0 // disable dcache
+
+ mrc p15, 0, r0, c1, c0, 0 // cr1
orr r0, #(1<<2)
mcr p15, 0, r0, c1, c0, 0 // enable dcache
@@ -211,6 +203,10 @@
mcr p15, 0, r0, c7, c5, 0 // invalidate icache to PoU
mrc p15, 0, r0, c1, c0, 0 // cr1
+ bic r0, #(1<<12)
+ mcr p15, 0, r0, c1, c0, 0 // disable icache
+
+ mrc p15, 0, r0, c1, c0, 0 // cr1
orr r0, #(1<<12)
mcr p15, 0, r0, c1, c0, 0 // enable icache