1	/* This is trivial with the new code... */
2	.globl		do_fpdis
3	.type		do_fpdis,#function
4do_fpdis:
5	sethi		%hi(TSTATE_PEF), %g4
6	rdpr		%tstate, %g5
7	andcc		%g5, %g4, %g0
8	be,pt		%xcc, 1f
9	 nop
10	rd		%fprs, %g5
11	andcc		%g5, FPRS_FEF, %g0
12	be,pt		%xcc, 1f
13	 nop
14
15	/* Legal state when DCR_IFPOE is set in Cheetah %dcr. */
16	sethi		%hi(109f), %g7
17	ba,pt		%xcc, etrap
18109:	 or		%g7, %lo(109b), %g7
19	add		%g0, %g0, %g0
20	ba,a,pt		%xcc, rtrap
21
221:	TRAP_LOAD_THREAD_REG(%g6, %g1)
23	ldub		[%g6 + TI_FPSAVED], %g5
24	wr		%g0, FPRS_FEF, %fprs
25	andcc		%g5, FPRS_FEF, %g0
26	be,a,pt		%icc, 1f
27	 clr		%g7
28	ldx		[%g6 + TI_GSR], %g7
291:	andcc		%g5, FPRS_DL, %g0
30	bne,pn		%icc, 2f
31	 fzero		%f0
32	andcc		%g5, FPRS_DU, %g0
33	bne,pn		%icc, 1f
34	 fzero		%f2
35	faddd		%f0, %f2, %f4
36	fmuld		%f0, %f2, %f6
37	faddd		%f0, %f2, %f8
38	fmuld		%f0, %f2, %f10
39	faddd		%f0, %f2, %f12
40	fmuld		%f0, %f2, %f14
41	faddd		%f0, %f2, %f16
42	fmuld		%f0, %f2, %f18
43	faddd		%f0, %f2, %f20
44	fmuld		%f0, %f2, %f22
45	faddd		%f0, %f2, %f24
46	fmuld		%f0, %f2, %f26
47	faddd		%f0, %f2, %f28
48	fmuld		%f0, %f2, %f30
49	faddd		%f0, %f2, %f32
50	fmuld		%f0, %f2, %f34
51	faddd		%f0, %f2, %f36
52	fmuld		%f0, %f2, %f38
53	faddd		%f0, %f2, %f40
54	fmuld		%f0, %f2, %f42
55	faddd		%f0, %f2, %f44
56	fmuld		%f0, %f2, %f46
57	faddd		%f0, %f2, %f48
58	fmuld		%f0, %f2, %f50
59	faddd		%f0, %f2, %f52
60	fmuld		%f0, %f2, %f54
61	faddd		%f0, %f2, %f56
62	fmuld		%f0, %f2, %f58
63	b,pt		%xcc, fpdis_exit2
64	 faddd		%f0, %f2, %f60
651:	mov		SECONDARY_CONTEXT, %g3
66	add		%g6, TI_FPREGS + 0x80, %g1
67	faddd		%f0, %f2, %f4
68	fmuld		%f0, %f2, %f6
69
70661:	ldxa		[%g3] ASI_DMMU, %g5
71	.section	.sun4v_1insn_patch, "ax"
72	.word		661b
73	ldxa		[%g3] ASI_MMU, %g5
74	.previous
75
76	sethi		%hi(sparc64_kern_sec_context), %g2
77	ldx		[%g2 + %lo(sparc64_kern_sec_context)], %g2
78
79661:	stxa		%g2, [%g3] ASI_DMMU
80	.section	.sun4v_1insn_patch, "ax"
81	.word		661b
82	stxa		%g2, [%g3] ASI_MMU
83	.previous
84
85	membar		#Sync
86	add		%g6, TI_FPREGS + 0xc0, %g2
87	faddd		%f0, %f2, %f8
88	fmuld		%f0, %f2, %f10
89	membar		#Sync
90	ldda		[%g1] ASI_BLK_S, %f32
91	ldda		[%g2] ASI_BLK_S, %f48
92	membar		#Sync
93	faddd		%f0, %f2, %f12
94	fmuld		%f0, %f2, %f14
95	faddd		%f0, %f2, %f16
96	fmuld		%f0, %f2, %f18
97	faddd		%f0, %f2, %f20
98	fmuld		%f0, %f2, %f22
99	faddd		%f0, %f2, %f24
100	fmuld		%f0, %f2, %f26
101	faddd		%f0, %f2, %f28
102	fmuld		%f0, %f2, %f30
103	b,pt		%xcc, fpdis_exit
104	 nop
1052:	andcc		%g5, FPRS_DU, %g0
106	bne,pt		%icc, 3f
107	 fzero		%f32
108	mov		SECONDARY_CONTEXT, %g3
109	fzero		%f34
110
111661:	ldxa		[%g3] ASI_DMMU, %g5
112	.section	.sun4v_1insn_patch, "ax"
113	.word		661b
114	ldxa		[%g3] ASI_MMU, %g5
115	.previous
116
117	add		%g6, TI_FPREGS, %g1
118	sethi		%hi(sparc64_kern_sec_context), %g2
119	ldx		[%g2 + %lo(sparc64_kern_sec_context)], %g2
120
121661:	stxa		%g2, [%g3] ASI_DMMU
122	.section	.sun4v_1insn_patch, "ax"
123	.word		661b
124	stxa		%g2, [%g3] ASI_MMU
125	.previous
126
127	membar		#Sync
128	add		%g6, TI_FPREGS + 0x40, %g2
129	faddd		%f32, %f34, %f36
130	fmuld		%f32, %f34, %f38
131	membar		#Sync
132	ldda		[%g1] ASI_BLK_S, %f0
133	ldda		[%g2] ASI_BLK_S, %f16
134	membar		#Sync
135	faddd		%f32, %f34, %f40
136	fmuld		%f32, %f34, %f42
137	faddd		%f32, %f34, %f44
138	fmuld		%f32, %f34, %f46
139	faddd		%f32, %f34, %f48
140	fmuld		%f32, %f34, %f50
141	faddd		%f32, %f34, %f52
142	fmuld		%f32, %f34, %f54
143	faddd		%f32, %f34, %f56
144	fmuld		%f32, %f34, %f58
145	faddd		%f32, %f34, %f60
146	fmuld		%f32, %f34, %f62
147	ba,pt		%xcc, fpdis_exit
148	 nop
1493:	mov		SECONDARY_CONTEXT, %g3
150	add		%g6, TI_FPREGS, %g1
151
152661:	ldxa		[%g3] ASI_DMMU, %g5
153	.section	.sun4v_1insn_patch, "ax"
154	.word		661b
155	ldxa		[%g3] ASI_MMU, %g5
156	.previous
157
158	sethi		%hi(sparc64_kern_sec_context), %g2
159	ldx		[%g2 + %lo(sparc64_kern_sec_context)], %g2
160
161661:	stxa		%g2, [%g3] ASI_DMMU
162	.section	.sun4v_1insn_patch, "ax"
163	.word		661b
164	stxa		%g2, [%g3] ASI_MMU
165	.previous
166
167	membar		#Sync
168	mov		0x40, %g2
169	membar		#Sync
170	ldda		[%g1] ASI_BLK_S, %f0
171	ldda		[%g1 + %g2] ASI_BLK_S, %f16
172	add		%g1, 0x80, %g1
173	ldda		[%g1] ASI_BLK_S, %f32
174	ldda		[%g1 + %g2] ASI_BLK_S, %f48
175	membar		#Sync
176fpdis_exit:
177
178661:	stxa		%g5, [%g3] ASI_DMMU
179	.section	.sun4v_1insn_patch, "ax"
180	.word		661b
181	stxa		%g5, [%g3] ASI_MMU
182	.previous
183
184	membar		#Sync
185fpdis_exit2:
186	wr		%g7, 0, %gsr
187	ldx		[%g6 + TI_XFSR], %fsr
188	rdpr		%tstate, %g3
189	or		%g3, %g4, %g3		! anal...
190	wrpr		%g3, %tstate
191	wr		%g0, FPRS_FEF, %fprs	! clean DU/DL bits
192	retry
193	.size		do_fpdis,.-do_fpdis
194
195	.align		32
196	.type		fp_other_bounce,#function
197fp_other_bounce:
198	call		do_fpother
199	 add		%sp, PTREGS_OFF, %o0
200	ba,pt		%xcc, rtrap
201	 nop
202	.size		fp_other_bounce,.-fp_other_bounce
203
204	.align		32
205	.globl		do_fpother_check_fitos
206	.type		do_fpother_check_fitos,#function
207do_fpother_check_fitos:
208	TRAP_LOAD_THREAD_REG(%g6, %g1)
209	sethi		%hi(fp_other_bounce - 4), %g7
210	or		%g7, %lo(fp_other_bounce - 4), %g7
211
212	/* NOTE: Need to preserve %g7 until we fully commit
213	 *       to the fitos fixup.
214	 */
215	stx		%fsr, [%g6 + TI_XFSR]
216	rdpr		%tstate, %g3
217	andcc		%g3, TSTATE_PRIV, %g0
218	bne,pn		%xcc, do_fptrap_after_fsr
219	 nop
220	ldx		[%g6 + TI_XFSR], %g3
221	srlx		%g3, 14, %g1
222	and		%g1, 7, %g1
223	cmp		%g1, 2			! Unfinished FP-OP
224	bne,pn		%xcc, do_fptrap_after_fsr
225	 sethi		%hi(1 << 23), %g1	! Inexact
226	andcc		%g3, %g1, %g0
227	bne,pn		%xcc, do_fptrap_after_fsr
228	 rdpr		%tpc, %g1
229	lduwa		[%g1] ASI_AIUP, %g3	! This cannot ever fail
230#define FITOS_MASK	0xc1f83fe0
231#define FITOS_COMPARE	0x81a01880
232	sethi		%hi(FITOS_MASK), %g1
233	or		%g1, %lo(FITOS_MASK), %g1
234	and		%g3, %g1, %g1
235	sethi		%hi(FITOS_COMPARE), %g2
236	or		%g2, %lo(FITOS_COMPARE), %g2
237	cmp		%g1, %g2
238	bne,pn		%xcc, do_fptrap_after_fsr
239	 nop
240	std		%f62, [%g6 + TI_FPREGS + (62 * 4)]
241	sethi		%hi(fitos_table_1), %g1
242	and		%g3, 0x1f, %g2
243	or		%g1, %lo(fitos_table_1),  %g1
244	sllx		%g2, 2, %g2
245	jmpl		%g1 + %g2, %g0
246	 ba,pt		%xcc, fitos_emul_continue
247
248fitos_table_1:
249	fitod		%f0, %f62
250	fitod		%f1, %f62
251	fitod		%f2, %f62
252	fitod		%f3, %f62
253	fitod		%f4, %f62
254	fitod		%f5, %f62
255	fitod		%f6, %f62
256	fitod		%f7, %f62
257	fitod		%f8, %f62
258	fitod		%f9, %f62
259	fitod		%f10, %f62
260	fitod		%f11, %f62
261	fitod		%f12, %f62
262	fitod		%f13, %f62
263	fitod		%f14, %f62
264	fitod		%f15, %f62
265	fitod		%f16, %f62
266	fitod		%f17, %f62
267	fitod		%f18, %f62
268	fitod		%f19, %f62
269	fitod		%f20, %f62
270	fitod		%f21, %f62
271	fitod		%f22, %f62
272	fitod		%f23, %f62
273	fitod		%f24, %f62
274	fitod		%f25, %f62
275	fitod		%f26, %f62
276	fitod		%f27, %f62
277	fitod		%f28, %f62
278	fitod		%f29, %f62
279	fitod		%f30, %f62
280	fitod		%f31, %f62
281
282fitos_emul_continue:
283	sethi		%hi(fitos_table_2), %g1
284	srl		%g3, 25, %g2
285	or		%g1, %lo(fitos_table_2), %g1
286	and		%g2, 0x1f, %g2
287	sllx		%g2, 2, %g2
288	jmpl		%g1 + %g2, %g0
289	 ba,pt		%xcc, fitos_emul_fini
290
291fitos_table_2:
292	fdtos		%f62, %f0
293	fdtos		%f62, %f1
294	fdtos		%f62, %f2
295	fdtos		%f62, %f3
296	fdtos		%f62, %f4
297	fdtos		%f62, %f5
298	fdtos		%f62, %f6
299	fdtos		%f62, %f7
300	fdtos		%f62, %f8
301	fdtos		%f62, %f9
302	fdtos		%f62, %f10
303	fdtos		%f62, %f11
304	fdtos		%f62, %f12
305	fdtos		%f62, %f13
306	fdtos		%f62, %f14
307	fdtos		%f62, %f15
308	fdtos		%f62, %f16
309	fdtos		%f62, %f17
310	fdtos		%f62, %f18
311	fdtos		%f62, %f19
312	fdtos		%f62, %f20
313	fdtos		%f62, %f21
314	fdtos		%f62, %f22
315	fdtos		%f62, %f23
316	fdtos		%f62, %f24
317	fdtos		%f62, %f25
318	fdtos		%f62, %f26
319	fdtos		%f62, %f27
320	fdtos		%f62, %f28
321	fdtos		%f62, %f29
322	fdtos		%f62, %f30
323	fdtos		%f62, %f31
324
325fitos_emul_fini:
326	ldd		[%g6 + TI_FPREGS + (62 * 4)], %f62
327	done
328	.size		do_fpother_check_fitos,.-do_fpother_check_fitos
329
330	.align		32
331	.globl		do_fptrap
332	.type		do_fptrap,#function
333do_fptrap:
334	TRAP_LOAD_THREAD_REG(%g6, %g1)
335	stx		%fsr, [%g6 + TI_XFSR]
336do_fptrap_after_fsr:
337	ldub		[%g6 + TI_FPSAVED], %g3
338	rd		%fprs, %g1
339	or		%g3, %g1, %g3
340	stb		%g3, [%g6 + TI_FPSAVED]
341	rd		%gsr, %g3
342	stx		%g3, [%g6 + TI_GSR]
343	mov		SECONDARY_CONTEXT, %g3
344
345661:	ldxa		[%g3] ASI_DMMU, %g5
346	.section	.sun4v_1insn_patch, "ax"
347	.word		661b
348	ldxa		[%g3] ASI_MMU, %g5
349	.previous
350
351	sethi		%hi(sparc64_kern_sec_context), %g2
352	ldx		[%g2 + %lo(sparc64_kern_sec_context)], %g2
353
354661:	stxa		%g2, [%g3] ASI_DMMU
355	.section	.sun4v_1insn_patch, "ax"
356	.word		661b
357	stxa		%g2, [%g3] ASI_MMU
358	.previous
359
360	membar		#Sync
361	add		%g6, TI_FPREGS, %g2
362	andcc		%g1, FPRS_DL, %g0
363	be,pn		%icc, 4f
364	 mov		0x40, %g3
365	stda		%f0, [%g2] ASI_BLK_S
366	stda		%f16, [%g2 + %g3] ASI_BLK_S
367	andcc		%g1, FPRS_DU, %g0
368	be,pn		%icc, 5f
3694:       add		%g2, 128, %g2
370	stda		%f32, [%g2] ASI_BLK_S
371	stda		%f48, [%g2 + %g3] ASI_BLK_S
3725:	mov		SECONDARY_CONTEXT, %g1
373	membar		#Sync
374
375661:	stxa		%g5, [%g1] ASI_DMMU
376	.section	.sun4v_1insn_patch, "ax"
377	.word		661b
378	stxa		%g5, [%g1] ASI_MMU
379	.previous
380
381	membar		#Sync
382	ba,pt		%xcc, etrap
383	 wr		%g0, 0, %fprs
384	.size		do_fptrap,.-do_fptrap
385