fsl_booke_entry_mapping.S 5.4 KB

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