Initial import

47 files changed, 3134 insertions, 0 deletions

diff --git a/arch/powerpc/math-emu/Makefile b/arch/powerpc/math-emu/Makefile
new file mode 100644
index 000000000..1b46ab4f6
--- /dev/null
+++ b/arch/powerpc/math-emu/Makefile
@@ -0,0 +1,19 @@
+math-emu-common-objs = math.o fre.o fsqrt.o fsqrts.o frsqrtes.o mtfsf.o mtfsfi.o
+obj-$(CONFIG_MATH_EMULATION_HW_UNIMPLEMENTED) += $(math-emu-common-objs)
+obj-$(CONFIG_MATH_EMULATION_FULL) += $(math-emu-common-objs) fabs.o fadd.o \
+					fadds.o fcmpo.o fcmpu.o fctiw.o \
+					fctiwz.o fdiv.o fdivs.o  fmadd.o \
+					fmadds.o fmsub.o fmsubs.o fmul.o \
+					fmuls.o fnabs.o fneg.o fnmadd.o \
+					fnmadds.o fnmsub.o fnmsubs.o fres.o \
+					frsp.o fsel.o lfs.o frsqrte.o fsub.o \
+					fsubs.o  mcrfs.o mffs.o mtfsb0.o \
+					mtfsb1.o stfiwx.o stfs.o math.o \
+					fmr.o lfd.o stfd.o
+
+obj-$(CONFIG_SPE)		+= math_efp.o
+
+CFLAGS_fabs.o = -fno-builtin-fabs
+CFLAGS_math.o = -fno-builtin-fabs
+
+ccflags-y = -I. -Iinclude/math-emu -w
diff --git a/arch/powerpc/math-emu/fabs.c b/arch/powerpc/math-emu/fabs.c
new file mode 100644
index 000000000..549baba59
--- /dev/null
+++ b/arch/powerpc/math-emu/fabs.c
@@ -0,0 +1,18 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int
+fabs(u32 *frD, u32 *frB)
+{
+	frD[0] = frB[0] & 0x7fffffff;
+	frD[1] = frB[1];
+
+#ifdef DEBUG
+	printk("%s: D %p, B %p: ", __func__, frD, frB);
+	dump_double(frD);
+	printk("\n");
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fadd.c b/arch/powerpc/math-emu/fadd.c
new file mode 100644
index 000000000..0158a16e2
--- /dev/null
+++ b/arch/powerpc/math-emu/fadd.c
@@ -0,0 +1,38 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fadd(void *frD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p\n", __func__, frD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	FP_ADD_D(R, A, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fadds.c b/arch/powerpc/math-emu/fadds.c
new file mode 100644
index 000000000..5930f40a8
--- /dev/null
+++ b/arch/powerpc/math-emu/fadds.c
@@ -0,0 +1,39 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fadds(void *frD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p\n", __func__, frD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	FP_ADD_D(R, A, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fcmpo.c b/arch/powerpc/math-emu/fcmpo.c
new file mode 100644
index 000000000..5bce011c2
--- /dev/null
+++ b/arch/powerpc/math-emu/fcmpo.c
@@ -0,0 +1,47 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fcmpo(u32 *ccr, int crfD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_EX;
+	int code[4] = { (1 << 3), (1 << 1), (1 << 2), (1 << 0) };
+	long cmp;
+
+#ifdef DEBUG
+	printk("%s: %p (%08x) %d %p %p\n", __func__, ccr, *ccr, crfD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	if (A_c == FP_CLS_NAN || B_c == FP_CLS_NAN)
+		FP_SET_EXCEPTION(EFLAG_VXVC);
+
+	FP_CMP_D(cmp, A, B, 2);
+	cmp = code[(cmp + 1) & 3];
+
+	__FPU_FPSCR &= ~(0x1f000);
+	__FPU_FPSCR |= (cmp << 12);
+
+	*ccr &= ~(15 << ((7 - crfD) << 2));
+	*ccr |= (cmp << ((7 - crfD) << 2));
+
+#ifdef DEBUG
+	printk("CR: %08x\n", *ccr);
+#endif
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fcmpu.c b/arch/powerpc/math-emu/fcmpu.c
new file mode 100644
index 000000000..d4fb1babc
--- /dev/null
+++ b/arch/powerpc/math-emu/fcmpu.c
@@ -0,0 +1,44 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fcmpu(u32 *ccr, int crfD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_EX;
+	int code[4] = { (1 << 3), (1 << 1), (1 << 2), (1 << 0) };
+	long cmp;
+
+#ifdef DEBUG
+	printk("%s: %p (%08x) %d %p %p\n", __func__, ccr, *ccr, crfD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	FP_CMP_D(cmp, A, B, 2);
+	cmp = code[(cmp + 1) & 3];
+
+	__FPU_FPSCR &= ~(0x1f000);
+	__FPU_FPSCR |= (cmp << 12);
+
+	*ccr &= ~(15 << ((7 - crfD) << 2));
+	*ccr |= (cmp << ((7 - crfD) << 2));
+
+#ifdef DEBUG
+	printk("CR: %08x\n", *ccr);
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fctiw.c b/arch/powerpc/math-emu/fctiw.c
new file mode 100644
index 000000000..f694440dd
--- /dev/null
+++ b/arch/powerpc/math-emu/fctiw.c
@@ -0,0 +1,27 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fctiw(u32 *frD, void *frB)
+{
+	FP_DECL_D(B);
+	FP_DECL_EX;
+	unsigned int r;
+
+	FP_UNPACK_DP(B, frB);
+	FP_TO_INT_D(r, B, 32, 1);
+	frD[1] = r;
+
+#ifdef DEBUG
+	printk("%s: D %p, B %p: ", __func__, frD, frB);
+	dump_double(frD);
+	printk("\n");
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fctiwz.c b/arch/powerpc/math-emu/fctiwz.c
new file mode 100644
index 000000000..71e782fd4
--- /dev/null
+++ b/arch/powerpc/math-emu/fctiwz.c
@@ -0,0 +1,34 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fctiwz(u32 *frD, void *frB)
+{
+	FP_DECL_D(B);
+	FP_DECL_EX;
+	u32 fpscr;
+	unsigned int r;
+
+	fpscr = __FPU_FPSCR;
+	__FPU_FPSCR &= ~(3);
+	__FPU_FPSCR |= FP_RND_ZERO;
+
+	FP_UNPACK_DP(B, frB);
+	FP_TO_INT_D(r, B, 32, 1);
+	frD[1] = r;
+
+	__FPU_FPSCR = fpscr;
+
+#ifdef DEBUG
+	printk("%s: D %p, B %p: ", __func__, frD, frB);
+	dump_double(frD);
+	printk("\n");
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fdiv.c b/arch/powerpc/math-emu/fdiv.c
new file mode 100644
index 000000000..a29239c05
--- /dev/null
+++ b/arch/powerpc/math-emu/fdiv.c
@@ -0,0 +1,56 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fdiv(void *frD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p\n", __func__, frD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	if (A_c == FP_CLS_ZERO && B_c == FP_CLS_ZERO) {
+		FP_SET_EXCEPTION(EFLAG_VXZDZ);
+#ifdef DEBUG
+		printk("%s: FPSCR_VXZDZ raised\n", __func__);
+#endif
+	}
+	if (A_c == FP_CLS_INF && B_c == FP_CLS_INF) {
+		FP_SET_EXCEPTION(EFLAG_VXIDI);
+#ifdef DEBUG
+		printk("%s: FPSCR_VXIDI raised\n", __func__);
+#endif
+	}
+
+	if (B_c == FP_CLS_ZERO && A_c != FP_CLS_ZERO) {
+		FP_SET_EXCEPTION(EFLAG_DIVZERO);
+		if (__FPU_TRAP_P(EFLAG_DIVZERO))
+			return FP_CUR_EXCEPTIONS;
+	}
+	FP_DIV_D(R, A, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fdivs.c b/arch/powerpc/math-emu/fdivs.c
new file mode 100644
index 000000000..526bc2612
--- /dev/null
+++ b/arch/powerpc/math-emu/fdivs.c
@@ -0,0 +1,58 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fdivs(void *frD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p\n", __func__, frD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	if (A_c == FP_CLS_ZERO && B_c == FP_CLS_ZERO) {
+		FP_SET_EXCEPTION(EFLAG_VXZDZ);
+#ifdef DEBUG
+		printk("%s: FPSCR_VXZDZ raised\n", __func__);
+#endif
+	}
+	if (A_c == FP_CLS_INF && B_c == FP_CLS_INF) {
+		FP_SET_EXCEPTION(EFLAG_VXIDI);
+#ifdef DEBUG
+		printk("%s: FPSCR_VXIDI raised\n", __func__);
+#endif
+	}
+
+	if (B_c == FP_CLS_ZERO && A_c != FP_CLS_ZERO) {
+		FP_SET_EXCEPTION(EFLAG_DIVZERO);
+		if (__FPU_TRAP_P(EFLAG_DIVZERO))
+			return FP_CUR_EXCEPTIONS;
+	}
+
+	FP_DIV_D(R, A, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fmadd.c b/arch/powerpc/math-emu/fmadd.c
new file mode 100644
index 000000000..8c3f20aa5
--- /dev/null
+++ b/arch/powerpc/math-emu/fmadd.c
@@ -0,0 +1,51 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fmadd(void *frD, void *frA, void *frB, void *frC)
+{
+	FP_DECL_D(R);
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(C);
+	FP_DECL_D(T);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+	FP_UNPACK_DP(C, frC);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+	printk("C: %ld %lu %lu %ld (%ld)\n", C_s, C_f1, C_f0, C_e, C_c);
+#endif
+
+	if ((A_c == FP_CLS_INF && C_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && C_c == FP_CLS_INF))
+                FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(T, A, C);
+
+	if (T_s != B_s && T_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, T, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fmadds.c b/arch/powerpc/math-emu/fmadds.c
new file mode 100644
index 000000000..794fb31e5
--- /dev/null
+++ b/arch/powerpc/math-emu/fmadds.c
@@ -0,0 +1,52 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fmadds(void *frD, void *frA, void *frB, void *frC)
+{
+	FP_DECL_D(R);
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(C);
+	FP_DECL_D(T);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+	FP_UNPACK_DP(C, frC);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+	printk("C: %ld %lu %lu %ld (%ld)\n", C_s, C_f1, C_f0, C_e, C_c);
+#endif
+
+	if ((A_c == FP_CLS_INF && C_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && C_c == FP_CLS_INF))
+                FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(T, A, C);
+
+	if (T_s != B_s && T_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, T, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fmr.c b/arch/powerpc/math-emu/fmr.c
new file mode 100644
index 000000000..bd55384b8
--- /dev/null
+++ b/arch/powerpc/math-emu/fmr.c
@@ -0,0 +1,18 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int
+fmr(u32 *frD, u32 *frB)
+{
+	frD[0] = frB[0];
+	frD[1] = frB[1];
+
+#ifdef DEBUG
+	printk("%s: D %p, B %p: ", __func__, frD, frB);
+	dump_double(frD);
+	printk("\n");
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fmsub.c b/arch/powerpc/math-emu/fmsub.c
new file mode 100644
index 000000000..626f6fed8
--- /dev/null
+++ b/arch/powerpc/math-emu/fmsub.c
@@ -0,0 +1,54 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fmsub(void *frD, void *frA, void *frB, void *frC)
+{
+	FP_DECL_D(R);
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(C);
+	FP_DECL_D(T);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+	FP_UNPACK_DP(C, frC);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+	printk("C: %ld %lu %lu %ld (%ld)\n", C_s, C_f1, C_f0, C_e, C_c);
+#endif
+
+	if ((A_c == FP_CLS_INF && C_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && C_c == FP_CLS_INF))
+		FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(T, A, C);
+
+	if (B_c != FP_CLS_NAN)
+		B_s ^= 1;
+
+	if (T_s != B_s && T_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, T, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fmsubs.c b/arch/powerpc/math-emu/fmsubs.c
new file mode 100644
index 000000000..3425bc899
--- /dev/null
+++ b/arch/powerpc/math-emu/fmsubs.c
@@ -0,0 +1,55 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fmsubs(void *frD, void *frA, void *frB, void *frC)
+{
+	FP_DECL_D(R);
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(C);
+	FP_DECL_D(T);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+	FP_UNPACK_DP(C, frC);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+	printk("C: %ld %lu %lu %ld (%ld)\n", C_s, C_f1, C_f0, C_e, C_c);
+#endif
+
+	if ((A_c == FP_CLS_INF && C_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && C_c == FP_CLS_INF))
+		FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(T, A, C);
+
+	if (B_c != FP_CLS_NAN)
+		B_s ^= 1;
+
+	if (T_s != B_s && T_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, T, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fmul.c b/arch/powerpc/math-emu/fmul.c
new file mode 100644
index 000000000..2c1929779
--- /dev/null
+++ b/arch/powerpc/math-emu/fmul.c
@@ -0,0 +1,45 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fmul(void *frD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p\n", __func__, frD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld) [%08lx.%08lx %lx]\n",
+	       A_s, A_f1, A_f0, A_e, A_c, A_f1, A_f0, A_e + 1023);
+	printk("B: %ld %lu %lu %ld (%ld) [%08lx.%08lx %lx]\n",
+	       B_s, B_f1, B_f0, B_e, B_c, B_f1, B_f0, B_e + 1023);
+#endif
+
+	if ((A_c == FP_CLS_INF && B_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && B_c == FP_CLS_INF))
+		FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(R, A, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld) [%08lx.%08lx %lx]\n",
+	       R_s, R_f1, R_f0, R_e, R_c, R_f1, R_f0, R_e + 1023);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fmuls.c b/arch/powerpc/math-emu/fmuls.c
new file mode 100644
index 000000000..f5ad5c9c7
--- /dev/null
+++ b/arch/powerpc/math-emu/fmuls.c
@@ -0,0 +1,46 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fmuls(void *frD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p\n", __func__, frD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld) [%08lx.%08lx %lx]\n",
+	       A_s, A_f1, A_f0, A_e, A_c, A_f1, A_f0, A_e + 1023);
+	printk("B: %ld %lu %lu %ld (%ld) [%08lx.%08lx %lx]\n",
+	       B_s, B_f1, B_f0, B_e, B_c, B_f1, B_f0, B_e + 1023);
+#endif
+
+	if ((A_c == FP_CLS_INF && B_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && B_c == FP_CLS_INF))
+		FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(R, A, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld) [%08lx.%08lx %lx]\n",
+	       R_s, R_f1, R_f0, R_e, R_c, R_f1, R_f0, R_e + 1023);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fnabs.c b/arch/powerpc/math-emu/fnabs.c
new file mode 100644
index 000000000..a7d34f3d9
--- /dev/null
+++ b/arch/powerpc/math-emu/fnabs.c
@@ -0,0 +1,18 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int
+fnabs(u32 *frD, u32 *frB)
+{
+	frD[0] = frB[0] | 0x80000000;
+	frD[1] = frB[1];
+
+#ifdef DEBUG
+	printk("%s: D %p, B %p: ", __func__, frD, frB);
+	dump_double(frD);
+	printk("\n");
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fneg.c b/arch/powerpc/math-emu/fneg.c
new file mode 100644
index 000000000..1e988cd9c
--- /dev/null
+++ b/arch/powerpc/math-emu/fneg.c
@@ -0,0 +1,18 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int
+fneg(u32 *frD, u32 *frB)
+{
+	frD[0] = frB[0] ^ 0x80000000;
+	frD[1] = frB[1];
+
+#ifdef DEBUG
+	printk("%s: D %p, B %p: ", __func__, frD, frB);
+	dump_double(frD);
+	printk("\n");
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fnmadd.c b/arch/powerpc/math-emu/fnmadd.c
new file mode 100644
index 000000000..e817bc545
--- /dev/null
+++ b/arch/powerpc/math-emu/fnmadd.c
@@ -0,0 +1,54 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fnmadd(void *frD, void *frA, void *frB, void *frC)
+{
+	FP_DECL_D(R);
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(C);
+	FP_DECL_D(T);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+	FP_UNPACK_DP(C, frC);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+	printk("C: %ld %lu %lu %ld (%ld)\n", C_s, C_f1, C_f0, C_e, C_c);
+#endif
+
+	if ((A_c == FP_CLS_INF && C_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && C_c == FP_CLS_INF))
+                FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(T, A, C);
+
+	if (T_s != B_s && T_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, T, B);
+
+	if (R_c != FP_CLS_NAN)
+		R_s ^= 1;
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fnmadds.c b/arch/powerpc/math-emu/fnmadds.c
new file mode 100644
index 000000000..4db4b7d9b
--- /dev/null
+++ b/arch/powerpc/math-emu/fnmadds.c
@@ -0,0 +1,55 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fnmadds(void *frD, void *frA, void *frB, void *frC)
+{
+	FP_DECL_D(R);
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(C);
+	FP_DECL_D(T);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+	FP_UNPACK_DP(C, frC);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+	printk("C: %ld %lu %lu %ld (%ld)\n", C_s, C_f1, C_f0, C_e, C_c);
+#endif
+
+	if ((A_c == FP_CLS_INF && C_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && C_c == FP_CLS_INF))
+                FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(T, A, C);
+
+	if (T_s != B_s && T_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, T, B);
+
+	if (R_c != FP_CLS_NAN)
+		R_s ^= 1;
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fnmsub.c b/arch/powerpc/math-emu/fnmsub.c
new file mode 100644
index 000000000..f65979fa7
--- /dev/null
+++ b/arch/powerpc/math-emu/fnmsub.c
@@ -0,0 +1,57 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fnmsub(void *frD, void *frA, void *frB, void *frC)
+{
+	FP_DECL_D(R);
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(C);
+	FP_DECL_D(T);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+	FP_UNPACK_DP(C, frC);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+	printk("C: %ld %lu %lu %ld (%ld)\n", C_s, C_f1, C_f0, C_e, C_c);
+#endif
+
+	if ((A_c == FP_CLS_INF && C_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && C_c == FP_CLS_INF))
+		FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(T, A, C);
+
+	if (B_c != FP_CLS_NAN)
+		B_s ^= 1;
+
+	if (T_s != B_s && T_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, T, B);
+
+	if (R_c != FP_CLS_NAN)
+		R_s ^= 1;
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fnmsubs.c b/arch/powerpc/math-emu/fnmsubs.c
new file mode 100644
index 000000000..9021dacc0
--- /dev/null
+++ b/arch/powerpc/math-emu/fnmsubs.c
@@ -0,0 +1,58 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fnmsubs(void *frD, void *frA, void *frB, void *frC)
+{
+	FP_DECL_D(R);
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(C);
+	FP_DECL_D(T);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+	FP_UNPACK_DP(C, frC);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+	printk("C: %ld %lu %lu %ld (%ld)\n", C_s, C_f1, C_f0, C_e, C_c);
+#endif
+
+	if ((A_c == FP_CLS_INF && C_c == FP_CLS_ZERO) ||
+	    (A_c == FP_CLS_ZERO && C_c == FP_CLS_INF))
+		FP_SET_EXCEPTION(EFLAG_VXIMZ);
+
+	FP_MUL_D(T, A, C);
+
+	if (B_c != FP_CLS_NAN)
+		B_s ^= 1;
+
+	if (T_s != B_s && T_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, T, B);
+
+	if (R_c != FP_CLS_NAN)
+		R_s ^= 1;
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fre.c b/arch/powerpc/math-emu/fre.c
new file mode 100644
index 000000000..49ccf2cc6
--- /dev/null
+++ b/arch/powerpc/math-emu/fre.c
@@ -0,0 +1,11 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int fre(void *frD, void *frB)
+{
+#ifdef DEBUG
+	printk("%s: %p %p\n", __func__, frD, frB);
+#endif
+	return -ENOSYS;
+}
diff --git a/arch/powerpc/math-emu/fres.c b/arch/powerpc/math-emu/fres.c
new file mode 100644
index 000000000..10ecbd08b
--- /dev/null
+++ b/arch/powerpc/math-emu/fres.c
@@ -0,0 +1,12 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int
+fres(void *frD, void *frB)
+{
+#ifdef DEBUG
+	printk("%s: %p %p\n", __func__, frD, frB);
+#endif
+	return -ENOSYS;
+}
diff --git a/arch/powerpc/math-emu/frsp.c b/arch/powerpc/math-emu/frsp.c
new file mode 100644
index 000000000..ddcc14664
--- /dev/null
+++ b/arch/powerpc/math-emu/frsp.c
@@ -0,0 +1,29 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+frsp(void *frD, void *frB)
+{
+	FP_DECL_D(B);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: D %p, B %p\n", __func__, frD, frB);
+#endif
+
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	__FP_PACK_DS(frD, B);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/frsqrte.c b/arch/powerpc/math-emu/frsqrte.c
new file mode 100644
index 000000000..1d0a3a0fd
--- /dev/null
+++ b/arch/powerpc/math-emu/frsqrte.c
@@ -0,0 +1,12 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int
+frsqrte(void *frD, void *frB)
+{
+#ifdef DEBUG
+	printk("%s: %p %p\n", __func__, frD, frB);
+#endif
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/frsqrtes.c b/arch/powerpc/math-emu/frsqrtes.c
new file mode 100644
index 000000000..7e838e380
--- /dev/null
+++ b/arch/powerpc/math-emu/frsqrtes.c
@@ -0,0 +1,11 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int frsqrtes(void *frD, void *frB)
+{
+#ifdef DEBUG
+	printk("%s: %p %p\n", __func__, frD, frB);
+#endif
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fsel.c b/arch/powerpc/math-emu/fsel.c
new file mode 100644
index 000000000..1b0c14498
--- /dev/null
+++ b/arch/powerpc/math-emu/fsel.c
@@ -0,0 +1,40 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fsel(u32 *frD, void *frA, u32 *frB, u32 *frC)
+{
+	FP_DECL_D(A);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frA, frB, frC);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %08x %08x\n", frB[0], frB[1]);
+	printk("C: %08x %08x\n", frC[0], frC[1]);
+#endif
+
+	if (A_c == FP_CLS_NAN || (A_c != FP_CLS_ZERO && A_s)) {
+		frD[0] = frB[0];
+		frD[1] = frB[1];
+	} else {
+		frD[0] = frC[0];
+		frD[1] = frC[1];
+	}
+
+#ifdef DEBUG
+	printk("D: %08x.%08x\n", frD[0], frD[1]);
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/fsqrt.c b/arch/powerpc/math-emu/fsqrt.c
new file mode 100644
index 000000000..a55fc7d49
--- /dev/null
+++ b/arch/powerpc/math-emu/fsqrt.c
@@ -0,0 +1,40 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fsqrt(void *frD, void *frB)
+{
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frB);
+#endif
+
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	if (B_s && B_c != FP_CLS_ZERO)
+		FP_SET_EXCEPTION(EFLAG_VXSQRT);
+	if (B_c == FP_CLS_NAN)
+		FP_SET_EXCEPTION(EFLAG_VXSNAN);
+
+	FP_SQRT_D(R, B);
+
+#ifdef DEBUG
+	printk("R: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fsqrts.c b/arch/powerpc/math-emu/fsqrts.c
new file mode 100644
index 000000000..31dccbfc3
--- /dev/null
+++ b/arch/powerpc/math-emu/fsqrts.c
@@ -0,0 +1,41 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fsqrts(void *frD, void *frB)
+{
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p %p\n", __func__, frD, frB);
+#endif
+
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	if (B_s && B_c != FP_CLS_ZERO)
+		FP_SET_EXCEPTION(EFLAG_VXSQRT);
+	if (B_c == FP_CLS_NAN)
+		FP_SET_EXCEPTION(EFLAG_VXSNAN);
+
+	FP_SQRT_D(R, B);
+
+#ifdef DEBUG
+	printk("R: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fsub.c b/arch/powerpc/math-emu/fsub.c
new file mode 100644
index 000000000..02c5dff45
--- /dev/null
+++ b/arch/powerpc/math-emu/fsub.c
@@ -0,0 +1,44 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+
+int
+fsub(void *frD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p\n", __func__, frD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	if (B_c != FP_CLS_NAN)
+		B_s ^= 1;
+
+	if (A_s != B_s && A_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, A, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_D(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/fsubs.c b/arch/powerpc/math-emu/fsubs.c
new file mode 100644
index 000000000..5d9b18c35
--- /dev/null
+++ b/arch/powerpc/math-emu/fsubs.c
@@ -0,0 +1,45 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+fsubs(void *frD, void *frA, void *frB)
+{
+	FP_DECL_D(A);
+	FP_DECL_D(B);
+	FP_DECL_D(R);
+	FP_DECL_EX;
+
+#ifdef DEBUG
+	printk("%s: %p %p %p\n", __func__, frD, frA, frB);
+#endif
+
+	FP_UNPACK_DP(A, frA);
+	FP_UNPACK_DP(B, frB);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+	printk("B: %ld %lu %lu %ld (%ld)\n", B_s, B_f1, B_f0, B_e, B_c);
+#endif
+
+	if (B_c != FP_CLS_NAN)
+		B_s ^= 1;
+
+	if (A_s != B_s && A_c == FP_CLS_INF && B_c == FP_CLS_INF)
+		FP_SET_EXCEPTION(EFLAG_VXISI);
+
+	FP_ADD_D(R, A, B);
+
+#ifdef DEBUG
+	printk("D: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	__FP_PACK_DS(frD, R);
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/lfd.c b/arch/powerpc/math-emu/lfd.c
new file mode 100644
index 000000000..79ac76d59
--- /dev/null
+++ b/arch/powerpc/math-emu/lfd.c
@@ -0,0 +1,19 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/double.h>
+
+int
+lfd(void *frD, void *ea)
+{
+	if (copy_from_user(frD, ea, sizeof(double)))
+		return -EFAULT;
+#ifdef DEBUG
+	printk("%s: D %p, ea %p: ", __func__, frD, ea);
+	dump_double(frD);
+	printk("\n");
+#endif
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/lfs.c b/arch/powerpc/math-emu/lfs.c
new file mode 100644
index 000000000..434ed27be
--- /dev/null
+++ b/arch/powerpc/math-emu/lfs.c
@@ -0,0 +1,46 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+lfs(void *frD, void *ea)
+{
+	FP_DECL_D(R);
+	FP_DECL_S(A);
+	FP_DECL_EX;
+	float f;
+
+#ifdef DEBUG
+	printk("%s: D %p, ea %p\n", __func__, frD, ea);
+#endif
+
+	if (copy_from_user(&f, ea, sizeof(float)))
+		return -EFAULT;
+
+	FP_UNPACK_S(A, f);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %ld (%ld) [%08lx]\n", A_s, A_f, A_e, A_c,
+	       *(unsigned long *)&f);
+#endif
+
+	FP_CONV(D, S, 2, 1, R, A);
+
+#ifdef DEBUG
+	printk("R: %ld %lu %lu %ld (%ld)\n", R_s, R_f1, R_f0, R_e, R_c);
+#endif
+
+	if (R_c == FP_CLS_NAN) {
+		R_e = _FP_EXPMAX_D;
+		_FP_PACK_RAW_2_P(D, frD, R);
+	} else {
+		__FP_PACK_D(frD, R);
+	}
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/math.c b/arch/powerpc/math-emu/math.c
new file mode 100644
index 000000000..ab151f040
--- /dev/null
+++ b/arch/powerpc/math-emu/math.c
@@ -0,0 +1,460 @@
+/*
+ * 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/switch_to.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/double.h>
+
+#define FLOATFUNC(x)	extern int x(void *, void *, void *, void *)
+
+/* The instructions list which may be not implemented by a hardware FPU */
+FLOATFUNC(fre);
+FLOATFUNC(frsqrtes);
+FLOATFUNC(fsqrt);
+FLOATFUNC(fsqrts);
+FLOATFUNC(mtfsf);
+FLOATFUNC(mtfsfi);
+
+#ifdef CONFIG_MATH_EMULATION_HW_UNIMPLEMENTED
+#undef FLOATFUNC(x)
+#define FLOATFUNC(x)	static inline int x(void *op1, void *op2, void *op3, \
+						 void *op4) { }
+#endif
+
+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(lfd);
+FLOATFUNC(lfs);
+
+FLOATFUNC(stfd);
+FLOATFUNC(stfs);
+FLOATFUNC(stfiwx);
+
+FLOATFUNC(fabs);
+FLOATFUNC(fmr);
+FLOATFUNC(fnabs);
+FLOATFUNC(fneg);
+
+/* Optional */
+FLOATFUNC(fres);
+FLOATFUNC(frsqrte);
+FLOATFUNC(fsel);
+
+
+#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
+
+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;
+}
+
+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;
+	int (*func)(void *, void *, void *, void *);
+	int type = 0;
+	int eflag, trap;
+
+	if (get_user(insn, (u32 *)pc))
+		return -EFAULT;
+
+	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);
+		op1 = (void *)((idx ? regs->gpr[idx] : 0)
+				+ regs->gpr[(insn >> 11) & 0x1f]);
+		break;
+
+	case XEU:
+		idx = (insn >> 16) & 0x1f;
+		if (!idx)
+			goto illegal;
+		op0 = (void *)&current->thread.TS_FPR((insn >> 21) & 0x1f);
+		op1 = (void *)(regs->gpr[idx]
+				+ 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;
+	}
+
+	/*
+	 * If we support a HW FPU, we need to ensure the FP state
+	 * is flushed into the thread_struct before attempting
+	 * emulation
+	 */
+	flush_fp_to_thread(current);
+
+	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;
+	}
+
+	regs->nip += 4;
+	return 0;
+
+illegal:
+	return -ENOSYS;
+}
diff --git a/arch/powerpc/math-emu/math_efp.c b/arch/powerpc/math-emu/math_efp.c
new file mode 100644
index 000000000..28337c970
--- /dev/null
+++ b/arch/powerpc/math-emu/math_efp.c
@@ -0,0 +1,931 @@
+/*
+ * arch/powerpc/math-emu/math_efp.c
+ *
+ * Copyright (C) 2006-2008, 2010 Freescale Semiconductor, Inc.
+ *
+ * Author: Ebony Zhu,	<ebony.zhu@freescale.com>
+ *         Yu Liu,	<yu.liu@freescale.com>
+ *
+ * Derived from arch/alpha/math-emu/math.c
+ *              arch/powerpc/math-emu/math.c
+ *
+ * Description:
+ * This file is the exception handler to make E500 SPE instructions
+ * fully comply with IEEE-754 floating point standard.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License
+ * as published by the Free Software Foundation; either version
+ * 2 of the License, or (at your option) any later version.
+ */
+
+#include <linux/types.h>
+#include <linux/prctl.h>
+
+#include <asm/uaccess.h>
+#include <asm/reg.h>
+
+#define FP_EX_BOOKE_E500_SPE
+#include <asm/sfp-machine.h>
+
+#include <math-emu/soft-fp.h>
+#include <math-emu/single.h>
+#include <math-emu/double.h>
+
+#define EFAPU		0x4
+
+#define VCT		0x4
+#define SPFP		0x6
+#define DPFP		0x7
+
+#define EFSADD		0x2c0
+#define EFSSUB		0x2c1
+#define EFSABS		0x2c4
+#define EFSNABS		0x2c5
+#define EFSNEG		0x2c6
+#define EFSMUL		0x2c8
+#define EFSDIV		0x2c9
+#define EFSCMPGT	0x2cc
+#define EFSCMPLT	0x2cd
+#define EFSCMPEQ	0x2ce
+#define EFSCFD		0x2cf
+#define EFSCFSI		0x2d1
+#define EFSCTUI		0x2d4
+#define EFSCTSI		0x2d5
+#define EFSCTUF		0x2d6
+#define EFSCTSF		0x2d7
+#define EFSCTUIZ	0x2d8
+#define EFSCTSIZ	0x2da
+
+#define EVFSADD		0x280
+#define EVFSSUB		0x281
+#define EVFSABS		0x284
+#define EVFSNABS	0x285
+#define EVFSNEG		0x286
+#define EVFSMUL		0x288
+#define EVFSDIV		0x289
+#define EVFSCMPGT	0x28c
+#define EVFSCMPLT	0x28d
+#define EVFSCMPEQ	0x28e
+#define EVFSCTUI	0x294
+#define EVFSCTSI	0x295
+#define EVFSCTUF	0x296
+#define EVFSCTSF	0x297
+#define EVFSCTUIZ	0x298
+#define EVFSCTSIZ	0x29a
+
+#define EFDADD		0x2e0
+#define EFDSUB		0x2e1
+#define EFDABS		0x2e4
+#define EFDNABS		0x2e5
+#define EFDNEG		0x2e6
+#define EFDMUL		0x2e8
+#define EFDDIV		0x2e9
+#define EFDCTUIDZ	0x2ea
+#define EFDCTSIDZ	0x2eb
+#define EFDCMPGT	0x2ec
+#define EFDCMPLT	0x2ed
+#define EFDCMPEQ	0x2ee
+#define EFDCFS		0x2ef
+#define EFDCTUI		0x2f4
+#define EFDCTSI		0x2f5
+#define EFDCTUF		0x2f6
+#define EFDCTSF		0x2f7
+#define EFDCTUIZ	0x2f8
+#define EFDCTSIZ	0x2fa
+
+#define AB	2
+#define XA	3
+#define XB	4
+#define XCR	5
+#define NOTYPE	0
+
+#define SIGN_BIT_S	(1UL << 31)
+#define SIGN_BIT_D	(1ULL << 63)
+#define FP_EX_MASK	(FP_EX_INEXACT | FP_EX_INVALID | FP_EX_DIVZERO | \
+			FP_EX_UNDERFLOW | FP_EX_OVERFLOW)
+
+static int have_e500_cpu_a005_erratum;
+
+union dw_union {
+	u64 dp[1];
+	u32 wp[2];
+};
+
+static unsigned long insn_type(unsigned long speinsn)
+{
+	unsigned long ret = NOTYPE;
+
+	switch (speinsn & 0x7ff) {
+	case EFSABS:	ret = XA;	break;
+	case EFSADD:	ret = AB;	break;
+	case EFSCFD:	ret = XB;	break;
+	case EFSCMPEQ:	ret = XCR;	break;
+	case EFSCMPGT:	ret = XCR;	break;
+	case EFSCMPLT:	ret = XCR;	break;
+	case EFSCTSF:	ret = XB;	break;
+	case EFSCTSI:	ret = XB;	break;
+	case EFSCTSIZ:	ret = XB;	break;
+	case EFSCTUF:	ret = XB;	break;
+	case EFSCTUI:	ret = XB;	break;
+	case EFSCTUIZ:	ret = XB;	break;
+	case EFSDIV:	ret = AB;	break;
+	case EFSMUL:	ret = AB;	break;
+	case EFSNABS:	ret = XA;	break;
+	case EFSNEG:	ret = XA;	break;
+	case EFSSUB:	ret = AB;	break;
+	case EFSCFSI:	ret = XB;	break;
+
+	case EVFSABS:	ret = XA;	break;
+	case EVFSADD:	ret = AB;	break;
+	case EVFSCMPEQ:	ret = XCR;	break;
+	case EVFSCMPGT:	ret = XCR;	break;
+	case EVFSCMPLT:	ret = XCR;	break;
+	case EVFSCTSF:	ret = XB;	break;
+	case EVFSCTSI:	ret = XB;	break;
+	case EVFSCTSIZ:	ret = XB;	break;
+	case EVFSCTUF:	ret = XB;	break;
+	case EVFSCTUI:	ret = XB;	break;
+	case EVFSCTUIZ:	ret = XB;	break;
+	case EVFSDIV:	ret = AB;	break;
+	case EVFSMUL:	ret = AB;	break;
+	case EVFSNABS:	ret = XA;	break;
+	case EVFSNEG:	ret = XA;	break;
+	case EVFSSUB:	ret = AB;	break;
+
+	case EFDABS:	ret = XA;	break;
+	case EFDADD:	ret = AB;	break;
+	case EFDCFS:	ret = XB;	break;
+	case EFDCMPEQ:	ret = XCR;	break;
+	case EFDCMPGT:	ret = XCR;	break;
+	case EFDCMPLT:	ret = XCR;	break;
+	case EFDCTSF:	ret = XB;	break;
+	case EFDCTSI:	ret = XB;	break;
+	case EFDCTSIDZ:	ret = XB;	break;
+	case EFDCTSIZ:	ret = XB;	break;
+	case EFDCTUF:	ret = XB;	break;
+	case EFDCTUI:	ret = XB;	break;
+	case EFDCTUIDZ:	ret = XB;	break;
+	case EFDCTUIZ:	ret = XB;	break;
+	case EFDDIV:	ret = AB;	break;
+	case EFDMUL:	ret = AB;	break;
+	case EFDNABS:	ret = XA;	break;
+	case EFDNEG:	ret = XA;	break;
+	case EFDSUB:	ret = AB;	break;
+	}
+
+	return ret;
+}
+
+int do_spe_mathemu(struct pt_regs *regs)
+{
+	FP_DECL_EX;
+	int IR, cmp;
+
+	unsigned long type, func, fc, fa, fb, src, speinsn;
+	union dw_union vc, va, vb;
+
+	if (get_user(speinsn, (unsigned int __user *) regs->nip))
+		return -EFAULT;
+	if ((speinsn >> 26) != EFAPU)
+		return -EINVAL;         /* not an spe instruction */
+
+	type = insn_type(speinsn);
+	if (type == NOTYPE)
+		goto illegal;
+
+	func = speinsn & 0x7ff;
+	fc = (speinsn >> 21) & 0x1f;
+	fa = (speinsn >> 16) & 0x1f;
+	fb = (speinsn >> 11) & 0x1f;
+	src = (speinsn >> 5) & 0x7;
+
+	vc.wp[0] = current->thread.evr[fc];
+	vc.wp[1] = regs->gpr[fc];
+	va.wp[0] = current->thread.evr[fa];
+	va.wp[1] = regs->gpr[fa];
+	vb.wp[0] = current->thread.evr[fb];
+	vb.wp[1] = regs->gpr[fb];
+
+	__FPU_FPSCR = mfspr(SPRN_SPEFSCR);
+
+	pr_debug("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
+	pr_debug("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
+	pr_debug("va: %08x  %08x\n", va.wp[0], va.wp[1]);
+	pr_debug("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);
+
+	switch (src) {
+	case SPFP: {
+		FP_DECL_S(SA); FP_DECL_S(SB); FP_DECL_S(SR);
+
+		switch (type) {
+		case AB:
+		case XCR:
+			FP_UNPACK_SP(SA, va.wp + 1);
+		case XB:
+			FP_UNPACK_SP(SB, vb.wp + 1);
+			break;
+		case XA:
+			FP_UNPACK_SP(SA, va.wp + 1);
+			break;
+		}
+
+		pr_debug("SA: %ld %08lx %ld (%ld)\n", SA_s, SA_f, SA_e, SA_c);
+		pr_debug("SB: %ld %08lx %ld (%ld)\n", SB_s, SB_f, SB_e, SB_c);
+
+		switch (func) {
+		case EFSABS:
+			vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
+			goto update_regs;
+
+		case EFSNABS:
+			vc.wp[1] = va.wp[1] | SIGN_BIT_S;
+			goto update_regs;
+
+		case EFSNEG:
+			vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
+			goto update_regs;
+
+		case EFSADD:
+			FP_ADD_S(SR, SA, SB);
+			goto pack_s;
+
+		case EFSSUB:
+			FP_SUB_S(SR, SA, SB);
+			goto pack_s;
+
+		case EFSMUL:
+			FP_MUL_S(SR, SA, SB);
+			goto pack_s;
+
+		case EFSDIV:
+			FP_DIV_S(SR, SA, SB);
+			goto pack_s;
+
+		case EFSCMPEQ:
+			cmp = 0;
+			goto cmp_s;
+
+		case EFSCMPGT:
+			cmp = 1;
+			goto cmp_s;
+
+		case EFSCMPLT:
+			cmp = -1;
+			goto cmp_s;
+
+		case EFSCTSF:
+		case EFSCTUF:
+			if (SB_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				SB_e += (func == EFSCTSF ? 31 : 32);
+				FP_TO_INT_ROUND_S(vc.wp[1], SB, 32,
+						(func == EFSCTSF));
+			}
+			goto update_regs;
+
+		case EFSCFD: {
+			FP_DECL_D(DB);
+			FP_CLEAR_EXCEPTIONS;
+			FP_UNPACK_DP(DB, vb.dp);
+
+			pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
+					DB_s, DB_f1, DB_f0, DB_e, DB_c);
+
+			FP_CONV(S, D, 1, 2, SR, DB);
+			goto pack_s;
+		}
+
+		case EFSCTSI:
+		case EFSCTUI:
+			if (SB_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_ROUND_S(vc.wp[1], SB, 32,
+						((func & 0x3) != 0));
+			}
+			goto update_regs;
+
+		case EFSCTSIZ:
+		case EFSCTUIZ:
+			if (SB_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_S(vc.wp[1], SB, 32,
+						((func & 0x3) != 0));
+			}
+			goto update_regs;
+
+		default:
+			goto illegal;
+		}
+		break;
+
+pack_s:
+		pr_debug("SR: %ld %08lx %ld (%ld)\n", SR_s, SR_f, SR_e, SR_c);
+
+		FP_PACK_SP(vc.wp + 1, SR);
+		goto update_regs;
+
+cmp_s:
+		FP_CMP_S(IR, SA, SB, 3);
+		if (IR == 3 && (FP_ISSIGNAN_S(SA) || FP_ISSIGNAN_S(SB)))
+			FP_SET_EXCEPTION(FP_EX_INVALID);
+		if (IR == cmp) {
+			IR = 0x4;
+		} else {
+			IR = 0;
+		}
+		goto update_ccr;
+	}
+
+	case DPFP: {
+		FP_DECL_D(DA); FP_DECL_D(DB); FP_DECL_D(DR);
+
+		switch (type) {
+		case AB:
+		case XCR:
+			FP_UNPACK_DP(DA, va.dp);
+		case XB:
+			FP_UNPACK_DP(DB, vb.dp);
+			break;
+		case XA:
+			FP_UNPACK_DP(DA, va.dp);
+			break;
+		}
+
+		pr_debug("DA: %ld %08lx %08lx %ld (%ld)\n",
+				DA_s, DA_f1, DA_f0, DA_e, DA_c);
+		pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
+				DB_s, DB_f1, DB_f0, DB_e, DB_c);
+
+		switch (func) {
+		case EFDABS:
+			vc.dp[0] = va.dp[0] & ~SIGN_BIT_D;
+			goto update_regs;
+
+		case EFDNABS:
+			vc.dp[0] = va.dp[0] | SIGN_BIT_D;
+			goto update_regs;
+
+		case EFDNEG:
+			vc.dp[0] = va.dp[0] ^ SIGN_BIT_D;
+			goto update_regs;
+
+		case EFDADD:
+			FP_ADD_D(DR, DA, DB);
+			goto pack_d;
+
+		case EFDSUB:
+			FP_SUB_D(DR, DA, DB);
+			goto pack_d;
+
+		case EFDMUL:
+			FP_MUL_D(DR, DA, DB);
+			goto pack_d;
+
+		case EFDDIV:
+			FP_DIV_D(DR, DA, DB);
+			goto pack_d;
+
+		case EFDCMPEQ:
+			cmp = 0;
+			goto cmp_d;
+
+		case EFDCMPGT:
+			cmp = 1;
+			goto cmp_d;
+
+		case EFDCMPLT:
+			cmp = -1;
+			goto cmp_d;
+
+		case EFDCTSF:
+		case EFDCTUF:
+			if (DB_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				DB_e += (func == EFDCTSF ? 31 : 32);
+				FP_TO_INT_ROUND_D(vc.wp[1], DB, 32,
+						(func == EFDCTSF));
+			}
+			goto update_regs;
+
+		case EFDCFS: {
+			FP_DECL_S(SB);
+			FP_CLEAR_EXCEPTIONS;
+			FP_UNPACK_SP(SB, vb.wp + 1);
+
+			pr_debug("SB: %ld %08lx %ld (%ld)\n",
+					SB_s, SB_f, SB_e, SB_c);
+
+			FP_CONV(D, S, 2, 1, DR, SB);
+			goto pack_d;
+		}
+
+		case EFDCTUIDZ:
+		case EFDCTSIDZ:
+			if (DB_c == FP_CLS_NAN) {
+				vc.dp[0] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_D(vc.dp[0], DB, 64,
+						((func & 0x1) == 0));
+			}
+			goto update_regs;
+
+		case EFDCTUI:
+		case EFDCTSI:
+			if (DB_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_ROUND_D(vc.wp[1], DB, 32,
+						((func & 0x3) != 0));
+			}
+			goto update_regs;
+
+		case EFDCTUIZ:
+		case EFDCTSIZ:
+			if (DB_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_D(vc.wp[1], DB, 32,
+						((func & 0x3) != 0));
+			}
+			goto update_regs;
+
+		default:
+			goto illegal;
+		}
+		break;
+
+pack_d:
+		pr_debug("DR: %ld %08lx %08lx %ld (%ld)\n",
+				DR_s, DR_f1, DR_f0, DR_e, DR_c);
+
+		FP_PACK_DP(vc.dp, DR);
+		goto update_regs;
+
+cmp_d:
+		FP_CMP_D(IR, DA, DB, 3);
+		if (IR == 3 && (FP_ISSIGNAN_D(DA) || FP_ISSIGNAN_D(DB)))
+			FP_SET_EXCEPTION(FP_EX_INVALID);
+		if (IR == cmp) {
+			IR = 0x4;
+		} else {
+			IR = 0;
+		}
+		goto update_ccr;
+
+	}
+
+	case VCT: {
+		FP_DECL_S(SA0); FP_DECL_S(SB0); FP_DECL_S(SR0);
+		FP_DECL_S(SA1); FP_DECL_S(SB1); FP_DECL_S(SR1);
+		int IR0, IR1;
+
+		switch (type) {
+		case AB:
+		case XCR:
+			FP_UNPACK_SP(SA0, va.wp);
+			FP_UNPACK_SP(SA1, va.wp + 1);
+		case XB:
+			FP_UNPACK_SP(SB0, vb.wp);
+			FP_UNPACK_SP(SB1, vb.wp + 1);
+			break;
+		case XA:
+			FP_UNPACK_SP(SA0, va.wp);
+			FP_UNPACK_SP(SA1, va.wp + 1);
+			break;
+		}
+
+		pr_debug("SA0: %ld %08lx %ld (%ld)\n",
+				SA0_s, SA0_f, SA0_e, SA0_c);
+		pr_debug("SA1: %ld %08lx %ld (%ld)\n",
+				SA1_s, SA1_f, SA1_e, SA1_c);
+		pr_debug("SB0: %ld %08lx %ld (%ld)\n",
+				SB0_s, SB0_f, SB0_e, SB0_c);
+		pr_debug("SB1: %ld %08lx %ld (%ld)\n",
+				SB1_s, SB1_f, SB1_e, SB1_c);
+
+		switch (func) {
+		case EVFSABS:
+			vc.wp[0] = va.wp[0] & ~SIGN_BIT_S;
+			vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
+			goto update_regs;
+
+		case EVFSNABS:
+			vc.wp[0] = va.wp[0] | SIGN_BIT_S;
+			vc.wp[1] = va.wp[1] | SIGN_BIT_S;
+			goto update_regs;
+
+		case EVFSNEG:
+			vc.wp[0] = va.wp[0] ^ SIGN_BIT_S;
+			vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
+			goto update_regs;
+
+		case EVFSADD:
+			FP_ADD_S(SR0, SA0, SB0);
+			FP_ADD_S(SR1, SA1, SB1);
+			goto pack_vs;
+
+		case EVFSSUB:
+			FP_SUB_S(SR0, SA0, SB0);
+			FP_SUB_S(SR1, SA1, SB1);
+			goto pack_vs;
+
+		case EVFSMUL:
+			FP_MUL_S(SR0, SA0, SB0);
+			FP_MUL_S(SR1, SA1, SB1);
+			goto pack_vs;
+
+		case EVFSDIV:
+			FP_DIV_S(SR0, SA0, SB0);
+			FP_DIV_S(SR1, SA1, SB1);
+			goto pack_vs;
+
+		case EVFSCMPEQ:
+			cmp = 0;
+			goto cmp_vs;
+
+		case EVFSCMPGT:
+			cmp = 1;
+			goto cmp_vs;
+
+		case EVFSCMPLT:
+			cmp = -1;
+			goto cmp_vs;
+
+		case EVFSCTUF:
+		case EVFSCTSF:
+			if (SB0_c == FP_CLS_NAN) {
+				vc.wp[0] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				SB0_e += (func == EVFSCTSF ? 31 : 32);
+				FP_TO_INT_ROUND_S(vc.wp[0], SB0, 32,
+						(func == EVFSCTSF));
+			}
+			if (SB1_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				SB1_e += (func == EVFSCTSF ? 31 : 32);
+				FP_TO_INT_ROUND_S(vc.wp[1], SB1, 32,
+						(func == EVFSCTSF));
+			}
+			goto update_regs;
+
+		case EVFSCTUI:
+		case EVFSCTSI:
+			if (SB0_c == FP_CLS_NAN) {
+				vc.wp[0] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_ROUND_S(vc.wp[0], SB0, 32,
+						((func & 0x3) != 0));
+			}
+			if (SB1_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_ROUND_S(vc.wp[1], SB1, 32,
+						((func & 0x3) != 0));
+			}
+			goto update_regs;
+
+		case EVFSCTUIZ:
+		case EVFSCTSIZ:
+			if (SB0_c == FP_CLS_NAN) {
+				vc.wp[0] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_S(vc.wp[0], SB0, 32,
+						((func & 0x3) != 0));
+			}
+			if (SB1_c == FP_CLS_NAN) {
+				vc.wp[1] = 0;
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			} else {
+				FP_TO_INT_S(vc.wp[1], SB1, 32,
+						((func & 0x3) != 0));
+			}
+			goto update_regs;
+
+		default:
+			goto illegal;
+		}
+		break;
+
+pack_vs:
+		pr_debug("SR0: %ld %08lx %ld (%ld)\n",
+				SR0_s, SR0_f, SR0_e, SR0_c);
+		pr_debug("SR1: %ld %08lx %ld (%ld)\n",
+				SR1_s, SR1_f, SR1_e, SR1_c);
+
+		FP_PACK_SP(vc.wp, SR0);
+		FP_PACK_SP(vc.wp + 1, SR1);
+		goto update_regs;
+
+cmp_vs:
+		{
+			int ch, cl;
+
+			FP_CMP_S(IR0, SA0, SB0, 3);
+			FP_CMP_S(IR1, SA1, SB1, 3);
+			if (IR0 == 3 && (FP_ISSIGNAN_S(SA0) || FP_ISSIGNAN_S(SB0)))
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			if (IR1 == 3 && (FP_ISSIGNAN_S(SA1) || FP_ISSIGNAN_S(SB1)))
+				FP_SET_EXCEPTION(FP_EX_INVALID);
+			ch = (IR0 == cmp) ? 1 : 0;
+			cl = (IR1 == cmp) ? 1 : 0;
+			IR = (ch << 3) | (cl << 2) | ((ch | cl) << 1) |
+				((ch & cl) << 0);
+			goto update_ccr;
+		}
+	}
+	default:
+		return -EINVAL;
+	}
+
+update_ccr:
+	regs->ccr &= ~(15 << ((7 - ((speinsn >> 23) & 0x7)) << 2));
+	regs->ccr |= (IR << ((7 - ((speinsn >> 23) & 0x7)) << 2));
+
+update_regs:
+	/*
+	 * If the "invalid" exception sticky bit was set by the
+	 * processor for non-finite input, but was not set before the
+	 * instruction being emulated, clear it.  Likewise for the
+	 * "underflow" bit, which may have been set by the processor
+	 * for exact underflow, not just inexact underflow when the
+	 * flag should be set for IEEE 754 semantics.  Other sticky
+	 * exceptions will only be set by the processor when they are
+	 * correct according to IEEE 754 semantics, and we must not
+	 * clear sticky bits that were already set before the emulated
+	 * instruction as they represent the user-visible sticky
+	 * exception status.  "inexact" traps to kernel are not
+	 * required for IEEE semantics and are not enabled by default,
+	 * so the "inexact" sticky bit may have been set by a previous
+	 * instruction without the kernel being aware of it.
+	 */
+	__FPU_FPSCR
+	  &= ~(FP_EX_INVALID | FP_EX_UNDERFLOW) | current->thread.spefscr_last;
+	__FPU_FPSCR |= (FP_CUR_EXCEPTIONS & FP_EX_MASK);
+	mtspr(SPRN_SPEFSCR, __FPU_FPSCR);
+	current->thread.spefscr_last = __FPU_FPSCR;
+
+	current->thread.evr[fc] = vc.wp[0];
+	regs->gpr[fc] = vc.wp[1];
+
+	pr_debug("ccr = %08lx\n", regs->ccr);
+	pr_debug("cur exceptions = %08x spefscr = %08lx\n",
+			FP_CUR_EXCEPTIONS, __FPU_FPSCR);
+	pr_debug("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
+	pr_debug("va: %08x  %08x\n", va.wp[0], va.wp[1]);
+	pr_debug("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);
+
+	if (current->thread.fpexc_mode & PR_FP_EXC_SW_ENABLE) {
+		if ((FP_CUR_EXCEPTIONS & FP_EX_DIVZERO)
+		    && (current->thread.fpexc_mode & PR_FP_EXC_DIV))
+			return 1;
+		if ((FP_CUR_EXCEPTIONS & FP_EX_OVERFLOW)
+		    && (current->thread.fpexc_mode & PR_FP_EXC_OVF))
+			return 1;
+		if ((FP_CUR_EXCEPTIONS & FP_EX_UNDERFLOW)
+		    && (current->thread.fpexc_mode & PR_FP_EXC_UND))
+			return 1;
+		if ((FP_CUR_EXCEPTIONS & FP_EX_INEXACT)
+		    && (current->thread.fpexc_mode & PR_FP_EXC_RES))
+			return 1;
+		if ((FP_CUR_EXCEPTIONS & FP_EX_INVALID)
+		    && (current->thread.fpexc_mode & PR_FP_EXC_INV))
+			return 1;
+	}
+	return 0;
+
+illegal:
+	if (have_e500_cpu_a005_erratum) {
+		/* according to e500 cpu a005 erratum, reissue efp inst */
+		regs->nip -= 4;
+		pr_debug("re-issue efp inst: %08lx\n", speinsn);
+		return 0;
+	}
+
+	printk(KERN_ERR "\nOoops! IEEE-754 compliance handler encountered un-supported instruction.\ninst code: %08lx\n", speinsn);
+	return -ENOSYS;
+}
+
+int speround_handler(struct pt_regs *regs)
+{
+	union dw_union fgpr;
+	int s_lo, s_hi;
+	int lo_inexact, hi_inexact;
+	int fp_result;
+	unsigned long speinsn, type, fb, fc, fptype, func;
+
+	if (get_user(speinsn, (unsigned int __user *) regs->nip))
+		return -EFAULT;
+	if ((speinsn >> 26) != 4)
+		return -EINVAL;         /* not an spe instruction */
+
+	func = speinsn & 0x7ff;
+	type = insn_type(func);
+	if (type == XCR) return -ENOSYS;
+
+	__FPU_FPSCR = mfspr(SPRN_SPEFSCR);
+	pr_debug("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
+
+	fptype = (speinsn >> 5) & 0x7;
+
+	/* No need to round if the result is exact */
+	lo_inexact = __FPU_FPSCR & (SPEFSCR_FG | SPEFSCR_FX);
+	hi_inexact = __FPU_FPSCR & (SPEFSCR_FGH | SPEFSCR_FXH);
+	if (!(lo_inexact || (hi_inexact && fptype == VCT)))
+		return 0;
+
+	fc = (speinsn >> 21) & 0x1f;
+	s_lo = regs->gpr[fc] & SIGN_BIT_S;
+	s_hi = current->thread.evr[fc] & SIGN_BIT_S;
+	fgpr.wp[0] = current->thread.evr[fc];
+	fgpr.wp[1] = regs->gpr[fc];
+
+	fb = (speinsn >> 11) & 0x1f;
+	switch (func) {
+	case EFSCTUIZ:
+	case EFSCTSIZ:
+	case EVFSCTUIZ:
+	case EVFSCTSIZ:
+	case EFDCTUIDZ:
+	case EFDCTSIDZ:
+	case EFDCTUIZ:
+	case EFDCTSIZ:
+		/*
+		 * These instructions always round to zero,
+		 * independent of the rounding mode.
+		 */
+		return 0;
+
+	case EFSCTUI:
+	case EFSCTUF:
+	case EVFSCTUI:
+	case EVFSCTUF:
+	case EFDCTUI:
+	case EFDCTUF:
+		fp_result = 0;
+		s_lo = 0;
+		s_hi = 0;
+		break;
+
+	case EFSCTSI:
+	case EFSCTSF:
+		fp_result = 0;
+		/* Recover the sign of a zero result if possible.  */
+		if (fgpr.wp[1] == 0)
+			s_lo = regs->gpr[fb] & SIGN_BIT_S;
+		break;
+
+	case EVFSCTSI:
+	case EVFSCTSF:
+		fp_result = 0;
+		/* Recover the sign of a zero result if possible.  */
+		if (fgpr.wp[1] == 0)
+			s_lo = regs->gpr[fb] & SIGN_BIT_S;
+		if (fgpr.wp[0] == 0)
+			s_hi = current->thread.evr[fb] & SIGN_BIT_S;
+		break;
+
+	case EFDCTSI:
+	case EFDCTSF:
+		fp_result = 0;
+		s_hi = s_lo;
+		/* Recover the sign of a zero result if possible.  */
+		if (fgpr.wp[1] == 0)
+			s_hi = current->thread.evr[fb] & SIGN_BIT_S;
+		break;
+
+	default:
+		fp_result = 1;
+		break;
+	}
+
+	pr_debug("round fgpr: %08x  %08x\n", fgpr.wp[0], fgpr.wp[1]);
+
+	switch (fptype) {
+	/* Since SPE instructions on E500 core can handle round to nearest
+	 * and round toward zero with IEEE-754 complied, we just need
+	 * to handle round toward +Inf and round toward -Inf by software.
+	 */
+	case SPFP:
+		if ((FP_ROUNDMODE) == FP_RND_PINF) {
+			if (!s_lo) fgpr.wp[1]++; /* Z > 0, choose Z1 */
+		} else { /* round to -Inf */
+			if (s_lo) {
+				if (fp_result)
+					fgpr.wp[1]++; /* Z < 0, choose Z2 */
+				else
+					fgpr.wp[1]--; /* Z < 0, choose Z2 */
+			}
+		}
+		break;
+
+	case DPFP:
+		if (FP_ROUNDMODE == FP_RND_PINF) {
+			if (!s_hi) {
+				if (fp_result)
+					fgpr.dp[0]++; /* Z > 0, choose Z1 */
+				else
+					fgpr.wp[1]++; /* Z > 0, choose Z1 */
+			}
+		} else { /* round to -Inf */
+			if (s_hi) {
+				if (fp_result)
+					fgpr.dp[0]++; /* Z < 0, choose Z2 */
+				else
+					fgpr.wp[1]--; /* Z < 0, choose Z2 */
+			}
+		}
+		break;
+
+	case VCT:
+		if (FP_ROUNDMODE == FP_RND_PINF) {
+			if (lo_inexact && !s_lo)
+				fgpr.wp[1]++; /* Z_low > 0, choose Z1 */
+			if (hi_inexact && !s_hi)
+				fgpr.wp[0]++; /* Z_high word > 0, choose Z1 */
+		} else { /* round to -Inf */
+			if (lo_inexact && s_lo) {
+				if (fp_result)
+					fgpr.wp[1]++; /* Z_low < 0, choose Z2 */
+				else
+					fgpr.wp[1]--; /* Z_low < 0, choose Z2 */
+			}
+			if (hi_inexact && s_hi) {
+				if (fp_result)
+					fgpr.wp[0]++; /* Z_high < 0, choose Z2 */
+				else
+					fgpr.wp[0]--; /* Z_high < 0, choose Z2 */
+			}
+		}
+		break;
+
+	default:
+		return -EINVAL;
+	}
+
+	current->thread.evr[fc] = fgpr.wp[0];
+	regs->gpr[fc] = fgpr.wp[1];
+
+	pr_debug("  to fgpr: %08x  %08x\n", fgpr.wp[0], fgpr.wp[1]);
+
+	if (current->thread.fpexc_mode & PR_FP_EXC_SW_ENABLE)
+		return (current->thread.fpexc_mode & PR_FP_EXC_RES) ? 1 : 0;
+	return 0;
+}
+
+int __init spe_mathemu_init(void)
+{
+	u32 pvr, maj, min;
+
+	pvr = mfspr(SPRN_PVR);
+
+	if ((PVR_VER(pvr) == PVR_VER_E500V1) ||
+	    (PVR_VER(pvr) == PVR_VER_E500V2)) {
+		maj = PVR_MAJ(pvr);
+		min = PVR_MIN(pvr);
+
+		/*
+		 * E500 revision below 1.1, 2.3, 3.1, 4.1, 5.1
+		 * need cpu a005 errata workaround
+		 */
+		switch (maj) {
+		case 1:
+			if (min < 1)
+				have_e500_cpu_a005_erratum = 1;
+			break;
+		case 2:
+			if (min < 3)
+				have_e500_cpu_a005_erratum = 1;
+			break;
+		case 3:
+		case 4:
+		case 5:
+			if (min < 1)
+				have_e500_cpu_a005_erratum = 1;
+			break;
+		default:
+			break;
+		}
+	}
+
+	return 0;
+}
+
+module_init(spe_mathemu_init);
diff --git a/arch/powerpc/math-emu/mcrfs.c b/arch/powerpc/math-emu/mcrfs.c
new file mode 100644
index 000000000..e948d5708
--- /dev/null
+++ b/arch/powerpc/math-emu/mcrfs.c
@@ -0,0 +1,32 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+
+int
+mcrfs(u32 *ccr, u32 crfD, u32 crfS)
+{
+	u32 value, clear;
+
+#ifdef DEBUG
+	printk("%s: %p (%08x) %d %d\n", __func__, ccr, *ccr, crfD, crfS);
+#endif
+
+	clear = 15 << ((7 - crfS) << 2);
+	if (!crfS)
+		clear = 0x90000000;
+
+	value = (__FPU_FPSCR >> ((7 - crfS) << 2)) & 15;
+	__FPU_FPSCR &= ~(clear);
+
+	*ccr &= ~(15 << ((7 - crfD) << 2));
+	*ccr |= (value << ((7 - crfD) << 2));
+
+#ifdef DEBUG
+	printk("CR: %08x\n", __func__, *ccr);
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/mffs.c b/arch/powerpc/math-emu/mffs.c
new file mode 100644
index 000000000..5526cf96e
--- /dev/null
+++ b/arch/powerpc/math-emu/mffs.c
@@ -0,0 +1,18 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+
+int
+mffs(u32 *frD)
+{
+	frD[1] = __FPU_FPSCR;
+
+#ifdef DEBUG
+	printk("%s: frD %p: %08x.%08x\n", __func__, frD, frD[0], frD[1]);
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/mtfsb0.c b/arch/powerpc/math-emu/mtfsb0.c
new file mode 100644
index 000000000..bc985585b
--- /dev/null
+++ b/arch/powerpc/math-emu/mtfsb0.c
@@ -0,0 +1,19 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+
+int
+mtfsb0(int crbD)
+{
+	if ((crbD != 1) && (crbD != 2))
+		__FPU_FPSCR &= ~(1 << (31 - crbD));
+
+#ifdef DEBUG
+	printk("%s: %d %08lx\n", __func__, crbD, __FPU_FPSCR);
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/mtfsb1.c b/arch/powerpc/math-emu/mtfsb1.c
new file mode 100644
index 000000000..fe6ed5ac8
--- /dev/null
+++ b/arch/powerpc/math-emu/mtfsb1.c
@@ -0,0 +1,19 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+
+int
+mtfsb1(int crbD)
+{
+	if ((crbD != 1) && (crbD != 2))
+		__FPU_FPSCR |= (1 << (31 - crbD));
+
+#ifdef DEBUG
+	printk("%s: %d %08lx\n", __func__, crbD, __FPU_FPSCR);
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/mtfsf.c b/arch/powerpc/math-emu/mtfsf.c
new file mode 100644
index 000000000..44b0fc821
--- /dev/null
+++ b/arch/powerpc/math-emu/mtfsf.c
@@ -0,0 +1,51 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+
+int
+mtfsf(unsigned int FM, u32 *frB)
+{
+	u32 mask;
+	u32 fpscr;
+
+	if (likely(FM == 1))
+		mask = 0x0f;
+	else if (likely(FM == 0xff))
+		mask = ~0;
+	else {
+		mask = ((FM & 1) |
+				((FM << 3) & 0x10) |
+				((FM << 6) & 0x100) |
+				((FM << 9) & 0x1000) |
+				((FM << 12) & 0x10000) |
+				((FM << 15) & 0x100000) |
+				((FM << 18) & 0x1000000) |
+				((FM << 21) & 0x10000000)) * 15;
+	}
+
+	fpscr = ((__FPU_FPSCR & ~mask) | (frB[1] & mask)) &
+		~(FPSCR_VX | FPSCR_FEX | 0x800);
+
+	if (fpscr & (FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI |
+		     FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC |
+		     FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI))
+		fpscr |= FPSCR_VX;
+
+	/* The bit order of exception enables and exception status
+	 * is the same. Simply shift and mask to check for enabled
+	 * exceptions.
+	 */
+	if (fpscr & (fpscr >> 22) &  0xf8)
+		fpscr |= FPSCR_FEX;
+
+	__FPU_FPSCR = fpscr;
+
+#ifdef DEBUG
+	printk("%s: %02x %p: %08lx\n", __func__, FM, frB, __FPU_FPSCR);
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/mtfsfi.c b/arch/powerpc/math-emu/mtfsfi.c
new file mode 100644
index 000000000..fd2acc268
--- /dev/null
+++ b/arch/powerpc/math-emu/mtfsfi.c
@@ -0,0 +1,24 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+
+int
+mtfsfi(unsigned int crfD, unsigned int IMM)
+{
+	u32 mask = 0xf;
+
+	if (!crfD)
+		mask = 9;
+
+	__FPU_FPSCR &= ~(mask << ((7 - crfD) << 2));
+	__FPU_FPSCR |= (IMM & 0xf) << ((7 - crfD) << 2);
+
+#ifdef DEBUG
+	printk("%s: %d %x: %08lx\n", __func__, crfD, IMM, __FPU_FPSCR);
+#endif
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/stfd.c b/arch/powerpc/math-emu/stfd.c
new file mode 100644
index 000000000..33a165c8d
--- /dev/null
+++ b/arch/powerpc/math-emu/stfd.c
@@ -0,0 +1,20 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int
+stfd(void *frS, void *ea)
+{
+#if 0
+#ifdef DEBUG
+	printk("%s: S %p, ea %p: ", __func__, frS, ea);
+	dump_double(frS);
+	printk("\n");
+#endif
+#endif
+
+	if (copy_to_user(ea, frS, sizeof(double)))
+		return -EFAULT;
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/stfiwx.c b/arch/powerpc/math-emu/stfiwx.c
new file mode 100644
index 000000000..f15a35f67
--- /dev/null
+++ b/arch/powerpc/math-emu/stfiwx.c
@@ -0,0 +1,16 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+int
+stfiwx(u32 *frS, void *ea)
+{
+#ifdef DEBUG
+	printk("%s: %p %p\n", __func__, frS, ea);
+#endif
+
+	if (copy_to_user(ea, &frS[1], sizeof(frS[1])))
+		return -EFAULT;
+
+	return 0;
+}
diff --git a/arch/powerpc/math-emu/stfs.c b/arch/powerpc/math-emu/stfs.c
new file mode 100644
index 000000000..612214735
--- /dev/null
+++ b/arch/powerpc/math-emu/stfs.c
@@ -0,0 +1,42 @@
+#include <linux/types.h>
+#include <linux/errno.h>
+#include <asm/uaccess.h>
+
+#include <asm/sfp-machine.h>
+#include <math-emu/soft-fp.h>
+#include <math-emu/double.h>
+#include <math-emu/single.h>
+
+int
+stfs(void *frS, void *ea)
+{
+	FP_DECL_D(A);
+	FP_DECL_S(R);
+	FP_DECL_EX;
+	float f;
+
+#ifdef DEBUG
+	printk("%s: S %p, ea %p\n", __func__, frS, ea);
+#endif
+
+	FP_UNPACK_DP(A, frS);
+
+#ifdef DEBUG
+	printk("A: %ld %lu %lu %ld (%ld)\n", A_s, A_f1, A_f0, A_e, A_c);
+#endif
+
+	FP_CONV(S, D, 1, 2, R, A);
+
+#ifdef DEBUG
+	printk("R: %ld %lu %ld (%ld)\n", R_s, R_f, R_e, R_c);
+#endif
+
+	_FP_PACK_CANONICAL(S, 1, R);
+	if (!FP_CUR_EXCEPTIONS || !__FPU_TRAP_P(FP_CUR_EXCEPTIONS)) {
+		_FP_PACK_RAW_1_P(S, &f, R);
+		if (copy_to_user(ea, &f, sizeof(float)))
+			return -EFAULT;
+	}
+
+	return FP_CUR_EXCEPTIONS;
+}
diff --git a/arch/powerpc/math-emu/udivmodti4.c b/arch/powerpc/math-emu/udivmodti4.c
new file mode 100644
index 000000000..6172044ab
--- /dev/null
+++ b/arch/powerpc/math-emu/udivmodti4.c
@@ -0,0 +1,191 @@
+/* This has so very few changes over libgcc2's __udivmoddi4 it isn't funny.  */
+
+#include <math-emu/soft-fp.h>
+
+#undef count_leading_zeros
+#define count_leading_zeros  __FP_CLZ
+
+void
+_fp_udivmodti4(_FP_W_TYPE q[2], _FP_W_TYPE r[2],
+	       _FP_W_TYPE n1, _FP_W_TYPE n0,
+	       _FP_W_TYPE d1, _FP_W_TYPE d0)
+{
+  _FP_W_TYPE q0, q1, r0, r1;
+  _FP_I_TYPE b, bm;
+
+  if (d1 == 0)
+    {
+#if !UDIV_NEEDS_NORMALIZATION
+      if (d0 > n1)
+	{
+	  /* 0q = nn / 0D */
+
+	  udiv_qrnnd (q0, n0, n1, n0, d0);
+	  q1 = 0;
+
+	  /* Remainder in n0.  */
+	}
+      else
+	{
+	  /* qq = NN / 0d */
+
+	  if (d0 == 0)
+	    d0 = 1 / d0;	/* Divide intentionally by zero.  */
+
+	  udiv_qrnnd (q1, n1, 0, n1, d0);
+	  udiv_qrnnd (q0, n0, n1, n0, d0);
+
+	  /* Remainder in n0.  */
+	}
+
+      r0 = n0;
+      r1 = 0;
+
+#else /* UDIV_NEEDS_NORMALIZATION */
+
+      if (d0 > n1)
+	{
+	  /* 0q = nn / 0D */
+
+	  count_leading_zeros (bm, d0);
+
+	  if (bm != 0)
+	    {
+	      /* Normalize, i.e. make the most significant bit of the
+		 denominator set.  */
+
+	      d0 = d0 << bm;
+	      n1 = (n1 << bm) | (n0 >> (_FP_W_TYPE_SIZE - bm));
+	      n0 = n0 << bm;
+	    }
+
+	  udiv_qrnnd (q0, n0, n1, n0, d0);
+	  q1 = 0;
+
+	  /* Remainder in n0 >> bm.  */
+	}
+      else
+	{
+	  /* qq = NN / 0d */
+
+	  if (d0 == 0)
+	    d0 = 1 / d0;	/* Divide intentionally by zero.  */
+
+	  count_leading_zeros (bm, d0);
+
+	  if (bm == 0)
+	    {
+	      /* From (n1 >= d0) /\ (the most significant bit of d0 is set),
+		 conclude (the most significant bit of n1 is set) /\ (the
+		 leading quotient digit q1 = 1).
+
+		 This special case is necessary, not an optimization.
+		 (Shifts counts of SI_TYPE_SIZE are undefined.)  */
+
+	      n1 -= d0;
+	      q1 = 1;
+	    }
+	  else
+	    {
+	      _FP_W_TYPE n2;
+
+	      /* Normalize.  */
+
+	      b = _FP_W_TYPE_SIZE - bm;
+
+	      d0 = d0 << bm;
+	      n2 = n1 >> b;
+	      n1 = (n1 << bm) | (n0 >> b);
+	      n0 = n0 << bm;
+
+	      udiv_qrnnd (q1, n1, n2, n1, d0);
+	    }
+
+	  /* n1 != d0...  */
+
+	  udiv_qrnnd (q0, n0, n1, n0, d0);
+
+	  /* Remainder in n0 >> bm.  */
+	}
+
+      r0 = n0 >> bm;
+      r1 = 0;
+#endif /* UDIV_NEEDS_NORMALIZATION */
+    }
+  else
+    {
+      if (d1 > n1)
+	{
+	  /* 00 = nn / DD */
+
+	  q0 = 0;
+	  q1 = 0;
+
+	  /* Remainder in n1n0.  */
+	  r0 = n0;
+	  r1 = n1;
+	}
+      else
+	{
+	  /* 0q = NN / dd */
+
+	  count_leading_zeros (bm, d1);
+	  if (bm == 0)
+	    {
+	      /* From (n1 >= d1) /\ (the most significant bit of d1 is set),
+		 conclude (the most significant bit of n1 is set) /\ (the
+		 quotient digit q0 = 0 or 1).
+
+		 This special case is necessary, not an optimization.  */
+
+	      /* The condition on the next line takes advantage of that
+		 n1 >= d1 (true due to program flow).  */
+	      if (n1 > d1 || n0 >= d0)
+		{
+		  q0 = 1;
+		  sub_ddmmss (n1, n0, n1, n0, d1, d0);
+		}
+	      else
+		q0 = 0;
+
+	      q1 = 0;
+
+	      r0 = n0;
+	      r1 = n1;
+	    }
+	  else
+	    {
+	      _FP_W_TYPE m1, m0, n2;
+
+	      /* Normalize.  */
+
+	      b = _FP_W_TYPE_SIZE - bm;
+
+	      d1 = (d1 << bm) | (d0 >> b);
+	      d0 = d0 << bm;
+	      n2 = n1 >> b;
+	      n1 = (n1 << bm) | (n0 >> b);
+	      n0 = n0 << bm;
+
+	      udiv_qrnnd (q0, n1, n2, n1, d1);
+	      umul_ppmm (m1, m0, q0, d0);
+
+	      if (m1 > n1 || (m1 == n1 && m0 > n0))
+		{
+		  q0--;
+		  sub_ddmmss (m1, m0, m1, m0, d1, d0);
+		}
+
+	      q1 = 0;
+
+	      /* Remainder in (n1n0 - m1m0) >> bm.  */
+	      sub_ddmmss (n1, n0, n1, n0, m1, m0);
+	      r0 = (n1 << b) | (n0 >> bm);
+	      r1 = n1 >> bm;
+	    }
+	}
+    }
+
+  q[0] = q0; q[1] = q1;
+  r[0] = r0, r[1] = r1;
+}