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