1/*
2 * Copyright (C) 1999  Eddie C. Dost  (ecd@atecom.com)
3 */
4
5#include <linux/types.h>
6#include <linux/sched.h>
7
8#include <asm/uaccess.h>
9#include <asm/reg.h>
10#include <asm/switch_to.h>
11
12#include <asm/sfp-machine.h>
13#include <math-emu/double.h>
14
15#define FLOATFUNC(x)	extern int x(void *, void *, void *, void *)
16
17/* The instructions list which may be not implemented by a hardware FPU */
18FLOATFUNC(fre);
19FLOATFUNC(frsqrtes);
20FLOATFUNC(fsqrt);
21FLOATFUNC(fsqrts);
22FLOATFUNC(mtfsf);
23FLOATFUNC(mtfsfi);
24
25#ifdef CONFIG_MATH_EMULATION_HW_UNIMPLEMENTED
26#undef FLOATFUNC(x)
27#define FLOATFUNC(x)	static inline int x(void *op1, void *op2, void *op3, \
28						 void *op4) { }
29#endif
30
31FLOATFUNC(fadd);
32FLOATFUNC(fadds);
33FLOATFUNC(fdiv);
34FLOATFUNC(fdivs);
35FLOATFUNC(fmul);
36FLOATFUNC(fmuls);
37FLOATFUNC(fsub);
38FLOATFUNC(fsubs);
39
40FLOATFUNC(fmadd);
41FLOATFUNC(fmadds);
42FLOATFUNC(fmsub);
43FLOATFUNC(fmsubs);
44FLOATFUNC(fnmadd);
45FLOATFUNC(fnmadds);
46FLOATFUNC(fnmsub);
47FLOATFUNC(fnmsubs);
48
49FLOATFUNC(fctiw);
50FLOATFUNC(fctiwz);
51FLOATFUNC(frsp);
52
53FLOATFUNC(fcmpo);
54FLOATFUNC(fcmpu);
55
56FLOATFUNC(mcrfs);
57FLOATFUNC(mffs);
58FLOATFUNC(mtfsb0);
59FLOATFUNC(mtfsb1);
60
61FLOATFUNC(lfd);
62FLOATFUNC(lfs);
63
64FLOATFUNC(stfd);
65FLOATFUNC(stfs);
66FLOATFUNC(stfiwx);
67
68FLOATFUNC(fabs);
69FLOATFUNC(fmr);
70FLOATFUNC(fnabs);
71FLOATFUNC(fneg);
72
73/* Optional */
74FLOATFUNC(fres);
75FLOATFUNC(frsqrte);
76FLOATFUNC(fsel);
77
78
79#define OP31		0x1f		/*   31 */
80#define LFS		0x30		/*   48 */
81#define LFSU		0x31		/*   49 */
82#define LFD		0x32		/*   50 */
83#define LFDU		0x33		/*   51 */
84#define STFS		0x34		/*   52 */
85#define STFSU		0x35		/*   53 */
86#define STFD		0x36		/*   54 */
87#define STFDU		0x37		/*   55 */
88#define OP59		0x3b		/*   59 */
89#define OP63		0x3f		/*   63 */
90
91/* Opcode 31: */
92/* X-Form: */
93#define LFSX		0x217		/*  535 */
94#define LFSUX		0x237		/*  567 */
95#define LFDX		0x257		/*  599 */
96#define LFDUX		0x277		/*  631 */
97#define STFSX		0x297		/*  663 */
98#define STFSUX		0x2b7		/*  695 */
99#define STFDX		0x2d7		/*  727 */
100#define STFDUX		0x2f7		/*  759 */
101#define STFIWX		0x3d7		/*  983 */
102
103/* Opcode 59: */
104/* A-Form: */
105#define FDIVS		0x012		/*   18 */
106#define FSUBS		0x014		/*   20 */
107#define FADDS		0x015		/*   21 */
108#define FSQRTS		0x016		/*   22 */
109#define FRES		0x018		/*   24 */
110#define FMULS		0x019		/*   25 */
111#define FRSQRTES	0x01a		/*   26 */
112#define FMSUBS		0x01c		/*   28 */
113#define FMADDS		0x01d		/*   29 */
114#define FNMSUBS		0x01e		/*   30 */
115#define FNMADDS		0x01f		/*   31 */
116
117/* Opcode 63: */
118/* A-Form: */
119#define FDIV		0x012		/*   18 */
120#define FSUB		0x014		/*   20 */
121#define FADD		0x015		/*   21 */
122#define FSQRT		0x016		/*   22 */
123#define FSEL		0x017		/*   23 */
124#define FRE		0x018		/*   24 */
125#define FMUL		0x019		/*   25 */
126#define FRSQRTE		0x01a		/*   26 */
127#define FMSUB		0x01c		/*   28 */
128#define FMADD		0x01d		/*   29 */
129#define FNMSUB		0x01e		/*   30 */
130#define FNMADD		0x01f		/*   31 */
131
132/* X-Form: */
133#define FCMPU		0x000		/*    0	*/
134#define FRSP		0x00c		/*   12 */
135#define FCTIW		0x00e		/*   14 */
136#define FCTIWZ		0x00f		/*   15 */
137#define FCMPO		0x020		/*   32 */
138#define MTFSB1		0x026		/*   38 */
139#define FNEG		0x028		/*   40 */
140#define MCRFS		0x040		/*   64 */
141#define MTFSB0		0x046		/*   70 */
142#define FMR		0x048		/*   72 */
143#define MTFSFI		0x086		/*  134 */
144#define FNABS		0x088		/*  136 */
145#define FABS		0x108		/*  264 */
146#define MFFS		0x247		/*  583 */
147#define MTFSF		0x2c7		/*  711 */
148
149
150#define AB	2
151#define AC	3
152#define ABC	4
153#define D	5
154#define DU	6
155#define X	7
156#define XA	8
157#define XB	9
158#define XCR	11
159#define XCRB	12
160#define XCRI	13
161#define XCRL	16
162#define XE	14
163#define XEU	15
164#define XFLB	10
165
166static int
167record_exception(struct pt_regs *regs, int eflag)
168{
169	u32 fpscr;
170
171	fpscr = __FPU_FPSCR;
172
173	if (eflag) {
174		fpscr |= FPSCR_FX;
175		if (eflag & EFLAG_OVERFLOW)
176			fpscr |= FPSCR_OX;
177		if (eflag & EFLAG_UNDERFLOW)
178			fpscr |= FPSCR_UX;
179		if (eflag & EFLAG_DIVZERO)
180			fpscr |= FPSCR_ZX;
181		if (eflag & EFLAG_INEXACT)
182			fpscr |= FPSCR_XX;
183		if (eflag & EFLAG_INVALID)
184			fpscr |= FPSCR_VX;
185		if (eflag & EFLAG_VXSNAN)
186			fpscr |= FPSCR_VXSNAN;
187		if (eflag & EFLAG_VXISI)
188			fpscr |= FPSCR_VXISI;
189		if (eflag & EFLAG_VXIDI)
190			fpscr |= FPSCR_VXIDI;
191		if (eflag & EFLAG_VXZDZ)
192			fpscr |= FPSCR_VXZDZ;
193		if (eflag & EFLAG_VXIMZ)
194			fpscr |= FPSCR_VXIMZ;
195		if (eflag & EFLAG_VXVC)
196			fpscr |= FPSCR_VXVC;
197		if (eflag & EFLAG_VXSOFT)
198			fpscr |= FPSCR_VXSOFT;
199		if (eflag & EFLAG_VXSQRT)
200			fpscr |= FPSCR_VXSQRT;
201		if (eflag & EFLAG_VXCVI)
202			fpscr |= FPSCR_VXCVI;
203	}
204
205//	fpscr &= ~(FPSCR_VX);
206	if (fpscr & (FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI |
207		     FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC |
208		     FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI))
209		fpscr |= FPSCR_VX;
210
211	fpscr &= ~(FPSCR_FEX);
212	if (((fpscr & FPSCR_VX) && (fpscr & FPSCR_VE)) ||
213	    ((fpscr & FPSCR_OX) && (fpscr & FPSCR_OE)) ||
214	    ((fpscr & FPSCR_UX) && (fpscr & FPSCR_UE)) ||
215	    ((fpscr & FPSCR_ZX) && (fpscr & FPSCR_ZE)) ||
216	    ((fpscr & FPSCR_XX) && (fpscr & FPSCR_XE)))
217		fpscr |= FPSCR_FEX;
218
219	__FPU_FPSCR = fpscr;
220
221	return (fpscr & FPSCR_FEX) ? 1 : 0;
222}
223
224int
225do_mathemu(struct pt_regs *regs)
226{
227	void *op0 = 0, *op1 = 0, *op2 = 0, *op3 = 0;
228	unsigned long pc = regs->nip;
229	signed short sdisp;
230	u32 insn = 0;
231	int idx = 0;
232	int (*func)(void *, void *, void *, void *);
233	int type = 0;
234	int eflag, trap;
235
236	if (get_user(insn, (u32 *)pc))
237		return -EFAULT;
238
239	switch (insn >> 26) {
240	case LFS:	func = lfs;	type = D;	break;
241	case LFSU:	func = lfs;	type = DU;	break;
242	case LFD:	func = lfd;	type = D;	break;
243	case LFDU:	func = lfd;	type = DU;	break;
244	case STFS:	func = stfs;	type = D;	break;
245	case STFSU:	func = stfs;	type = DU;	break;
246	case STFD:	func = stfd;	type = D;	break;
247	case STFDU:	func = stfd;	type = DU;	break;
248
249	case OP31:
250		switch ((insn >> 1) & 0x3ff) {
251		case LFSX:	func = lfs;	type = XE;	break;
252		case LFSUX:	func = lfs;	type = XEU;	break;
253		case LFDX:	func = lfd;	type = XE;	break;
254		case LFDUX:	func = lfd;	type = XEU;	break;
255		case STFSX:	func = stfs;	type = XE;	break;
256		case STFSUX:	func = stfs;	type = XEU;	break;
257		case STFDX:	func = stfd;	type = XE;	break;
258		case STFDUX:	func = stfd;	type = XEU;	break;
259		case STFIWX:	func = stfiwx;	type = XE;	break;
260		default:
261			goto illegal;
262		}
263		break;
264
265	case OP59:
266		switch ((insn >> 1) & 0x1f) {
267		case FDIVS:	func = fdivs;	type = AB;	break;
268		case FSUBS:	func = fsubs;	type = AB;	break;
269		case FADDS:	func = fadds;	type = AB;	break;
270		case FSQRTS:	func = fsqrts;	type = XB;	break;
271		case FRES:	func = fres;	type = XB;	break;
272		case FMULS:	func = fmuls;	type = AC;	break;
273		case FRSQRTES:	func = frsqrtes;type = XB;	break;
274		case FMSUBS:	func = fmsubs;	type = ABC;	break;
275		case FMADDS:	func = fmadds;	type = ABC;	break;
276		case FNMSUBS:	func = fnmsubs;	type = ABC;	break;
277		case FNMADDS:	func = fnmadds;	type = ABC;	break;
278		default:
279			goto illegal;
280		}
281		break;
282
283	case OP63:
284		if (insn & 0x20) {
285			switch ((insn >> 1) & 0x1f) {
286			case FDIV:	func = fdiv;	type = AB;	break;
287			case FSUB:	func = fsub;	type = AB;	break;
288			case FADD:	func = fadd;	type = AB;	break;
289			case FSQRT:	func = fsqrt;	type = XB;	break;
290			case FRE:	func = fre;	type = XB;	break;
291			case FSEL:	func = fsel;	type = ABC;	break;
292			case FMUL:	func = fmul;	type = AC;	break;
293			case FRSQRTE:	func = frsqrte;	type = XB;	break;
294			case FMSUB:	func = fmsub;	type = ABC;	break;
295			case FMADD:	func = fmadd;	type = ABC;	break;
296			case FNMSUB:	func = fnmsub;	type = ABC;	break;
297			case FNMADD:	func = fnmadd;	type = ABC;	break;
298			default:
299				goto illegal;
300			}
301			break;
302		}
303
304		switch ((insn >> 1) & 0x3ff) {
305		case FCMPU:	func = fcmpu;	type = XCR;	break;
306		case FRSP:	func = frsp;	type = XB;	break;
307		case FCTIW:	func = fctiw;	type = XB;	break;
308		case FCTIWZ:	func = fctiwz;	type = XB;	break;
309		case FCMPO:	func = fcmpo;	type = XCR;	break;
310		case MTFSB1:	func = mtfsb1;	type = XCRB;	break;
311		case FNEG:	func = fneg;	type = XB;	break;
312		case MCRFS:	func = mcrfs;	type = XCRL;	break;
313		case MTFSB0:	func = mtfsb0;	type = XCRB;	break;
314		case FMR:	func = fmr;	type = XB;	break;
315		case MTFSFI:	func = mtfsfi;	type = XCRI;	break;
316		case FNABS:	func = fnabs;	type = XB;	break;
317		case FABS:	func = fabs;	type = XB;	break;
318		case MFFS:	func = mffs;	type = X;	break;
319		case MTFSF:	func = mtfsf;	type = XFLB;	break;
320		default:
321			goto illegal;
322		}
323		break;
324
325	default:
326		goto illegal;
327	}
328
329	switch (type) {
330	case AB:
331		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
332		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
333		op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
334		break;
335
336	case AC:
337		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
338		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
339		op2 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
340		break;
341
342	case ABC:
343		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
344		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
345		op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
346		op3 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
347		break;
348
349	case D:
350		idx = (insn >> 16) & 0x1f;
351		sdisp = (insn & 0xffff);
352		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
353		op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp);
354		break;
355
356	case DU:
357		idx = (insn >> 16) & 0x1f;
358		if (!idx)
359			goto illegal;
360
361		sdisp = (insn & 0xffff);
362		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
363		op1 = (void *)(regs->gpr[idx] + sdisp);
364		break;
365
366	case X:
367		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
368		break;
369
370	case XA:
371		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
372		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
373		break;
374
375	case XB:
376		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
377		op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
378		break;
379
380	case XE:
381		idx = (insn >> 16) & 0x1f;
382		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
383		op1 = (void *)((idx ? regs->gpr[idx] : 0)
384				+ regs->gpr[(insn >> 11) & 0x1f]);
385		break;
386
387	case XEU:
388		idx = (insn >> 16) & 0x1f;
389		if (!idx)
390			goto illegal;
391		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
392		op1 = (void *)(regs->gpr[idx]
393				+ regs->gpr[(insn >> 11) & 0x1f]);
394		break;
395
396	case XCR:
397		op0 = (void *)&regs->ccr;
398		op1 = (void *)((insn >> 23) & 0x7);
399		op2 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
400		op3 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
401		break;
402
403	case XCRL:
404		op0 = (void *)&regs->ccr;
405		op1 = (void *)((insn >> 23) & 0x7);
406		op2 = (void *)((insn >> 18) & 0x7);
407		break;
408
409	case XCRB:
410		op0 = (void *)((insn >> 21) & 0x1f);
411		break;
412
413	case XCRI:
414		op0 = (void *)((insn >> 23) & 0x7);
415		op1 = (void *)((insn >> 12) & 0xf);
416		break;
417
418	case XFLB:
419		op0 = (void *)((insn >> 17) & 0xff);
420		op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
421		break;
422
423	default:
424		goto illegal;
425	}
426
427	/*
428	 * If we support a HW FPU, we need to ensure the FP state
429	 * is flushed into the thread_struct before attempting
430	 * emulation
431	 */
432	flush_fp_to_thread(current);
433
434	eflag = func(op0, op1, op2, op3);
435
436	if (insn & 1) {
437		regs->ccr &= ~(0x0f000000);
438		regs->ccr |= (__FPU_FPSCR >> 4) & 0x0f000000;
439	}
440
441	trap = record_exception(regs, eflag);
442	if (trap)
443		return 1;
444
445	switch (type) {
446	case DU:
447	case XEU:
448		regs->gpr[idx] = (unsigned long)op1;
449		break;
450
451	default:
452		break;
453	}
454
455	regs->nip += 4;
456	return 0;
457
458illegal:
459	return -ENOSYS;
460}
461