1
2/* 1. Find the index of the entry we're executing in */
3	bl	invstr				/* Find our address */
4invstr:	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
35match_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 */
481:	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
59skpinv:	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 */
881:	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 */
1201:	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
1262:
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 */
1412:
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/*
156 * The mapping only needs to be cache-coherent on SMP, except on
157 * Freescale e500mc derivatives where it's also needed for coherent DMA.
158 */
159#if defined(CONFIG_SMP) || defined(CONFIG_PPC_E500MC)
160#define M_IF_NEEDED	MAS2_M
161#else
162#define M_IF_NEEDED	0
163#endif
164
165#if defined(ENTRY_MAPPING_BOOT_SETUP)
166
167/* 6. Setup KERNELBASE mapping in TLB1[0] */
168	lis	r6,0x1000		/* Set MAS0(TLBSEL) = TLB1(1), ESEL = 0 */
169	mtspr	SPRN_MAS0,r6
170	lis	r6,(MAS1_VALID|MAS1_IPROT)@h
171	ori	r6,r6,(MAS1_TSIZE(BOOK3E_PAGESZ_64M))@l
172	mtspr	SPRN_MAS1,r6
173	lis	r6,MAS2_VAL(PAGE_OFFSET, BOOK3E_PAGESZ_64M, M_IF_NEEDED)@h
174	ori	r6,r6,MAS2_VAL(PAGE_OFFSET, BOOK3E_PAGESZ_64M, M_IF_NEEDED)@l
175	mtspr	SPRN_MAS2,r6
176	mtspr	SPRN_MAS3,r8
177	tlbwe
178
179/* 7. Jump to KERNELBASE mapping */
180	lis	r6,(KERNELBASE & ~0xfff)@h
181	ori	r6,r6,(KERNELBASE & ~0xfff)@l
182	rlwinm	r7,r25,0,0x03ffffff
183	add	r6,r7,r6
184
185#elif defined(ENTRY_MAPPING_KEXEC_SETUP)
186/*
187 * 6. Setup a 1:1 mapping in TLB1. Esel 0 is unsued, 1 or 2 contains the tmp
188 * mapping so we start at 3. We setup 8 mappings, each 256MiB in size. This
189 * will cover the first 2GiB of memory.
190 */
191
192	lis r10, (MAS1_VALID|MAS1_IPROT)@h
193	ori r10,r10, (MAS1_TSIZE(BOOK3E_PAGESZ_256M))@l
194	li  r11, 0
195	li  r0, 8
196	mtctr   r0
197
198next_tlb_setup:
199	addi	r0, r11, 3
200	rlwinm  r0, r0, 16, 4, 15  // Compute esel
201	rlwinm  r9, r11, 28, 0, 3   // Compute [ER]PN
202	oris    r0, r0, (MAS0_TLBSEL(1))@h
203	mtspr   SPRN_MAS0,r0
204	mtspr   SPRN_MAS1,r10
205	mtspr   SPRN_MAS2,r9
206	ori r9, r9, (MAS3_SX|MAS3_SW|MAS3_SR)
207	mtspr   SPRN_MAS3,r9
208	tlbwe
209	addi    r11, r11, 1
210	bdnz+   next_tlb_setup
211
212/* 7. Jump to our 1:1 mapping */
213	mr	r6, r25
214#else
215	#error You need to specify the mapping or not use this at all.
216#endif
217
218	lis	r7,MSR_KERNEL@h
219	ori	r7,r7,MSR_KERNEL@l
220	bl	1f			/* Find our address */
2211:	mflr	r9
222	rlwimi	r6,r9,0,20,31
223	addi	r6,r6,(2f - 1b)
224	mtspr	SPRN_SRR0,r6
225	mtspr	SPRN_SRR1,r7
226	rfi				/* start execution out of TLB1[0] entry */
227
228/* 8. Clear out the temp mapping */
2292:	lis	r7,0x1000	/* Set MAS0(TLBSEL) = 1 */
230	rlwimi	r7,r5,16,4,15	/* Setup MAS0 = TLBSEL | ESEL(r5) */
231	mtspr	SPRN_MAS0,r7
232	tlbre
233	mfspr	r8,SPRN_MAS1
234	rlwinm	r8,r8,0,2,0	/* clear IPROT */
235	mtspr	SPRN_MAS1,r8
236	tlbwe
237	/* Invalidate TLB1 */
238	li	r9,0x0c
239	tlbivax 0,r9
240	TLBSYNC
241