[ARM] Invalidate TLB before and after setting up page tables

diff --git a/arch/arm/boot/compressed/head.S b/arch/arm/boot/compressed/head.S
index 824831c320b05812de6b6ce1a918aa3bfab8d43e..1fde5be64c242b5adfae83d9bf76d312d9423020 100644 (file)
--- a/arch/arm/boot/compressed/head.S
+++ b/arch/arm/boot/compressed/head.S
@@ -366,9 +366,12 @@ __armv4_cache_on:
                mcr     p15, 0, r0, c7, c10, 4  @ drain write buffer
                mcr     p15, 0, r0, c8, c7, 0   @ flush I,D TLBs
                mrc     p15, 0, r0, c1, c0, 0   @ read control reg
-               orr     r0, r0, #0x1000         @ I-cache enable
+               orr     r0, r0, #0x5000         @ I-cache enable, RR cache replacement
                orr     r0, r0, #0x0030
-               b       __common_cache_on
+               bl      __common_cache_on
+               mov     r0, #0
+               mcr     p15, 0, r0, c8, c7, 0   @ flush I,D TLBs
+               mov     pc, r12
 
 __arm6_cache_on:
                mov     r12, lr
@@ -377,6 +380,11 @@ __arm6_cache_on:
                mcr     p15, 0, r0, c7, c0, 0   @ invalidate whole cache v3
                mcr     p15, 0, r0, c5, c0, 0   @ invalidate whole TLB v3
                mov     r0, #0x30
+               bl      __common_cache_on
+               mov     r0, #0
+               mcr     p15, 0, r0, c5, c0, 0   @ invalidate whole TLB v3
+               mov     pc, r12
+
 __common_cache_on:
 #ifndef DEBUG
                orr     r0, r0, #0x000d         @ Write buffer, mmu
@@ -385,7 +393,7 @@ __common_cache_on:
                mcr     p15, 0, r3, c2, c0, 0   @ load page table pointer
                mcr     p15, 0, r1, c3, c0, 0   @ load domain access control
                mcr     p15, 0, r0, c1, c0, 0   @ load control register
-               mov     pc, r12
+               mov     pc, lr
 
 /*
  * All code following this line is relocatable.  It is relocated by
@@ -567,11 +575,12 @@ cache_clean_flush:
 __armv4_cache_flush:
                bic     r1, pc, #31
                add     r2, r1, #65536          @ 2x the largest dcache size
-1:             ldr     r12, [r1], #32          @ s/w flush D cache
+1:             ldr     r3, [r1], #32           @ s/w flush D cache
                teq     r1, r2
                bne     1b
 
-               mcr     p15, 0, r1, c7, c7, 0   @ flush I cache
+               mcr     p15, 0, r1, c7, c5, 0   @ flush I cache
+               mcr     p15, 0, r1, c7, c6, 0   @ flush D cache
                mcr     p15, 0, r1, c7, c10, 4  @ drain WB
                mov     pc, lr