diff options
Diffstat (limited to 'arch/arm/vfp')
| -rw-r--r-- | arch/arm/vfp/Makefile | 12 | ||||
| -rw-r--r-- | arch/arm/vfp/entry.S | 45 | ||||
| -rw-r--r-- | arch/arm/vfp/vfp.h | 344 | ||||
| -rw-r--r-- | arch/arm/vfp/vfpdouble.c | 1186 | ||||
| -rw-r--r-- | arch/arm/vfp/vfphw.S | 215 | ||||
| -rw-r--r-- | arch/arm/vfp/vfpinstr.h | 88 | ||||
| -rw-r--r-- | arch/arm/vfp/vfpmodule.c | 288 | ||||
| -rw-r--r-- | arch/arm/vfp/vfpsingle.c | 1224 |
8 files changed, 3402 insertions, 0 deletions
diff --git a/arch/arm/vfp/Makefile b/arch/arm/vfp/Makefile new file mode 100644 index 00000000000..afabac31dd1 --- /dev/null +++ b/arch/arm/vfp/Makefile @@ -0,0 +1,12 @@ +# +# linux/arch/arm/vfp/Makefile +# +# Copyright (C) 2001 ARM Limited +# + +# EXTRA_CFLAGS := -DDEBUG +# EXTRA_AFLAGS := -DDEBUG + +obj-y += vfp.o + +vfp-$(CONFIG_VFP) += entry.o vfpmodule.o vfphw.o vfpsingle.o vfpdouble.o diff --git a/arch/arm/vfp/entry.S b/arch/arm/vfp/entry.S new file mode 100644 index 00000000000..e73c8deca59 --- /dev/null +++ b/arch/arm/vfp/entry.S @@ -0,0 +1,45 @@ +/* + * linux/arch/arm/vfp/entry.S + * + * Copyright (C) 2004 ARM Limited. + * Written by Deep Blue Solutions Limited. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * Basic entry code, called from the kernel's undefined instruction trap. + * r0 = faulted instruction + * r5 = faulted PC+4 + * r9 = successful return + * r10 = thread_info structure + * lr = failure return + */ +#include <linux/linkage.h> +#include <linux/init.h> +#include <asm/constants.h> +#include <asm/vfpmacros.h> + + .globl do_vfp +do_vfp: + ldr r4, .LCvfp + add r10, r10, #TI_VFPSTATE @ r10 = workspace + ldr pc, [r4] @ call VFP entry point + +.LCvfp: + .word vfp_vector + +@ This code is called if the VFP does not exist. It needs to flag the +@ failure to the VFP initialisation code. + + __INIT + .globl vfp_testing_entry +vfp_testing_entry: + ldr r0, VFP_arch_address + str r5, [r0] @ known non-zero value + mov pc, r9 @ we have handled the fault + +VFP_arch_address: + .word VFP_arch + + __FINIT diff --git a/arch/arm/vfp/vfp.h b/arch/arm/vfp/vfp.h new file mode 100644 index 00000000000..55a02bc994a --- /dev/null +++ b/arch/arm/vfp/vfp.h @@ -0,0 +1,344 @@ +/* + * linux/arch/arm/vfp/vfp.h + * + * Copyright (C) 2004 ARM Limited. + * Written by Deep Blue Solutions Limited. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ + +static inline u32 vfp_shiftright32jamming(u32 val, unsigned int shift) +{ + if (shift) { + if (shift < 32) + val = val >> shift | ((val << (32 - shift)) != 0); + else + val = val != 0; + } + return val; +} + +static inline u64 vfp_shiftright64jamming(u64 val, unsigned int shift) +{ + if (shift) { + if (shift < 64) + val = val >> shift | ((val << (64 - shift)) != 0); + else + val = val != 0; + } + return val; +} + +static inline u32 vfp_hi64to32jamming(u64 val) +{ + u32 v; + + asm( + "cmp %Q1, #1 @ vfp_hi64to32jamming\n\t" + "movcc %0, %R1\n\t" + "orrcs %0, %R1, #1" + : "=r" (v) : "r" (val) : "cc"); + + return v; +} + +static inline void add128(u64 *resh, u64 *resl, u64 nh, u64 nl, u64 mh, u64 ml) +{ + asm( "adds %Q0, %Q2, %Q4\n\t" + "adcs %R0, %R2, %R4\n\t" + "adcs %Q1, %Q3, %Q5\n\t" + "adc %R1, %R3, %R5" + : "=r" (nl), "=r" (nh) + : "0" (nl), "1" (nh), "r" (ml), "r" (mh) + : "cc"); + *resh = nh; + *resl = nl; +} + +static inline void sub128(u64 *resh, u64 *resl, u64 nh, u64 nl, u64 mh, u64 ml) +{ + asm( "subs %Q0, %Q2, %Q4\n\t" + "sbcs %R0, %R2, %R4\n\t" + "sbcs %Q1, %Q3, %Q5\n\t" + "sbc %R1, %R3, %R5\n\t" + : "=r" (nl), "=r" (nh) + : "0" (nl), "1" (nh), "r" (ml), "r" (mh) + : "cc"); + *resh = nh; + *resl = nl; +} + +static inline void mul64to128(u64 *resh, u64 *resl, u64 n, u64 m) +{ + u32 nh, nl, mh, ml; + u64 rh, rma, rmb, rl; + + nl = n; + ml = m; + rl = (u64)nl * ml; + + nh = n >> 32; + rma = (u64)nh * ml; + + mh = m >> 32; + rmb = (u64)nl * mh; + rma += rmb; + + rh = (u64)nh * mh; + rh += ((u64)(rma < rmb) << 32) + (rma >> 32); + + rma <<= 32; + rl += rma; + rh += (rl < rma); + + *resl = rl; + *resh = rh; +} + +static inline void shift64left(u64 *resh, u64 *resl, u64 n) +{ + *resh = n >> 63; + *resl = n << 1; +} + +static inline u64 vfp_hi64multiply64(u64 n, u64 m) +{ + u64 rh, rl; + mul64to128(&rh, &rl, n, m); + return rh | (rl != 0); +} + +static inline u64 vfp_estimate_div128to64(u64 nh, u64 nl, u64 m) +{ + u64 mh, ml, remh, reml, termh, terml, z; + + if (nh >= m) + return ~0ULL; + mh = m >> 32; + z = (mh << 32 <= nh) ? 0xffffffff00000000ULL : (nh / mh) << 32; + mul64to128(&termh, &terml, m, z); + sub128(&remh, &reml, nh, nl, termh, terml); + ml = m << 32; + while ((s64)remh < 0) { + z -= 0x100000000ULL; + add128(&remh, &reml, remh, reml, mh, ml); + } + remh = (remh << 32) | (reml >> 32); + z |= (mh << 32 <= remh) ? 0xffffffff : remh / mh; + return z; +} + +/* + * Operations on unpacked elements + */ +#define vfp_sign_negate(sign) (sign ^ 0x8000) + +/* + * Single-precision + */ +struct vfp_single { + s16 exponent; + u16 sign; + u32 significand; +}; + +extern s32 vfp_get_float(unsigned int reg); +extern void vfp_put_float(unsigned int reg, s32 val); + +/* + * VFP_SINGLE_MANTISSA_BITS - number of bits in the mantissa + * VFP_SINGLE_EXPONENT_BITS - number of bits in the exponent + * VFP_SINGLE_LOW_BITS - number of low bits in the unpacked significand + * which are not propagated to the float upon packing. + */ +#define VFP_SINGLE_MANTISSA_BITS (23) +#define VFP_SINGLE_EXPONENT_BITS (8) +#define VFP_SINGLE_LOW_BITS (32 - VFP_SINGLE_MANTISSA_BITS - 2) +#define VFP_SINGLE_LOW_BITS_MASK ((1 << VFP_SINGLE_LOW_BITS) - 1) + +/* + * The bit in an unpacked float which indicates that it is a quiet NaN + */ +#define VFP_SINGLE_SIGNIFICAND_QNAN (1 << (VFP_SINGLE_MANTISSA_BITS - 1 + VFP_SINGLE_LOW_BITS)) + +/* + * Operations on packed single-precision numbers + */ +#define vfp_single_packed_sign(v) ((v) & 0x80000000) +#define vfp_single_packed_negate(v) ((v) ^ 0x80000000) +#define vfp_single_packed_abs(v) ((v) & ~0x80000000) +#define vfp_single_packed_exponent(v) (((v) >> VFP_SINGLE_MANTISSA_BITS) & ((1 << VFP_SINGLE_EXPONENT_BITS) - 1)) +#define vfp_single_packed_mantissa(v) ((v) & ((1 << VFP_SINGLE_MANTISSA_BITS) - 1)) + +/* + * Unpack a single-precision float. Note that this returns the magnitude + * of the single-precision float mantissa with the 1. if necessary, + * aligned to bit 30. + */ +static inline void vfp_single_unpack(struct vfp_single *s, s32 val) +{ + u32 significand; + + s->sign = vfp_single_packed_sign(val) >> 16, + s->exponent = vfp_single_packed_exponent(val); + + significand = (u32) val; + significand = (significand << (32 - VFP_SINGLE_MANTISSA_BITS)) >> 2; + if (s->exponent && s->exponent != 255) + significand |= 0x40000000; + s->significand = significand; +} + +/* + * Re-pack a single-precision float. This assumes that the float is + * already normalised such that the MSB is bit 30, _not_ bit 31. + */ +static inline s32 vfp_single_pack(struct vfp_single *s) +{ + u32 val; + val = (s->sign << 16) + + (s->exponent << VFP_SINGLE_MANTISSA_BITS) + + (s->significand >> VFP_SINGLE_LOW_BITS); + return (s32)val; +} + +#define VFP_NUMBER (1<<0) +#define VFP_ZERO (1<<1) +#define VFP_DENORMAL (1<<2) +#define VFP_INFINITY (1<<3) +#define VFP_NAN (1<<4) +#define VFP_NAN_SIGNAL (1<<5) + +#define VFP_QNAN (VFP_NAN) +#define VFP_SNAN (VFP_NAN|VFP_NAN_SIGNAL) + +static inline int vfp_single_type(struct vfp_single *s) +{ + int type = VFP_NUMBER; + if (s->exponent == 255) { + if (s->significand == 0) + type = VFP_INFINITY; + else if (s->significand & VFP_SINGLE_SIGNIFICAND_QNAN) + type = VFP_QNAN; + else + type = VFP_SNAN; + } else if (s->exponent == 0) { + if (s->significand == 0) + type |= VFP_ZERO; + else + type |= VFP_DENORMAL; + } + return type; +} + +#ifndef DEBUG +#define vfp_single_normaliseround(sd,vsd,fpscr,except,func) __vfp_single_normaliseround(sd,vsd,fpscr,except) +u32 __vfp_single_normaliseround(int sd, struct vfp_single *vs, u32 fpscr, u32 exceptions); +#else +u32 vfp_single_normaliseround(int sd, struct vfp_single *vs, u32 fpscr, u32 exceptions, const char *func); +#endif + +/* + * Double-precision + */ +struct vfp_double { + s16 exponent; + u16 sign; + u64 significand; +}; + +/* + * VFP_REG_ZERO is a special register number for vfp_get_double + * which returns (double)0.0. This is useful for the compare with + * zero instructions. + */ +#define VFP_REG_ZERO 16 +extern u64 vfp_get_double(unsigned int reg); +extern void vfp_put_double(unsigned int reg, u64 val); + +#define VFP_DOUBLE_MANTISSA_BITS (52) +#define VFP_DOUBLE_EXPONENT_BITS (11) +#define VFP_DOUBLE_LOW_BITS (64 - VFP_DOUBLE_MANTISSA_BITS - 2) +#define VFP_DOUBLE_LOW_BITS_MASK ((1 << VFP_DOUBLE_LOW_BITS) - 1) + +/* + * The bit in an unpacked double which indicates that it is a quiet NaN + */ +#define VFP_DOUBLE_SIGNIFICAND_QNAN (1ULL << (VFP_DOUBLE_MANTISSA_BITS - 1 + VFP_DOUBLE_LOW_BITS)) + +/* + * Operations on packed single-precision numbers + */ +#define vfp_double_packed_sign(v) ((v) & (1ULL << 63)) +#define vfp_double_packed_negate(v) ((v) ^ (1ULL << 63)) +#define vfp_double_packed_abs(v) ((v) & ~(1ULL << 63)) +#define vfp_double_packed_exponent(v) (((v) >> VFP_DOUBLE_MANTISSA_BITS) & ((1 << VFP_DOUBLE_EXPONENT_BITS) - 1)) +#define vfp_double_packed_mantissa(v) ((v) & ((1ULL << VFP_DOUBLE_MANTISSA_BITS) - 1)) + +/* + * Unpack a double-precision float. Note that this returns the magnitude + * of the double-precision float mantissa with the 1. if necessary, + * aligned to bit 62. + */ +static inline void vfp_double_unpack(struct vfp_double *s, s64 val) +{ + u64 significand; + + s->sign = vfp_double_packed_sign(val) >> 48; + s->exponent = vfp_double_packed_exponent(val); + + significand = (u64) val; + significand = (significand << (64 - VFP_DOUBLE_MANTISSA_BITS)) >> 2; + if (s->exponent && s->exponent != 2047) + significand |= (1ULL << 62); + s->significand = significand; +} + +/* + * Re-pack a double-precision float. This assumes that the float is + * already normalised such that the MSB is bit 30, _not_ bit 31. + */ +static inline s64 vfp_double_pack(struct vfp_double *s) +{ + u64 val; + val = ((u64)s->sign << 48) + + ((u64)s->exponent << VFP_DOUBLE_MANTISSA_BITS) + + (s->significand >> VFP_DOUBLE_LOW_BITS); + return (s64)val; +} + +static inline int vfp_double_type(struct vfp_double *s) +{ + int type = VFP_NUMBER; + if (s->exponent == 2047) { + if (s->significand == 0) + type = VFP_INFINITY; + else if (s->significand & VFP_DOUBLE_SIGNIFICAND_QNAN) + type = VFP_QNAN; + else + type = VFP_SNAN; + } else if (s->exponent == 0) { + if (s->significand == 0) + type |= VFP_ZERO; + else + type |= VFP_DENORMAL; + } + return type; +} + +u32 vfp_double_normaliseround(int dd, struct vfp_double *vd, u32 fpscr, u32 exceptions, const char *func); + +/* + * System registers + */ +extern u32 vfp_get_sys(unsigned int reg); +extern void vfp_put_sys(unsigned int reg, u32 val); + +u32 vfp_estimate_sqrt_significand(u32 exponent, u32 significand); + +/* + * A special flag to tell the normalisation code not to normalise. + */ +#define VFP_NAN_FLAG 0x100 diff --git a/arch/arm/vfp/vfpdouble.c b/arch/arm/vfp/vfpdouble.c new file mode 100644 index 00000000000..fa3053e84db --- /dev/null +++ b/arch/arm/vfp/vfpdouble.c @@ -0,0 +1,1186 @@ +/* + * linux/arch/arm/vfp/vfpdouble.c + * + * This code is derived in part from John R. Housers softfloat library, which + * carries the following notice: + * + * =========================================================================== + * This C source file is part of the SoftFloat IEC/IEEE Floating-point + * Arithmetic Package, Release 2. + * + * Written by John R. Hauser. This work was made possible in part by the + * International Computer Science Institute, located at Suite 600, 1947 Center + * Street, Berkeley, California 94704. Funding was partially provided by the + * National Science Foundation under grant MIP-9311980. The original version + * of this code was written as part of a project to build a fixed-point vector + * processor in collaboration with the University of California at Berkeley, + * overseen by Profs. Nelson Morgan and John Wawrzynek. More information + * is available through the web page `http://HTTP.CS.Berkeley.EDU/~jhauser/ + * arithmetic/softfloat.html'. + * + * THIS SOFTWARE IS DISTRIBUTED AS IS, FOR FREE. Although reasonable effort + * has been made to avoid it, THIS SOFTWARE MAY CONTAIN FAULTS THAT WILL AT + * TIMES RESULT IN INCORRECT BEHAVIOR. USE OF THIS SOFTWARE IS RESTRICTED TO + * PERSONS AND ORGANIZATIONS WHO CAN AND WILL TAKE FULL RESPONSIBILITY FOR ANY + * AND ALL LOSSES, COSTS, OR OTHER PROBLEMS ARISING FROM ITS USE. + * + * Derivative works are acceptable, even for commercial purposes, so long as + * (1) they include prominent notice that the work is derivative, and (2) they + * include prominent notice akin to these three paragraphs for those parts of + * this code that are retained. + * =========================================================================== + */ +#include <linux/kernel.h> +#include <linux/bitops.h> +#include <asm/ptrace.h> +#include <asm/vfp.h> + +#include "vfpinstr.h" +#include "vfp.h" + +static struct vfp_double vfp_double_default_qnan = { + .exponent = 2047, + .sign = 0, + .significand = VFP_DOUBLE_SIGNIFICAND_QNAN, +}; + +static void vfp_double_dump(const char *str, struct vfp_double *d) +{ + pr_debug("VFP: %s: sign=%d exponent=%d significand=%016llx\n", + str, d->sign != 0, d->exponent, d->significand); +} + +static void vfp_double_normalise_denormal(struct vfp_double *vd) +{ + int bits = 31 - fls(vd->significand >> 32); + if (bits == 31) + bits = 62 - fls(vd->significand); + + vfp_double_dump("normalise_denormal: in", vd); + + if (bits) { + vd->exponent -= bits - 1; + vd->significand <<= bits; + } + + vfp_double_dump("normalise_denormal: out", vd); +} + +u32 vfp_double_normaliseround(int dd, struct vfp_double *vd, u32 fpscr, u32 exceptions, const char *func) +{ + u64 significand, incr; + int exponent, shift, underflow; + u32 rmode; + + vfp_double_dump("pack: in", vd); + + /* + * Infinities and NaNs are a special case. + */ + if (vd->exponent == 2047 && (vd->significand == 0 || exceptions)) + goto pack; + + /* + * Special-case zero. + */ + if (vd->significand == 0) { + vd->exponent = 0; + goto pack; + } + + exponent = vd->exponent; + significand = vd->significand; + + shift = 32 - fls(significand >> 32); + if (shift == 32) + shift = 64 - fls(significand); + if (shift) { + exponent -= shift; + significand <<= shift; + } + +#ifdef DEBUG + vd->exponent = exponent; + vd->significand = significand; + vfp_double_dump("pack: normalised", vd); +#endif + + /* + * Tiny number? + */ + underflow = exponent < 0; + if (underflow) { + significand = vfp_shiftright64jamming(significand, -exponent); + exponent = 0; +#ifdef DEBUG + vd->exponent = exponent; + vd->significand = significand; + vfp_double_dump("pack: tiny number", vd); +#endif + if (!(significand & ((1ULL << (VFP_DOUBLE_LOW_BITS + 1)) - 1))) + underflow = 0; + } + + /* + * Select rounding increment. + */ + incr = 0; + rmode = fpscr & FPSCR_RMODE_MASK; + + if (rmode == FPSCR_ROUND_NEAREST) { + incr = 1ULL << VFP_DOUBLE_LOW_BITS; + if ((significand & (1ULL << (VFP_DOUBLE_LOW_BITS + 1))) == 0) + incr -= 1; + } else if (rmode == FPSCR_ROUND_TOZERO) { + incr = 0; + } else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vd->sign != 0)) + incr = (1ULL << (VFP_DOUBLE_LOW_BITS + 1)) - 1; + + pr_debug("VFP: rounding increment = 0x%08llx\n", incr); + + /* + * Is our rounding going to overflow? + */ + if ((significand + incr) < significand) { + exponent += 1; + significand = (significand >> 1) | (significand & 1); + incr >>= 1; +#ifdef DEBUG + vd->exponent = exponent; + vd->significand = significand; + vfp_double_dump("pack: overflow", vd); +#endif + } + + /* + * If any of the low bits (which will be shifted out of the + * number) are non-zero, the result is inexact. + */ + if (significand & ((1 << (VFP_DOUBLE_LOW_BITS + 1)) - 1)) + exceptions |= FPSCR_IXC; + + /* + * Do our rounding. + */ + significand += incr; + + /* + * Infinity? + */ + if (exponent >= 2046) { + exceptions |= FPSCR_OFC | FPSCR_IXC; + if (incr == 0) { + vd->exponent = 2045; + vd->significand = 0x7fffffffffffffffULL; + } else { + vd->exponent = 2047; /* infinity */ + vd->significand = 0; + } + } else { + if (significand >> (VFP_DOUBLE_LOW_BITS + 1) == 0) + exponent = 0; + if (exponent || significand > 0x8000000000000000ULL) + underflow = 0; + if (underflow) + exceptions |= FPSCR_UFC; + vd->exponent = exponent; + vd->significand = significand >> 1; + } + + pack: + vfp_double_dump("pack: final", vd); + { + s64 d = vfp_double_pack(vd); + pr_debug("VFP: %s: d(d%d)=%016llx exceptions=%08x\n", func, + dd, d, exceptions); + vfp_put_double(dd, d); + } + return exceptions & ~VFP_NAN_FLAG; +} + +/* + * Propagate the NaN, setting exceptions if it is signalling. + * 'n' is always a NaN. 'm' may be a number, NaN or infinity. + */ +static u32 +vfp_propagate_nan(struct vfp_double *vdd, struct vfp_double *vdn, + struct vfp_double *vdm, u32 fpscr) +{ + struct vfp_double *nan; + int tn, tm = 0; + + tn = vfp_double_type(vdn); + + if (vdm) + tm = vfp_double_type(vdm); + + if (fpscr & FPSCR_DEFAULT_NAN) + /* + * Default NaN mode - always returns a quiet NaN + */ + nan = &vfp_double_default_qnan; + else { + /* + * Contemporary mode - select the first signalling + * NAN, or if neither are signalling, the first + * quiet NAN. + */ + if (tn == VFP_SNAN || (tm != VFP_SNAN && tn == VFP_QNAN)) + nan = vdn; + else + nan = vdm; + /* + * Make the NaN quiet. + */ + nan->significand |= VFP_DOUBLE_SIGNIFICAND_QNAN; + } + + *vdd = *nan; + + /* + * If one was a signalling NAN, raise invalid operation. + */ + return tn == VFP_SNAN || tm == VFP_SNAN ? FPSCR_IOC : VFP_NAN_FLAG; +} + +/* + * Extended operations + */ +static u32 vfp_double_fabs(int dd, int unused, int dm, u32 fpscr) +{ + vfp_put_double(dd, vfp_double_packed_abs(vfp_get_double(dm))); + return 0; +} + +static u32 vfp_double_fcpy(int dd, int unused, int dm, u32 fpscr) +{ + vfp_put_double(dd, vfp_get_double(dm)); + return 0; +} + +static u32 vfp_double_fneg(int dd, int unused, int dm, u32 fpscr) +{ + vfp_put_double(dd, vfp_double_packed_negate(vfp_get_double(dm))); + return 0; +} + +static u32 vfp_double_fsqrt(int dd, int unused, int dm, u32 fpscr) +{ + struct vfp_double vdm, vdd; + int ret, tm; + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + tm = vfp_double_type(&vdm); + if (tm & (VFP_NAN|VFP_INFINITY)) { + struct vfp_double *vdp = &vdd; + + if (tm & VFP_NAN) + ret = vfp_propagate_nan(vdp, &vdm, NULL, fpscr); + else if (vdm.sign == 0) { + sqrt_copy: + vdp = &vdm; + ret = 0; + } else { + sqrt_invalid: + vdp = &vfp_double_default_qnan; + ret = FPSCR_IOC; + } + vfp_put_double(dd, vfp_double_pack(vdp)); + return ret; + } + + /* + * sqrt(+/- 0) == +/- 0 + */ + if (tm & VFP_ZERO) + goto sqrt_copy; + + /* + * Normalise a denormalised number + */ + if (tm & VFP_DENORMAL) + vfp_double_normalise_denormal(&vdm); + + /* + * sqrt(<0) = invalid + */ + if (vdm.sign) + goto sqrt_invalid; + + vfp_double_dump("sqrt", &vdm); + + /* + * Estimate the square root. + */ + vdd.sign = 0; + vdd.exponent = ((vdm.exponent - 1023) >> 1) + 1023; + vdd.significand = (u64)vfp_estimate_sqrt_significand(vdm.exponent, vdm.significand >> 32) << 31; + + vfp_double_dump("sqrt estimate1", &vdd); + + vdm.significand >>= 1 + (vdm.exponent & 1); + vdd.significand += 2 + vfp_estimate_div128to64(vdm.significand, 0, vdd.significand); + + vfp_double_dump("sqrt estimate2", &vdd); + + /* + * And now adjust. + */ + if ((vdd.significand & VFP_DOUBLE_LOW_BITS_MASK) <= 5) { + if (vdd.significand < 2) { + vdd.significand = ~0ULL; + } else { + u64 termh, terml, remh, reml; + vdm.significand <<= 2; + mul64to128(&termh, &terml, vdd.significand, vdd.significand); + sub128(&remh, &reml, vdm.significand, 0, termh, terml); + while ((s64)remh < 0) { + vdd.significand -= 1; + shift64left(&termh, &terml, vdd.significand); + terml |= 1; + add128(&remh, &reml, remh, reml, termh, terml); + } + vdd.significand |= (remh | reml) != 0; + } + } + vdd.significand = vfp_shiftright64jamming(vdd.significand, 1); + + return vfp_double_normaliseround(dd, &vdd, fpscr, 0, "fsqrt"); +} + +/* + * Equal := ZC + * Less than := N + * Greater than := C + * Unordered := CV + */ +static u32 vfp_compare(int dd, int signal_on_qnan, int dm, u32 fpscr) +{ + s64 d, m; + u32 ret = 0; + + m = vfp_get_double(dm); + if (vfp_double_packed_exponent(m) == 2047 && vfp_double_packed_mantissa(m)) { + ret |= FPSCR_C | FPSCR_V; + if (signal_on_qnan || !(vfp_double_packed_mantissa(m) & (1ULL << (VFP_DOUBLE_MANTISSA_BITS - 1)))) + /* + * Signalling NaN, or signalling on quiet NaN + */ + ret |= FPSCR_IOC; + } + + d = vfp_get_double(dd); + if (vfp_double_packed_exponent(d) == 2047 && vfp_double_packed_mantissa(d)) { + ret |= FPSCR_C | FPSCR_V; + if (signal_on_qnan || !(vfp_double_packed_mantissa(d) & (1ULL << (VFP_DOUBLE_MANTISSA_BITS - 1)))) + /* + * Signalling NaN, or signalling on quiet NaN + */ + ret |= FPSCR_IOC; + } + + if (ret == 0) { + if (d == m || vfp_double_packed_abs(d | m) == 0) { + /* + * equal + */ + ret |= FPSCR_Z | FPSCR_C; + } else if (vfp_double_packed_sign(d ^ m)) { + /* + * different signs + */ + if (vfp_double_packed_sign(d)) + /* + * d is negative, so d < m + */ + ret |= FPSCR_N; + else + /* + * d is positive, so d > m + */ + ret |= FPSCR_C; + } else if ((vfp_double_packed_sign(d) != 0) ^ (d < m)) { + /* + * d < m + */ + ret |= FPSCR_N; + } else if ((vfp_double_packed_sign(d) != 0) ^ (d > m)) { + /* + * d > m + */ + ret |= FPSCR_C; + } + } + + return ret; +} + +static u32 vfp_double_fcmp(int dd, int unused, int dm, u32 fpscr) +{ + return vfp_compare(dd, 0, dm, fpscr); +} + +static u32 vfp_double_fcmpe(int dd, int unused, int dm, u32 fpscr) +{ + return vfp_compare(dd, 1, dm, fpscr); +} + +static u32 vfp_double_fcmpz(int dd, int unused, int dm, u32 fpscr) +{ + return vfp_compare(dd, 0, VFP_REG_ZERO, fpscr); +} + +static u32 vfp_double_fcmpez(int dd, int unused, int dm, u32 fpscr) +{ + return vfp_compare(dd, 1, VFP_REG_ZERO, fpscr); +} + +static u32 vfp_double_fcvts(int sd, int unused, int dm, u32 fpscr) +{ + struct vfp_double vdm; + struct vfp_single vsd; + int tm; + u32 exceptions = 0; + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + + tm = vfp_double_type(&vdm); + + /* + * If we have a signalling NaN, signal invalid operation. + */ + if (tm == VFP_SNAN) + exceptions = FPSCR_IOC; + + if (tm & VFP_DENORMAL) + vfp_double_normalise_denormal(&vdm); + + vsd.sign = vdm.sign; + vsd.significand = vfp_hi64to32jamming(vdm.significand); + + /* + * If we have an infinity or a NaN, the exponent must be 255 + */ + if (tm & (VFP_INFINITY|VFP_NAN)) { + vsd.exponent = 255; + if (tm & VFP_NAN) + vsd.significand |= VFP_SINGLE_SIGNIFICAND_QNAN; + goto pack_nan; + } else if (tm & VFP_ZERO) + vsd.exponent = 0; + else + vsd.exponent = vdm.exponent - (1023 - 127); + + return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, "fcvts"); + + pack_nan: + vfp_put_float(sd, vfp_single_pack(&vsd)); + return exceptions; +} + +static u32 vfp_double_fuito(int dd, int unused, int dm, u32 fpscr) +{ + struct vfp_double vdm; + u32 m = vfp_get_float(dm); + + vdm.sign = 0; + vdm.exponent = 1023 + 63 - 1; + vdm.significand = (u64)m; + + return vfp_double_normaliseround(dd, &vdm, fpscr, 0, "fuito"); +} + +static u32 vfp_double_fsito(int dd, int unused, int dm, u32 fpscr) +{ + struct vfp_double vdm; + u32 m = vfp_get_float(dm); + + vdm.sign = (m & 0x80000000) >> 16; + vdm.exponent = 1023 + 63 - 1; + vdm.significand = vdm.sign ? -m : m; + + return vfp_double_normaliseround(dd, &vdm, fpscr, 0, "fsito"); +} + +static u32 vfp_double_ftoui(int sd, int unused, int dm, u32 fpscr) +{ + struct vfp_double vdm; + u32 d, exceptions = 0; + int rmode = fpscr & FPSCR_RMODE_MASK; + int tm; + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + + /* + * Do we have a denormalised number? + */ + tm = vfp_double_type(&vdm); + if (tm & VFP_DENORMAL) + exceptions |= FPSCR_IDC; + + if (tm & VFP_NAN) + vdm.sign = 0; + + if (vdm.exponent >= 1023 + 32) { + d = vdm.sign ? 0 : 0xffffffff; + exceptions = FPSCR_IOC; + } else if (vdm.exponent >= 1023 - 1) { + int shift = 1023 + 63 - vdm.exponent; + u64 rem, incr = 0; + + /* + * 2^0 <= m < 2^32-2^8 + */ + d = (vdm.significand << 1) >> shift; + rem = vdm.significand << (65 - shift); + + if (rmode == FPSCR_ROUND_NEAREST) { + incr = 0x8000000000000000ULL; + if ((d & 1) == 0) + incr -= 1; + } else if (rmode == FPSCR_ROUND_TOZERO) { + incr = 0; + } else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vdm.sign != 0)) { + incr = ~0ULL; + } + + if ((rem + incr) < rem) { + if (d < 0xffffffff) + d += 1; + else + exceptions |= FPSCR_IOC; + } + + if (d && vdm.sign) { + d = 0; + exceptions |= FPSCR_IOC; + } else if (rem) + exceptions |= FPSCR_IXC; + } else { + d = 0; + if (vdm.exponent | vdm.significand) { + exceptions |= FPSCR_IXC; + if (rmode == FPSCR_ROUND_PLUSINF && vdm.sign == 0) + d = 1; + else if (rmode == FPSCR_ROUND_MINUSINF && vdm.sign) { + d = 0; + exceptions |= FPSCR_IOC; + } + } + } + + pr_debug("VFP: ftoui: d(s%d)=%08x exceptions=%08x\n", sd, d, exceptions); + + vfp_put_float(sd, d); + + return exceptions; +} + +static u32 vfp_double_ftouiz(int sd, int unused, int dm, u32 fpscr) +{ + return vfp_double_ftoui(sd, unused, dm, FPSCR_ROUND_TOZERO); +} + +static u32 vfp_double_ftosi(int sd, int unused, int dm, u32 fpscr) +{ + struct vfp_double vdm; + u32 d, exceptions = 0; + int rmode = fpscr & FPSCR_RMODE_MASK; + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + vfp_double_dump("VDM", &vdm); + + /* + * Do we have denormalised number? + */ + if (vfp_double_type(&vdm) & VFP_DENORMAL) + exceptions |= FPSCR_IDC; + + if (vdm.exponent >= 1023 + 32) { + d = 0x7fffffff; + if (vdm.sign) + d = ~d; + exceptions |= FPSCR_IOC; + } else if (vdm.exponent >= 1023 - 1) { + int shift = 1023 + 63 - vdm.exponent; /* 58 */ + u64 rem, incr = 0; + + d = (vdm.significand << 1) >> shift; + rem = vdm.significand << (65 - shift); + + if (rmode == FPSCR_ROUND_NEAREST) { + incr = 0x8000000000000000ULL; + if ((d & 1) == 0) + incr -= 1; + } else if (rmode == FPSCR_ROUND_TOZERO) { + incr = 0; + } else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vdm.sign != 0)) { + incr = ~0ULL; + } + + if ((rem + incr) < rem && d < 0xffffffff) + d += 1; + if (d > 0x7fffffff + (vdm.sign != 0)) { + d = 0x7fffffff + (vdm.sign != 0); + exceptions |= FPSCR_IOC; + } else if (rem) + exceptions |= FPSCR_IXC; + + if (vdm.sign) + d = -d; + } else { + d = 0; + if (vdm.exponent | vdm.significand) { + exceptions |= FPSCR_IXC; + if (rmode == FPSCR_ROUND_PLUSINF && vdm.sign == 0) + d = 1; + else if (rmode == FPSCR_ROUND_MINUSINF && vdm.sign) + d = -1; + } + } + + pr_debug("VFP: ftosi: d(s%d)=%08x exceptions=%08x\n", sd, d, exceptions); + + vfp_put_float(sd, (s32)d); + + return exceptions; +} + +static u32 vfp_double_ftosiz(int dd, int unused, int dm, u32 fpscr) +{ + return vfp_double_ftosi(dd, unused, dm, FPSCR_ROUND_TOZERO); +} + + +static u32 (* const fop_extfns[32])(int dd, int unused, int dm, u32 fpscr) = { + [FEXT_TO_IDX(FEXT_FCPY)] = vfp_double_fcpy, + [FEXT_TO_IDX(FEXT_FABS)] = vfp_double_fabs, + [FEXT_TO_IDX(FEXT_FNEG)] = vfp_double_fneg, + [FEXT_TO_IDX(FEXT_FSQRT)] = vfp_double_fsqrt, + [FEXT_TO_IDX(FEXT_FCMP)] = vfp_double_fcmp, + [FEXT_TO_IDX(FEXT_FCMPE)] = vfp_double_fcmpe, + [FEXT_TO_IDX(FEXT_FCMPZ)] = vfp_double_fcmpz, + [FEXT_TO_IDX(FEXT_FCMPEZ)] = vfp_double_fcmpez, + [FEXT_TO_IDX(FEXT_FCVT)] = vfp_double_fcvts, + [FEXT_TO_IDX(FEXT_FUITO)] = vfp_double_fuito, + [FEXT_TO_IDX(FEXT_FSITO)] = vfp_double_fsito, + [FEXT_TO_IDX(FEXT_FTOUI)] = vfp_double_ftoui, + [FEXT_TO_IDX(FEXT_FTOUIZ)] = vfp_double_ftouiz, + [FEXT_TO_IDX(FEXT_FTOSI)] = vfp_double_ftosi, + [FEXT_TO_IDX(FEXT_FTOSIZ)] = vfp_double_ftosiz, +}; + + + + +static u32 +vfp_double_fadd_nonnumber(struct vfp_double *vdd, struct vfp_double *vdn, + struct vfp_double *vdm, u32 fpscr) +{ + struct vfp_double *vdp; + u32 exceptions = 0; + int tn, tm; + + tn = vfp_double_type(vdn); + tm = vfp_double_type(vdm); + + if (tn & tm & VFP_INFINITY) { + /* + * Two infinities. Are they different signs? + */ + if (vdn->sign ^ vdm->sign) { + /* + * different signs -> invalid + */ + exceptions = FPSCR_IOC; + vdp = &vfp_double_default_qnan; + } else { + /* + * same signs -> valid + */ + vdp = vdn; + } + } else if (tn & VFP_INFINITY && tm & VFP_NUMBER) { + /* + * One infinity and one number -> infinity + */ + vdp = vdn; + } else { + /* + * 'n' is a NaN of some type + */ + return vfp_propagate_nan(vdd, vdn, vdm, fpscr); + } + *vdd = *vdp; + return exceptions; +} + +static u32 +vfp_double_add(struct vfp_double *vdd, struct vfp_double *vdn, + struct vfp_double *vdm, u32 fpscr) +{ + u32 exp_diff; + u64 m_sig; + + if (vdn->significand & (1ULL << 63) || + vdm->significand & (1ULL << 63)) { + pr_info("VFP: bad FP values in %s\n", __func__); + vfp_double_dump("VDN", vdn); + vfp_double_dump("VDM", vdm); + } + + /* + * Ensure that 'n' is the largest magnitude number. Note that + * if 'n' and 'm' have equal exponents, we do not swap them. + * This ensures that NaN propagation works correctly. + */ + if (vdn->exponent < vdm->exponent) { + struct vfp_double *t = vdn; + vdn = vdm; + vdm = t; + } + + /* + * Is 'n' an infinity or a NaN? Note that 'm' may be a number, + * infinity or a NaN here. + */ + if (vdn->exponent == 2047) + return vfp_double_fadd_nonnumber(vdd, vdn, vdm, fpscr); + + /* + * We have two proper numbers, where 'vdn' is the larger magnitude. + * + * Copy 'n' to 'd' before doing the arithmetic. + */ + *vdd = *vdn; + + /* + * Align 'm' with the result. + */ + exp_diff = vdn->exponent - vdm->exponent; + m_sig = vfp_shiftright64jamming(vdm->significand, exp_diff); + + /* + * If the signs are different, we are really subtracting. + */ + if (vdn->sign ^ vdm->sign) { + m_sig = vdn->significand - m_sig; + if ((s64)m_sig < 0) { + vdd->sign = vfp_sign_negate(vdd->sign); + m_sig = -m_sig; + } + } else { + m_sig += vdn->significand; + } + vdd->significand = m_sig; + + return 0; +} + +static u32 +vfp_double_multiply(struct vfp_double *vdd, struct vfp_double *vdn, + struct vfp_double *vdm, u32 fpscr) +{ + vfp_double_dump("VDN", vdn); + vfp_double_dump("VDM", vdm); + + /* + * Ensure that 'n' is the largest magnitude number. Note that + * if 'n' and 'm' have equal exponents, we do not swap them. + * This ensures that NaN propagation works correctly. + */ + if (vdn->exponent < vdm->exponent) { + struct vfp_double *t = vdn; + vdn = vdm; + vdm = t; + pr_debug("VFP: swapping M <-> N\n"); + } + + vdd->sign = vdn->sign ^ vdm->sign; + + /* + * If 'n' is an infinity or NaN, handle it. 'm' may be anything. + */ + if (vdn->exponent == 2047) { + if (vdn->significand || (vdm->exponent == 2047 && vdm->significand)) + return vfp_propagate_nan(vdd, vdn, vdm, fpscr); + if ((vdm->exponent | vdm->significand) == 0) { + *vdd = vfp_double_default_qnan; + return FPSCR_IOC; + } + vdd->exponent = vdn->exponent; + vdd->significand = 0; + return 0; + } + + /* + * If 'm' is zero, the result is always zero. In this case, + * 'n' may be zero or a number, but it doesn't matter which. + */ + if ((vdm->exponent | vdm->significand) == 0) { + vdd->exponent = 0; + vdd->significand = 0; + return 0; + } + + /* + * We add 2 to the destination exponent for the same reason + * as the addition case - though this time we have +1 from + * each input operand. + */ + vdd->exponent = vdn->exponent + vdm->exponent - 1023 + 2; + vdd->significand = vfp_hi64multiply64(vdn->significand, vdm->significand); + + vfp_double_dump("VDD", vdd); + return 0; +} + +#define NEG_MULTIPLY (1 << 0) +#define NEG_SUBTRACT (1 << 1) + +static u32 +vfp_double_multiply_accumulate(int dd, int dn, int dm, u32 fpscr, u32 negate, char *func) +{ + struct vfp_double vdd, vdp, vdn, vdm; + u32 exceptions; + + vfp_double_unpack(&vdn, vfp_get_double(dn)); + if (vdn.exponent == 0 && vdn.significand) + vfp_double_normalise_denormal(&vdn); + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + if (vdm.exponent == 0 && vdm.significand) + vfp_double_normalise_denormal(&vdm); + + exceptions = vfp_double_multiply(&vdp, &vdn, &vdm, fpscr); + if (negate & NEG_MULTIPLY) + vdp.sign = vfp_sign_negate(vdp.sign); + + vfp_double_unpack(&vdn, vfp_get_double(dd)); + if (negate & NEG_SUBTRACT) + vdn.sign = vfp_sign_negate(vdn.sign); + + exceptions |= vfp_double_add(&vdd, &vdn, &vdp, fpscr); + + return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, func); +} + +/* + * Standard operations + */ + +/* + * sd = sd + (sn * sm) + */ +static u32 vfp_double_fmac(int dd, int dn, int dm, u32 fpscr) +{ + return vfp_double_multiply_accumulate(dd, dn, dm, fpscr, 0, "fmac"); +} + +/* + * sd = sd - (sn * sm) + */ +static u32 vfp_double_fnmac(int dd, int dn, int dm, u32 fpscr) +{ + return vfp_double_multiply_accumulate(dd, dn, dm, fpscr, NEG_MULTIPLY, "fnmac"); +} + +/* + * sd = -sd + (sn * sm) + */ +static u32 vfp_double_fmsc(int dd, int dn, int dm, u32 fpscr) +{ + return vfp_double_multiply_accumulate(dd, dn, dm, fpscr, NEG_SUBTRACT, "fmsc"); +} + +/* + * sd = -sd - (sn * sm) + */ +static u32 vfp_double_fnmsc(int dd, int dn, int dm, u32 fpscr) +{ + return vfp_double_multiply_accumulate(dd, dn, dm, fpscr, NEG_SUBTRACT | NEG_MULTIPLY, "fnmsc"); +} + +/* + * sd = sn * sm + */ +static u32 vfp_double_fmul(int dd, int dn, int dm, u32 fpscr) +{ + struct vfp_double vdd, vdn, vdm; + u32 exceptions; + + vfp_double_unpack(&vdn, vfp_get_double(dn)); + if (vdn.exponent == 0 && vdn.significand) + vfp_double_normalise_denormal(&vdn); + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + if (vdm.exponent == 0 && vdm.significand) + vfp_double_normalise_denormal(&vdm); + + exceptions = vfp_double_multiply(&vdd, &vdn, &vdm, fpscr); + return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fmul"); +} + +/* + * sd = -(sn * sm) + */ +static u32 vfp_double_fnmul(int dd, int dn, int dm, u32 fpscr) +{ + struct vfp_double vdd, vdn, vdm; + u32 exceptions; + + vfp_double_unpack(&vdn, vfp_get_double(dn)); + if (vdn.exponent == 0 && vdn.significand) + vfp_double_normalise_denormal(&vdn); + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + if (vdm.exponent == 0 && vdm.significand) + vfp_double_normalise_denormal(&vdm); + + exceptions = vfp_double_multiply(&vdd, &vdn, &vdm, fpscr); + vdd.sign = vfp_sign_negate(vdd.sign); + + return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fnmul"); +} + +/* + * sd = sn + sm + */ +static u32 vfp_double_fadd(int dd, int dn, int dm, u32 fpscr) +{ + struct vfp_double vdd, vdn, vdm; + u32 exceptions; + + vfp_double_unpack(&vdn, vfp_get_double(dn)); + if (vdn.exponent == 0 && vdn.significand) + vfp_double_normalise_denormal(&vdn); + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + if (vdm.exponent == 0 && vdm.significand) + vfp_double_normalise_denormal(&vdm); + + exceptions = vfp_double_add(&vdd, &vdn, &vdm, fpscr); + + return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fadd"); +} + +/* + * sd = sn - sm + */ +static u32 vfp_double_fsub(int dd, int dn, int dm, u32 fpscr) +{ + struct vfp_double vdd, vdn, vdm; + u32 exceptions; + + vfp_double_unpack(&vdn, vfp_get_double(dn)); + if (vdn.exponent == 0 && vdn.significand) + vfp_double_normalise_denormal(&vdn); + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + if (vdm.exponent == 0 && vdm.significand) + vfp_double_normalise_denormal(&vdm); + + /* + * Subtraction is like addition, but with a negated operand. + */ + vdm.sign = vfp_sign_negate(vdm.sign); + + exceptions = vfp_double_add(&vdd, &vdn, &vdm, fpscr); + + return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fsub"); +} + +/* + * sd = sn / sm + */ +static u32 vfp_double_fdiv(int dd, int dn, int dm, u32 fpscr) +{ + struct vfp_double vdd, vdn, vdm; + u32 exceptions = 0; + int tm, tn; + + vfp_double_unpack(&vdn, vfp_get_double(dn)); + vfp_double_unpack(&vdm, vfp_get_double(dm)); + + vdd.sign = vdn.sign ^ vdm.sign; + + tn = vfp_double_type(&vdn); + tm = vfp_double_type(&vdm); + + /* + * Is n a NAN? + */ + if (tn & VFP_NAN) + goto vdn_nan; + + /* + * Is m a NAN? + */ + if (tm & VFP_NAN) + goto vdm_nan; + + /* + * If n and m are infinity, the result is invalid + * If n and m are zero, the result is invalid + */ + if (tm & tn & (VFP_INFINITY|VFP_ZERO)) + goto invalid; + + /* + * If n is infinity, the result is infinity + */ + if (tn & VFP_INFINITY) + goto infinity; + + /* + * If m is zero, raise div0 exceptions + */ + if (tm & VFP_ZERO) + goto divzero; + + /* + * If m is infinity, or n is zero, the result is zero + */ + if (tm & VFP_INFINITY || tn & VFP_ZERO) + goto zero; + + if (tn & VFP_DENORMAL) + vfp_double_normalise_denormal(&vdn); + if (tm & VFP_DENORMAL) + vfp_double_normalise_denormal(&vdm); + + /* + * Ok, we have two numbers, we can perform division. + */ + vdd.exponent = vdn.exponent - vdm.exponent + 1023 - 1; + vdm.significand <<= 1; + if (vdm.significand <= (2 * vdn.significand)) { + vdn.significand >>= 1; + vdd.exponent++; + } + vdd.significand = vfp_estimate_div128to64(vdn.significand, 0, vdm.significand); + if ((vdd.significand & 0x1ff) <= 2) { + u64 termh, terml, remh, reml; + mul64to128(&termh, &terml, vdm.significand, vdd.significand); + sub128(&remh, &reml, vdn.significand, 0, termh, terml); + while ((s64)remh < 0) { + vdd.significand -= 1; + add128(&remh, &reml, remh, reml, 0, vdm.significand); + } + vdd.significand |= (reml != 0); + } + return vfp_double_normaliseround(dd, &vdd, fpscr, 0, "fdiv"); + + vdn_nan: + exceptions = vfp_propagate_nan(&vdd, &vdn, &vdm, fpscr); + pack: + vfp_put_double(dd, vfp_double_pack(&vdd)); + return exceptions; + + vdm_nan: + exceptions = vfp_propagate_nan(&vdd, &vdm, &vdn, fpscr); + goto pack; + + zero: + vdd.exponent = 0; + vdd.significand = 0; + goto pack; + + divzero: + exceptions = FPSCR_DZC; + infinity: + vdd.exponent = 2047; + vdd.significand = 0; + goto pack; + + invalid: + vfp_put_double(dd, vfp_double_pack(&vfp_double_default_qnan)); + return FPSCR_IOC; +} + +static u32 (* const fop_fns[16])(int dd, int dn, int dm, u32 fpscr) = { + [FOP_TO_IDX(FOP_FMAC)] = vfp_double_fmac, + [FOP_TO_IDX(FOP_FNMAC)] = vfp_double_fnmac, + [FOP_TO_IDX(FOP_FMSC)] = vfp_double_fmsc, + [FOP_TO_IDX(FOP_FNMSC)] = vfp_double_fnmsc, + [FOP_TO_IDX(FOP_FMUL)] = vfp_double_fmul, + [FOP_TO_IDX(FOP_FNMUL)] = vfp_double_fnmul, + [FOP_TO_IDX(FOP_FADD)] = vfp_double_fadd, + [FOP_TO_IDX(FOP_FSUB)] = vfp_double_fsub, + [FOP_TO_IDX(FOP_FDIV)] = vfp_double_fdiv, +}; + +#define FREG_BANK(x) ((x) & 0x0c) +#define FREG_IDX(x) ((x) & 3) + +u32 vfp_double_cpdo(u32 inst, u32 fpscr) +{ + u32 op = inst & FOP_MASK; + u32 exceptions = 0; + unsigned int dd = vfp_get_sd(inst); + unsigned int dn = vfp_get_sn(inst); + unsigned int dm = vfp_get_sm(inst); + unsigned int vecitr, veclen, vecstride; + u32 (*fop)(int, int, s32, u32); + + veclen = fpscr & FPSCR_LENGTH_MASK; + vecstride = (1 + ((fpscr & FPSCR_STRIDE_MASK) == FPSCR_STRIDE_MASK)) * 2; + + /* + * If destination bank is zero, vector length is always '1'. + * ARM DDI0100F C5.1.3, C5.3.2. + */ + if (FREG_BANK(dd) == 0) + veclen = 0; + + pr_debug("VFP: vecstride=%u veclen=%u\n", vecstride, + (veclen >> FPSCR_LENGTH_BIT) + 1); + + fop = (op == FOP_EXT) ? fop_extfns[dn] : fop_fns[FOP_TO_IDX(op)]; + if (!fop) + goto invalid; + + for (vecitr = 0; vecitr <= veclen; vecitr += 1 << FPSCR_LENGTH_BIT) { + u32 except; + + if (op == FOP_EXT) + pr_debug("VFP: itr%d (d%u.%u) = op[%u] (d%u.%u)\n", + vecitr >> FPSCR_LENGTH_BIT, + dd >> 1, dd & 1, dn, + dm >> 1, dm & 1); + else + pr_debug("VFP: itr%d (d%u.%u) = (d%u.%u) op[%u] (d%u.%u)\n", + vecitr >> FPSCR_LENGTH_BIT, + dd >> 1, dd & 1, + dn >> 1, dn & 1, + FOP_TO_IDX(op), + dm >> 1, dm & 1); + + except = fop(dd, dn, dm, fpscr); + pr_debug("VFP: itr%d: exceptions=%08x\n", + vecitr >> FPSCR_LENGTH_BIT, except); + + exceptions |= except; + + /* + * This ensures that comparisons only operate on scalars; + * comparisons always return with one FPSCR status bit set. + */ + if (except & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V)) + break; + + /* + * CHECK: It appears to be undefined whether we stop when + * we encounter an exception. We continue. + */ + + dd = FREG_BANK(dd) + ((FREG_IDX(dd) + vecstride) & 6); + dn = FREG_BANK(dn) + ((FREG_IDX(dn) + vecstride) & 6); + if (FREG_BANK(dm) != 0) + dm = FREG_BANK(dm) + ((FREG_IDX(dm) + vecstride) & 6); + } + return exceptions; + + invalid: + return ~0; +} diff --git a/arch/arm/vfp/vfphw.S b/arch/arm/vfp/vfphw.S new file mode 100644 index 00000000000..de4ca1223c5 --- /dev/null +++ b/arch/arm/vfp/vfphw.S @@ -0,0 +1,215 @@ +/* + * linux/arch/arm/vfp/vfphw.S + * + * Copyright (C) 2004 ARM Limited. + * Written by Deep Blue Solutions Limited. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * This code is called from the kernel's undefined instruction trap. + * r9 holds the return address for successful handling. + * lr holds the return address for unrecognised instructions. + * r10 points at the start of the private FP workspace in the thread structure + * sp points to a struct pt_regs (as defined in include/asm/proc/ptrace.h) + */ +#include <asm/thread_info.h> +#include <asm/vfpmacros.h> +#include "../kernel/entry-header.S" + + .macro DBGSTR, str +#ifdef DEBUG + stmfd sp!, {r0-r3, ip, lr} + add r0, pc, #4 + bl printk + b 1f + .asciz "<7>VFP: \str\n" + .balign 4 +1: ldmfd sp!, {r0-r3, ip, lr} +#endif + .endm + + .macro DBGSTR1, str, arg +#ifdef DEBUG + stmfd sp!, {r0-r3, ip, lr} + mov r1, \arg + add r0, pc, #4 + bl printk + b 1f + .asciz "<7>VFP: \str\n" + .balign 4 +1: ldmfd sp!, {r0-r3, ip, lr} +#endif + .endm + + .macro DBGSTR3, str, arg1, arg2, arg3 +#ifdef DEBUG + stmfd sp!, {r0-r3, ip, lr} + mov r3, \arg3 + mov r2, \arg2 + mov r1, \arg1 + add r0, pc, #4 + bl printk + b 1f + .asciz "<7>VFP: \str\n" + .balign 4 +1: ldmfd sp!, {r0-r3, ip, lr} +#endif + .endm + + +@ VFP hardware support entry point. +@ +@ r0 = faulted instruction +@ r2 = faulted PC+4 +@ r9 = successful return +@ r10 = vfp_state union +@ lr = failure return + + .globl vfp_support_entry +vfp_support_entry: + DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10 + + VFPFMRX r1, FPEXC @ Is the VFP enabled? + DBGSTR1 "fpexc %08x", r1 + tst r1, #FPEXC_ENABLE + bne look_for_VFP_exceptions @ VFP is already enabled + + DBGSTR1 "enable %x", r10 + ldr r3, last_VFP_context_address + orr r1, r1, #FPEXC_ENABLE @ user FPEXC has the enable bit set + ldr r4, [r3] @ last_VFP_context pointer + bic r5, r1, #FPEXC_EXCEPTION @ make sure exceptions are disabled + cmp r4, r10 + beq check_for_exception @ we are returning to the same + @ process, so the registers are + @ still there. In this case, we do + @ not want to drop a pending exception. + + VFPFMXR FPEXC, r5 @ enable VFP, disable any pending + @ exceptions, so we can get at the + @ rest of it + + @ Save out the current registers to the old thread state + + DBGSTR1 "save old state %p", r4 + cmp r4, #0 + beq no_old_VFP_process + VFPFMRX r5, FPSCR @ current status + VFPFMRX r6, FPINST @ FPINST (always there, rev0 onwards) + tst r1, #FPEXC_FPV2 @ is there an FPINST2 to read? + VFPFMRX r8, FPINST2, NE @ FPINST2 if needed - avoids reading + @ nonexistant reg on rev0 + VFPFSTMIA r4 @ save the working registers + add r4, r4, #8*16+4 + stmia r4, {r1, r5, r6, r8} @ save FPEXC, FPSCR, FPINST, FPINST2 + @ and point r4 at the word at the + @ start of the register dump + +no_old_VFP_process: + DBGSTR1 "load state %p", r10 + str r10, [r3] @ update the last_VFP_context pointer + @ Load the saved state back into the VFP + add r4, r10, #8*16+4 + ldmia r4, {r1, r5, r6, r8} @ load FPEXC, FPSCR, FPINST, FPINST2 + VFPFLDMIA r10 @ reload the working registers while + @ FPEXC is in a safe state + tst r1, #FPEXC_FPV2 @ is there an FPINST2 to write? + VFPFMXR FPINST2, r8, NE @ FPINST2 if needed - avoids writing + @ nonexistant reg on rev0 + VFPFMXR FPINST, r6 + VFPFMXR FPSCR, r5 @ restore status + +check_for_exception: + tst r1, #FPEXC_EXCEPTION + bne process_exception @ might as well handle the pending + @ exception before retrying branch + @ out before setting an FPEXC that + @ stops us reading stuff + VFPFMXR FPEXC, r1 @ restore FPEXC last + sub r2, r2, #4 + str r2, [sp, #S_PC] @ retry the instruction + mov pc, r9 @ we think we have handled things + + +look_for_VFP_exceptions: + tst r1, #FPEXC_EXCEPTION + bne process_exception + VFPFMRX r5, FPSCR + tst r5, #FPSCR_IXE @ IXE doesn't set FPEXC_EXCEPTION ! + bne process_exception + + @ Fall into hand on to next handler - appropriate coproc instr + @ not recognised by VFP + + DBGSTR "not VFP" + mov pc, lr + +process_exception: + DBGSTR "bounce" + sub r2, r2, #4 + str r2, [sp, #S_PC] @ retry the instruction on exit from + @ the imprecise exception handling in + @ the support code + mov r2, sp @ nothing stacked - regdump is at TOS + mov lr, r9 @ setup for a return to the user code. + + @ Now call the C code to package up the bounce to the support code + @ r0 holds the trigger instruction + @ r1 holds the FPEXC value + @ r2 pointer to register dump + b VFP9_bounce @ we have handled this - the support + @ code will raise an exception if + @ required. If not, the user code will + @ retry the faulted instruction + +last_VFP_context_address: + .word last_VFP_context + + .globl vfp_get_float +vfp_get_float: + add pc, pc, r0, lsl #3 + mov r0, r0 + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 + mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0 + mov pc, lr + mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1 + mov pc, lr + .endr + + .globl vfp_put_float +vfp_put_float: + add pc, pc, r0, lsl #3 + mov r0, r0 + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 + mcr p10, 0, r1, c\dr, c0, 0 @ fmsr r0, s0 + mov pc, lr + mcr p10, 0, r1, c\dr, c0, 4 @ fmsr r0, s1 + mov pc, lr + .endr + + .globl vfp_get_double +vfp_get_double: + mov r0, r0, lsr #1 + add pc, pc, r0, lsl #3 + mov r0, r0 + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 + mrrc p10, 1, r0, r1, c\dr @ fmrrd r0, r1, d\dr + mov pc, lr + .endr + + @ virtual register 16 for compare with zero + mov r0, #0 + mov r1, #0 + mov pc, lr + + .globl vfp_put_double +vfp_put_double: + mov r0, r0, lsr #1 + add pc, pc, r0, lsl #3 + mov r0, r0 + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 + mcrr p10, 1, r1, r2, c\dr @ fmrrd r1, r2, d\dr + mov pc, lr + .endr diff --git a/arch/arm/vfp/vfpinstr.h b/arch/arm/vfp/vfpinstr.h new file mode 100644 index 00000000000..6c819aeae00 --- /dev/null +++ b/arch/arm/vfp/vfpinstr.h @@ -0,0 +1,88 @@ +/* + * linux/arch/arm/vfp/vfpinstr.h + * + * Copyright (C) 2004 ARM Limited. + * Written by Deep Blue Solutions Limited. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + * + * VFP instruction masks. + */ +#define INST_CPRTDO(inst) (((inst) & 0x0f000000) == 0x0e000000) +#define INST_CPRT(inst) ((inst) & (1 << 4)) +#define INST_CPRT_L(inst) ((inst) & (1 << 20)) +#define INST_CPRT_Rd(inst) (((inst) & (15 << 12)) >> 12) +#define INST_CPRT_OP(inst) (((inst) >> 21) & 7) +#define INST_CPNUM(inst) ((inst) & 0xf00) +#define CPNUM(cp) ((cp) << 8) + +#define FOP_MASK (0x00b00040) +#define FOP_FMAC (0x00000000) +#define FOP_FNMAC (0x00000040) +#define FOP_FMSC (0x00100000) +#define FOP_FNMSC (0x00100040) +#define FOP_FMUL (0x00200000) +#define FOP_FNMUL (0x00200040) +#define FOP_FADD (0x00300000) +#define FOP_FSUB (0x00300040) +#define FOP_FDIV (0x00800000) +#define FOP_EXT (0x00b00040) + +#define FOP_TO_IDX(inst) ((inst & 0x00b00000) >> 20 | (inst & (1 << 6)) >> 4) + +#define FEXT_MASK (0x000f0080) +#define FEXT_FCPY (0x00000000) +#define FEXT_FABS (0x00000080) +#define FEXT_FNEG (0x00010000) +#define FEXT_FSQRT (0x00010080) +#define FEXT_FCMP (0x00040000) +#define FEXT_FCMPE (0x00040080) +#define FEXT_FCMPZ (0x00050000) +#define FEXT_FCMPEZ (0x00050080) +#define FEXT_FCVT (0x00070080) +#define FEXT_FUITO (0x00080000) +#define FEXT_FSITO (0x00080080) +#define FEXT_FTOUI (0x000c0000) +#define FEXT_FTOUIZ (0x000c0080) +#define FEXT_FTOSI (0x000d0000) +#define FEXT_FTOSIZ (0x000d0080) + +#define FEXT_TO_IDX(inst) ((inst & 0x000f0000) >> 15 | (inst & (1 << 7)) >> 7) + +#define vfp_get_sd(inst) ((inst & 0x0000f000) >> 11 | (inst & (1 << 22)) >> 22) +#define vfp_get_dd(inst) ((inst & 0x0000f000) >> 12) +#define vfp_get_sm(inst) ((inst & 0x0000000f) << 1 | (inst & (1 << 5)) >> 5) +#define vfp_get_dm(inst) ((inst & 0x0000000f)) +#define vfp_get_sn(inst) ((inst & 0x000f0000) >> 15 | (inst & (1 << 7)) >> 7) +#define vfp_get_dn(inst) ((inst & 0x000f0000) >> 16) + +#define vfp_single(inst) (((inst) & 0x0000f00) == 0xa00) + +#define FPSCR_N (1 << 31) +#define FPSCR_Z (1 << 30) +#define FPSCR_C (1 << 29) +#define FPSCR_V (1 << 28) + +/* + * Since we aren't building with -mfpu=vfp, we need to code + * these instructions using their MRC/MCR equivalents. + */ +#define vfpreg(_vfp_) #_vfp_ + +#define fmrx(_vfp_) ({ \ + u32 __v; \ + asm("mrc%? p10, 7, %0, " vfpreg(_vfp_) ", cr0, 0 @ fmrx %0, " #_vfp_ \ + : "=r" (__v)); \ + __v; \ + }) + +#define fmxr(_vfp_,_var_) \ + asm("mcr%? p10, 7, %0, " vfpreg(_vfp_) ", cr0, 0 @ fmxr " #_vfp_ ", %0" \ + : : "r" (_var_)) + +u32 vfp_single_cpdo(u32 inst, u32 fpscr); +u32 vfp_single_cprt(u32 inst, u32 fpscr, struct pt_regs *regs); + +u32 vfp_double_cpdo(u32 inst, u32 fpscr); diff --git a/arch/arm/vfp/vfpmodule.c b/arch/arm/vfp/vfpmodule.c new file mode 100644 index 00000000000..3aeedd2afc7 --- /dev/null +++ b/arch/arm/vfp/vfpmodule.c @@ -0,0 +1,288 @@ +/* + * linux/arch/arm/vfp/vfpmodule.c + * + * Copyright (C) 2004 ARM Limited. + * Written by Deep Blue Solutions Limited. + * + * This program is free software; you can redistribute it and/or modify + * it under the terms of the GNU General Public License version 2 as + * published by the Free Software Foundation. + */ +#include <linux/module.h> +#include <linux/config.h> +#include <linux/types.h> +#include <linux/kernel.h> +#include <linux/signal.h> +#include <linux/sched.h> +#include <linux/init.h> +#include <asm/vfp.h> + +#include "vfpinstr.h" +#include "vfp.h" + +/* + * Our undef handlers (in entry.S) + */ +void vfp_testing_entry(void); +void vfp_support_entry(void); + +void (*vfp_vector)(void) = vfp_testing_entry; +union vfp_state *last_VFP_context; + +/* + * Dual-use variable. + * Used in startup: set to non-zero if VFP checks fail + * After startup, holds VFP architecture + */ +unsigned int VFP_arch; + +/* + * Per-thread VFP initialisation. + */ +void vfp_flush_thread(union vfp_state *vfp) +{ + memset(vfp, 0, sizeof(union vfp_state)); + + vfp->hard.fpexc = FPEXC_ENABLE; + vfp->hard.fpscr = FPSCR_ROUND_NEAREST; + + /* + * Disable VFP to ensure we initialise it first. + */ + fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_ENABLE); + + /* + * Ensure we don't try to overwrite our newly initialised + * state information on the first fault. + */ + if (last_VFP_context == vfp) + last_VFP_context = NULL; +} + +/* + * Per-thread VFP cleanup. + */ +void vfp_release_thread(union vfp_state *vfp) +{ + if (last_VFP_context == vfp) + last_VFP_context = NULL; +} + +/* + * Raise a SIGFPE for the current process. + * sicode describes the signal being raised. + */ +void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs) +{ + siginfo_t info; + + memset(&info, 0, sizeof(info)); + + info.si_signo = SIGFPE; + info.si_code = sicode; + info.si_addr = (void *)(instruction_pointer(regs) - 4); + + /* + * This is the same as NWFPE, because it's not clear what + * this is used for + */ + current->thread.error_code = 0; + current->thread.trap_no = 6; + + force_sig_info(SIGFPE, &info, current); +} + +static void vfp_panic(char *reason) +{ + int i; + + printk(KERN_ERR "VFP: Error: %s\n", reason); + printk(KERN_ERR "VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n", + fmrx(FPEXC), fmrx(FPSCR), fmrx(FPINST)); + for (i = 0; i < 32; i += 2) + printk(KERN_ERR "VFP: s%2u: 0x%08x s%2u: 0x%08x\n", + i, vfp_get_float(i), i+1, vfp_get_float(i+1)); +} + +/* + * Process bitmask of exception conditions. + */ +static void vfp_raise_exceptions(u32 exceptions, u32 inst, u32 fpscr, struct pt_regs *regs) +{ + int si_code = 0; + + pr_debug("VFP: raising exceptions %08x\n", exceptions); + + if (exceptions == (u32)-1) { + vfp_panic("unhandled bounce"); + vfp_raise_sigfpe(0, regs); + return; + } + + /* + * If any of the status flags are set, update the FPSCR. + * Comparison instructions always return at least one of + * these flags set. + */ + if (exceptions & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V)) + fpscr &= ~(FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V); + + fpscr |= exceptions; + + fmxr(FPSCR, fpscr); + +#define RAISE(stat,en,sig) \ + if (exceptions & stat && fpscr & en) \ + si_code = sig; + + /* + * These are arranged in priority order, least to highest. + */ + RAISE(FPSCR_IXC, FPSCR_IXE, FPE_FLTRES); + RAISE(FPSCR_UFC, FPSCR_UFE, FPE_FLTUND); + RAISE(FPSCR_OFC, FPSCR_OFE, FPE_FLTOVF); + RAISE(FPSCR_IOC, FPSCR_IOE, FPE_FLTINV); + + if (si_code) + vfp_raise_sigfpe(si_code, regs); +} + +/* + * Emulate a VFP instruction. + */ +static u32 vfp_emulate_instruction(u32 inst, u32 fpscr, struct pt_regs *regs) +{ + u32 exceptions = (u32)-1; + + pr_debug("VFP: emulate: INST=0x%08x SCR=0x%08x\n", inst, fpscr); + + if (INST_CPRTDO(inst)) { + if (!INST_CPRT(inst)) { + /* + * CPDO + */ + if (vfp_single(inst)) { + exceptions = vfp_single_cpdo(inst, fpscr); + } else { + exceptions = vfp_double_cpdo(inst, fpscr); + } + } else { + /* + * A CPRT instruction can not appear in FPINST2, nor + * can it cause an exception. Therefore, we do not + * have to emulate it. + */ + } + } else { + /* + * A CPDT instruction can not appear in FPINST2, nor can + * it cause an exception. Therefore, we do not have to + * emulate it. + */ + } + return exceptions; +} + +/* + * Package up a bounce condition. + */ +void VFP9_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs) +{ + u32 fpscr, orig_fpscr, exceptions, inst; + + pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc); + + /* + * Enable access to the VFP so we can handle the bounce. + */ + fmxr(FPEXC, fpexc & ~(FPEXC_EXCEPTION|FPEXC_INV|FPEXC_UFC|FPEXC_IOC)); + + orig_fpscr = fpscr = fmrx(FPSCR); + + /* + * If we are running with inexact exceptions enabled, we need to + * emulate the trigger instruction. Note that as we're emulating + * the trigger instruction, we need to increment PC. + */ + if (fpscr & FPSCR_IXE) { + regs->ARM_pc += 4; + goto emulate; + } + + barrier(); + + /* + * Modify fpscr to indicate the number of iterations remaining + */ + if (fpexc & FPEXC_EXCEPTION) { + u32 len; + + len = fpexc + (1 << FPEXC_LENGTH_BIT); + + fpscr &= ~FPSCR_LENGTH_MASK; + fpscr |= (len & FPEXC_LENGTH_MASK) << (FPSCR_LENGTH_BIT - FPEXC_LENGTH_BIT); + } + + /* + * Handle the first FP instruction. We used to take note of the + * FPEXC bounce reason, but this appears to be unreliable. + * Emulate the bounced instruction instead. + */ + inst = fmrx(FPINST); + exceptions = vfp_emulate_instruction(inst, fpscr, regs); + if (exceptions) + vfp_raise_exceptions(exceptions, inst, orig_fpscr, regs); + + /* + * If there isn't a second FP instruction, exit now. + */ + if (!(fpexc & FPEXC_FPV2)) + return; + + /* + * The barrier() here prevents fpinst2 being read + * before the condition above. + */ + barrier(); + trigger = fmrx(FPINST2); + fpscr = fmrx(FPSCR); + + emulate: + exceptions = vfp_emulate_instruction(trigger, fpscr, regs); + if (exceptions) + vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs); +} + +/* + * VFP support code initialisation. + */ +static int __init vfp_init(void) +{ + unsigned int vfpsid; + + /* + * First check that there is a VFP that we can use. + * The handler is already setup to just log calls, so + * we just need to read the VFPSID register. + */ + vfpsid = fmrx(FPSID); + + printk(KERN_INFO "VFP support v0.3: "); + if (VFP_arch) { + printk("not present\n"); + } else if (vfpsid & FPSID_NODOUBLE) { + printk("no double precision support\n"); + } else { + VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT; /* Extract the architecture version */ + printk("implementor %02x architecture %d part %02x variant %x rev %x\n", + (vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT, + (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT, + (vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT, + (vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT, + (vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT); + vfp_vector = vfp_support_entry; + } + return 0; +} + +late_initcall(vfp_init); diff --git a/arch/arm/vfp/vfpsingle.c b/arch/arm/vfp/vfpsingle.c new file mode 100644 index 00000000000..6849fe35cb2 --- /dev/null +++ b/arch/arm/vfp/vfpsingle.c @@ -0,0 +1,1224 @@ +/* + * linux/arch/arm/vfp/vfpsingle.c + * + * This code is derived in part from John R. Housers softfloat library, which + * carries the following notice: + * + * =========================================================================== + * This C source file is part of the SoftFloat IEC/IEEE Floating-point + * Arithmetic Package, Release 2. + * + * Written by John R. Hauser. This work was made possible in part by the + * International Computer Science Institute, located at Suite 600, 1947 Center + * Street, Berkeley, California 94704. Funding was partially provided by the + * National Science Foundation under grant MIP-9311980. The original version + * of this code was written as part of a project to build a fixed-point vector + * processor in collaboration with the University of California at Berkeley, + * overseen by Profs. Nelson Morgan and John Wawrzynek. More information + * is available through the web page `http://HTTP.CS.Berkeley.EDU/~jhauser/ + * arithmetic/softfloat.html'. + * + * THIS SOFTWARE IS DISTRIBUTED AS IS, FOR FREE. Although reasonable effort + * has been made to avoid it, THIS SOFTWARE MAY CONTAIN FAULTS THAT WILL AT + * TIMES RESULT IN INCORRECT BEHAVIOR. USE OF THIS SOFTWARE IS RESTRICTED TO + * PERSONS AND ORGANIZATIONS WHO CAN AND WILL TAKE FULL RESPONSIBILITY FOR ANY + * AND ALL LOSSES, COSTS, OR OTHER PROBLEMS ARISING FROM ITS USE. + * + * Derivative works are acceptable, even for commercial purposes, so long as + * (1) they include prominent notice that the work is derivative, and (2) they + * include prominent notice akin to these three paragraphs for those parts of + * this code that are retained. + * =========================================================================== + */ +#include <linux/kernel.h> +#include <linux/bitops.h> +#include <asm/ptrace.h> +#include <asm/vfp.h> + +#include "vfpinstr.h" +#include "vfp.h" + +static struct vfp_single vfp_single_default_qnan = { + .exponent = 255, + .sign = 0, + .significand = VFP_SINGLE_SIGNIFICAND_QNAN, +}; + +static void vfp_single_dump(const char *str, struct vfp_single *s) +{ + pr_debug("VFP: %s: sign=%d exponent=%d significand=%08x\n", + str, s->sign != 0, s->exponent, s->significand); +} + +static void vfp_single_normalise_denormal(struct vfp_single *vs) +{ + int bits = 31 - fls(vs->significand); + + vfp_single_dump("normalise_denormal: in", vs); + + if (bits) { + vs->exponent -= bits - 1; + vs->significand <<= bits; + } + + vfp_single_dump("normalise_denormal: out", vs); +} + +#ifndef DEBUG +#define vfp_single_normaliseround(sd,vsd,fpscr,except,func) __vfp_single_normaliseround(sd,vsd,fpscr,except) +u32 __vfp_single_normaliseround(int sd, struct vfp_single *vs, u32 fpscr, u32 exceptions) +#else +u32 vfp_single_normaliseround(int sd, struct vfp_single *vs, u32 fpscr, u32 exceptions, const char *func) +#endif +{ + u32 significand, incr, rmode; + int exponent, shift, underflow; + + vfp_single_dump("pack: in", vs); + + /* + * Infinities and NaNs are a special case. + */ + if (vs->exponent == 255 && (vs->significand == 0 || exceptions)) + goto pack; + + /* + * Special-case zero. + */ + if (vs->significand == 0) { + vs->exponent = 0; + goto pack; + } + + exponent = vs->exponent; + significand = vs->significand; + + /* + * Normalise first. Note that we shift the significand up to + * bit 31, so we have VFP_SINGLE_LOW_BITS + 1 below the least + * significant bit. + */ + shift = 32 - fls(significand); + if (shift < 32 && shift) { + exponent -= shift; + significand <<= shift; + } + +#ifdef DEBUG + vs->exponent = exponent; + vs->significand = significand; + vfp_single_dump("pack: normalised", vs); +#endif + + /* + * Tiny number? + */ + underflow = exponent < 0; + if (underflow) { + significand = vfp_shiftright32jamming(significand, -exponent); + exponent = 0; +#ifdef DEBUG + vs->exponent = exponent; + vs->significand = significand; + vfp_single_dump("pack: tiny number", vs); +#endif + if (!(significand & ((1 << (VFP_SINGLE_LOW_BITS + 1)) - 1))) + underflow = 0; + } + + /* + * Select rounding increment. + */ + incr = 0; + rmode = fpscr & FPSCR_RMODE_MASK; + + if (rmode == FPSCR_ROUND_NEAREST) { + incr = 1 << VFP_SINGLE_LOW_BITS; + if ((significand & (1 << (VFP_SINGLE_LOW_BITS + 1))) == 0) + incr -= 1; + } else if (rmode == FPSCR_ROUND_TOZERO) { + incr = 0; + } else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vs->sign != 0)) + incr = (1 << (VFP_SINGLE_LOW_BITS + 1)) - 1; + + pr_debug("VFP: rounding increment = 0x%08x\n", incr); + + /* + * Is our rounding going to overflow? + */ + if ((significand + incr) < significand) { + exponent += 1; + significand = (significand >> 1) | (significand & 1); + incr >>= 1; +#ifdef DEBUG + vs->exponent = exponent; + vs->significand = significand; + vfp_single_dump("pack: overflow", vs); +#endif + } + + /* + * If any of the low bits (which will be shifted out of the + * number) are non-zero, the result is inexact. + */ + if (significand & ((1 << (VFP_SINGLE_LOW_BITS + 1)) - 1)) + exceptions |= FPSCR_IXC; + + /* + * Do our rounding. + */ + significand += incr; + + /* + * Infinity? + */ + if (exponent >= 254) { + exceptions |= FPSCR_OFC | FPSCR_IXC; + if (incr == 0) { + vs->exponent = 253; + vs->significand = 0x7fffffff; + } else { + vs->exponent = 255; /* infinity */ + vs->significand = 0; + } + } else { + if (significand >> (VFP_SINGLE_LOW_BITS + 1) == 0) + exponent = 0; + if (exponent || significand > 0x80000000) + underflow = 0; + if (underflow) + exceptions |= FPSCR_UFC; + vs->exponent = exponent; + vs->significand = significand >> 1; + } + + pack: + vfp_single_dump("pack: final", vs); + { + s32 d = vfp_single_pack(vs); + pr_debug("VFP: %s: d(s%d)=%08x exceptions=%08x\n", func, + sd, d, exceptions); + vfp_put_float(sd, d); + } + + return exceptions & ~VFP_NAN_FLAG; +} + +/* + * Propagate the NaN, setting exceptions if it is signalling. + * 'n' is always a NaN. 'm' may be a number, NaN or infinity. + */ +static u32 +vfp_propagate_nan(struct vfp_single *vsd, struct vfp_single *vsn, + struct vfp_single *vsm, u32 fpscr) +{ + struct vfp_single *nan; + int tn, tm = 0; + + tn = vfp_single_type(vsn); + + if (vsm) + tm = vfp_single_type(vsm); + + if (fpscr & FPSCR_DEFAULT_NAN) + /* + * Default NaN mode - always returns a quiet NaN + */ + nan = &vfp_single_default_qnan; + else { + /* + * Contemporary mode - select the first signalling + * NAN, or if neither are signalling, the first + * quiet NAN. + */ + if (tn == VFP_SNAN || (tm != VFP_SNAN && tn == VFP_QNAN)) + nan = vsn; + else + nan = vsm; + /* + * Make the NaN quiet. + */ + nan->significand |= VFP_SINGLE_SIGNIFICAND_QNAN; + } + + *vsd = *nan; + + /* + * If one was a signalling NAN, raise invalid operation. + */ + return tn == VFP_SNAN || tm == VFP_SNAN ? FPSCR_IOC : VFP_NAN_FLAG; +} + + +/* + * Extended operations + */ +static u32 vfp_single_fabs(int sd, int unused, s32 m, u32 fpscr) +{ + vfp_put_float(sd, vfp_single_packed_abs(m)); + return 0; +} + +static u32 vfp_single_fcpy(int sd, int unused, s32 m, u32 fpscr) +{ + vfp_put_float(sd, m); + return 0; +} + +static u32 vfp_single_fneg(int sd, int unused, s32 m, u32 fpscr) +{ + vfp_put_float(sd, vfp_single_packed_negate(m)); + return 0; +} + +static const u16 sqrt_oddadjust[] = { + 0x0004, 0x0022, 0x005d, 0x00b1, 0x011d, 0x019f, 0x0236, 0x02e0, + 0x039c, 0x0468, 0x0545, 0x0631, 0x072b, 0x0832, 0x0946, 0x0a67 +}; + +static const u16 sqrt_evenadjust[] = { + 0x0a2d, 0x08af, 0x075a, 0x0629, 0x051a, 0x0429, 0x0356, 0x029e, + 0x0200, 0x0179, 0x0109, 0x00af, 0x0068, 0x0034, 0x0012, 0x0002 +}; + +u32 vfp_estimate_sqrt_significand(u32 exponent, u32 significand) +{ + int index; + u32 z, a; + + if ((significand & 0xc0000000) != 0x40000000) { + printk(KERN_WARNING "VFP: estimate_sqrt: invalid significand\n"); + } + + a = significand << 1; + index = (a >> 27) & 15; + if (exponent & 1) { + z = 0x4000 + (a >> 17) - sqrt_oddadjust[index]; + z = ((a / z) << 14) + (z << 15); + a >>= 1; + } else { + z = 0x8000 + (a >> 17) - sqrt_evenadjust[index]; + z = a / z + z; + z = (z >= 0x20000) ? 0xffff8000 : (z << 15); + if (z <= a) + return (s32)a >> 1; + } + return (u32)(((u64)a << 31) / z) + (z >> 1); +} + +static u32 vfp_single_fsqrt(int sd, int unused, s32 m, u32 fpscr) +{ + struct vfp_single vsm, vsd; + int ret, tm; + + vfp_single_unpack(&vsm, m); + tm = vfp_single_type(&vsm); + if (tm & (VFP_NAN|VFP_INFINITY)) { + struct vfp_single *vsp = &vsd; + + if (tm & VFP_NAN) + ret = vfp_propagate_nan(vsp, &vsm, NULL, fpscr); + else if (vsm.sign == 0) { + sqrt_copy: + vsp = &vsm; + ret = 0; + } else { + sqrt_invalid: + vsp = &vfp_single_default_qnan; + ret = FPSCR_IOC; + } + vfp_put_float(sd, vfp_single_pack(vsp)); + return ret; + } + + /* + * sqrt(+/- 0) == +/- 0 + */ + if (tm & VFP_ZERO) + goto sqrt_copy; + + /* + * Normalise a denormalised number + */ + if (tm & VFP_DENORMAL) + vfp_single_normalise_denormal(&vsm); + + /* + * sqrt(<0) = invalid + */ + if (vsm.sign) + goto sqrt_invalid; + + vfp_single_dump("sqrt", &vsm); + + /* + * Estimate the square root. + */ + vsd.sign = 0; + vsd.exponent = ((vsm.exponent - 127) >> 1) + 127; + vsd.significand = vfp_estimate_sqrt_significand(vsm.exponent, vsm.significand) + 2; + + vfp_single_dump("sqrt estimate", &vsd); + + /* + * And now adjust. + */ + if ((vsd.significand & VFP_SINGLE_LOW_BITS_MASK) <= 5) { + if (vsd.significand < 2) { + vsd.significand = 0xffffffff; + } else { + u64 term; + s64 rem; + vsm.significand <<= !(vsm.exponent & 1); + term = (u64)vsd.significand * vsd.significand; + rem = ((u64)vsm.significand << 32) - term; + + pr_debug("VFP: term=%016llx rem=%016llx\n", term, rem); + + while (rem < 0) { + vsd.significand -= 1; + rem += ((u64)vsd.significand << 1) | 1; + } + vsd.significand |= rem != 0; + } + } + vsd.significand = vfp_shiftright32jamming(vsd.significand, 1); + + return vfp_single_normaliseround(sd, &vsd, fpscr, 0, "fsqrt"); +} + +/* + * Equal := ZC + * Less than := N + * Greater than := C + * Unordered := CV + */ +static u32 vfp_compare(int sd, int signal_on_qnan, s32 m, u32 fpscr) +{ + s32 d; + u32 ret = 0; + + d = vfp_get_float(sd); + if (vfp_single_packed_exponent(m) == 255 && vfp_single_packed_mantissa(m)) { + ret |= FPSCR_C | FPSCR_V; + if (signal_on_qnan || !(vfp_single_packed_mantissa(m) & (1 << (VFP_SINGLE_MANTISSA_BITS - 1)))) + /* + * Signalling NaN, or signalling on quiet NaN + */ + ret |= FPSCR_IOC; + } + + if (vfp_single_packed_exponent(d) == 255 && vfp_single_packed_mantissa(d)) { + ret |= FPSCR_C | FPSCR_V; + if (signal_on_qnan || !(vfp_single_packed_mantissa(d) & (1 << (VFP_SINGLE_MANTISSA_BITS - 1)))) + /* + * Signalling NaN, or signalling on quiet NaN + */ + ret |= FPSCR_IOC; + } + + if (ret == 0) { + if (d == m || vfp_single_packed_abs(d | m) == 0) { + /* + * equal + */ + ret |= FPSCR_Z | FPSCR_C; + } else if (vfp_single_packed_sign(d ^ m)) { + /* + * different signs + */ + if (vfp_single_packed_sign(d)) + /* + * d is negative, so d < m + */ + ret |= FPSCR_N; + else + /* + * d is positive, so d > m + */ + ret |= FPSCR_C; + } else if ((vfp_single_packed_sign(d) != 0) ^ (d < m)) { + /* + * d < m + */ + ret |= FPSCR_N; + } else if ((vfp_single_packed_sign(d) != 0) ^ (d > m)) { + /* + * d > m + */ + ret |= FPSCR_C; + } + } + return ret; +} + +static u32 vfp_single_fcmp(int sd, int unused, s32 m, u32 fpscr) +{ + return vfp_compare(sd, 0, m, fpscr); +} + +static u32 vfp_single_fcmpe(int sd, int unused, s32 m, u32 fpscr) +{ + return vfp_compare(sd, 1, m, fpscr); +} + +static u32 vfp_single_fcmpz(int sd, int unused, s32 m, u32 fpscr) +{ + return vfp_compare(sd, 0, 0, fpscr); +} + +static u32 vfp_single_fcmpez(int sd, int unused, s32 m, u32 fpscr) +{ + return vfp_compare(sd, 1, 0, fpscr); +} + +static u32 vfp_single_fcvtd(int dd, int unused, s32 m, u32 fpscr) +{ + struct vfp_single vsm; + struct vfp_double vdd; + int tm; + u32 exceptions = 0; + + vfp_single_unpack(&vsm, m); + + tm = vfp_single_type(&vsm); + + /* + * If we have a signalling NaN, signal invalid operation. + */ + if (tm == VFP_SNAN) + exceptions = FPSCR_IOC; + + if (tm & VFP_DENORMAL) + vfp_single_normalise_denormal(&vsm); + + vdd.sign = vsm.sign; + vdd.significand = (u64)vsm.significand << 32; + + /* + * If we have an infinity or NaN, the exponent must be 2047. + */ + if (tm & (VFP_INFINITY|VFP_NAN)) { + vdd.exponent = 2047; + if (tm & VFP_NAN) + vdd.significand |= VFP_DOUBLE_SIGNIFICAND_QNAN; + goto pack_nan; + } else if (tm & VFP_ZERO) + vdd.exponent = 0; + else + vdd.exponent = vsm.exponent + (1023 - 127); + + /* + * Technically, if bit 0 of dd is set, this is an invalid + * instruction. However, we ignore this for efficiency. + */ + return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fcvtd"); + + pack_nan: + vfp_put_double(dd, vfp_double_pack(&vdd)); + return exceptions; +} + +static u32 vfp_single_fuito(int sd, int unused, s32 m, u32 fpscr) +{ + struct vfp_single vs; + + vs.sign = 0; + vs.exponent = 127 + 31 - 1; + vs.significand = (u32)m; + + return vfp_single_normaliseround(sd, &vs, fpscr, 0, "fuito"); +} + +static u32 vfp_single_fsito(int sd, int unused, s32 m, u32 fpscr) +{ + struct vfp_single vs; + + vs.sign = (m & 0x80000000) >> 16; + vs.exponent = 127 + 31 - 1; + vs.significand = vs.sign ? -m : m; + + return vfp_single_normaliseround(sd, &vs, fpscr, 0, "fsito"); +} + +static u32 vfp_single_ftoui(int sd, int unused, s32 m, u32 fpscr) +{ + struct vfp_single vsm; + u32 d, exceptions = 0; + int rmode = fpscr & FPSCR_RMODE_MASK; + int tm; + + vfp_single_unpack(&vsm, m); + vfp_single_dump("VSM", &vsm); + + /* + * Do we have a denormalised number? + */ + tm = vfp_single_type(&vsm); + if (tm & VFP_DENORMAL) + exceptions |= FPSCR_IDC; + + if (tm & VFP_NAN) + vsm.sign = 0; + + if (vsm.exponent >= 127 + 32) { + d = vsm.sign ? 0 : 0xffffffff; + exceptions = FPSCR_IOC; + } else if (vsm.exponent >= 127 - 1) { + int shift = 127 + 31 - vsm.exponent; + u32 rem, incr = 0; + + /* + * 2^0 <= m < 2^32-2^8 + */ + d = (vsm.significand << 1) >> shift; + rem = vsm.significand << (33 - shift); + + if (rmode == FPSCR_ROUND_NEAREST) { + incr = 0x80000000; + if ((d & 1) == 0) + incr -= 1; + } else if (rmode == FPSCR_ROUND_TOZERO) { + incr = 0; + } else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vsm.sign != 0)) { + incr = ~0; + } + + if ((rem + incr) < rem) { + if (d < 0xffffffff) + d += 1; + else + exceptions |= FPSCR_IOC; + } + + if (d && vsm.sign) { + d = 0; + exceptions |= FPSCR_IOC; + } else if (rem) + exceptions |= FPSCR_IXC; + } else { + d = 0; + if (vsm.exponent | vsm.significand) { + exceptions |= FPSCR_IXC; + if (rmode == FPSCR_ROUND_PLUSINF && vsm.sign == 0) + d = 1; + else if (rmode == FPSCR_ROUND_MINUSINF && vsm.sign) { + d = 0; + exceptions |= FPSCR_IOC; + } + } + } + + pr_debug("VFP: ftoui: d(s%d)=%08x exceptions=%08x\n", sd, d, exceptions); + + vfp_put_float(sd, d); + + return exceptions; +} + +static u32 vfp_single_ftouiz(int sd, int unused, s32 m, u32 fpscr) +{ + return vfp_single_ftoui(sd, unused, m, FPSCR_ROUND_TOZERO); +} + +static u32 vfp_single_ftosi(int sd, int unused, s32 m, u32 fpscr) +{ + struct vfp_single vsm; + u32 d, exceptions = 0; + int rmode = fpscr & FPSCR_RMODE_MASK; + + vfp_single_unpack(&vsm, m); + vfp_single_dump("VSM", &vsm); + + /* + * Do we have a denormalised number? + */ + if (vfp_single_type(&vsm) & VFP_DENORMAL) + exceptions |= FPSCR_IDC; + + if (vsm.exponent >= 127 + 32) { + /* + * m >= 2^31-2^7: invalid + */ + d = 0x7fffffff; + if (vsm.sign) + d = ~d; + exceptions |= FPSCR_IOC; + } else if (vsm.exponent >= 127 - 1) { + int shift = 127 + 31 - vsm.exponent; + u32 rem, incr = 0; + + /* 2^0 <= m <= 2^31-2^7 */ + d = (vsm.significand << 1) >> shift; + rem = vsm.significand << (33 - shift); + + if (rmode == FPSCR_ROUND_NEAREST) { + incr = 0x80000000; + if ((d & 1) == 0) + incr -= 1; + } else if (rmode == FPSCR_ROUND_TOZERO) { + incr = 0; + } else if ((rmode == FPSCR_ROUND_PLUSINF) ^ (vsm.sign != 0)) { + incr = ~0; + } + + if ((rem + incr) < rem && d < 0xffffffff) + d += 1; + if (d > 0x7fffffff + (vsm.sign != 0)) { + d = 0x7fffffff + (vsm.sign != 0); + exceptions |= FPSCR_IOC; + } else if (rem) + exceptions |= FPSCR_IXC; + + if (vsm.sign) + d = -d; + } else { + d = 0; + if (vsm.exponent | vsm.significand) { + exceptions |= FPSCR_IXC; + if (rmode == FPSCR_ROUND_PLUSINF && vsm.sign == 0) + d = 1; + else if (rmode == FPSCR_ROUND_MINUSINF && vsm.sign) + d = -1; + } + } + + pr_debug("VFP: ftosi: d(s%d)=%08x exceptions=%08x\n", sd, d, exceptions); + + vfp_put_float(sd, (s32)d); + + return exceptions; +} + +static u32 vfp_single_ftosiz(int sd, int unused, s32 m, u32 fpscr) +{ + return vfp_single_ftosi(sd, unused, m, FPSCR_ROUND_TOZERO); +} + +static u32 (* const fop_extfns[32])(int sd, int unused, s32 m, u32 fpscr) = { + [FEXT_TO_IDX(FEXT_FCPY)] = vfp_single_fcpy, + [FEXT_TO_IDX(FEXT_FABS)] = vfp_single_fabs, + [FEXT_TO_IDX(FEXT_FNEG)] = vfp_single_fneg, + [FEXT_TO_IDX(FEXT_FSQRT)] = vfp_single_fsqrt, + [FEXT_TO_IDX(FEXT_FCMP)] = vfp_single_fcmp, + [FEXT_TO_IDX(FEXT_FCMPE)] = vfp_single_fcmpe, + [FEXT_TO_IDX(FEXT_FCMPZ)] = vfp_single_fcmpz, + [FEXT_TO_IDX(FEXT_FCMPEZ)] = vfp_single_fcmpez, + [FEXT_TO_IDX(FEXT_FCVT)] = vfp_single_fcvtd, + [FEXT_TO_IDX(FEXT_FUITO)] = vfp_single_fuito, + [FEXT_TO_IDX(FEXT_FSITO)] = vfp_single_fsito, + [FEXT_TO_IDX(FEXT_FTOUI)] = vfp_single_ftoui, + [FEXT_TO_IDX(FEXT_FTOUIZ)] = vfp_single_ftouiz, + [FEXT_TO_IDX(FEXT_FTOSI)] = vfp_single_ftosi, + [FEXT_TO_IDX(FEXT_FTOSIZ)] = vfp_single_ftosiz, +}; + + + + + +static u32 +vfp_single_fadd_nonnumber(struct vfp_single *vsd, struct vfp_single *vsn, + struct vfp_single *vsm, u32 fpscr) +{ + struct vfp_single *vsp; + u32 exceptions = 0; + int tn, tm; + + tn = vfp_single_type(vsn); + tm = vfp_single_type(vsm); + + if (tn & tm & VFP_INFINITY) { + /* + * Two infinities. Are they different signs? + */ + if (vsn->sign ^ vsm->sign) { + /* + * different signs -> invalid + */ + exceptions = FPSCR_IOC; + vsp = &vfp_single_default_qnan; + } else { + /* + * same signs -> valid + */ + vsp = vsn; + } + } else if (tn & VFP_INFINITY && tm & VFP_NUMBER) { + /* + * One infinity and one number -> infinity + */ + vsp = vsn; + } else { + /* + * 'n' is a NaN of some type + */ + return vfp_propagate_nan(vsd, vsn, vsm, fpscr); + } + *vsd = *vsp; + return exceptions; +} + +static u32 +vfp_single_add(struct vfp_single *vsd, struct vfp_single *vsn, + struct vfp_single *vsm, u32 fpscr) +{ + u32 exp_diff, m_sig; + + if (vsn->significand & 0x80000000 || + vsm->significand & 0x80000000) { + pr_info("VFP: bad FP values in %s\n", __func__); + vfp_single_dump("VSN", vsn); + vfp_single_dump("VSM", vsm); + } + + /* + * Ensure that 'n' is the largest magnitude number. Note that + * if 'n' and 'm' have equal exponents, we do not swap them. + * This ensures that NaN propagation works correctly. + */ + if (vsn->exponent < vsm->exponent) { + struct vfp_single *t = vsn; + vsn = vsm; + vsm = t; + } + + /* + * Is 'n' an infinity or a NaN? Note that 'm' may be a number, + * infinity or a NaN here. + */ + if (vsn->exponent == 255) + return vfp_single_fadd_nonnumber(vsd, vsn, vsm, fpscr); + + /* + * We have two proper numbers, where 'vsn' is the larger magnitude. + * + * Copy 'n' to 'd' before doing the arithmetic. + */ + *vsd = *vsn; + + /* + * Align both numbers. + */ + exp_diff = vsn->exponent - vsm->exponent; + m_sig = vfp_shiftright32jamming(vsm->significand, exp_diff); + + /* + * If the signs are different, we are really subtracting. + */ + if (vsn->sign ^ vsm->sign) { + m_sig = vsn->significand - m_sig; + if ((s32)m_sig < 0) { + vsd->sign = vfp_sign_negate(vsd->sign); + m_sig = -m_sig; + } else if (m_sig == 0) { + vsd->sign = (fpscr & FPSCR_RMODE_MASK) == + FPSCR_ROUND_MINUSINF ? 0x8000 : 0; + } + } else { + m_sig = vsn->significand + m_sig; + } + vsd->significand = m_sig; + + return 0; +} + +static u32 +vfp_single_multiply(struct vfp_single *vsd, struct vfp_single *vsn, struct vfp_single *vsm, u32 fpscr) +{ + vfp_single_dump("VSN", vsn); + vfp_single_dump("VSM", vsm); + + /* + * Ensure that 'n' is the largest magnitude number. Note that + * if 'n' and 'm' have equal exponents, we do not swap them. + * This ensures that NaN propagation works correctly. + */ + if (vsn->exponent < vsm->exponent) { + struct vfp_single *t = vsn; + vsn = vsm; + vsm = t; + pr_debug("VFP: swapping M <-> N\n"); + } + + vsd->sign = vsn->sign ^ vsm->sign; + + /* + * If 'n' is an infinity or NaN, handle it. 'm' may be anything. + */ + if (vsn->exponent == 255) { + if (vsn->significand || (vsm->exponent == 255 && vsm->significand)) + return vfp_propagate_nan(vsd, vsn, vsm, fpscr); + if ((vsm->exponent | vsm->significand) == 0) { + *vsd = vfp_single_default_qnan; + return FPSCR_IOC; + } + vsd->exponent = vsn->exponent; + vsd->significand = 0; + return 0; + } + + /* + * If 'm' is zero, the result is always zero. In this case, + * 'n' may be zero or a number, but it doesn't matter which. + */ + if ((vsm->exponent | vsm->significand) == 0) { + vsd->exponent = 0; + vsd->significand = 0; + return 0; + } + + /* + * We add 2 to the destination exponent for the same reason as + * the addition case - though this time we have +1 from each + * input operand. + */ + vsd->exponent = vsn->exponent + vsm->exponent - 127 + 2; + vsd->significand = vfp_hi64to32jamming((u64)vsn->significand * vsm->significand); + + vfp_single_dump("VSD", vsd); + return 0; +} + +#define NEG_MULTIPLY (1 << 0) +#define NEG_SUBTRACT (1 << 1) + +static u32 +vfp_single_multiply_accumulate(int sd, int sn, s32 m, u32 fpscr, u32 negate, char *func) +{ + struct vfp_single vsd, vsp, vsn, vsm; + u32 exceptions; + s32 v; + + v = vfp_get_float(sn); + pr_debug("VFP: s%u = %08x\n", sn, v); + vfp_single_unpack(&vsn, v); + if (vsn.exponent == 0 && vsn.significand) + vfp_single_normalise_denormal(&vsn); + + vfp_single_unpack(&vsm, m); + if (vsm.exponent == 0 && vsm.significand) + vfp_single_normalise_denormal(&vsm); + + exceptions = vfp_single_multiply(&vsp, &vsn, &vsm, fpscr); + if (negate & NEG_MULTIPLY) + vsp.sign = vfp_sign_negate(vsp.sign); + + v = vfp_get_float(sd); + pr_debug("VFP: s%u = %08x\n", sd, v); + vfp_single_unpack(&vsn, v); + if (negate & NEG_SUBTRACT) + vsn.sign = vfp_sign_negate(vsn.sign); + + exceptions |= vfp_single_add(&vsd, &vsn, &vsp, fpscr); + + return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, func); +} + +/* + * Standard operations + */ + +/* + * sd = sd + (sn * sm) + */ +static u32 vfp_single_fmac(int sd, int sn, s32 m, u32 fpscr) +{ + return vfp_single_multiply_accumulate(sd, sn, m, fpscr, 0, "fmac"); +} + +/* + * sd = sd - (sn * sm) + */ +static u32 vfp_single_fnmac(int sd, int sn, s32 m, u32 fpscr) +{ + return vfp_single_multiply_accumulate(sd, sn, m, fpscr, NEG_MULTIPLY, "fnmac"); +} + +/* + * sd = -sd + (sn * sm) + */ +static u32 vfp_single_fmsc(int sd, int sn, s32 m, u32 fpscr) +{ + return vfp_single_multiply_accumulate(sd, sn, m, fpscr, NEG_SUBTRACT, "fmsc"); +} + +/* + * sd = -sd - (sn * sm) + */ +static u32 vfp_single_fnmsc(int sd, int sn, s32 m, u32 fpscr) +{ + return vfp_single_multiply_accumulate(sd, sn, m, fpscr, NEG_SUBTRACT | NEG_MULTIPLY, "fnmsc"); +} + +/* + * sd = sn * sm + */ +static u32 vfp_single_fmul(int sd, int sn, s32 m, u32 fpscr) +{ + struct vfp_single vsd, vsn, vsm; + u32 exceptions; + s32 n = vfp_get_float(sn); + + pr_debug("VFP: s%u = %08x\n", sn, n); + + vfp_single_unpack(&vsn, n); + if (vsn.exponent == 0 && vsn.significand) + vfp_single_normalise_denormal(&vsn); + + vfp_single_unpack(&vsm, m); + if (vsm.exponent == 0 && vsm.significand) + vfp_single_normalise_denormal(&vsm); + + exceptions = vfp_single_multiply(&vsd, &vsn, &vsm, fpscr); + return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, "fmul"); +} + +/* + * sd = -(sn * sm) + */ +static u32 vfp_single_fnmul(int sd, int sn, s32 m, u32 fpscr) +{ + struct vfp_single vsd, vsn, vsm; + u32 exceptions; + s32 n = vfp_get_float(sn); + + pr_debug("VFP: s%u = %08x\n", sn, n); + + vfp_single_unpack(&vsn, n); + if (vsn.exponent == 0 && vsn.significand) + vfp_single_normalise_denormal(&vsn); + + vfp_single_unpack(&vsm, m); + if (vsm.exponent == 0 && vsm.significand) + vfp_single_normalise_denormal(&vsm); + + exceptions = vfp_single_multiply(&vsd, &vsn, &vsm, fpscr); + vsd.sign = vfp_sign_negate(vsd.sign); + return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, "fnmul"); +} + +/* + * sd = sn + sm + */ +static u32 vfp_single_fadd(int sd, int sn, s32 m, u32 fpscr) +{ + struct vfp_single vsd, vsn, vsm; + u32 exceptions; + s32 n = vfp_get_float(sn); + + pr_debug("VFP: s%u = %08x\n", sn, n); + + /* + * Unpack and normalise denormals. + */ + vfp_single_unpack(&vsn, n); + if (vsn.exponent == 0 && vsn.significand) + vfp_single_normalise_denormal(&vsn); + + vfp_single_unpack(&vsm, m); + if (vsm.exponent == 0 && vsm.significand) + vfp_single_normalise_denormal(&vsm); + + exceptions = vfp_single_add(&vsd, &vsn, &vsm, fpscr); + + return vfp_single_normaliseround(sd, &vsd, fpscr, exceptions, "fadd"); +} + +/* + * sd = sn - sm + */ +static u32 vfp_single_fsub(int sd, int sn, s32 m, u32 fpscr) +{ + /* + * Subtraction is addition with one sign inverted. + */ + return vfp_single_fadd(sd, sn, vfp_single_packed_negate(m), fpscr); +} + +/* + * sd = sn / sm + */ +static u32 vfp_single_fdiv(int sd, int sn, s32 m, u32 fpscr) +{ + struct vfp_single vsd, vsn, vsm; + u32 exceptions = 0; + s32 n = vfp_get_float(sn); + int tm, tn; + + pr_debug("VFP: s%u = %08x\n", sn, n); + + vfp_single_unpack(&vsn, n); + vfp_single_unpack(&vsm, m); + + vsd.sign = vsn.sign ^ vsm.sign; + + tn = vfp_single_type(&vsn); + tm = vfp_single_type(&vsm); + + /* + * Is n a NAN? + */ + if (tn & VFP_NAN) + goto vsn_nan; + + /* + * Is m a NAN? + */ + if (tm & VFP_NAN) + goto vsm_nan; + + /* + * If n and m are infinity, the result is invalid + * If n and m are zero, the result is invalid + */ + if (tm & tn & (VFP_INFINITY|VFP_ZERO)) + goto invalid; + + /* + * If n is infinity, the result is infinity + */ + if (tn & VFP_INFINITY) + goto infinity; + + /* + * If m is zero, raise div0 exception + */ + if (tm & VFP_ZERO) + goto divzero; + + /* + * If m is infinity, or n is zero, the result is zero + */ + if (tm & VFP_INFINITY || tn & VFP_ZERO) + goto zero; + + if (tn & VFP_DENORMAL) + vfp_single_normalise_denormal(&vsn); + if (tm & VFP_DENORMAL) + vfp_single_normalise_denormal(&vsm); + + /* + * Ok, we have two numbers, we can perform division. + */ + vsd.exponent = vsn.exponent - vsm.exponent + 127 - 1; + vsm.significand <<= 1; + if (vsm.significand <= (2 * vsn.significand)) { + vsn.significand >>= 1; + vsd.exponent++; + } + vsd.significand = ((u64)vsn.significand << 32) / vsm.significand; + if ((vsd.significand & 0x3f) == 0) + vsd.significand |= ((u64)vsm.significand * vsd.significand != (u64)vsn.significand << 32); + + return vfp_single_normaliseround(sd, &vsd, fpscr, 0, "fdiv"); + + vsn_nan: + exceptions = vfp_propagate_nan(&vsd, &vsn, &vsm, fpscr); + pack: + vfp_put_float(sd, vfp_single_pack(&vsd)); + return exceptions; + + vsm_nan: + exceptions = vfp_propagate_nan(&vsd, &vsm, &vsn, fpscr); + goto pack; + + zero: + vsd.exponent = 0; + vsd.significand = 0; + goto pack; + + divzero: + exceptions = FPSCR_DZC; + infinity: + vsd.exponent = 255; + vsd.significand = 0; + goto pack; + + invalid: + vfp_put_float(sd, vfp_single_pack(&vfp_single_default_qnan)); + return FPSCR_IOC; +} + +static u32 (* const fop_fns[16])(int sd, int sn, s32 m, u32 fpscr) = { + [FOP_TO_IDX(FOP_FMAC)] = vfp_single_fmac, + [FOP_TO_IDX(FOP_FNMAC)] = vfp_single_fnmac, + [FOP_TO_IDX(FOP_FMSC)] = vfp_single_fmsc, + [FOP_TO_IDX(FOP_FNMSC)] = vfp_single_fnmsc, + [FOP_TO_IDX(FOP_FMUL)] = vfp_single_fmul, + [FOP_TO_IDX(FOP_FNMUL)] = vfp_single_fnmul, + [FOP_TO_IDX(FOP_FADD)] = vfp_single_fadd, + [FOP_TO_IDX(FOP_FSUB)] = vfp_single_fsub, + [FOP_TO_IDX(FOP_FDIV)] = vfp_single_fdiv, +}; + +#define FREG_BANK(x) ((x) & 0x18) +#define FREG_IDX(x) ((x) & 7) + +u32 vfp_single_cpdo(u32 inst, u32 fpscr) +{ + u32 op = inst & FOP_MASK; + u32 exceptions = 0; + unsigned int sd = vfp_get_sd(inst); + unsigned int sn = vfp_get_sn(inst); + unsigned int sm = vfp_get_sm(inst); + unsigned int vecitr, veclen, vecstride; + u32 (*fop)(int, int, s32, u32); + + veclen = fpscr & FPSCR_LENGTH_MASK; + vecstride = 1 + ((fpscr & FPSCR_STRIDE_MASK) == FPSCR_STRIDE_MASK); + + /* + * If destination bank is zero, vector length is always '1'. + * ARM DDI0100F C5.1.3, C5.3.2. + */ + if (FREG_BANK(sd) == 0) + veclen = 0; + + pr_debug("VFP: vecstride=%u veclen=%u\n", vecstride, + (veclen >> FPSCR_LENGTH_BIT) + 1); + + fop = (op == FOP_EXT) ? fop_extfns[sn] : fop_fns[FOP_TO_IDX(op)]; + if (!fop) + goto invalid; + + for (vecitr = 0; vecitr <= veclen; vecitr += 1 << FPSCR_LENGTH_BIT) { + s32 m = vfp_get_float(sm); + u32 except; + + if (op == FOP_EXT) + pr_debug("VFP: itr%d (s%u) = op[%u] (s%u=%08x)\n", + vecitr >> FPSCR_LENGTH_BIT, sd, sn, sm, m); + else + pr_debug("VFP: itr%d (s%u) = (s%u) op[%u] (s%u=%08x)\n", + vecitr >> FPSCR_LENGTH_BIT, sd, sn, + FOP_TO_IDX(op), sm, m); + + except = fop(sd, sn, m, fpscr); + pr_debug("VFP: itr%d: exceptions=%08x\n", + vecitr >> FPSCR_LENGTH_BIT, except); + + exceptions |= except; + + /* + * This ensures that comparisons only operate on scalars; + * comparisons always return with one FPSCR status bit set. + */ + if (except & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V)) + break; + + /* + * CHECK: It appears to be undefined whether we stop when + * we encounter an exception. We continue. + */ + + sd = FREG_BANK(sd) + ((FREG_IDX(sd) + vecstride) & 7); + sn = FREG_BANK(sn) + ((FREG_IDX(sn) + vecstride) & 7); + if (FREG_BANK(sm) != 0) + sm = FREG_BANK(sm) + ((FREG_IDX(sm) + vecstride) & 7); + } + return exceptions; + + invalid: + return (u32)-1; +} |