2 files changed, 15 insertions, 9 deletions

diff --git a/arch/powerpc/include/asm/mmu-fsl-booke.h b/arch/powerpc/include/asm/mmu-fsl-booke.h
index 925d93cf64d8..5588a41f439c 100644
--- a/arch/powerpc/include/asm/mmu-fsl-booke.h
+++ b/arch/powerpc/include/asm/mmu-fsl-booke.h
@@ -40,6 +40,8 @@
 #define MAS2_M          0x00000004
 #define MAS2_G          0x00000002
 #define MAS2_E          0x00000001
+#define MAS2_EPN_MASK(size)             (~0 << (2*(size) + 10))
+#define MAS2_VAL(addr, size, flags)     ((addr) & MAS2_EPN_MASK(size) | (flags))
 
 #define MAS3_RPN        0xFFFFF000
 #define MAS3_U0         0x00000200
diff --git a/arch/powerpc/kernel/head_fsl_booke.S b/arch/powerpc/kernel/head_fsl_booke.S
index 837e3cc9cc85..8942d7a82faf 100644
--- a/arch/powerpc/kernel/head_fsl_booke.S
+++ b/arch/powerpc/kernel/head_fsl_booke.S
@@ -235,36 +235,40 @@ skpinv:	addi	r6,r6,1				/* Increment */
         tlbivax 0,r9
         TLBSYNC
 
+/* The mapping only needs to be cache-coherent on SMP */
+#ifdef CONFIG_SMP
+#define M_IF_SMP        MAS2_M
+#else
+#define M_IF_SMP        0
+#endif
+
 /* 6. Setup KERNELBASE mapping in TLB1[0] */
         lis     r6,0x1000               /* Set MAS0(TLBSEL) = TLB1(1), ESEL = 0 */
         mtspr   SPRN_MAS0,r6
         lis     r6,(MAS1_VALID|MAS1_IPROT)@h
         ori     r6,r6,(MAS1_TSIZE(BOOKE_PAGESZ_64M))@l
         mtspr   SPRN_MAS1,r6
-        li      r7,0
-        lis     r6,PAGE_OFFSET@h
-        ori     r6,r6,PAGE_OFFSET@l
-        rlwimi  r6,r7,0,20,31
+        lis     r6,MAS2_VAL(PAGE_OFFSET, BOOKE_PAGESZ_64M, M_IF_SMP)@h
+        ori     r6,r6,MAS2_VAL(PAGE_OFFSET, BOOKE_PAGESZ_64M, M_IF_SMP)@l
         mtspr   SPRN_MAS2,r6
         mtspr   SPRN_MAS3,r8
         tlbwe
 
 /* 7. Jump to KERNELBASE mapping */
-        lis     r6,KERNELBASE@h
-        ori     r6,r6,KERNELBASE@l
-        rlwimi  r6,r7,0,20,31
+        lis     r6,(KERNELBASE & ~0xfff)@h
+        ori     r6,r6,(KERNELBASE & ~0xfff)@l
         lis     r7,MSR_KERNEL@h
         ori     r7,r7,MSR_KERNEL@l
         bl      1f                      /* Find our address */
 1:      mflr    r9
         rlwimi  r6,r9,0,20,31
-        addi    r6,r6,24
+        addi    r6,r6,(2f - 1b)
         mtspr   SPRN_SRR0,r6
         mtspr   SPRN_SRR1,r7
         rfi                             /* start execution out of TLB1[0] entry */
 
 /* 8. Clear out the temp mapping */
-        lis     r7,0x1000       /* Set MAS0(TLBSEL) = 1 */
+2:      lis     r7,0x1000       /* Set MAS0(TLBSEL) = 1 */
         rlwimi  r7,r5,16,4,15   /* Setup MAS0 = TLBSEL | ESEL(r5) */
         mtspr   SPRN_MAS0,r7
         tlbre