[PATCH] m68k FPU emulator

M68k FPU emulator: Add fgetman, fgetexp and fsqrt (from Michael Müller and
Roman Zippel)

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__ */
 
 /*

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;
 }