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

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