From 62068f4cab38860d7b1325c5a59c09cf1f380336 Mon Sep 17 00:00:00 2001
From: Geert Uytterhoeven <geert@linux-m68k.org>
Date: Fri, 25 Jul 2003 19:37:07 -0700
Subject: [PATCH] [PATCH] m68k FPU emulator
MIME-Version: 1.0
Content-Type: text/plain; charset=utf8
Content-Transfer-Encoding: 8bit

M68k FPU emulator: Add fgetman, fgetexp and fsqrt (from Michael Müller and
Roman Zippel)
---
 arch/m68k/math-emu/fp_emu.h |  9 ++++
 arch/m68k/math-emu/fp_log.c | 87 +++++++++++++++++++++++++++++++++++--
 2 files changed, 93 insertions(+), 3 deletions(-)

diff --git a/arch/m68k/math-emu/fp_emu.h b/arch/m68k/math-emu/fp_emu.h
index 55ecb417e3d08..586226d1042ec 100644
--- a/arch/m68k/math-emu/fp_emu.h
+++ b/arch/m68k/math-emu/fp_emu.h
@@ -115,6 +115,15 @@ extern const struct fp_ext fp_Inf;
 	__res;							\
 })
 
+#define fp_conv_long2ext(dest, src) ({				\
+	register struct fp_ext *__dest asm ("a0") = dest;	\
+	register int __src asm ("d0") = src;			\
+								\
+	asm volatile ("jsr fp_conv_ext2long"			\
+			: : "d" (__src), "a" (__dest)		\
+			: "a1", "d1", "d2", "memory");		\
+})
+
 #else /* __ASSEMBLY__ */
 
 /*
diff --git a/arch/m68k/math-emu/fp_log.c b/arch/m68k/math-emu/fp_log.c
index 8e1c96420ec07..327999c0f79f1 100644
--- a/arch/m68k/math-emu/fp_log.c
+++ b/arch/m68k/math-emu/fp_log.c
@@ -17,10 +17,22 @@
 
 #include "fp_emu.h"
 
+static const struct fp_ext fp_one =
+{
+	0, 0, 0x3fff, { 0 }
+};
+
+extern struct fp_ext *fp_fadd(struct fp_ext *dest, const struct fp_ext *src);
+extern struct fp_ext *fp_fdiv(struct fp_ext *dest, const struct fp_ext *src);
+extern struct fp_ext *fp_fmul(struct fp_ext *dest, const struct fp_ext *src);
+
 struct fp_ext *
 fp_fsqrt(struct fp_ext *dest, struct fp_ext *src)
 {
-	uprint("fsqrt\n");
+	struct fp_ext tmp, src2;
+	int i, exp;
+
+	dprint(PINSTR, "fsqrt\n");
 
 	fp_monadic_check(dest, src);
 
@@ -34,6 +46,56 @@ fp_fsqrt(struct fp_ext *dest, struct fp_ext *src)
 	if (IS_INF(dest))
 		return dest;
 
+	/*
+	 *		 sqrt(m) * 2^(p)	, if e = 2*p
+	 * sqrt(m*2^e) = 
+	 *		 sqrt(2*m) * 2^(p)	, if e = 2*p + 1
+	 *
+	 * So we use the last bit of the exponent to decide wether to
+	 * use the m or 2*m.
+	 *
+	 * Since only the fractional part of the mantissa is stored and
+	 * the integer part is assumed to be one, we place a 1 or 2 into
+	 * the fixed point representation.
+	 */
+	exp = dest->exp;
+	dest->exp = 0x3FFF;
+	if (!(exp & 1))		/* lowest bit of exponent is set */
+		dest->exp++;
+	fp_copy_ext(&src2, dest);
+
+	/*
+	 * The taylor row arround a for sqrt(x) is:
+	 *	sqrt(x) = sqrt(a) + 1/(2*sqrt(a))*(x-a) + R
+	 * With a=1 this gives:
+	 *	sqrt(x) = 1 + 1/2*(x-1)
+	 *		= 1/2*(1+x)
+	 */
+	fp_fadd(dest, &fp_one);
+	dest->exp--;		/* * 1/2 */
+
+	/*
+	 * We now apply the newton rule to the function
+	 *	f(x) := x^2 - r
+	 * which has a null point on x = sqrt(r).
+	 *
+	 * It gives:
+	 * 	x' := x - f(x)/f'(x)
+	 *	    = x - (x^2 -r)/(2*x)
+	 *	    = x - (x - r/x)/2
+	 *          = (2*x - x + r/x)/2
+	 *	    = (x + r/x)/2
+	 */
+	for (i = 0; i < 9; i++) {
+		fp_copy_ext(&tmp, &src2);
+
+		fp_fdiv(&tmp, dest);
+		fp_fadd(dest, &tmp);
+		dest->exp--;
+	}
+
+	dest->exp += (exp - 0x3FFF) / 2;
+
 	return dest;
 }
 
@@ -123,20 +185,39 @@ fp_flog2(struct fp_ext *dest, struct fp_ext *src)
 struct fp_ext *
 fp_fgetexp(struct fp_ext *dest, struct fp_ext *src)
 {
-	uprint("fgetexp\n");
+	dprint(PINSTR, "fgetexp\n");
 
 	fp_monadic_check(dest, src);
 
+	if (IS_INF(dest)) {
+		fp_set_nan(dest);
+		return dest;
+	}
+	if (IS_ZERO(dest))
+		return dest;
+
+	fp_conv_long2ext(dest, (int)dest->exp - 0x3FFF);
+
+	fp_normalize_ext(dest);
+
 	return dest;
 }
 
 struct fp_ext *
 fp_fgetman(struct fp_ext *dest, struct fp_ext *src)
 {
-	uprint("fgetman\n");
+	dprint(PINSTR, "fgetman\n");
 
 	fp_monadic_check(dest, src);
 
+	if (IS_ZERO(dest))
+		return dest;
+
+	if (IS_INF(dest))
+		return dest;
+
+	dest->exp = 0x3FFF;
+
 	return dest;
 }
 
-- 
2.53.0