1 files changed, 35 insertions, 9 deletions

diff --git a/arch/powerpc/kernel/head_fsl_booke.S b/arch/powerpc/kernel/head_fsl_booke.S
index d9cc2c288d9e..4ff744143566 100644
--- a/arch/powerpc/kernel/head_fsl_booke.S
+++ b/arch/powerpc/kernel/head_fsl_booke.S
@@ -68,7 +68,9 @@ _ENTRY(_start);
         mr      r29,r5
         mr      r28,r6
         mr      r27,r7
+        li      r25,0           /* phys kernel start (low) */
         li      r24,0           /* CPU number */
+        li      r23,0           /* phys kernel start (high) */
 
 /* We try to not make any assumptions about how the boot loader
  * setup or used the TLBs.  We invalidate all mappings from the
@@ -167,7 +169,28 @@ skpinv:	addi	r6,r6,1				/* Increment */
         mtspr   SPRN_MAS0,r7
         tlbre
 
-        /* Just modify the entry ID, EPN and RPN for the temp mapping */
+        /* grab and fixup the RPN */
+        mfspr   r6,SPRN_MAS1    /* extract MAS1[SIZE] */
+        rlwinm  r6,r6,25,27,30
+        li      r8,-1
+        addi    r6,r6,10
+        slw     r6,r8,r6        /* convert to mask */
+
+        bl      1f              /* Find our address */
+1:      mflr    r7
+
+        mfspr   r8,SPRN_MAS3
+#ifdef CONFIG_PHYS_64BIT
+        mfspr   r23,SPRN_MAS7
+#endif
+        and     r8,r6,r8
+        subfic  r9,r6,-4096
+        and     r9,r9,r7
+
+        or      r25,r8,r9
+        ori     r8,r25,(MAS3_SX|MAS3_SW|MAS3_SR)
+
+        /* Just modify the entry ID and EPN for the temp mapping */
         lis     r7,0x1000       /* Set MAS0(TLBSEL) = 1 */
         rlwimi  r7,r5,16,4,15   /* Setup MAS0 = TLBSEL | ESEL(r5) */
         mtspr   SPRN_MAS0,r7
@@ -177,12 +200,10 @@ skpinv:	addi	r6,r6,1				/* Increment */
         ori     r6,r6,(MAS1_TSIZE(BOOKE_PAGESZ_4K))@l
         mtspr   SPRN_MAS1,r6
         mfspr   r6,SPRN_MAS2
-        lis     r7,PHYSICAL_START@h
+        li      r7,0            /* temp EPN = 0 */
         rlwimi  r7,r6,0,20,31
         mtspr   SPRN_MAS2,r7
-        mfspr   r6,SPRN_MAS3
-        rlwimi  r7,r6,0,20,31
-        mtspr   SPRN_MAS3,r7
+        mtspr   SPRN_MAS3,r8
         tlbwe
 
         xori    r6,r4,1
@@ -232,8 +253,7 @@ skpinv:	addi	r6,r6,1				/* Increment */
         ori     r6,r6,PAGE_OFFSET@l
         rlwimi  r6,r7,0,20,31
         mtspr   SPRN_MAS2,r6
-        li      r7,(MAS3_SX|MAS3_SW|MAS3_SR)
-        mtspr   SPRN_MAS3,r7
+        mtspr   SPRN_MAS3,r8
         tlbwe
 
 /* 7. Jump to KERNELBASE mapping */
@@ -283,7 +303,10 @@ skpinv:	addi	r6,r6,1				/* Increment */
         SET_IVOR(12, WatchdogTimer);
         SET_IVOR(13, DataTLBError);
         SET_IVOR(14, InstructionTLBError);
-        SET_IVOR(15, Debug);
+        SET_IVOR(15, DebugDebug);
+#if defined(CONFIG_E500)
+        SET_IVOR(15, DebugCrit);
+#endif
         SET_IVOR(32, SPEUnavailable);
         SET_IVOR(33, SPEFloatingPointData);
         SET_IVOR(34, SPEFloatingPointRound);
@@ -718,7 +741,10 @@ interrupt_base:
 
 
         /* Debug Interrupt */
-        DEBUG_EXCEPTION
+        DEBUG_DEBUG_EXCEPTION
+#if defined(CONFIG_E500)
+        DEBUG_CRIT_EXCEPTION
+#endif
 
 /*
  * Local functions