vfpdouble.c

LINUX 2.6.17.4的源码

	 */
	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(sd, (s32)d);

	return exceptions;
}

static u32 vfp_double_ftosiz(int dd, int unused, int dm, u32 fpscr)
{
	return vfp_double_ftosi(dd, unused, dm, FPSCR_ROUND_TOZERO);
}


static u32 (* const fop_extfns[32])(int dd, int unused, int dm, u32 fpscr) = {
	[FEXT_TO_IDX(FEXT_FCPY)]	= vfp_double_fcpy,
	[FEXT_TO_IDX(FEXT_FABS)]	= vfp_double_fabs,
	[FEXT_TO_IDX(FEXT_FNEG)]	= vfp_double_fneg,
	[FEXT_TO_IDX(FEXT_FSQRT)]	= vfp_double_fsqrt,
	[FEXT_TO_IDX(FEXT_FCMP)]	= vfp_double_fcmp,
	[FEXT_TO_IDX(FEXT_FCMPE)]	= vfp_double_fcmpe,
	[FEXT_TO_IDX(FEXT_FCMPZ)]	= vfp_double_fcmpz,
	[FEXT_TO_IDX(FEXT_FCMPEZ)]	= vfp_double_fcmpez,
	[FEXT_TO_IDX(FEXT_FCVT)]	= vfp_double_fcvts,
	[FEXT_TO_IDX(FEXT_FUITO)]	= vfp_double_fuito,
	[FEXT_TO_IDX(FEXT_FSITO)]	= vfp_double_fsito,
	[FEXT_TO_IDX(FEXT_FTOUI)]	= vfp_double_ftoui,
	[FEXT_TO_IDX(FEXT_FTOUIZ)]	= vfp_double_ftouiz,
	[FEXT_TO_IDX(FEXT_FTOSI)]	= vfp_double_ftosi,
	[FEXT_TO_IDX(FEXT_FTOSIZ)]	= vfp_double_ftosiz,
};




static u32
vfp_double_fadd_nonnumber(struct vfp_double *vdd, struct vfp_double *vdn,
			  struct vfp_double *vdm, u32 fpscr)
{
	struct vfp_double *vdp;
	u32 exceptions = 0;
	int tn, tm;

	tn = vfp_double_type(vdn);
	tm = vfp_double_type(vdm);

	if (tn & tm & VFP_INFINITY) {
		/*
		 * Two infinities.  Are they different signs?
		 */
		if (vdn->sign ^ vdm->sign) {
			/*
			 * different signs -> invalid
			 */
			exceptions = FPSCR_IOC;
			vdp = &vfp_double_default_qnan;
		} else {
			/*
			 * same signs -> valid
			 */
			vdp = vdn;
		}
	} else if (tn & VFP_INFINITY && tm & VFP_NUMBER) {
		/*
		 * One infinity and one number -> infinity
		 */
		vdp = vdn;
	} else {
		/*
		 * 'n' is a NaN of some type
		 */
		return vfp_propagate_nan(vdd, vdn, vdm, fpscr);
	}
	*vdd = *vdp;
	return exceptions;
}

static u32
vfp_double_add(struct vfp_double *vdd, struct vfp_double *vdn,
	       struct vfp_double *vdm, u32 fpscr)
{
	u32 exp_diff;
	u64 m_sig;

	if (vdn->significand & (1ULL << 63) ||
	    vdm->significand & (1ULL << 63)) {
		pr_info("VFP: bad FP values in %s\n", __func__);
		vfp_double_dump("VDN", vdn);
		vfp_double_dump("VDM", vdm);
	}

	/*
	 * Ensure that 'n' is the largest magnitude number.  Note that
	 * if 'n' and 'm' have equal exponents, we do not swap them.
	 * This ensures that NaN propagation works correctly.
	 */
	if (vdn->exponent < vdm->exponent) {
		struct vfp_double *t = vdn;
		vdn = vdm;
		vdm = t;
	}

	/*
	 * Is 'n' an infinity or a NaN?  Note that 'm' may be a number,
	 * infinity or a NaN here.
	 */
	if (vdn->exponent == 2047)
		return vfp_double_fadd_nonnumber(vdd, vdn, vdm, fpscr);

	/*
	 * We have two proper numbers, where 'vdn' is the larger magnitude.
	 *
	 * Copy 'n' to 'd' before doing the arithmetic.
	 */
	*vdd = *vdn;

	/*
	 * Align 'm' with the result.
	 */
	exp_diff = vdn->exponent - vdm->exponent;
	m_sig = vfp_shiftright64jamming(vdm->significand, exp_diff);

	/*
	 * If the signs are different, we are really subtracting.
	 */
	if (vdn->sign ^ vdm->sign) {
		m_sig = vdn->significand - m_sig;
		if ((s64)m_sig < 0) {
			vdd->sign = vfp_sign_negate(vdd->sign);
			m_sig = -m_sig;
		} else if (m_sig == 0) {
			vdd->sign = (fpscr & FPSCR_RMODE_MASK) ==
				      FPSCR_ROUND_MINUSINF ? 0x8000 : 0;
		}
	} else {
		m_sig += vdn->significand;
	}
	vdd->significand = m_sig;

	return 0;
}

static u32
vfp_double_multiply(struct vfp_double *vdd, struct vfp_double *vdn,
		    struct vfp_double *vdm, u32 fpscr)
{
	vfp_double_dump("VDN", vdn);
	vfp_double_dump("VDM", vdm);

	/*
	 * Ensure that 'n' is the largest magnitude number.  Note that
	 * if 'n' and 'm' have equal exponents, we do not swap them.
	 * This ensures that NaN propagation works correctly.
	 */
	if (vdn->exponent < vdm->exponent) {
		struct vfp_double *t = vdn;
		vdn = vdm;
		vdm = t;
		pr_debug("VFP: swapping M <-> N\n");
	}

	vdd->sign = vdn->sign ^ vdm->sign;

	/*
	 * If 'n' is an infinity or NaN, handle it.  'm' may be anything.
	 */
	if (vdn->exponent == 2047) {
		if (vdn->significand || (vdm->exponent == 2047 && vdm->significand))
			return vfp_propagate_nan(vdd, vdn, vdm, fpscr);
		if ((vdm->exponent | vdm->significand) == 0) {
			*vdd = vfp_double_default_qnan;
			return FPSCR_IOC;
		}
		vdd->exponent = vdn->exponent;
		vdd->significand = 0;
		return 0;
	}

	/*
	 * If 'm' is zero, the result is always zero.  In this case,
	 * 'n' may be zero or a number, but it doesn't matter which.
	 */
	if ((vdm->exponent | vdm->significand) == 0) {
		vdd->exponent = 0;
		vdd->significand = 0;
		return 0;
	}

	/*
	 * We add 2 to the destination exponent for the same reason
	 * as the addition case - though this time we have +1 from
	 * each input operand.
	 */
	vdd->exponent = vdn->exponent + vdm->exponent - 1023 + 2;
	vdd->significand = vfp_hi64multiply64(vdn->significand, vdm->significand);

	vfp_double_dump("VDD", vdd);
	return 0;
}

#define NEG_MULTIPLY	(1 << 0)
#define NEG_SUBTRACT	(1 << 1)

static u32
vfp_double_multiply_accumulate(int dd, int dn, int dm, u32 fpscr, u32 negate, char *func)
{
	struct vfp_double vdd, vdp, vdn, vdm;
	u32 exceptions;

	vfp_double_unpack(&vdn, vfp_get_double(dn));
	if (vdn.exponent == 0 && vdn.significand)
		vfp_double_normalise_denormal(&vdn);

	vfp_double_unpack(&vdm, vfp_get_double(dm));
	if (vdm.exponent == 0 && vdm.significand)
		vfp_double_normalise_denormal(&vdm);

	exceptions = vfp_double_multiply(&vdp, &vdn, &vdm, fpscr);
	if (negate & NEG_MULTIPLY)
		vdp.sign = vfp_sign_negate(vdp.sign);

	vfp_double_unpack(&vdn, vfp_get_double(dd));
	if (negate & NEG_SUBTRACT)
		vdn.sign = vfp_sign_negate(vdn.sign);

	exceptions |= vfp_double_add(&vdd, &vdn, &vdp, fpscr);

	return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, func);
}

/*
 * Standard operations
 */

/*
 * sd = sd + (sn * sm)
 */
static u32 vfp_double_fmac(int dd, int dn, int dm, u32 fpscr)
{
	return vfp_double_multiply_accumulate(dd, dn, dm, fpscr, 0, "fmac");
}

/*
 * sd = sd - (sn * sm)
 */
static u32 vfp_double_fnmac(int dd, int dn, int dm, u32 fpscr)
{
	return vfp_double_multiply_accumulate(dd, dn, dm, fpscr, NEG_MULTIPLY, "fnmac");
}

/*
 * sd = -sd + (sn * sm)
 */
static u32 vfp_double_fmsc(int dd, int dn, int dm, u32 fpscr)
{
	return vfp_double_multiply_accumulate(dd, dn, dm, fpscr, NEG_SUBTRACT, "fmsc");
}

/*
 * sd = -sd - (sn * sm)
 */
static u32 vfp_double_fnmsc(int dd, int dn, int dm, u32 fpscr)
{
	return vfp_double_multiply_accumulate(dd, dn, dm, fpscr, NEG_SUBTRACT | NEG_MULTIPLY, "fnmsc");
}

/*
 * sd = sn * sm
 */
static u32 vfp_double_fmul(int dd, int dn, int dm, u32 fpscr)
{
	struct vfp_double vdd, vdn, vdm;
	u32 exceptions;

	vfp_double_unpack(&vdn, vfp_get_double(dn));
	if (vdn.exponent == 0 && vdn.significand)
		vfp_double_normalise_denormal(&vdn);

	vfp_double_unpack(&vdm, vfp_get_double(dm));
	if (vdm.exponent == 0 && vdm.significand)
		vfp_double_normalise_denormal(&vdm);

	exceptions = vfp_double_multiply(&vdd, &vdn, &vdm, fpscr);
	return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fmul");
}

/*
 * sd = -(sn * sm)
 */
static u32 vfp_double_fnmul(int dd, int dn, int dm, u32 fpscr)
{
	struct vfp_double vdd, vdn, vdm;
	u32 exceptions;

	vfp_double_unpack(&vdn, vfp_get_double(dn));
	if (vdn.exponent == 0 && vdn.significand)
		vfp_double_normalise_denormal(&vdn);

	vfp_double_unpack(&vdm, vfp_get_double(dm));
	if (vdm.exponent == 0 && vdm.significand)
		vfp_double_normalise_denormal(&vdm);

	exceptions = vfp_double_multiply(&vdd, &vdn, &vdm, fpscr);
	vdd.sign = vfp_sign_negate(vdd.sign);

	return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fnmul");
}

/*
 * sd = sn + sm
 */
static u32 vfp_double_fadd(int dd, int dn, int dm, u32 fpscr)
{
	struct vfp_double vdd, vdn, vdm;
	u32 exceptions;

	vfp_double_unpack(&vdn, vfp_get_double(dn));
	if (vdn.exponent == 0 && vdn.significand)
		vfp_double_normalise_denormal(&vdn);

	vfp_double_unpack(&vdm, vfp_get_double(dm));
	if (vdm.exponent == 0 && vdm.significand)
		vfp_double_normalise_denormal(&vdm);

	exceptions = vfp_double_add(&vdd, &vdn, &vdm, fpscr);

	return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fadd");
}

/*
 * sd = sn - sm
 */
static u32 vfp_double_fsub(int dd, int dn, int dm, u32 fpscr)
{
	struct vfp_double vdd, vdn, vdm;
	u32 exceptions;

	vfp_double_unpack(&vdn, vfp_get_double(dn));
	if (vdn.exponent == 0 && vdn.significand)
		vfp_double_normalise_denormal(&vdn);

	vfp_double_unpack(&vdm, vfp_get_double(dm));
	if (vdm.exponent == 0 && vdm.significand)
		vfp_double_normalise_denormal(&vdm);

	/*
	 * Subtraction is like addition, but with a negated operand.
	 */
	vdm.sign = vfp_sign_negate(vdm.sign);

	exceptions = vfp_double_add(&vdd, &vdn, &vdm, fpscr);

	return vfp_double_normaliseround(dd, &vdd, fpscr, exceptions, "fsub");
}

/*
 * sd = sn / sm
 */
static u32 vfp_double_fdiv(int dd, int dn, int dm, u32 fpscr)
{
	struct vfp_double vdd, vdn, vdm;
	u32 exceptions = 0;
	int tm, tn;

	vfp_double_unpack(&vdn, vfp_get_double(dn));
	vfp_double_unpack(&vdm, vfp_get_double(dm));

	vdd.sign = vdn.sign ^ vdm.sign;

	tn = vfp_double_type(&vdn);
	tm = vfp_double_type(&vdm);

	/*
	 * Is n a NAN?
	 */
	if (tn & VFP_NAN)
		goto vdn_nan;

	/*
	 * Is m a NAN?
	 */
	if (tm & VFP_NAN)
		goto vdm_nan;

	/*
	 * If n and m are infinity, the result is invalid
	 * If n and m are zero, the result is invalid
	 */
	if (tm & tn & (VFP_INFINITY|VFP_ZERO))
		goto invalid;

	/*
	 * If n is infinity, the result is infinity
	 */
	if (tn & VFP_INFINITY)
		goto infinity;

	/*
	 * If m is zero, raise div0 exceptions
	 */
	if (tm & VFP_ZERO)
		goto divzero;

	/*
	 * If m is infinity, or n is zero, the result is zero
	 */
	if (tm & VFP_INFINITY || tn & VFP_ZERO)
		goto zero;

	if (tn & VFP_DENORMAL)
		vfp_double_normalise_denormal(&vdn);
	if (tm & VFP_DENORMAL)
		vfp_double_normalise_denormal(&vdm);

	/*
	 * Ok, we have two numbers, we can perform division.
	 */
	vdd.exponent = vdn.exponent - vdm.exponent + 1023 - 1;
	vdm.significand <<= 1;
	if (vdm.significand <= (2 * vdn.significand)) {
		vdn.significand >>= 1;
		vdd.exponent++;
	}
	vdd.significand = vfp_estimate_div128to64(vdn.significand, 0, vdm.significand);
	if ((vdd.significand & 0x1ff) <= 2) {
		u64 termh, terml, remh, reml;
		mul64to128(&termh, &terml, vdm.significand, vdd.significand);
		sub128(&remh, &reml, vdn.significand, 0, termh, terml);
		while ((s64)remh < 0) {
			vdd.significand -= 1;
			add128(&remh, &reml, remh, reml, 0, vdm.significand);
		}
		vdd.significand |= (reml != 0);
	}
	return vfp_double_normaliseround(dd, &vdd, fpscr, 0, "fdiv");

 vdn_nan:
	exceptions = vfp_propagate_nan(&vdd, &vdn, &vdm, fpscr);
 pack:
	vfp_put_double(dd, vfp_double_pack(&vdd));
	return exceptions;

 vdm_nan:
	exceptions = vfp_propagate_nan(&vdd, &vdm, &vdn, fpscr);
	goto pack;

 zero:
	vdd.exponent = 0;
	vdd.significand = 0;
	goto pack;

 divzero:
	exceptions = FPSCR_DZC;
 infinity:
	vdd.exponent = 2047;
	vdd.significand = 0;
	goto pack;

 invalid:
	vfp_put_double(dd, vfp_double_pack(&vfp_double_default_qnan));
	return FPSCR_IOC;
}

static u32 (* const fop_fns[16])(int dd, int dn, int dm, u32 fpscr) = {
	[FOP_TO_IDX(FOP_FMAC)]	= vfp_double_fmac,
	[FOP_TO_IDX(FOP_FNMAC)]	= vfp_double_fnmac,
	[FOP_TO_IDX(FOP_FMSC)]	= vfp_double_fmsc,
	[FOP_TO_IDX(FOP_FNMSC)]	= vfp_double_fnmsc,
	[FOP_TO_IDX(FOP_FMUL)]	= vfp_double_fmul,
	[FOP_TO_IDX(FOP_FNMUL)]	= vfp_double_fnmul,
	[FOP_TO_IDX(FOP_FADD)]	= vfp_double_fadd,
	[FOP_TO_IDX(FOP_FSUB)]	= vfp_double_fsub,
	[FOP_TO_IDX(FOP_FDIV)]	= vfp_double_fdiv,
};

#define FREG_BANK(x)	((x) & 0x0c)
#define FREG_IDX(x)	((x) & 3)

u32 vfp_double_cpdo(u32 inst, u32 fpscr)
{
	u32 op = inst & FOP_MASK;
	u32 exceptions = 0;
	unsigned int dd = vfp_get_dd(inst);
	unsigned int dn = vfp_get_dn(inst);
	unsigned int dm = vfp_get_dm(inst);
	unsigned int vecitr, veclen, vecstride;
	u32 (*fop)(int, int, s32, u32);

	veclen = fpscr & FPSCR_LENGTH_MASK;
	vecstride = (1 + ((fpscr & FPSCR_STRIDE_MASK) == FPSCR_STRIDE_MASK)) * 2;

	/*
	 * If destination bank is zero, vector length is always '1'.
	 * ARM DDI0100F C5.1.3, C5.3.2.
	 */
	if (FREG_BANK(dd) == 0)
		veclen = 0;

	pr_debug("VFP: vecstride=%u veclen=%u\n", vecstride,
		 (veclen >> FPSCR_LENGTH_BIT) + 1);

	fop = (op == FOP_EXT) ? fop_extfns[FEXT_TO_IDX(inst)] : fop_fns[FOP_TO_IDX(op)];
	if (!fop)
		goto invalid;

	for (vecitr = 0; vecitr <= veclen; vecitr += 1 << FPSCR_LENGTH_BIT) {
		u32 except;

		if (op == FOP_EXT)
			pr_debug("VFP: itr%d (d%u) = op[%u] (d%u)\n",
				 vecitr >> FPSCR_LENGTH_BIT,
				 dd, dn, dm);
		else
			pr_debug("VFP: itr%d (d%u) = (d%u) op[%u] (d%u)\n",
				 vecitr >> FPSCR_LENGTH_BIT,
				 dd, dn, FOP_TO_IDX(op), dm);

		except = fop(dd, dn, dm, fpscr);
		pr_debug("VFP: itr%d: exceptions=%08x\n",
			 vecitr >> FPSCR_LENGTH_BIT, except);

		exceptions |= except;

		/*
		 * This ensures that comparisons only operate on scalars;
		 * comparisons always return with one FPSCR status bit set.
		 */
		if (except & (FPSCR_N|FPSCR_Z|FPSCR_C|FPSCR_V))
			break;

		/*
		 * CHECK: It appears to be undefined whether we stop when
		 * we encounter an exception.  We continue.
		 */

		dd = FREG_BANK(dd) + ((FREG_IDX(dd) + vecstride) & 6);
		dn = FREG_BANK(dn) + ((FREG_IDX(dn) + vecstride) & 6);
		if (FREG_BANK(dm) != 0)
			dm = FREG_BANK(dm) + ((FREG_IDX(dm) + vecstride) & 6);
	}
	return exceptions;

 invalid:
	return ~0;
}