Sebastian Andrzej Siewior | 7c08ce7 | 2010-04-04 22:19:02 +0200 | [diff] [blame] | 1 | |
| 2 | /* 1. Find the index of the entry we're executing in */ |
| 3 | bl invstr /* Find our address */ |
| 4 | invstr: mflr r6 /* Make it accessible */ |
| 5 | mfmsr r7 |
| 6 | rlwinm r4,r7,27,31,31 /* extract MSR[IS] */ |
| 7 | mfspr r7, SPRN_PID0 |
| 8 | slwi r7,r7,16 |
| 9 | or r7,r7,r4 |
| 10 | mtspr SPRN_MAS6,r7 |
| 11 | tlbsx 0,r6 /* search MSR[IS], SPID=PID0 */ |
| 12 | mfspr r7,SPRN_MAS1 |
| 13 | andis. r7,r7,MAS1_VALID@h |
| 14 | bne match_TLB |
| 15 | |
| 16 | mfspr r7,SPRN_MMUCFG |
| 17 | rlwinm r7,r7,21,28,31 /* extract MMUCFG[NPIDS] */ |
| 18 | cmpwi r7,3 |
| 19 | bne match_TLB /* skip if NPIDS != 3 */ |
| 20 | |
| 21 | mfspr r7,SPRN_PID1 |
| 22 | slwi r7,r7,16 |
| 23 | or r7,r7,r4 |
| 24 | mtspr SPRN_MAS6,r7 |
| 25 | tlbsx 0,r6 /* search MSR[IS], SPID=PID1 */ |
| 26 | mfspr r7,SPRN_MAS1 |
| 27 | andis. r7,r7,MAS1_VALID@h |
| 28 | bne match_TLB |
| 29 | mfspr r7, SPRN_PID2 |
| 30 | slwi r7,r7,16 |
| 31 | or r7,r7,r4 |
| 32 | mtspr SPRN_MAS6,r7 |
| 33 | tlbsx 0,r6 /* Fall through, we had to match */ |
| 34 | |
| 35 | match_TLB: |
| 36 | mfspr r7,SPRN_MAS0 |
| 37 | rlwinm r3,r7,16,20,31 /* Extract MAS0(Entry) */ |
| 38 | |
| 39 | mfspr r7,SPRN_MAS1 /* Insure IPROT set */ |
| 40 | oris r7,r7,MAS1_IPROT@h |
| 41 | mtspr SPRN_MAS1,r7 |
| 42 | tlbwe |
| 43 | |
| 44 | /* 2. Invalidate all entries except the entry we're executing in */ |
| 45 | mfspr r9,SPRN_TLB1CFG |
| 46 | andi. r9,r9,0xfff |
| 47 | li r6,0 /* Set Entry counter to 0 */ |
| 48 | 1: lis r7,0x1000 /* Set MAS0(TLBSEL) = 1 */ |
| 49 | rlwimi r7,r6,16,4,15 /* Setup MAS0 = TLBSEL | ESEL(r6) */ |
| 50 | mtspr SPRN_MAS0,r7 |
| 51 | tlbre |
| 52 | mfspr r7,SPRN_MAS1 |
| 53 | rlwinm r7,r7,0,2,31 /* Clear MAS1 Valid and IPROT */ |
| 54 | cmpw r3,r6 |
| 55 | beq skpinv /* Dont update the current execution TLB */ |
| 56 | mtspr SPRN_MAS1,r7 |
| 57 | tlbwe |
| 58 | isync |
| 59 | skpinv: addi r6,r6,1 /* Increment */ |
| 60 | cmpw r6,r9 /* Are we done? */ |
| 61 | bne 1b /* If not, repeat */ |
| 62 | |
| 63 | /* Invalidate TLB0 */ |
| 64 | li r6,0x04 |
| 65 | tlbivax 0,r6 |
| 66 | TLBSYNC |
| 67 | /* Invalidate TLB1 */ |
| 68 | li r6,0x0c |
| 69 | tlbivax 0,r6 |
| 70 | TLBSYNC |
| 71 | |
| 72 | /* 3. Setup a temp mapping and jump to it */ |
| 73 | andi. r5, r3, 0x1 /* Find an entry not used and is non-zero */ |
| 74 | addi r5, r5, 0x1 |
| 75 | lis r7,0x1000 /* Set MAS0(TLBSEL) = 1 */ |
| 76 | rlwimi r7,r3,16,4,15 /* Setup MAS0 = TLBSEL | ESEL(r3) */ |
| 77 | mtspr SPRN_MAS0,r7 |
| 78 | tlbre |
| 79 | |
| 80 | /* grab and fixup the RPN */ |
| 81 | mfspr r6,SPRN_MAS1 /* extract MAS1[SIZE] */ |
| 82 | rlwinm r6,r6,25,27,31 |
| 83 | li r8,-1 |
| 84 | addi r6,r6,10 |
| 85 | slw r6,r8,r6 /* convert to mask */ |
| 86 | |
| 87 | bl 1f /* Find our address */ |
| 88 | 1: mflr r7 |
| 89 | |
| 90 | mfspr r8,SPRN_MAS3 |
| 91 | #ifdef CONFIG_PHYS_64BIT |
| 92 | mfspr r23,SPRN_MAS7 |
| 93 | #endif |
| 94 | and r8,r6,r8 |
| 95 | subfic r9,r6,-4096 |
| 96 | and r9,r9,r7 |
| 97 | |
| 98 | or r25,r8,r9 |
| 99 | ori r8,r25,(MAS3_SX|MAS3_SW|MAS3_SR) |
| 100 | |
| 101 | /* Just modify the entry ID and EPN for the temp mapping */ |
| 102 | lis r7,0x1000 /* Set MAS0(TLBSEL) = 1 */ |
| 103 | rlwimi r7,r5,16,4,15 /* Setup MAS0 = TLBSEL | ESEL(r5) */ |
| 104 | mtspr SPRN_MAS0,r7 |
| 105 | xori r6,r4,1 /* Setup TMP mapping in the other Address space */ |
| 106 | slwi r6,r6,12 |
| 107 | oris r6,r6,(MAS1_VALID|MAS1_IPROT)@h |
| 108 | ori r6,r6,(MAS1_TSIZE(BOOK3E_PAGESZ_4K))@l |
| 109 | mtspr SPRN_MAS1,r6 |
| 110 | mfspr r6,SPRN_MAS2 |
| 111 | li r7,0 /* temp EPN = 0 */ |
| 112 | rlwimi r7,r6,0,20,31 |
| 113 | mtspr SPRN_MAS2,r7 |
| 114 | mtspr SPRN_MAS3,r8 |
| 115 | tlbwe |
| 116 | |
| 117 | xori r6,r4,1 |
| 118 | slwi r6,r6,5 /* setup new context with other address space */ |
| 119 | bl 1f /* Find our address */ |
| 120 | 1: mflr r9 |
| 121 | rlwimi r7,r9,0,20,31 |
| 122 | addi r7,r7,(2f - 1b) |
| 123 | mtspr SPRN_SRR0,r7 |
| 124 | mtspr SPRN_SRR1,r6 |
| 125 | rfi |
| 126 | 2: |
| 127 | /* 4. Clear out PIDs & Search info */ |
| 128 | li r6,0 |
| 129 | mtspr SPRN_MAS6,r6 |
| 130 | mtspr SPRN_PID0,r6 |
| 131 | |
| 132 | mfspr r7,SPRN_MMUCFG |
| 133 | rlwinm r7,r7,21,28,31 /* extract MMUCFG[NPIDS] */ |
| 134 | cmpwi r7,3 |
| 135 | bne 2f /* skip if NPIDS != 3 */ |
| 136 | |
| 137 | mtspr SPRN_PID1,r6 |
| 138 | mtspr SPRN_PID2,r6 |
| 139 | |
| 140 | /* 5. Invalidate mapping we started in */ |
| 141 | 2: |
| 142 | lis r7,0x1000 /* Set MAS0(TLBSEL) = 1 */ |
| 143 | rlwimi r7,r3,16,4,15 /* Setup MAS0 = TLBSEL | ESEL(r3) */ |
| 144 | mtspr SPRN_MAS0,r7 |
| 145 | tlbre |
| 146 | mfspr r6,SPRN_MAS1 |
| 147 | rlwinm r6,r6,0,2,0 /* clear IPROT */ |
| 148 | mtspr SPRN_MAS1,r6 |
| 149 | tlbwe |
| 150 | /* Invalidate TLB1 */ |
| 151 | li r9,0x0c |
| 152 | tlbivax 0,r9 |
| 153 | TLBSYNC |
| 154 | |
| 155 | /* The mapping only needs to be cache-coherent on SMP */ |
| 156 | #ifdef CONFIG_SMP |
| 157 | #define M_IF_SMP MAS2_M |
| 158 | #else |
| 159 | #define M_IF_SMP 0 |
| 160 | #endif |
| 161 | |
Sebastian Andrzej Siewior | b3df895 | 2010-04-04 22:19:03 +0200 | [diff] [blame] | 162 | #if defined(ENTRY_MAPPING_BOOT_SETUP) |
| 163 | |
Sebastian Andrzej Siewior | 7c08ce7 | 2010-04-04 22:19:02 +0200 | [diff] [blame] | 164 | /* 6. Setup KERNELBASE mapping in TLB1[0] */ |
| 165 | lis r6,0x1000 /* Set MAS0(TLBSEL) = TLB1(1), ESEL = 0 */ |
| 166 | mtspr SPRN_MAS0,r6 |
| 167 | lis r6,(MAS1_VALID|MAS1_IPROT)@h |
| 168 | ori r6,r6,(MAS1_TSIZE(BOOK3E_PAGESZ_64M))@l |
| 169 | mtspr SPRN_MAS1,r6 |
| 170 | lis r6,MAS2_VAL(PAGE_OFFSET, BOOK3E_PAGESZ_64M, M_IF_SMP)@h |
| 171 | ori r6,r6,MAS2_VAL(PAGE_OFFSET, BOOK3E_PAGESZ_64M, M_IF_SMP)@l |
| 172 | mtspr SPRN_MAS2,r6 |
| 173 | mtspr SPRN_MAS3,r8 |
| 174 | tlbwe |
| 175 | |
| 176 | /* 7. Jump to KERNELBASE mapping */ |
| 177 | lis r6,(KERNELBASE & ~0xfff)@h |
| 178 | ori r6,r6,(KERNELBASE & ~0xfff)@l |
Sebastian Andrzej Siewior | b3df895 | 2010-04-04 22:19:03 +0200 | [diff] [blame] | 179 | |
| 180 | #elif defined(ENTRY_MAPPING_KEXEC_SETUP) |
| 181 | /* |
| 182 | * 6. Setup a 1:1 mapping in TLB1. Esel 0 is unsued, 1 or 2 contains the tmp |
| 183 | * mapping so we start at 3. We setup 8 mappings, each 256MiB in size. This |
| 184 | * will cover the first 2GiB of memory. |
| 185 | */ |
| 186 | |
| 187 | lis r10, (MAS1_VALID|MAS1_IPROT)@h |
| 188 | ori r10,r10, (MAS1_TSIZE(BOOK3E_PAGESZ_256M))@l |
| 189 | li r11, 0 |
| 190 | li r0, 8 |
| 191 | mtctr r0 |
| 192 | |
| 193 | next_tlb_setup: |
| 194 | addi r0, r11, 3 |
| 195 | rlwinm r0, r0, 16, 4, 15 // Compute esel |
| 196 | rlwinm r9, r11, 28, 0, 3 // Compute [ER]PN |
| 197 | oris r0, r0, (MAS0_TLBSEL(1))@h |
| 198 | mtspr SPRN_MAS0,r0 |
| 199 | mtspr SPRN_MAS1,r10 |
| 200 | mtspr SPRN_MAS2,r9 |
| 201 | ori r9, r9, (MAS3_SX|MAS3_SW|MAS3_SR) |
| 202 | mtspr SPRN_MAS3,r9 |
| 203 | tlbwe |
| 204 | addi r11, r11, 1 |
| 205 | bdnz+ next_tlb_setup |
| 206 | |
| 207 | /* 7. Jump to our 1:1 mapping */ |
| 208 | li r6, 0 |
| 209 | |
| 210 | #else |
| 211 | #error You need to specify the mapping or not use this at all. |
| 212 | #endif |
| 213 | |
Sebastian Andrzej Siewior | 7c08ce7 | 2010-04-04 22:19:02 +0200 | [diff] [blame] | 214 | lis r7,MSR_KERNEL@h |
| 215 | ori r7,r7,MSR_KERNEL@l |
| 216 | bl 1f /* Find our address */ |
| 217 | 1: mflr r9 |
| 218 | rlwimi r6,r9,0,20,31 |
| 219 | addi r6,r6,(2f - 1b) |
| 220 | add r6, r6, r25 |
| 221 | mtspr SPRN_SRR0,r6 |
| 222 | mtspr SPRN_SRR1,r7 |
| 223 | rfi /* start execution out of TLB1[0] entry */ |
| 224 | |
| 225 | /* 8. Clear out the temp mapping */ |
| 226 | 2: lis r7,0x1000 /* Set MAS0(TLBSEL) = 1 */ |
| 227 | rlwimi r7,r5,16,4,15 /* Setup MAS0 = TLBSEL | ESEL(r5) */ |
| 228 | mtspr SPRN_MAS0,r7 |
| 229 | tlbre |
| 230 | mfspr r8,SPRN_MAS1 |
| 231 | rlwinm r8,r8,0,2,0 /* clear IPROT */ |
| 232 | mtspr SPRN_MAS1,r8 |
| 233 | tlbwe |
| 234 | /* Invalidate TLB1 */ |
| 235 | li r9,0x0c |
| 236 | tlbivax 0,r9 |
| 237 | TLBSYNC |