powerpc/fsl_booke: set the tlb entry for the kernel address in AS1

We use the tlb1 entries to map low mem to the kernel space. In the
current code, it assumes that the first tlb entry would cover the
kernel image. But this is not true for some special cases, such as
when we run a relocatable kernel above the 64M or set
CONFIG_KERNEL_START above 64M. So we choose to switch to address
space 1 before setting these tlb entries.

Signed-off-by: Kevin Hao <haokexin@gmail.com>
Signed-off-by: Scott Wood <scottwood@freescale.com>

1 files changed, 81 insertions, 0 deletions

diff --git a/arch/powerpc/kernel/head_fsl_booke.S b/arch/powerpc/kernel/head_fsl_booke.S
index 19bd574bda9d..75f0223e6d0d 100644
--- a/arch/powerpc/kernel/head_fsl_booke.S
+++ b/arch/powerpc/kernel/head_fsl_booke.S
@@ -1157,6 +1157,87 @@ __secondary_hold_acknowledge:
 #endif
 
 /*
+ * Create a tlb entry with the same effective and physical address as
+ * the tlb entry used by the current running code. But set the TS to 1.
+ * Then switch to the address space 1. It will return with the r3 set to
+ * the ESEL of the new created tlb.
+ */
+_GLOBAL(switch_to_as1)
+        mflr    r5
+
+        /* Find a entry not used */
+        mfspr   r3,SPRN_TLB1CFG
+        andi.   r3,r3,0xfff
+        mfspr   r4,SPRN_PID
+        rlwinm  r4,r4,16,0x3fff0000     /* turn PID into MAS6[SPID] */
+        mtspr   SPRN_MAS6,r4
+1:      lis     r4,0x1000               /* Set MAS0(TLBSEL) = 1 */
+        addi    r3,r3,-1
+        rlwimi  r4,r3,16,4,15           /* Setup MAS0 = TLBSEL | ESEL(r3) */
+        mtspr   SPRN_MAS0,r4
+        tlbre
+        mfspr   r4,SPRN_MAS1
+        andis.  r4,r4,MAS1_VALID@h
+        bne     1b
+
+        /* Get the tlb entry used by the current running code */
+        bl      0f
+0:      mflr    r4
+        tlbsx   0,r4
+
+        mfspr   r4,SPRN_MAS1
+        ori     r4,r4,MAS1_TS           /* Set the TS = 1 */
+        mtspr   SPRN_MAS1,r4
+
+        mfspr   r4,SPRN_MAS0
+        rlwinm  r4,r4,0,~MAS0_ESEL_MASK
+        rlwimi  r4,r3,16,4,15           /* Setup MAS0 = TLBSEL | ESEL(r3) */
+        mtspr   SPRN_MAS0,r4
+        tlbwe
+        isync
+        sync
+
+        mfmsr   r4
+        ori     r4,r4,MSR_IS | MSR_DS
+        mtspr   SPRN_SRR0,r5
+        mtspr   SPRN_SRR1,r4
+        sync
+        rfi
+
+/*
+ * Restore to the address space 0 and also invalidate the tlb entry created
+ * by switch_to_as1.
+*/
+_GLOBAL(restore_to_as0)
+        mflr    r0
+
+        bl      0f
+0:      mflr    r9
+        addi    r9,r9,1f - 0b
+
+        mfmsr   r7
+        li      r8,(MSR_IS | MSR_DS)
+        andc    r7,r7,r8
+
+        mtspr   SPRN_SRR0,r9
+        mtspr   SPRN_SRR1,r7
+        sync
+        rfi
+
+        /* Invalidate the temporary tlb entry for AS1 */
+1:      lis     r9,0x1000               /* Set MAS0(TLBSEL) = 1 */
+        rlwimi  r9,r3,16,4,15           /* Setup MAS0 = TLBSEL | ESEL(r3) */
+        mtspr   SPRN_MAS0,r9
+        tlbre
+        mfspr   r9,SPRN_MAS1
+        rlwinm  r9,r9,0,2,31            /* Clear MAS1 Valid and IPPROT */
+        mtspr   SPRN_MAS1,r9
+        tlbwe
+        isync
+        mtlr    r0
+        blr
+
+/*
  * We put a few things here that have to be page-aligned. This stuff
  * goes at the beginning of the data segment, which is page-aligned.
  */