[deliverable/linux.git] / arch / powerpc / math-emu / math.c

/*
 * Copyright (C) 1999  Eddie C. Dost  (ecd@atecom.com)
 */

#include <linux/types.h>
#include <linux/sched.h>

#include <asm/uaccess.h>
#include <asm/reg.h>

#include <asm/sfp-machine.h>
#include <math-emu/double.h>

#define FLOATFUNC(x)	extern int x(void *, void *, void *, void *)

FLOATFUNC(fadd);
FLOATFUNC(fadds);
FLOATFUNC(fdiv);
FLOATFUNC(fdivs);
FLOATFUNC(fmul);
FLOATFUNC(fmuls);
FLOATFUNC(fsub);
FLOATFUNC(fsubs);

FLOATFUNC(fmadd);
FLOATFUNC(fmadds);
FLOATFUNC(fmsub);
FLOATFUNC(fmsubs);
FLOATFUNC(fnmadd);
FLOATFUNC(fnmadds);
FLOATFUNC(fnmsub);
FLOATFUNC(fnmsubs);

FLOATFUNC(fctiw);
FLOATFUNC(fctiwz);
FLOATFUNC(frsp);

FLOATFUNC(fcmpo);
FLOATFUNC(fcmpu);

FLOATFUNC(mcrfs);
FLOATFUNC(mffs);
FLOATFUNC(mtfsb0);
FLOATFUNC(mtfsb1);
FLOATFUNC(mtfsf);
FLOATFUNC(mtfsfi);

FLOATFUNC(lfd);
FLOATFUNC(lfs);

FLOATFUNC(stfd);
FLOATFUNC(stfs);
FLOATFUNC(stfiwx);

FLOATFUNC(fabs);
FLOATFUNC(fmr);
FLOATFUNC(fnabs);
FLOATFUNC(fneg);

/* Optional */
FLOATFUNC(fre);
FLOATFUNC(fres);
FLOATFUNC(frsqrte);
FLOATFUNC(frsqrtes);
FLOATFUNC(fsel);
FLOATFUNC(fsqrt);
FLOATFUNC(fsqrts);


#define OP31		0x1f		/*   31 */
#define LFS		0x30		/*   48 */
#define LFSU		0x31		/*   49 */
#define LFD		0x32		/*   50 */
#define LFDU		0x33		/*   51 */
#define STFS		0x34		/*   52 */
#define STFSU		0x35		/*   53 */
#define STFD		0x36		/*   54 */
#define STFDU		0x37		/*   55 */
#define OP59		0x3b		/*   59 */
#define OP63		0x3f		/*   63 */

/* Opcode 31: */
/* X-Form: */
#define LFSX		0x217		/*  535 */
#define LFSUX		0x237		/*  567 */
#define LFDX		0x257		/*  599 */
#define LFDUX		0x277		/*  631 */
#define STFSX		0x297		/*  663 */
#define STFSUX		0x2b7		/*  695 */
#define STFDX		0x2d7		/*  727 */
#define STFDUX		0x2f7		/*  759 */
#define STFIWX		0x3d7		/*  983 */

/* Opcode 59: */
/* A-Form: */
#define FDIVS		0x012		/*   18 */
#define FSUBS		0x014		/*   20 */
#define FADDS		0x015		/*   21 */
#define FSQRTS		0x016		/*   22 */
#define FRES		0x018		/*   24 */
#define FMULS		0x019		/*   25 */
#define FRSQRTES	0x01a		/*   26 */
#define FMSUBS		0x01c		/*   28 */
#define FMADDS		0x01d		/*   29 */
#define FNMSUBS		0x01e		/*   30 */
#define FNMADDS		0x01f		/*   31 */

/* Opcode 63: */
/* A-Form: */
#define FDIV		0x012		/*   18 */
#define FSUB		0x014		/*   20 */
#define FADD		0x015		/*   21 */
#define FSQRT		0x016		/*   22 */
#define FSEL		0x017		/*   23 */
#define FRE		0x018		/*   24 */
#define FMUL		0x019		/*   25 */
#define FRSQRTE		0x01a		/*   26 */
#define FMSUB		0x01c		/*   28 */
#define FMADD		0x01d		/*   29 */
#define FNMSUB		0x01e		/*   30 */
#define FNMADD		0x01f		/*   31 */

/* X-Form: */
#define FCMPU		0x000		/*    0	*/
#define FRSP		0x00c		/*   12 */
#define FCTIW		0x00e		/*   14 */
#define FCTIWZ		0x00f		/*   15 */
#define FCMPO		0x020		/*   32 */
#define MTFSB1		0x026		/*   38 */
#define FNEG		0x028		/*   40 */
#define MCRFS		0x040		/*   64 */
#define MTFSB0		0x046		/*   70 */
#define FMR		0x048		/*   72 */
#define MTFSFI		0x086		/*  134 */
#define FNABS		0x088		/*  136 */
#define FABS		0x108		/*  264 */
#define MFFS		0x247		/*  583 */
#define MTFSF		0x2c7		/*  711 */


#define AB	2
#define AC	3
#define ABC	4
#define D	5
#define DU	6
#define X	7
#define XA	8
#define XB	9
#define XCR	11
#define XCRB	12
#define XCRI	13
#define XCRL	16
#define XE	14
#define XEU	15
#define XFLB	10

#ifdef CONFIG_MATH_EMULATION
static int
record_exception(struct pt_regs *regs, int eflag)
{
	u32 fpscr;

	fpscr = __FPU_FPSCR;

	if (eflag) {
		fpscr |= FPSCR_FX;
		if (eflag & EFLAG_OVERFLOW)
			fpscr |= FPSCR_OX;
		if (eflag & EFLAG_UNDERFLOW)
			fpscr |= FPSCR_UX;
		if (eflag & EFLAG_DIVZERO)
			fpscr |= FPSCR_ZX;
		if (eflag & EFLAG_INEXACT)
			fpscr |= FPSCR_XX;
		if (eflag & EFLAG_INVALID)
			fpscr |= FPSCR_VX;
		if (eflag & EFLAG_VXSNAN)
			fpscr |= FPSCR_VXSNAN;
		if (eflag & EFLAG_VXISI)
			fpscr |= FPSCR_VXISI;
		if (eflag & EFLAG_VXIDI)
			fpscr |= FPSCR_VXIDI;
		if (eflag & EFLAG_VXZDZ)
			fpscr |= FPSCR_VXZDZ;
		if (eflag & EFLAG_VXIMZ)
			fpscr |= FPSCR_VXIMZ;
		if (eflag & EFLAG_VXVC)
			fpscr |= FPSCR_VXVC;
		if (eflag & EFLAG_VXSOFT)
			fpscr |= FPSCR_VXSOFT;
		if (eflag & EFLAG_VXSQRT)
			fpscr |= FPSCR_VXSQRT;
		if (eflag & EFLAG_VXCVI)
			fpscr |= FPSCR_VXCVI;
	}

//	fpscr &= ~(FPSCR_VX);
	if (fpscr & (FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI |
		     FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC |
		     FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI))
		fpscr |= FPSCR_VX;

	fpscr &= ~(FPSCR_FEX);
	if (((fpscr & FPSCR_VX) && (fpscr & FPSCR_VE)) ||
	    ((fpscr & FPSCR_OX) && (fpscr & FPSCR_OE)) ||
	    ((fpscr & FPSCR_UX) && (fpscr & FPSCR_UE)) ||
	    ((fpscr & FPSCR_ZX) && (fpscr & FPSCR_ZE)) ||
	    ((fpscr & FPSCR_XX) && (fpscr & FPSCR_XE)))
		fpscr |= FPSCR_FEX;

	__FPU_FPSCR = fpscr;

	return (fpscr & FPSCR_FEX) ? 1 : 0;
}
#endif /* CONFIG_MATH_EMULATION */

int
do_mathemu(struct pt_regs *regs)
{
	void *op0 = 0, *op1 = 0, *op2 = 0, *op3 = 0;
	unsigned long pc = regs->nip;
	signed short sdisp;
	u32 insn = 0;
	int idx = 0;
#ifdef CONFIG_MATH_EMULATION
	int (*func)(void *, void *, void *, void *);
	int type = 0;
	int eflag, trap;
#endif

	if (get_user(insn, (u32 *)pc))
		return -EFAULT;

#ifndef CONFIG_MATH_EMULATION
	switch (insn >> 26) {
	case LFD:
		idx = (insn >> 16) & 0x1f;
		sdisp = (insn & 0xffff);
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp);
		lfd(op0, op1, op2, op3);
		break;
	case LFDU:
		idx = (insn >> 16) & 0x1f;
		sdisp = (insn & 0xffff);
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp);
		lfd(op0, op1, op2, op3);
		regs->gpr[idx] = (unsigned long)op1;
		break;
	case STFD:
		idx = (insn >> 16) & 0x1f;
		sdisp = (insn & 0xffff);
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp);
		stfd(op0, op1, op2, op3);
		break;
	case STFDU:
		idx = (insn >> 16) & 0x1f;
		sdisp = (insn & 0xffff);
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp);
		stfd(op0, op1, op2, op3);
		regs->gpr[idx] = (unsigned long)op1;
		break;
	case OP63:
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
		fmr(op0, op1, op2, op3);
		break;
	default:
		goto illegal;
	}
#else /* CONFIG_MATH_EMULATION */
	switch (insn >> 26) {
	case LFS:	func = lfs;	type = D;	break;
	case LFSU:	func = lfs;	type = DU;	break;
	case LFD:	func = lfd;	type = D;	break;
	case LFDU:	func = lfd;	type = DU;	break;
	case STFS:	func = stfs;	type = D;	break;
	case STFSU:	func = stfs;	type = DU;	break;
	case STFD:	func = stfd;	type = D;	break;
	case STFDU:	func = stfd;	type = DU;	break;

	case OP31:
		switch ((insn >> 1) & 0x3ff) {
		case LFSX:	func = lfs;	type = XE;	break;
		case LFSUX:	func = lfs;	type = XEU;	break;
		case LFDX:	func = lfd;	type = XE;	break;
		case LFDUX:	func = lfd;	type = XEU;	break;
		case STFSX:	func = stfs;	type = XE;	break;
		case STFSUX:	func = stfs;	type = XEU;	break;
		case STFDX:	func = stfd;	type = XE;	break;
		case STFDUX:	func = stfd;	type = XEU;	break;
		case STFIWX:	func = stfiwx;	type = XE;	break;
		default:
			goto illegal;
		}
		break;

	case OP59:
		switch ((insn >> 1) & 0x1f) {
		case FDIVS:	func = fdivs;	type = AB;	break;
		case FSUBS:	func = fsubs;	type = AB;	break;
		case FADDS:	func = fadds;	type = AB;	break;
		case FSQRTS:	func = fsqrts;	type = XB;	break;
		case FRES:	func = fres;	type = XB;	break;
		case FMULS:	func = fmuls;	type = AC;	break;
		case FRSQRTES:	func = frsqrtes;type = XB;	break;
		case FMSUBS:	func = fmsubs;	type = ABC;	break;
		case FMADDS:	func = fmadds;	type = ABC;	break;
		case FNMSUBS:	func = fnmsubs;	type = ABC;	break;
		case FNMADDS:	func = fnmadds;	type = ABC;	break;
		default:
			goto illegal;
		}
		break;

	case OP63:
		if (insn & 0x20) {
			switch ((insn >> 1) & 0x1f) {
			case FDIV:	func = fdiv;	type = AB;	break;
			case FSUB:	func = fsub;	type = AB;	break;
			case FADD:	func = fadd;	type = AB;	break;
			case FSQRT:	func = fsqrt;	type = XB;	break;
			case FRE:	func = fre;	type = XB;	break;
			case FSEL:	func = fsel;	type = ABC;	break;
			case FMUL:	func = fmul;	type = AC;	break;
			case FRSQRTE:	func = frsqrte;	type = XB;	break;
			case FMSUB:	func = fmsub;	type = ABC;	break;
			case FMADD:	func = fmadd;	type = ABC;	break;
			case FNMSUB:	func = fnmsub;	type = ABC;	break;
			case FNMADD:	func = fnmadd;	type = ABC;	break;
			default:
				goto illegal;
			}
			break;
		}

		switch ((insn >> 1) & 0x3ff) {
		case FCMPU:	func = fcmpu;	type = XCR;	break;
		case FRSP:	func = frsp;	type = XB;	break;
		case FCTIW:	func = fctiw;	type = XB;	break;
		case FCTIWZ:	func = fctiwz;	type = XB;	break;
		case FCMPO:	func = fcmpo;	type = XCR;	break;
		case MTFSB1:	func = mtfsb1;	type = XCRB;	break;
		case FNEG:	func = fneg;	type = XB;	break;
		case MCRFS:	func = mcrfs;	type = XCRL;	break;
		case MTFSB0:	func = mtfsb0;	type = XCRB;	break;
		case FMR:	func = fmr;	type = XB;	break;
		case MTFSFI:	func = mtfsfi;	type = XCRI;	break;
		case FNABS:	func = fnabs;	type = XB;	break;
		case FABS:	func = fabs;	type = XB;	break;
		case MFFS:	func = mffs;	type = X;	break;
		case MTFSF:	func = mtfsf;	type = XFLB;	break;
		default:
			goto illegal;
		}
		break;

	default:
		goto illegal;
	}

	switch (type) {
	case AB:
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
		op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
		break;

	case AC:
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
		op2 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
		break;

	case ABC:
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
		op2 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
		op3 = (void *)&current->thread.TS_FPR((insn >>  6) & 0x1f);
		break;

	case D:
		idx = (insn >> 16) & 0x1f;
		sdisp = (insn & 0xffff);
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp);
		break;

	case DU:
		idx = (insn >> 16) & 0x1f;
		if (!idx)
			goto illegal;

		sdisp = (insn & 0xffff);
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)(regs->gpr[idx] + sdisp);
		break;

	case X:
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		break;

	case XA:
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
		break;

	case XB:
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
		break;

	case XE:
		idx = (insn >> 16) & 0x1f;
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		if (!idx) {
			if (((insn >> 1) & 0x3ff) == STFIWX)
				op1 = (void *)(regs->gpr[(insn >> 11) & 0x1f]);
			else
				goto illegal;
		} else {
			op1 = (void *)(regs->gpr[idx] + regs->gpr[(insn >> 11) & 0x1f]);
		}

		break;

	case XEU:
		idx = (insn >> 16) & 0x1f;
		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
		op1 = (void *)((idx ? regs->gpr[idx] : 0)
				+ regs->gpr[(insn >> 11) & 0x1f]);
		break;

	case XCR:
		op0 = (void *)&regs->ccr;
		op1 = (void *)((insn >> 23) & 0x7);
		op2 = (void *)&current->thread.TS_FPR((insn >> 16) & 0x1f);
		op3 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
		break;

	case XCRL:
		op0 = (void *)&regs->ccr;
		op1 = (void *)((insn >> 23) & 0x7);
		op2 = (void *)((insn >> 18) & 0x7);
		break;

	case XCRB:
		op0 = (void *)((insn >> 21) & 0x1f);
		break;

	case XCRI:
		op0 = (void *)((insn >> 23) & 0x7);
		op1 = (void *)((insn >> 12) & 0xf);
		break;

	case XFLB:
		op0 = (void *)((insn >> 17) & 0xff);
		op1 = (void *)&current->thread.TS_FPR((insn >> 11) & 0x1f);
		break;

	default:
		goto illegal;
	}

	eflag = func(op0, op1, op2, op3);

	if (insn & 1) {
		regs->ccr &= ~(0x0f000000);
		regs->ccr |= (__FPU_FPSCR >> 4) & 0x0f000000;
	}

	trap = record_exception(regs, eflag);
	if (trap)
		return 1;

	switch (type) {
	case DU:
	case XEU:
		regs->gpr[idx] = (unsigned long)op1;
		break;

	default:
		break;
	}
#endif /* CONFIG_MATH_EMULATION */

	regs->nip += 4;
	return 0;

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