diff options
Diffstat (limited to 'arch/arm/vfp')
| -rw-r--r-- | arch/arm/vfp/Makefile | 15 | ||||
| -rw-r--r-- | arch/arm/vfp/entry.S | 70 | ||||
| -rw-r--r-- | arch/arm/vfp/vfp.h | 380 | ||||
| -rw-r--r-- | arch/arm/vfp/vfpdouble.c | 1204 | ||||
| -rw-r--r-- | arch/arm/vfp/vfphw.S | 284 | ||||
| -rw-r--r-- | arch/arm/vfp/vfpinstr.h | 88 | ||||
| -rw-r--r-- | arch/arm/vfp/vfpmodule.c | 613 | ||||
| -rw-r--r-- | arch/arm/vfp/vfpsingle.c | 1244 |
8 files changed, 3898 insertions, 0 deletions
diff --git a/arch/arm/vfp/Makefile b/arch/arm/vfp/Makefile new file mode 100644 index 00000000..6de73aab --- /dev/null +++ b/arch/arm/vfp/Makefile @@ -0,0 +1,15 @@ +# +# linux/arch/arm/vfp/Makefile +# +# Copyright (C) 2001 ARM Limited +# + +# ccflags-y := -DDEBUG +# asflags-y := -DDEBUG + +KBUILD_AFLAGS :=$(KBUILD_AFLAGS:-msoft-float=-Wa,-mfpu=softvfp+vfp) +LDFLAGS +=--no-warn-mismatch + +obj-y += vfp.o + +vfp-$(CONFIG_VFP) += vfpmodule.o entry.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 00000000..c1a97840 --- /dev/null +++ b/arch/arm/vfp/entry.S @@ -0,0 +1,70 @@ +/* + * 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 + * r2 = faulted PC+4 + * r9 = successful return + * r10 = thread_info structure + * lr = failure return + */ +#include <asm/thread_info.h> +#include <asm/vfpmacros.h> +#include "../kernel/entry-header.S" + +ENTRY(do_vfp) +#ifdef CONFIG_PREEMPT + ldr r4, [r10, #TI_PREEMPT] @ get preempt count + add r11, r4, #1 @ increment it + str r11, [r10, #TI_PREEMPT] +#endif + enable_irq + str r2, [sp, #S_PC] @ update regs->ARM_pc for Thumb 2 case + ldr r4, .LCvfp + ldr r11, [r10, #TI_CPU] @ CPU number + add r10, r10, #TI_VFPSTATE @ r10 = workspace + ldr pc, [r4] @ call VFP entry point +ENDPROC(do_vfp) + +ENTRY(vfp_null_entry) +#ifdef CONFIG_PREEMPT + get_thread_info r10 + ldr r4, [r10, #TI_PREEMPT] @ get preempt count + sub r11, r4, #1 @ decrement it + str r11, [r10, #TI_PREEMPT] +#endif + mov pc, lr +ENDPROC(vfp_null_entry) + + .align 2 +.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 +ENTRY(vfp_testing_entry) +#ifdef CONFIG_PREEMPT + get_thread_info r10 + ldr r4, [r10, #TI_PREEMPT] @ get preempt count + sub r11, r4, #1 @ decrement it + str r11, [r10, #TI_PREEMPT] +#endif + ldr r0, VFP_arch_address + str r5, [r0] @ known non-zero value + mov pc, r9 @ we have handled the fault +ENDPROC(vfp_testing_entry) + + .align 2 +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 00000000..c8c98dd4 --- /dev/null +++ b/arch/arm/vfp/vfp.h @@ -0,0 +1,380 @@ +/* + * 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; + if (mh << 32 <= nh) { + z = 0xffffffff00000000ULL; + } else { + z = nh; + do_div(z, mh); + z <<= 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); + if (mh << 32 <= remh) { + z |= 0xffffffff; + } else { + do_div(remh, mh); + z |= remh; + } + 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(s32 val, unsigned int reg); + +/* + * 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. + */ +#ifdef CONFIG_VFPv3 +#define VFP_REG_ZERO 32 +#else +#define VFP_REG_ZERO 16 +#endif +extern u64 vfp_get_double(unsigned int reg); +extern void vfp_put_double(u64 val, unsigned int reg); + +#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); + +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 + +/* + * A bit pattern used to indicate the initial (unset) value of the + * exception mask, in case nothing handles an instruction. This + * doesn't include the NAN flag, which get masked out before + * we check for an error. + */ +#define VFP_EXCEPTION_ERROR ((u32)-1 & ~VFP_NAN_FLAG) + +/* + * A flag to tell vfp instruction type. + * OP_SCALAR - this operation always operates in scalar mode + * OP_SD - the instruction exceptionally writes to a single precision result. + * OP_DD - the instruction exceptionally writes to a double precision result. + * OP_SM - the instruction exceptionally reads from a single precision operand. + */ +#define OP_SCALAR (1 << 0) +#define OP_SD (1 << 1) +#define OP_DD (1 << 1) +#define OP_SM (1 << 2) + +struct op { + u32 (* const fn)(int dd, int dn, int dm, u32 fpscr); + u32 flags; +}; + +extern void vfp_save_state(void *location, u32 fpexc); diff --git a/arch/arm/vfp/vfpdouble.c b/arch/arm/vfp/vfpdouble.c new file mode 100644 index 00000000..6cac43bd --- /dev/null +++ b/arch/arm/vfp/vfpdouble.c @@ -0,0 +1,1204 @@ +/* + * 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/div64.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 = 63 - 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(d, dd); + } + return exceptions; +} + +/* + * 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(vfp_double_packed_abs(vfp_get_double(dm)), dd); + return 0; +} + +static u32 vfp_double_fcpy(int dd, int unused, int dm, u32 fpscr) +{ + vfp_put_double(vfp_get_double(dm), dd); + return 0; +} + +static u32 vfp_double_fneg(int dd, int unused, int dm, u32 fpscr) +{ + vfp_put_double(vfp_double_packed_negate(vfp_get_double(dm)), dd); + 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(vfp_double_pack(vdp), dd); + 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_QNAN) + 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(vfp_single_pack(&vsd), sd); + 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(d, sd); + + 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; + int tm; + + vfp_double_unpack(&vdm, vfp_get_double(dm)); + vfp_double_dump("VDM", &vdm); + + /* + * Do we have denormalised number? + */ + tm = vfp_double_type(&vdm); + if (tm & VFP_DENORMAL) + exceptions |= FPSCR_IDC; + + if (tm & VFP_NAN) { + d = 0; + exceptions |= FPSCR_IOC; + } else 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((s32)d, sd); + + 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 struct op fops_ext[32] = { + [FEXT_TO_IDX(FEXT_FCPY)] = { vfp_double_fcpy, 0 }, + [FEXT_TO_IDX(FEXT_FABS)] = { vfp_double_fabs, 0 }, + [FEXT_TO_IDX(FEXT_FNEG)] = { vfp_double_fneg, 0 }, + [FEXT_TO_IDX(FEXT_FSQRT)] = { vfp_double_fsqrt, 0 }, + [FEXT_TO_IDX(FEXT_FCMP)] = { vfp_double_fcmp, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FCMPE)] = { vfp_double_fcmpe, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FCMPZ)] = { vfp_double_fcmpz, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FCMPEZ)] = { vfp_double_fcmpez, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FCVT)] = { vfp_double_fcvts, OP_SCALAR|OP_SD }, + [FEXT_TO_IDX(FEXT_FUITO)] = { vfp_double_fuito, OP_SCALAR|OP_SM }, + [FEXT_TO_IDX(FEXT_FSITO)] = { vfp_double_fsito, OP_SCALAR|OP_SM }, + [FEXT_TO_IDX(FEXT_FTOUI)] = { vfp_double_ftoui, OP_SCALAR|OP_SD }, + [FEXT_TO_IDX(FEXT_FTOUIZ)] = { vfp_double_ftouiz, OP_SCALAR|OP_SD }, + [FEXT_TO_IDX(FEXT_FTOSI)] = { vfp_double_ftosi, OP_SCALAR|OP_SD }, + [FEXT_TO_IDX(FEXT_FTOSIZ)] = { vfp_double_ftosiz, OP_SCALAR|OP_SD }, +}; + + + + +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 if (m_sig == 0) { + vdd->sign = (fpscr & FPSCR_RMODE_MASK) == + FPSCR_ROUND_MINUSINF ? 0x8000 : 0; + } + } 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(vfp_double_pack(&vdd), dd); + 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(vfp_double_pack(&vfp_double_default_qnan), dd); + return FPSCR_IOC; +} + +static struct op fops[16] = { + [FOP_TO_IDX(FOP_FMAC)] = { vfp_double_fmac, 0 }, + [FOP_TO_IDX(FOP_FNMAC)] = { vfp_double_fnmac, 0 }, + [FOP_TO_IDX(FOP_FMSC)] = { vfp_double_fmsc, 0 }, + [FOP_TO_IDX(FOP_FNMSC)] = { vfp_double_fnmsc, 0 }, + [FOP_TO_IDX(FOP_FMUL)] = { vfp_double_fmul, 0 }, + [FOP_TO_IDX(FOP_FNMUL)] = { vfp_double_fnmul, 0 }, + [FOP_TO_IDX(FOP_FADD)] = { vfp_double_fadd, 0 }, + [FOP_TO_IDX(FOP_FSUB)] = { vfp_double_fsub, 0 }, + [FOP_TO_IDX(FOP_FDIV)] = { vfp_double_fdiv, 0 }, +}; + +#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 dest; + unsigned int dn = vfp_get_dn(inst); + unsigned int dm; + unsigned int vecitr, veclen, vecstride; + struct op *fop; + + vecstride = (1 + ((fpscr & FPSCR_STRIDE_MASK) == FPSCR_STRIDE_MASK)); + + fop = (op == FOP_EXT) ? &fops_ext[FEXT_TO_IDX(inst)] : &fops[FOP_TO_IDX(op)]; + + /* + * fcvtds takes an sN register number as destination, not dN. + * It also always operates on scalars. + */ + if (fop->flags & OP_SD) + dest = vfp_get_sd(inst); + else + dest = vfp_get_dd(inst); + + /* + * f[us]ito takes a sN operand, not a dN operand. + */ + if (fop->flags & OP_SM) + dm = vfp_get_sm(inst); + else + dm = vfp_get_dm(inst); + + /* + * If destination bank is zero, vector length is always '1'. + * ARM DDI0100F C5.1.3, C5.3.2. + */ + if ((fop->flags & OP_SCALAR) || (FREG_BANK(dest) == 0)) + veclen = 0; + else + veclen = fpscr & FPSCR_LENGTH_MASK; + + pr_debug("VFP: vecstride=%u veclen=%u\n", vecstride, + (veclen >> FPSCR_LENGTH_BIT) + 1); + + if (!fop->fn) + goto invalid; + + for (vecitr = 0; vecitr <= veclen; vecitr += 1 << FPSCR_LENGTH_BIT) { + u32 except; + char type; + + type = fop->flags & OP_SD ? 's' : 'd'; + if (op == FOP_EXT) + pr_debug("VFP: itr%d (%c%u) = op[%u] (d%u)\n", + vecitr >> FPSCR_LENGTH_BIT, + type, dest, dn, dm); + else + pr_debug("VFP: itr%d (%c%u) = (d%u) op[%u] (d%u)\n", + vecitr >> FPSCR_LENGTH_BIT, + type, dest, dn, FOP_TO_IDX(op), dm); + + except = fop->fn(dest, dn, dm, fpscr); + pr_debug("VFP: itr%d: exceptions=%08x\n", + vecitr >> FPSCR_LENGTH_BIT, except); + + exceptions |= except; + + /* + * CHECK: It appears to be undefined whether we stop when + * we encounter an exception. We continue. + */ + dest = FREG_BANK(dest) + ((FREG_IDX(dest) + vecstride) & 3); + dn = FREG_BANK(dn) + ((FREG_IDX(dn) + vecstride) & 3); + if (FREG_BANK(dm) != 0) + dm = FREG_BANK(dm) + ((FREG_IDX(dm) + vecstride) & 3); + } + return exceptions; + + invalid: + return ~0; +} diff --git a/arch/arm/vfp/vfphw.S b/arch/arm/vfp/vfphw.S new file mode 100644 index 00000000..404538ae --- /dev/null +++ b/arch/arm/vfp/vfphw.S @@ -0,0 +1,284 @@ +/* + * 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 +@ r11 = CPU number +@ lr = failure return + +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_EN + bne look_for_VFP_exceptions @ VFP is already enabled + + DBGSTR1 "enable %x", r10 + ldr r3, vfp_current_hw_state_address + orr r1, r1, #FPEXC_EN @ user FPEXC has the enable bit set + ldr r4, [r3, r11, lsl #2] @ vfp_current_hw_state pointer + bic r5, r1, #FPEXC_EX @ make sure exceptions are disabled + cmp r4, r10 @ this thread owns the hw context? + beq vfp_hw_state_valid + + VFPFMXR FPEXC, r5 @ enable VFP, disable any pending + @ exceptions, so we can get at the + @ rest of it + +#ifndef CONFIG_SMP + @ Save out the current registers to the old thread state + @ No need for SMP since this is not done lazily + + DBGSTR1 "save old state %p", r4 + cmp r4, #0 + beq no_old_VFP_process + VFPFSTMIA r4, r5 @ save the working registers + VFPFMRX r5, FPSCR @ current status +#ifndef CONFIG_CPU_FEROCEON + tst r1, #FPEXC_EX @ is there additional state to save? + beq 1f + VFPFMRX r6, FPINST @ FPINST (only if FPEXC.EX is set) + tst r1, #FPEXC_FP2V @ is there an FPINST2 to read? + beq 1f + VFPFMRX r8, FPINST2 @ FPINST2 if needed (and present) +1: +#endif + stmia r4, {r1, r5, r6, r8} @ save FPEXC, FPSCR, FPINST, FPINST2 + @ and point r4 at the word at the + @ start of the register dump +#endif + +no_old_VFP_process: + DBGSTR1 "load state %p", r10 + str r10, [r3, r11, lsl #2] @ update the vfp_current_hw_state pointer + @ Load the saved state back into the VFP + VFPFLDMIA r10, r5 @ reload the working registers while + @ FPEXC is in a safe state + ldmia r10, {r1, r5, r6, r8} @ load FPEXC, FPSCR, FPINST, FPINST2 +#ifndef CONFIG_CPU_FEROCEON + tst r1, #FPEXC_EX @ is there additional state to restore? + beq 1f + VFPFMXR FPINST, r6 @ restore FPINST (only if FPEXC.EX is set) + tst r1, #FPEXC_FP2V @ is there an FPINST2 to write? + beq 1f + VFPFMXR FPINST2, r8 @ FPINST2 if needed (and present) +1: +#endif + VFPFMXR FPSCR, r5 @ restore status + +@ The context stored in the VFP hardware is up to date with this thread +vfp_hw_state_valid: + tst r1, #FPEXC_EX + 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 +#ifdef CONFIG_PREEMPT + get_thread_info r10 + ldr r4, [r10, #TI_PREEMPT] @ get preempt count + sub r11, r4, #1 @ decrement it + str r11, [r10, #TI_PREEMPT] +#endif + mov pc, r9 @ we think we have handled things + + +look_for_VFP_exceptions: + @ Check for synchronous or asynchronous exception + tst r1, #FPEXC_EX | FPEXC_DEX + bne process_exception + @ On some implementations of the VFP subarch 1, setting FPSCR.IXE + @ causes all the CDP instructions to be bounced synchronously without + @ setting the FPEXC.EX bit + VFPFMRX r5, FPSCR + tst r5, #FPSCR_IXE + bne process_exception + + @ Fall into hand on to next handler - appropriate coproc instr + @ not recognised by VFP + + DBGSTR "not VFP" +#ifdef CONFIG_PREEMPT + get_thread_info r10 + ldr r4, [r10, #TI_PREEMPT] @ get preempt count + sub r11, r4, #1 @ decrement it + str r11, [r10, #TI_PREEMPT] +#endif + mov pc, lr + +process_exception: + DBGSTR "bounce" + 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 VFP_bounce @ we have handled this - the support + @ code will raise an exception if + @ required. If not, the user code will + @ retry the faulted instruction +ENDPROC(vfp_support_entry) + +ENTRY(vfp_save_state) + @ Save the current VFP state + @ r0 - save location + @ r1 - FPEXC + DBGSTR1 "save VFP state %p", r0 + VFPFSTMIA r0, r2 @ save the working registers + VFPFMRX r2, FPSCR @ current status + tst r1, #FPEXC_EX @ is there additional state to save? + beq 1f + VFPFMRX r3, FPINST @ FPINST (only if FPEXC.EX is set) + tst r1, #FPEXC_FP2V @ is there an FPINST2 to read? + beq 1f + VFPFMRX r12, FPINST2 @ FPINST2 if needed (and present) +1: + stmia r0, {r1, r2, r3, r12} @ save FPEXC, FPSCR, FPINST, FPINST2 + mov pc, lr +ENDPROC(vfp_save_state) + + .align +vfp_current_hw_state_address: + .word vfp_current_hw_state + + .macro tbl_branch, base, tmp, shift +#ifdef CONFIG_THUMB2_KERNEL + adr \tmp, 1f + add \tmp, \tmp, \base, lsl \shift + mov pc, \tmp +#else + add pc, pc, \base, lsl \shift + mov r0, r0 +#endif +1: + .endm + +ENTRY(vfp_get_float) + tbl_branch r0, r3, #3 + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 +1: mrc p10, 0, r0, c\dr, c0, 0 @ fmrs r0, s0 + mov pc, lr + .org 1b + 8 +1: mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1 + mov pc, lr + .org 1b + 8 + .endr +ENDPROC(vfp_get_float) + +ENTRY(vfp_put_float) + tbl_branch r1, r3, #3 + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 +1: mcr p10, 0, r0, c\dr, c0, 0 @ fmsr r0, s0 + mov pc, lr + .org 1b + 8 +1: mcr p10, 0, r0, c\dr, c0, 4 @ fmsr r0, s1 + mov pc, lr + .org 1b + 8 + .endr +ENDPROC(vfp_put_float) + +ENTRY(vfp_get_double) + tbl_branch r0, r3, #3 + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 +1: fmrrd r0, r1, d\dr + mov pc, lr + .org 1b + 8 + .endr +#ifdef CONFIG_VFPv3 + @ d16 - d31 registers + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 +1: mrrc p11, 3, r0, r1, c\dr @ fmrrd r0, r1, d\dr + mov pc, lr + .org 1b + 8 + .endr +#endif + + @ virtual register 16 (or 32 if VFPv3) for compare with zero + mov r0, #0 + mov r1, #0 + mov pc, lr +ENDPROC(vfp_get_double) + +ENTRY(vfp_put_double) + tbl_branch r2, r3, #3 + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 +1: fmdrr d\dr, r0, r1 + mov pc, lr + .org 1b + 8 + .endr +#ifdef CONFIG_VFPv3 + @ d16 - d31 registers + .irp dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15 +1: mcrr p11, 3, r0, r1, c\dr @ fmdrr r0, r1, d\dr + mov pc, lr + .org 1b + 8 + .endr +#endif +ENDPROC(vfp_put_double) diff --git a/arch/arm/vfp/vfpinstr.h b/arch/arm/vfp/vfpinstr.h new file mode 100644 index 00000000..15b95b5a --- /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 | (inst & (1 << 22)) >> 18) +#define vfp_get_sm(inst) ((inst & 0x0000000f) << 1 | (inst & (1 << 5)) >> 5) +#define vfp_get_dm(inst) ((inst & 0x0000000f) | (inst & (1 << 5)) >> 1) +#define vfp_get_sn(inst) ((inst & 0x000f0000) >> 15 | (inst & (1 << 7)) >> 7) +#define vfp_get_dn(inst) ((inst & 0x000f0000) >> 16 | (inst & (1 << 7)) >> 3) + +#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) : : "cc"); \ + __v; \ + }) + +#define fmxr(_vfp_,_var_) \ + asm("mcr p10, 7, %0, " vfpreg(_vfp_) ", cr0, 0 @ fmxr " #_vfp_ ", %0" \ + : : "r" (_var_) : "cc") + +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 00000000..2bd4980b --- /dev/null +++ b/arch/arm/vfp/vfpmodule.c @@ -0,0 +1,613 @@ +/* + * 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/types.h> +#include <linux/cpu.h> +#include <linux/hardirq.h> +#include <linux/kernel.h> +#include <linux/notifier.h> +#include <linux/signal.h> +#include <linux/sched.h> +#include <linux/smp.h> +#include <linux/init.h> + +#include <asm/cputype.h> +#include <asm/thread_notify.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_null_entry(void); + +void (*vfp_vector)(void) = vfp_null_entry; + +/* + * The pointer to the vfpstate structure of the thread which currently + * owns the context held in the VFP hardware, or NULL if the hardware + * context is invalid. + */ +union vfp_state *vfp_current_hw_state[NR_CPUS]; + +/* + * 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 initialization. + */ +static void vfp_thread_flush(struct thread_info *thread) +{ + union vfp_state *vfp = &thread->vfpstate; + unsigned int cpu; + + memset(vfp, 0, sizeof(union vfp_state)); + + vfp->hard.fpexc = FPEXC_EN; + vfp->hard.fpscr = FPSCR_ROUND_NEAREST; + + /* + * Disable VFP to ensure we initialize it first. We must ensure + * that the modification of vfp_current_hw_state[] and hardware disable + * are done for the same CPU and without preemption. + */ + cpu = get_cpu(); + if (vfp_current_hw_state[cpu] == vfp) + vfp_current_hw_state[cpu] = NULL; + fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN); + put_cpu(); +} + +static void vfp_thread_exit(struct thread_info *thread) +{ + /* release case: Per-thread VFP cleanup. */ + union vfp_state *vfp = &thread->vfpstate; + unsigned int cpu = get_cpu(); + + if (vfp_current_hw_state[cpu] == vfp) + vfp_current_hw_state[cpu] = NULL; + put_cpu(); +} + +static void vfp_thread_copy(struct thread_info *thread) +{ + struct thread_info *parent = current_thread_info(); + + vfp_sync_hwstate(parent); + thread->vfpstate = parent->vfpstate; +} + +/* + * When this function is called with the following 'cmd's, the following + * is true while this function is being run: + * THREAD_NOFTIFY_SWTICH: + * - the previously running thread will not be scheduled onto another CPU. + * - the next thread to be run (v) will not be running on another CPU. + * - thread->cpu is the local CPU number + * - not preemptible as we're called in the middle of a thread switch + * THREAD_NOTIFY_FLUSH: + * - the thread (v) will be running on the local CPU, so + * v === current_thread_info() + * - thread->cpu is the local CPU number at the time it is accessed, + * but may change at any time. + * - we could be preempted if tree preempt rcu is enabled, so + * it is unsafe to use thread->cpu. + * THREAD_NOTIFY_EXIT + * - the thread (v) will be running on the local CPU, so + * v === current_thread_info() + * - thread->cpu is the local CPU number at the time it is accessed, + * but may change at any time. + * - we could be preempted if tree preempt rcu is enabled, so + * it is unsafe to use thread->cpu. + */ +static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v) +{ + struct thread_info *thread = v; + u32 fpexc; +#ifdef CONFIG_SMP + unsigned int cpu; +#endif + + switch (cmd) { + case THREAD_NOTIFY_SWITCH: + fpexc = fmrx(FPEXC); + +#ifdef CONFIG_SMP + cpu = thread->cpu; + + /* + * On SMP, if VFP is enabled, save the old state in + * case the thread migrates to a different CPU. The + * restoring is done lazily. + */ + if ((fpexc & FPEXC_EN) && vfp_current_hw_state[cpu]) { + vfp_save_state(vfp_current_hw_state[cpu], fpexc); + vfp_current_hw_state[cpu]->hard.cpu = cpu; + } + /* + * Thread migration, just force the reloading of the + * state on the new CPU in case the VFP registers + * contain stale data. + */ + if (thread->vfpstate.hard.cpu != cpu) + vfp_current_hw_state[cpu] = NULL; +#endif + + /* + * Always disable VFP so we can lazily save/restore the + * old state. + */ + fmxr(FPEXC, fpexc & ~FPEXC_EN); + break; + + case THREAD_NOTIFY_FLUSH: + vfp_thread_flush(thread); + break; + + case THREAD_NOTIFY_EXIT: + vfp_thread_exit(thread); + break; + + case THREAD_NOTIFY_COPY: + vfp_thread_copy(thread); + break; + } + + return NOTIFY_DONE; +} + +static struct notifier_block vfp_notifier_block = { + .notifier_call = vfp_notifier, +}; + +/* + * Raise a SIGFPE for the current process. + * sicode describes the signal being raised. + */ +static 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 __user *)(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; + + send_sig_info(SIGFPE, &info, current); +} + +static void vfp_panic(char *reason, u32 inst) +{ + 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), inst); + 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 == VFP_EXCEPTION_ERROR) { + vfp_panic("unhandled bounce", inst); + 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_DZC, FPSCR_DZE, FPE_FLTDIV); + 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 = VFP_EXCEPTION_ERROR; + + 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 & ~VFP_NAN_FLAG; +} + +/* + * Package up a bounce condition. + */ +void VFP_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs) +{ + u32 fpscr, orig_fpscr, fpsid, exceptions; + + pr_debug("VFP: bounce: trigger %08x fpexc %08x\n", trigger, fpexc); + + /* + * At this point, FPEXC can have the following configuration: + * + * EX DEX IXE + * 0 1 x - synchronous exception + * 1 x 0 - asynchronous exception + * 1 x 1 - sychronous on VFP subarch 1 and asynchronous on later + * 0 0 1 - synchronous on VFP9 (non-standard subarch 1 + * implementation), undefined otherwise + * + * Clear various bits and enable access to the VFP so we can + * handle the bounce. + */ + fmxr(FPEXC, fpexc & ~(FPEXC_EX|FPEXC_DEX|FPEXC_FP2V|FPEXC_VV|FPEXC_TRAP_MASK)); + + fpsid = fmrx(FPSID); + orig_fpscr = fpscr = fmrx(FPSCR); + + /* + * Check for the special VFP subarch 1 and FPSCR.IXE bit case + */ + if ((fpsid & FPSID_ARCH_MASK) == (1 << FPSID_ARCH_BIT) + && (fpscr & FPSCR_IXE)) { + /* + * Synchronous exception, emulate the trigger instruction + */ + goto emulate; + } + + if (fpexc & FPEXC_EX) { +#ifndef CONFIG_CPU_FEROCEON + /* + * Asynchronous exception. The instruction is read from FPINST + * and the interrupted instruction has to be restarted. + */ + trigger = fmrx(FPINST); + regs->ARM_pc -= 4; +#endif + } else if (!(fpexc & FPEXC_DEX)) { + /* + * Illegal combination of bits. It can be caused by an + * unallocated VFP instruction but with FPSCR.IXE set and not + * on VFP subarch 1. + */ + vfp_raise_exceptions(VFP_EXCEPTION_ERROR, trigger, fpscr, regs); + goto exit; + } + + /* + * Modify fpscr to indicate the number of iterations remaining. + * If FPEXC.EX is 0, FPEXC.DEX is 1 and the FPEXC.VV bit indicates + * whether FPEXC.VECITR or FPSCR.LEN is used. + */ + if (fpexc & (FPEXC_EX | FPEXC_VV)) { + 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. + */ + exceptions = vfp_emulate_instruction(trigger, fpscr, regs); + if (exceptions) + vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs); + + /* + * If there isn't a second FP instruction, exit now. Note that + * the FPEXC.FP2V bit is valid only if FPEXC.EX is 1. + */ + if (fpexc ^ (FPEXC_EX | FPEXC_FP2V)) + goto exit; + + /* + * The barrier() here prevents fpinst2 being read + * before the condition above. + */ + barrier(); + trigger = fmrx(FPINST2); + + emulate: + exceptions = vfp_emulate_instruction(trigger, orig_fpscr, regs); + if (exceptions) + vfp_raise_exceptions(exceptions, trigger, orig_fpscr, regs); + exit: + preempt_enable(); +} + +static void vfp_enable(void *unused) +{ + u32 access; + + BUG_ON(preemptible()); + access = get_copro_access(); + + /* + * Enable full access to VFP (cp10 and cp11) + */ + set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11)); +} + +#ifdef CONFIG_PM +#include <linux/syscore_ops.h> + +static int vfp_pm_suspend(void) +{ + struct thread_info *ti = current_thread_info(); + u32 fpexc = fmrx(FPEXC); + + /* if vfp is on, then save state for resumption */ + if (fpexc & FPEXC_EN) { + printk(KERN_DEBUG "%s: saving vfp state\n", __func__); + vfp_save_state(&ti->vfpstate, fpexc); + + /* disable, just in case */ + fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN); + } else if (vfp_current_hw_state[ti->cpu]) { + fmxr(FPEXC, fpexc | FPEXC_EN); + vfp_save_state(vfp_current_hw_state[ti->cpu], fpexc); + fmxr(FPEXC, fpexc); + } + + /* clear any information we had about last context state */ + memset(vfp_current_hw_state, 0, sizeof(vfp_current_hw_state)); + + return 0; +} + +static void vfp_pm_resume(void) +{ + /* ensure we have access to the vfp */ + vfp_enable(NULL); + + /* and disable it to ensure the next usage restores the state */ + fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN); +} + +static struct syscore_ops vfp_pm_syscore_ops = { + .suspend = vfp_pm_suspend, + .resume = vfp_pm_resume, +}; + +static void vfp_pm_init(void) +{ + register_syscore_ops(&vfp_pm_syscore_ops); +} + +#else +static inline void vfp_pm_init(void) { } +#endif /* CONFIG_PM */ + +void vfp_sync_hwstate(struct thread_info *thread) +{ + unsigned int cpu = get_cpu(); + + /* + * If the thread we're interested in is the current owner of the + * hardware VFP state, then we need to save its state. + */ + if (vfp_current_hw_state[cpu] == &thread->vfpstate) { + u32 fpexc = fmrx(FPEXC); + + /* + * Save the last VFP state on this CPU. + */ + fmxr(FPEXC, fpexc | FPEXC_EN); + vfp_save_state(&thread->vfpstate, fpexc | FPEXC_EN); + fmxr(FPEXC, fpexc); + } + + put_cpu(); +} + +void vfp_flush_hwstate(struct thread_info *thread) +{ + unsigned int cpu = get_cpu(); + + /* + * If the thread we're interested in is the current owner of the + * hardware VFP state, then we need to save its state. + */ + if (vfp_current_hw_state[cpu] == &thread->vfpstate) { + u32 fpexc = fmrx(FPEXC); + + fmxr(FPEXC, fpexc & ~FPEXC_EN); + + /* + * Set the context to NULL to force a reload the next time + * the thread uses the VFP. + */ + vfp_current_hw_state[cpu] = NULL; + } + +#ifdef CONFIG_SMP + /* + * For SMP we still have to take care of the case where the thread + * migrates to another CPU and then back to the original CPU on which + * the last VFP user is still the same thread. Mark the thread VFP + * state as belonging to a non-existent CPU so that the saved one will + * be reloaded in the above case. + */ + thread->vfpstate.hard.cpu = NR_CPUS; +#endif + put_cpu(); +} + +/* + * VFP hardware can lose all context when a CPU goes offline. + * As we will be running in SMP mode with CPU hotplug, we will save the + * hardware state at every thread switch. We clear our held state when + * a CPU has been killed, indicating that the VFP hardware doesn't contain + * a threads VFP state. When a CPU starts up, we re-enable access to the + * VFP hardware. + * + * Both CPU_DYING and CPU_STARTING are called on the CPU which + * is being offlined/onlined. + */ +static int vfp_hotplug(struct notifier_block *b, unsigned long action, + void *hcpu) +{ + if (action == CPU_DYING || action == CPU_DYING_FROZEN) { + unsigned int cpu = (long)hcpu; + vfp_current_hw_state[cpu] = NULL; + } else if (action == CPU_STARTING || action == CPU_STARTING_FROZEN) + vfp_enable(NULL); + return NOTIFY_OK; +} + +/* + * VFP support code initialisation. + */ +static int __init vfp_init(void) +{ + unsigned int vfpsid; + unsigned int cpu_arch = cpu_architecture(); + + if (cpu_arch >= CPU_ARCH_ARMv6) + on_each_cpu(vfp_enable, NULL, 1); + + /* + * 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. + */ + vfp_vector = vfp_testing_entry; + barrier(); + vfpsid = fmrx(FPSID); + barrier(); + vfp_vector = vfp_null_entry; + + 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 { + hotcpu_notifier(vfp_hotplug, 0); + + 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; + + thread_register_notifier(&vfp_notifier_block); + vfp_pm_init(); + + /* + * We detected VFP, and the support code is + * in place; report VFP support to userspace. + */ + elf_hwcap |= HWCAP_VFP; +#ifdef CONFIG_VFPv3 + if (VFP_arch >= 2) { + elf_hwcap |= HWCAP_VFPv3; + + /* + * Check for VFPv3 D16. CPUs in this configuration + * only have 16 x 64bit registers. + */ + if (((fmrx(MVFR0) & MVFR0_A_SIMD_MASK)) == 1) + elf_hwcap |= HWCAP_VFPv3D16; + } +#endif +#ifdef CONFIG_NEON + /* + * Check for the presence of the Advanced SIMD + * load/store instructions, integer and single + * precision floating point operations. Only check + * for NEON if the hardware has the MVFR registers. + */ + if ((read_cpuid_id() & 0x000f0000) == 0x000f0000) { + if ((fmrx(MVFR1) & 0x000fff00) == 0x00011100) + elf_hwcap |= HWCAP_NEON; + } +#endif + } + 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 00000000..b252631b --- /dev/null +++ b/arch/arm/vfp/vfpsingle.c @@ -0,0 +1,1244 @@ +/* + * 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/div64.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); +#ifdef DEBUG + pr_debug("VFP: %s: d(s%d)=%08x exceptions=%08x\n", func, + sd, d, exceptions); +#endif + vfp_put_float(d, sd); + } + + return exceptions; +} + +/* + * 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(vfp_single_packed_abs(m), sd); + return 0; +} + +static u32 vfp_single_fcpy(int sd, int unused, s32 m, u32 fpscr) +{ + vfp_put_float(m, sd); + return 0; +} + +static u32 vfp_single_fneg(int sd, int unused, s32 m, u32 fpscr) +{ + vfp_put_float(vfp_single_packed_negate(m), sd); + 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; + } + { + u64 v = (u64)a << 31; + do_div(v, z); + return v + (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(vfp_single_pack(vsp), sd); + 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_QNAN) + vdd.significand |= VFP_DOUBLE_SIGNIFICAND_QNAN; + goto pack_nan; + } else if (tm & VFP_ZERO) + vdd.exponent = 0; + else + vdd.exponent = vsm.exponent + (1023 - 127); + + return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fcvtd"); + + pack_nan: + vfp_put_double(vfp_double_pack(&vdd), dd); + 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(d, sd); + + 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; + int tm; + + vfp_single_unpack(&vsm, m); + vfp_single_dump("VSM", &vsm); + + /* + * Do we have a denormalised number? + */ + tm = vfp_single_type(&vsm); + if (vfp_single_type(&vsm) & VFP_DENORMAL) + exceptions |= FPSCR_IDC; + + if (tm & VFP_NAN) { + d = 0; + exceptions |= FPSCR_IOC; + } else 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((s32)d, sd); + + 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 struct op fops_ext[32] = { + [FEXT_TO_IDX(FEXT_FCPY)] = { vfp_single_fcpy, 0 }, + [FEXT_TO_IDX(FEXT_FABS)] = { vfp_single_fabs, 0 }, + [FEXT_TO_IDX(FEXT_FNEG)] = { vfp_single_fneg, 0 }, + [FEXT_TO_IDX(FEXT_FSQRT)] = { vfp_single_fsqrt, 0 }, + [FEXT_TO_IDX(FEXT_FCMP)] = { vfp_single_fcmp, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FCMPE)] = { vfp_single_fcmpe, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FCMPZ)] = { vfp_single_fcmpz, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FCMPEZ)] = { vfp_single_fcmpez, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FCVT)] = { vfp_single_fcvtd, OP_SCALAR|OP_DD }, + [FEXT_TO_IDX(FEXT_FUITO)] = { vfp_single_fuito, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FSITO)] = { vfp_single_fsito, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FTOUI)] = { vfp_single_ftoui, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FTOUIZ)] = { vfp_single_ftouiz, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FTOSI)] = { vfp_single_ftosi, OP_SCALAR }, + [FEXT_TO_IDX(FEXT_FTOSIZ)] = { vfp_single_ftosiz, OP_SCALAR }, +}; + + + + + +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++; + } + { + u64 significand = (u64)vsn.significand << 32; + do_div(significand, vsm.significand); + vsd.significand = 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(vfp_single_pack(&vsd), sd); + 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(vfp_single_pack(&vfp_single_default_qnan), sd); + return FPSCR_IOC; +} + +static struct op fops[16] = { + [FOP_TO_IDX(FOP_FMAC)] = { vfp_single_fmac, 0 }, + [FOP_TO_IDX(FOP_FNMAC)] = { vfp_single_fnmac, 0 }, + [FOP_TO_IDX(FOP_FMSC)] = { vfp_single_fmsc, 0 }, + [FOP_TO_IDX(FOP_FNMSC)] = { vfp_single_fnmsc, 0 }, + [FOP_TO_IDX(FOP_FMUL)] = { vfp_single_fmul, 0 }, + [FOP_TO_IDX(FOP_FNMUL)] = { vfp_single_fnmul, 0 }, + [FOP_TO_IDX(FOP_FADD)] = { vfp_single_fadd, 0 }, + [FOP_TO_IDX(FOP_FSUB)] = { vfp_single_fsub, 0 }, + [FOP_TO_IDX(FOP_FDIV)] = { vfp_single_fdiv, 0 }, +}; + +#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 dest; + unsigned int sn = vfp_get_sn(inst); + unsigned int sm = vfp_get_sm(inst); + unsigned int vecitr, veclen, vecstride; + struct op *fop; + + vecstride = 1 + ((fpscr & FPSCR_STRIDE_MASK) == FPSCR_STRIDE_MASK); + + fop = (op == FOP_EXT) ? &fops_ext[FEXT_TO_IDX(inst)] : &fops[FOP_TO_IDX(op)]; + + /* + * fcvtsd takes a dN register number as destination, not sN. + * Technically, if bit 0 of dd is set, this is an invalid + * instruction. However, we ignore this for efficiency. + * It also only operates on scalars. + */ + if (fop->flags & OP_DD) + dest = vfp_get_dd(inst); + else + dest = vfp_get_sd(inst); + + /* + * If destination bank is zero, vector length is always '1'. + * ARM DDI0100F C5.1.3, C5.3.2. + */ + if ((fop->flags & OP_SCALAR) || FREG_BANK(dest) == 0) + veclen = 0; + else + veclen = fpscr & FPSCR_LENGTH_MASK; + + pr_debug("VFP: vecstride=%u veclen=%u\n", vecstride, + (veclen >> FPSCR_LENGTH_BIT) + 1); + + if (!fop->fn) + goto invalid; + + for (vecitr = 0; vecitr <= veclen; vecitr += 1 << FPSCR_LENGTH_BIT) { + s32 m = vfp_get_float(sm); + u32 except; + char type; + + type = fop->flags & OP_DD ? 'd' : 's'; + if (op == FOP_EXT) + pr_debug("VFP: itr%d (%c%u) = op[%u] (s%u=%08x)\n", + vecitr >> FPSCR_LENGTH_BIT, type, dest, sn, + sm, m); + else + pr_debug("VFP: itr%d (%c%u) = (s%u) op[%u] (s%u=%08x)\n", + vecitr >> FPSCR_LENGTH_BIT, type, dest, sn, + FOP_TO_IDX(op), sm, m); + + except = fop->fn(dest, sn, m, fpscr); + pr_debug("VFP: itr%d: exceptions=%08x\n", + vecitr >> FPSCR_LENGTH_BIT, except); + + exceptions |= except; + + /* + * CHECK: It appears to be undefined whether we stop when + * we encounter an exception. We continue. + */ + dest = FREG_BANK(dest) + ((FREG_IDX(dest) + 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; +} |