2 files changed, 202 insertions, 4 deletions

diff --git a/arch/powerpc/include/asm/reg_booke.h b/arch/powerpc/include/asm/reg_booke.h
index 9bb81d99b765..3bf783505528 100644
--- a/arch/powerpc/include/asm/reg_booke.h
+++ b/arch/powerpc/include/asm/reg_booke.h
@@ -108,6 +108,8 @@
 #define SPRN_PID2       0x27A   /* Process ID Register 2 */
 #define SPRN_TLB0CFG    0x2B0   /* TLB 0 Config Register */
 #define SPRN_TLB1CFG    0x2B1   /* TLB 1 Config Register */
+#define SPRN_TLB2CFG    0x2B2   /* TLB 2 Config Register */
+#define SPRN_TLB3CFG    0x2B3   /* TLB 3 Config Register */
 #define SPRN_EPR        0x2BE   /* External Proxy Register */
 #define SPRN_CCR1       0x378   /* Core Configuration Register 1 */
 #define SPRN_ZPR        0x3B0   /* Zone Protection Register (40x) */
diff --git a/arch/powerpc/kernel/exceptions-64e.S b/arch/powerpc/kernel/exceptions-64e.S
index 662236c72244..9048f96237f6 100644
--- a/arch/powerpc/kernel/exceptions-64e.S
+++ b/arch/powerpc/kernel/exceptions-64e.S
@@ -616,18 +616,214 @@ bad_stack_book3e:
  * Setup the initial TLB for a core. This current implementation
  * assume that whatever we are running off will not conflict with
  * the new mapping at PAGE_OFFSET.
- * We also make various assumptions about the processor we run on,
- * this might have to be made more flexible based on the content
- * of MMUCFG and friends.
  */
 _GLOBAL(initial_tlb_book3e)
 
+        /* Look for the first TLB with IPROT set */
+        mfspr   r4,SPRN_TLB0CFG
+        andi.   r3,r4,TLBnCFG_IPROT
+        lis     r3,MAS0_TLBSEL(0)@h
+        bne     found_iprot
+
+        mfspr   r4,SPRN_TLB1CFG
+        andi.   r3,r4,TLBnCFG_IPROT
+        lis     r3,MAS0_TLBSEL(1)@h
+        bne     found_iprot
+
+        mfspr   r4,SPRN_TLB2CFG
+        andi.   r3,r4,TLBnCFG_IPROT
+        lis     r3,MAS0_TLBSEL(2)@h
+        bne     found_iprot
+
+        lis     r3,MAS0_TLBSEL(3)@h
+        mfspr   r4,SPRN_TLB3CFG
+        /* fall through */
+
+found_iprot:
+        andi.   r5,r4,TLBnCFG_HES
+        bne     have_hes
+
+        mflr    r8                              /* save LR */
+/* 1. Find the index of the entry we're executing in
+ *
+ * r3 = MAS0_TLBSEL (for the iprot array)
+ * r4 = SPRN_TLBnCFG
+ */
+        bl      invstr                          /* Find our address */
+invstr: mflr    r6                              /* Make it accessible */
+        mfmsr   r7
+        rlwinm  r5,r7,27,31,31                  /* extract MSR[IS] */
+        mfspr   r7,SPRN_PID
+        slwi    r7,r7,16
+        or      r7,r7,r5
+        mtspr   SPRN_MAS6,r7
+        tlbsx   0,r6                            /* search MSR[IS], SPID=PID */
+
+        mfspr   r3,SPRN_MAS0
+        rlwinm  r5,r3,16,20,31                  /* Extract MAS0(Entry) */
+
+        mfspr   r7,SPRN_MAS1                    /* Insure IPROT set */
+        oris    r7,r7,MAS1_IPROT@h
+        mtspr   SPRN_MAS1,r7
+        tlbwe
+
+/* 2. Invalidate all entries except the entry we're executing in
+ *
+ * r3 = MAS0 w/TLBSEL & ESEL for the entry we are running in
+ * r4 = SPRN_TLBnCFG
+ * r5 = ESEL of entry we are running in
+ */
+        andi.   r4,r4,TLBnCFG_N_ENTRY           /* Extract # entries */
+        li      r6,0                            /* Set Entry counter to 0 */
+1:      mr      r7,r3                           /* Set MAS0(TLBSEL) */
+        rlwimi  r7,r6,16,4,15                   /* Setup MAS0 = TLBSEL | ESEL(r6) */
+        mtspr   SPRN_MAS0,r7
+        tlbre
+        mfspr   r7,SPRN_MAS1
+        rlwinm  r7,r7,0,2,31                    /* Clear MAS1 Valid and IPROT */
+        cmpw    r5,r6
+        beq     skpinv                          /* Dont update the current execution TLB */
+        mtspr   SPRN_MAS1,r7
+        tlbwe
+        isync
+skpinv: addi    r6,r6,1                         /* Increment */
+        cmpw    r6,r4                           /* Are we done? */
+        bne     1b                              /* If not, repeat */
+
+        /* Invalidate all TLBs */
+        PPC_TLBILX_ALL(0,0)
+        sync
+        isync
+
+/* 3. Setup a temp mapping and jump to it
+ *
+ * r3 = MAS0 w/TLBSEL & ESEL for the entry we are running in
+ * r5 = ESEL of entry we are running in
+ */
+        andi.   r7,r5,0x1       /* Find an entry not used and is non-zero */
+        addi    r7,r7,0x1
+        mr      r4,r3           /* Set MAS0(TLBSEL) = 1 */
+        mtspr   SPRN_MAS0,r4
+        tlbre
+
+        rlwimi  r4,r7,16,4,15   /* Setup MAS0 = TLBSEL | ESEL(r7) */
+        mtspr   SPRN_MAS0,r4
+
+        mfspr   r7,SPRN_MAS1
+        xori    r6,r7,MAS1_TS           /* Setup TMP mapping in the other Address space */
+        mtspr   SPRN_MAS1,r6
+
+        tlbwe
+
+        mfmsr   r6
+        xori    r6,r6,MSR_IS
+        mtspr   SPRN_SRR1,r6
+        bl      1f              /* Find our address */
+1:      mflr    r6
+        addi    r6,r6,(2f - 1b)
+        mtspr   SPRN_SRR0,r6
+        rfi
+2:
+
+/* 4. Clear out PIDs & Search info
+ *
+ * r3 = MAS0 w/TLBSEL & ESEL for the entry we started in
+ * r4 = MAS0 w/TLBSEL & ESEL for the temp mapping
+ * r5 = MAS3
+ */
+        li      r6,0
+        mtspr   SPRN_MAS6,r6
+        mtspr   SPRN_PID,r6
+
+/* 5. Invalidate mapping we started in
+ *
+ * r3 = MAS0 w/TLBSEL & ESEL for the entry we started in
+ * r4 = MAS0 w/TLBSEL & ESEL for the temp mapping
+ * r5 = MAS3
+ */
+        mtspr   SPRN_MAS0,r3
+        tlbre
+        mfspr   r6,SPRN_MAS1
+        rlwinm  r6,r6,0,2,0     /* clear IPROT */
+        mtspr   SPRN_MAS1,r6
+        tlbwe
+
+        /* Invalidate TLB1 */
+        PPC_TLBILX_ALL(0,0)
+        sync
+        isync
+
+/* 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 TLB[0]
+ *
+ * r3 = MAS0 w/TLBSEL & ESEL for the entry we started in
+ * r4 = MAS0 w/TLBSEL & ESEL for the temp mapping
+ * r5 = MAS3
+ */
+        rlwinm  r3,r3,0,16,3    /* clear ESEL */
+        mtspr   SPRN_MAS0,r3
+        lis     r6,(MAS1_VALID|MAS1_IPROT)@h
+        ori     r6,r6,(MAS1_TSIZE(BOOK3E_PAGESZ_1GB))@l
+        mtspr   SPRN_MAS1,r6
+
+        LOAD_REG_IMMEDIATE(r6, PAGE_OFFSET | M_IF_SMP)
+        mtspr   SPRN_MAS2,r6
+
+        rlwinm  r5,r5,0,0,25
+        ori     r5,r5,MAS3_SR | MAS3_SW | MAS3_SX
+        mtspr   SPRN_MAS3,r5
+        li      r5,-1
+        rlwinm  r5,r5,0,0,25
+
+        tlbwe
+
+/* 7. Jump to KERNELBASE mapping
+ *
+ * r4 = MAS0 w/TLBSEL & ESEL for the temp mapping
+ */
+        /* Now we branch the new virtual address mapped by this entry */
+        LOAD_REG_IMMEDIATE(r6,2f)
+        lis     r7,MSR_KERNEL@h
+        ori     r7,r7,MSR_KERNEL@l
+        mtspr   SPRN_SRR0,r6
+        mtspr   SPRN_SRR1,r7
+        rfi                             /* start execution out of TLB1[0] entry */
+2:
+
+/* 8. Clear out the temp mapping
+ *
+ * r4 = MAS0 w/TLBSEL & ESEL for the entry we are running in
+ */
+        mtspr   SPRN_MAS0,r4
+        tlbre
+        mfspr   r5,SPRN_MAS1
+        rlwinm  r5,r5,0,2,0     /* clear IPROT */
+        mtspr   SPRN_MAS1,r5
+        tlbwe
+
+        /* Invalidate TLB1 */
+        PPC_TLBILX_ALL(0,0)
+        sync
+        isync
+
+        /* We translate LR and return */
+        tovirt(r8,r8)
+        mtlr    r8
+        blr
+
+have_hes:
         /* Setup MAS 0,1,2,3 and 7 for tlbwe of a 1G entry that maps the
          * kernel linear mapping. We also set MAS8 once for all here though
          * that will have to be made dependent on whether we are running under
          * a hypervisor I suppose.
          */
-        li      r3,MAS0_HES | MAS0_WQ_ALLWAYS
+        ori     r3,r3,MAS0_HES | MAS0_WQ_ALLWAYS
         mtspr   SPRN_MAS0,r3
         lis     r3,(MAS1_VALID | MAS1_IPROT)@h
         ori     r3,r3,BOOK3E_PAGESZ_1GB << MAS1_TSIZE_SHIFT