summaryrefslogtreecommitdiffstats
path: root/kernel/arch/arm/vfp
diff options
context:
space:
mode:
authorYunhong Jiang <yunhong.jiang@intel.com>2015-08-04 12:17:53 -0700
committerYunhong Jiang <yunhong.jiang@intel.com>2015-08-04 15:44:42 -0700
commit9ca8dbcc65cfc63d6f5ef3312a33184e1d726e00 (patch)
tree1c9cafbcd35f783a87880a10f85d1a060db1a563 /kernel/arch/arm/vfp
parent98260f3884f4a202f9ca5eabed40b1354c489b29 (diff)
Add the rt linux 4.1.3-rt3 as base
Import the rt linux 4.1.3-rt3 as OPNFV kvm base. It's from git://git.kernel.org/pub/scm/linux/kernel/git/rt/linux-rt-devel.git linux-4.1.y-rt and the base is: commit 0917f823c59692d751951bf5ea699a2d1e2f26a2 Author: Sebastian Andrzej Siewior <bigeasy@linutronix.de> Date: Sat Jul 25 12:13:34 2015 +0200 Prepare v4.1.3-rt3 Signed-off-by: Sebastian Andrzej Siewior <bigeasy@linutronix.de> We lose all the git history this way and it's not good. We should apply another opnfv project repo in future. Change-Id: I87543d81c9df70d99c5001fbdf646b202c19f423 Signed-off-by: Yunhong Jiang <yunhong.jiang@intel.com>
Diffstat (limited to 'kernel/arch/arm/vfp')
-rw-r--r--kernel/arch/arm/vfp/Makefile15
-rw-r--r--kernel/arch/arm/vfp/entry.S59
-rw-r--r--kernel/arch/arm/vfp/vfp.h380
-rw-r--r--kernel/arch/arm/vfp/vfpdouble.c1206
-rw-r--r--kernel/arch/arm/vfp/vfphw.S323
-rw-r--r--kernel/arch/arm/vfp/vfpinstr.h88
-rw-r--r--kernel/arch/arm/vfp/vfpmodule.c811
-rw-r--r--kernel/arch/arm/vfp/vfpsingle.c1246
8 files changed, 4128 insertions, 0 deletions
diff --git a/kernel/arch/arm/vfp/Makefile b/kernel/arch/arm/vfp/Makefile
new file mode 100644
index 000000000..a81404c09
--- /dev/null
+++ b/kernel/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 -mfloat-abi=soft)
+LDFLAGS +=--no-warn-mismatch
+
+obj-y += vfp.o
+
+vfp-$(CONFIG_VFP) += vfpmodule.o entry.o vfphw.o vfpsingle.o vfpdouble.o
diff --git a/kernel/arch/arm/vfp/entry.S b/kernel/arch/arm/vfp/entry.S
new file mode 100644
index 000000000..2e78760f3
--- /dev/null
+++ b/kernel/arch/arm/vfp/entry.S
@@ -0,0 +1,59 @@
+/*
+ * 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.
+ */
+#include <linux/init.h>
+#include <linux/linkage.h>
+#include <asm/thread_info.h>
+#include <asm/vfpmacros.h>
+#include <asm/assembler.h>
+#include <asm/asm-offsets.h>
+
+@ VFP entry point.
+@
+@ r0 = instruction opcode (32-bit ARM or two 16-bit Thumb)
+@ r2 = PC value to resume execution after successful emulation
+@ r9 = normal "successful" return address
+@ r10 = this threads thread_info structure
+@ lr = unrecognised instruction return address
+@ IRQs enabled.
+@
+ENTRY(do_vfp)
+ inc_preempt_count r10, r4
+ 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)
+ dec_preempt_count_ti r10, r4
+ ret 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)
+ dec_preempt_count_ti r10, r4
+ ldr r0, VFP_arch_address
+ str r0, [r0] @ set to non-zero value
+ ret r9 @ we have handled the fault
+ENDPROC(vfp_testing_entry)
+
+ .align 2
+VFP_arch_address:
+ .word VFP_arch
+
+ __FINIT
diff --git a/kernel/arch/arm/vfp/vfp.h b/kernel/arch/arm/vfp/vfp.h
new file mode 100644
index 000000000..c8c98dd44
--- /dev/null
+++ b/kernel/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/kernel/arch/arm/vfp/vfpdouble.c b/kernel/arch/arm/vfp/vfpdouble.c
new file mode 100644
index 000000000..423f56dd4
--- /dev/null
+++ b/kernel/arch/arm/vfp/vfpdouble.c
@@ -0,0 +1,1206 @@
+/*
+ * 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 (vdn.exponent == 0 && vdn.significand)
+ vfp_double_normalise_denormal(&vdn);
+ 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/kernel/arch/arm/vfp/vfphw.S b/kernel/arch/arm/vfp/vfphw.S
new file mode 100644
index 000000000..f74a8f7e5
--- /dev/null
+++ b/kernel/arch/arm/vfp/vfphw.S
@@ -0,0 +1,323 @@
+/*
+ * 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 <linux/init.h>
+#include <linux/linkage.h>
+#include <asm/thread_info.h>
+#include <asm/vfpmacros.h>
+#include <linux/kern_levels.h>
+#include <asm/assembler.h>
+#include <asm/asm-offsets.h>
+
+ .macro DBGSTR, str
+#ifdef DEBUG
+ stmfd sp!, {r0-r3, ip, lr}
+ ldr r0, =1f
+ bl printk
+ ldmfd sp!, {r0-r3, ip, lr}
+
+ .pushsection .rodata, "a"
+1: .ascii KERN_DEBUG "VFP: \str\n"
+ .byte 0
+ .previous
+#endif
+ .endm
+
+ .macro DBGSTR1, str, arg
+#ifdef DEBUG
+ stmfd sp!, {r0-r3, ip, lr}
+ mov r1, \arg
+ ldr r0, =1f
+ bl printk
+ ldmfd sp!, {r0-r3, ip, lr}
+
+ .pushsection .rodata, "a"
+1: .ascii KERN_DEBUG "VFP: \str\n"
+ .byte 0
+ .previous
+#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
+ ldr r0, =1f
+ bl printk
+ ldmfd sp!, {r0-r3, ip, lr}
+
+ .pushsection .rodata, "a"
+1: .ascii KERN_DEBUG "VFP: \str\n"
+ .byte 0
+ .previous
+#endif
+ .endm
+
+
+@ VFP hardware support entry point.
+@
+@ r0 = instruction opcode (32-bit ARM or two 16-bit Thumb)
+@ r2 = PC value to resume execution after successful emulation
+@ r9 = normal "successful" return address
+@ r10 = vfp_state union
+@ r11 = CPU number
+@ lr = unrecognised instruction return address
+@ IRQs enabled.
+ENTRY(vfp_support_entry)
+ DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10
+
+ ldr r3, [sp, #S_PSR] @ Neither lazy restore nor FP exceptions
+ and r3, r3, #MODE_MASK @ are supported in kernel mode
+ teq r3, #USR_MODE
+ bne vfp_kmode_exception @ Returns through lr
+
+ 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?
+#ifndef CONFIG_SMP
+ @ For UP, checking that this thread owns the hw context is
+ @ sufficient to determine that the hardware state is valid.
+ beq vfp_hw_state_valid
+
+ @ On UP, we lazily save the VFP context. As a different
+ @ thread wants ownership of the VFP hardware, save the old
+ @ state if there was a previous (valid) owner.
+
+ VFPFMXR FPEXC, r5 @ enable VFP, disable any pending
+ @ exceptions, so we can get at the
+ @ rest of it
+
+ DBGSTR1 "save old state %p", r4
+ cmp r4, #0 @ if the vfp_current_hw_state is NULL
+ beq vfp_reload_hw @ then the hw state needs reloading
+ 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
+vfp_reload_hw:
+
+#else
+ @ For SMP, if this thread does not own the hw context, then we
+ @ need to reload it. No need to save the old state as on SMP,
+ @ we always save the state when we switch away from a thread.
+ bne vfp_reload_hw
+
+ @ This thread has ownership of the current hardware context.
+ @ However, it may have been migrated to another CPU, in which
+ @ case the saved state is newer than the hardware context.
+ @ Check this by looking at the CPU number which the state was
+ @ last loaded onto.
+ ldr ip, [r10, #VFP_CPU]
+ teq ip, r11
+ beq vfp_hw_state_valid
+
+vfp_reload_hw:
+ @ We're loading this threads state into the VFP hardware. Update
+ @ the CPU number which contains the most up to date VFP context.
+ str r11, [r10, #VFP_CPU]
+
+ VFPFMXR FPEXC, r5 @ enable VFP, disable any pending
+ @ exceptions, so we can get at the
+ @ rest of it
+#endif
+
+ 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 @ Retry current instruction - if Thumb
+ str r2, [sp, #S_PC] @ mode it's two 16-bit instructions,
+ @ else it's one 32-bit instruction, so
+ @ always subtract 4 from the following
+ @ instruction address.
+ dec_preempt_count_ti r10, r4
+ ret 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
+
+ tst r5, #FPSCR_LENGTH_MASK
+ beq skip
+ orr r1, r1, #FPEXC_DEX
+ b process_exception
+skip:
+
+ @ Fall into hand on to next handler - appropriate coproc instr
+ @ not recognised by VFP
+
+ DBGSTR "not VFP"
+ dec_preempt_count_ti r10, r4
+ ret 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
+ ret 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
+ ret \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
+ ret lr
+ .org 1b + 8
+1: mrc p10, 0, r0, c\dr, c0, 4 @ fmrs r0, s1
+ ret 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
+ ret lr
+ .org 1b + 8
+1: mcr p10, 0, r0, c\dr, c0, 4 @ fmsr r0, s1
+ ret 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
+ ret 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
+ ret lr
+ .org 1b + 8
+ .endr
+#endif
+
+ @ virtual register 16 (or 32 if VFPv3) for compare with zero
+ mov r0, #0
+ mov r1, #0
+ ret 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
+ ret 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
+ ret lr
+ .org 1b + 8
+ .endr
+#endif
+ENDPROC(vfp_put_double)
diff --git a/kernel/arch/arm/vfp/vfpinstr.h b/kernel/arch/arm/vfp/vfpinstr.h
new file mode 100644
index 000000000..15b95b5ab
--- /dev/null
+++ b/kernel/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/kernel/arch/arm/vfp/vfpmodule.c b/kernel/arch/arm/vfp/vfpmodule.c
new file mode 100644
index 000000000..f6e4d56ed
--- /dev/null
+++ b/kernel/arch/arm/vfp/vfpmodule.c
@@ -0,0 +1,811 @@
+/*
+ * 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/types.h>
+#include <linux/cpu.h>
+#include <linux/cpu_pm.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 <linux/uaccess.h>
+#include <linux/user.h>
+#include <linux/export.h>
+
+#include <asm/cp15.h>
+#include <asm/cputype.h>
+#include <asm/system_info.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;
+
+/*
+ * Dual-use variable.
+ * Used in startup: set to non-zero if VFP checks fail
+ * After startup, holds VFP architecture
+ */
+unsigned int VFP_arch;
+
+/*
+ * 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.
+ *
+ * For UP, this is sufficient to tell which thread owns the VFP context.
+ * However, for SMP, we also need to check the CPU number stored in the
+ * saved state too to catch migrations.
+ */
+union vfp_state *vfp_current_hw_state[NR_CPUS];
+
+/*
+ * Is 'thread's most up to date state stored in this CPUs hardware?
+ * Must be called from non-preemptible context.
+ */
+static bool vfp_state_in_hw(unsigned int cpu, struct thread_info *thread)
+{
+#ifdef CONFIG_SMP
+ if (thread->vfpstate.hard.cpu != cpu)
+ return false;
+#endif
+ return vfp_current_hw_state[cpu] == &thread->vfpstate;
+}
+
+/*
+ * Force a reload of the VFP context from the thread structure. We do
+ * this by ensuring that access to the VFP hardware is disabled, and
+ * clear vfp_current_hw_state. Must be called from non-preemptible context.
+ */
+static void vfp_force_reload(unsigned int cpu, struct thread_info *thread)
+{
+ if (vfp_state_in_hw(cpu, thread)) {
+ fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
+ vfp_current_hw_state[cpu] = NULL;
+ }
+#ifdef CONFIG_SMP
+ thread->vfpstate.hard.cpu = NR_CPUS;
+#endif
+}
+
+/*
+ * Per-thread VFP initialization.
+ */
+static void vfp_thread_flush(struct thread_info *thread)
+{
+ union vfp_state *vfp = &thread->vfpstate;
+ unsigned int cpu;
+
+ /*
+ * 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.
+ *
+ * Do this first to ensure that preemption won't overwrite our
+ * state saving should access to the VFP be enabled at this point.
+ */
+ cpu = get_cpu();
+ if (vfp_current_hw_state[cpu] == vfp)
+ vfp_current_hw_state[cpu] = NULL;
+ fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
+ put_cpu();
+
+ memset(vfp, 0, sizeof(union vfp_state));
+
+ vfp->hard.fpexc = FPEXC_EN;
+ vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
+#ifdef CONFIG_SMP
+ vfp->hard.cpu = NR_CPUS;
+#endif
+}
+
+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;
+#ifdef CONFIG_SMP
+ thread->vfpstate.hard.cpu = NR_CPUS;
+#endif
+}
+
+/*
+ * 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);
+#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;
+
+ pr_err("VFP: Error: %s\n", reason);
+ pr_err("VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n",
+ fmrx(FPEXC), fmrx(FPSCR), inst);
+ for (i = 0; i < 32; i += 2)
+ pr_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)) != (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_CPU_PM
+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) {
+ pr_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]) {
+#ifndef CONFIG_SMP
+ fmxr(FPEXC, fpexc | FPEXC_EN);
+ vfp_save_state(vfp_current_hw_state[ti->cpu], fpexc);
+ fmxr(FPEXC, fpexc);
+#endif
+ }
+
+ /* clear any information we had about last context state */
+ vfp_current_hw_state[ti->cpu] = NULL;
+
+ 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 int vfp_cpu_pm_notifier(struct notifier_block *self, unsigned long cmd,
+ void *v)
+{
+ switch (cmd) {
+ case CPU_PM_ENTER:
+ vfp_pm_suspend();
+ break;
+ case CPU_PM_ENTER_FAILED:
+ case CPU_PM_EXIT:
+ vfp_pm_resume();
+ break;
+ }
+ return NOTIFY_OK;
+}
+
+static struct notifier_block vfp_cpu_pm_notifier_block = {
+ .notifier_call = vfp_cpu_pm_notifier,
+};
+
+static void vfp_pm_init(void)
+{
+ cpu_pm_register_notifier(&vfp_cpu_pm_notifier_block);
+}
+
+#else
+static inline void vfp_pm_init(void) { }
+#endif /* CONFIG_CPU_PM */
+
+/*
+ * Ensure that the VFP state stored in 'thread->vfpstate' is up to date
+ * with the hardware state.
+ */
+void vfp_sync_hwstate(struct thread_info *thread)
+{
+ unsigned int cpu = get_cpu();
+
+ if (vfp_state_in_hw(cpu, thread)) {
+ 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();
+}
+
+/* Ensure that the thread reloads the hardware VFP state on the next use. */
+void vfp_flush_hwstate(struct thread_info *thread)
+{
+ unsigned int cpu = get_cpu();
+
+ vfp_force_reload(cpu, thread);
+
+ put_cpu();
+}
+
+/*
+ * Save the current VFP state into the provided structures and prepare
+ * for entry into a new function (signal handler).
+ */
+int vfp_preserve_user_clear_hwstate(struct user_vfp __user *ufp,
+ struct user_vfp_exc __user *ufp_exc)
+{
+ struct thread_info *thread = current_thread_info();
+ struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;
+ int err = 0;
+
+ /* Ensure that the saved hwstate is up-to-date. */
+ vfp_sync_hwstate(thread);
+
+ /*
+ * Copy the floating point registers. There can be unused
+ * registers see asm/hwcap.h for details.
+ */
+ err |= __copy_to_user(&ufp->fpregs, &hwstate->fpregs,
+ sizeof(hwstate->fpregs));
+ /*
+ * Copy the status and control register.
+ */
+ __put_user_error(hwstate->fpscr, &ufp->fpscr, err);
+
+ /*
+ * Copy the exception registers.
+ */
+ __put_user_error(hwstate->fpexc, &ufp_exc->fpexc, err);
+ __put_user_error(hwstate->fpinst, &ufp_exc->fpinst, err);
+ __put_user_error(hwstate->fpinst2, &ufp_exc->fpinst2, err);
+
+ if (err)
+ return -EFAULT;
+
+ /* Ensure that VFP is disabled. */
+ vfp_flush_hwstate(thread);
+
+ /*
+ * As per the PCS, clear the length and stride bits for function
+ * entry.
+ */
+ hwstate->fpscr &= ~(FPSCR_LENGTH_MASK | FPSCR_STRIDE_MASK);
+ return 0;
+}
+
+/* Sanitise and restore the current VFP state from the provided structures. */
+int vfp_restore_user_hwstate(struct user_vfp __user *ufp,
+ struct user_vfp_exc __user *ufp_exc)
+{
+ struct thread_info *thread = current_thread_info();
+ struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;
+ unsigned long fpexc;
+ int err = 0;
+
+ /* Disable VFP to avoid corrupting the new thread state. */
+ vfp_flush_hwstate(thread);
+
+ /*
+ * Copy the floating point registers. There can be unused
+ * registers see asm/hwcap.h for details.
+ */
+ err |= __copy_from_user(&hwstate->fpregs, &ufp->fpregs,
+ sizeof(hwstate->fpregs));
+ /*
+ * Copy the status and control register.
+ */
+ __get_user_error(hwstate->fpscr, &ufp->fpscr, err);
+
+ /*
+ * Sanitise and restore the exception registers.
+ */
+ __get_user_error(fpexc, &ufp_exc->fpexc, err);
+
+ /* Ensure the VFP is enabled. */
+ fpexc |= FPEXC_EN;
+
+ /* Ensure FPINST2 is invalid and the exception flag is cleared. */
+ fpexc &= ~(FPEXC_EX | FPEXC_FP2V);
+ hwstate->fpexc = fpexc;
+
+ __get_user_error(hwstate->fpinst, &ufp_exc->fpinst, err);
+ __get_user_error(hwstate->fpinst2, &ufp_exc->fpinst2, err);
+
+ return err ? -EFAULT : 0;
+}
+
+/*
+ * 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)
+ vfp_current_hw_state[(long)hcpu] = NULL;
+ else if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
+ vfp_enable(NULL);
+ return NOTIFY_OK;
+}
+
+void vfp_kmode_exception(void)
+{
+ /*
+ * If we reach this point, a floating point exception has been raised
+ * while running in kernel mode. If the NEON/VFP unit was enabled at the
+ * time, it means a VFP instruction has been issued that requires
+ * software assistance to complete, something which is not currently
+ * supported in kernel mode.
+ * If the NEON/VFP unit was disabled, and the location pointed to below
+ * is properly preceded by a call to kernel_neon_begin(), something has
+ * caused the task to be scheduled out and back in again. In this case,
+ * rebuilding and running with CONFIG_DEBUG_ATOMIC_SLEEP enabled should
+ * be helpful in localizing the problem.
+ */
+ if (fmrx(FPEXC) & FPEXC_EN)
+ pr_crit("BUG: unsupported FP instruction in kernel mode\n");
+ else
+ pr_crit("BUG: FP instruction issued in kernel mode with FP unit disabled\n");
+}
+
+#ifdef CONFIG_KERNEL_MODE_NEON
+
+/*
+ * Kernel-side NEON support functions
+ */
+void kernel_neon_begin(void)
+{
+ struct thread_info *thread = current_thread_info();
+ unsigned int cpu;
+ u32 fpexc;
+
+ /*
+ * Kernel mode NEON is only allowed outside of interrupt context
+ * with preemption disabled. This will make sure that the kernel
+ * mode NEON register contents never need to be preserved.
+ */
+ BUG_ON(in_interrupt());
+ cpu = get_cpu();
+
+ fpexc = fmrx(FPEXC) | FPEXC_EN;
+ fmxr(FPEXC, fpexc);
+
+ /*
+ * Save the userland NEON/VFP state. Under UP,
+ * the owner could be a task other than 'current'
+ */
+ if (vfp_state_in_hw(cpu, thread))
+ vfp_save_state(&thread->vfpstate, fpexc);
+#ifndef CONFIG_SMP
+ else if (vfp_current_hw_state[cpu] != NULL)
+ vfp_save_state(vfp_current_hw_state[cpu], fpexc);
+#endif
+ vfp_current_hw_state[cpu] = NULL;
+}
+EXPORT_SYMBOL(kernel_neon_begin);
+
+void kernel_neon_end(void)
+{
+ /* Disable the NEON/VFP unit. */
+ fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
+ put_cpu();
+}
+EXPORT_SYMBOL(kernel_neon_end);
+
+#endif /* CONFIG_KERNEL_MODE_NEON */
+
+/*
+ * 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;
+
+ pr_info("VFP support v0.3: ");
+ if (VFP_arch) {
+ pr_cont("not present\n");
+ return 0;
+ /* Extract the architecture on CPUID scheme */
+ } else if ((read_cpuid_id() & 0x000f0000) == 0x000f0000) {
+ VFP_arch = vfpsid & FPSID_CPUID_ARCH_MASK;
+ VFP_arch >>= FPSID_ARCH_BIT;
+ /*
+ * 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 (IS_ENABLED(CONFIG_NEON) &&
+ (fmrx(MVFR1) & 0x000fff00) == 0x00011100)
+ elf_hwcap |= HWCAP_NEON;
+
+ if (IS_ENABLED(CONFIG_VFPv3)) {
+ u32 mvfr0 = fmrx(MVFR0);
+ if (((mvfr0 & MVFR0_DP_MASK) >> MVFR0_DP_BIT) == 0x2 ||
+ ((mvfr0 & MVFR0_SP_MASK) >> MVFR0_SP_BIT) == 0x2) {
+ elf_hwcap |= HWCAP_VFPv3;
+ /*
+ * Check for VFPv3 D16 and VFPv4 D16. CPUs in
+ * this configuration only have 16 x 64bit
+ * registers.
+ */
+ if ((mvfr0 & MVFR0_A_SIMD_MASK) == 1)
+ /* also v4-D16 */
+ elf_hwcap |= HWCAP_VFPv3D16;
+ else
+ elf_hwcap |= HWCAP_VFPD32;
+ }
+
+ if ((fmrx(MVFR1) & 0xf0000000) == 0x10000000)
+ elf_hwcap |= HWCAP_VFPv4;
+ }
+ /* Extract the architecture version on pre-cpuid scheme */
+ } else {
+ if (vfpsid & FPSID_NODOUBLE) {
+ pr_cont("no double precision support\n");
+ return 0;
+ }
+
+ VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;
+ }
+
+ hotcpu_notifier(vfp_hotplug, 0);
+
+ 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;
+
+ pr_cont("implementor %02x architecture %d part %02x variant %x rev %x\n",
+ (vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
+ VFP_arch,
+ (vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
+ (vfpsid & FPSID_VARIANT_MASK) >> FPSID_VARIANT_BIT,
+ (vfpsid & FPSID_REV_MASK) >> FPSID_REV_BIT);
+
+ return 0;
+}
+
+core_initcall(vfp_init);
diff --git a/kernel/arch/arm/vfp/vfpsingle.c b/kernel/arch/arm/vfp/vfpsingle.c
new file mode 100644
index 000000000..f0465ba0f
--- /dev/null
+++ b/kernel/arch/arm/vfp/vfpsingle.c
@@ -0,0 +1,1246 @@
+/*
+ * 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) {
+ pr_warn("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 (vsn.exponent == 0 && vsn.significand)
+ vfp_single_normalise_denormal(&vsn);
+ 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;
+}