// SPDX-License-Identifier: GPL-2.0-only
/*
 * IEEE754 floating point arithmetic
 * double precision: MADDF.f (Fused Multiply Add)
 * MADDF.fmt: FPR[fd] = FPR[fd] + (FPR[fs] x FPR[ft])
 *
 * MIPS floating point support
 * Copyright (C) 2015 Imagination Technologies, Ltd.
 * Author: Markos Chandras <markos.chandras@imgtec.com>
 */

#include "ieee754dp.h"


/* 128 bits shift right logical with rounding. */
static void srl128(u64 *hptr, u64 *lptr, int count)
{
        u64 low;

        if (count >= 128) {
                *lptr = *hptr != 0 || *lptr != 0;
                *hptr = 0;
        } else if (count >= 64) {
                if (count == 64) {
                        *lptr = *hptr | (*lptr != 0);
                } else {
                        low = *lptr;
                        *lptr = *hptr >> (count - 64);
                        *lptr |= (*hptr << (128 - count)) != 0 || low != 0;
                }
                *hptr = 0;
        } else {
                low = *lptr;
                *lptr = low >> count | *hptr << (64 - count);
                *lptr |= (low << (64 - count)) != 0;
                *hptr = *hptr >> count;
        }
}

static union ieee754dp _dp_maddf(union ieee754dp z, union ieee754dp x,
                                 union ieee754dp y, enum maddf_flags flags)
{
        int re;
        int rs;
        unsigned int lxm;
        unsigned int hxm;
        unsigned int lym;
        unsigned int hym;
        u64 lrm;
        u64 hrm;
        u64 lzm;
        u64 hzm;
        u64 t;
        u64 at;
        int s;

        COMPXDP;
        COMPYDP;
        COMPZDP;

        EXPLODEXDP;
        EXPLODEYDP;
        EXPLODEZDP;

        FLUSHXDP;
        FLUSHYDP;
        FLUSHZDP;

        ieee754_clearcx();

        rs = xs ^ ys;
        if (flags & MADDF_NEGATE_PRODUCT)
                rs ^= 1;
        if (flags & MADDF_NEGATE_ADDITION)
                zs ^= 1;

        /*
         * Handle the cases when at least one of x, y or z is a NaN.
         * Order of precedence is sNaN, qNaN and z, x, y.
         */
        if (zc == IEEE754_CLASS_SNAN)
                return ieee754dp_nanxcpt(z);
        if (xc == IEEE754_CLASS_SNAN)
                return ieee754dp_nanxcpt(x);
        if (yc == IEEE754_CLASS_SNAN)
                return ieee754dp_nanxcpt(y);
        if (zc == IEEE754_CLASS_QNAN)
                return z;
        if (xc == IEEE754_CLASS_QNAN)
                return x;
        if (yc == IEEE754_CLASS_QNAN)
                return y;

        if (zc == IEEE754_CLASS_DNORM)
                DPDNORMZ;
        /* ZERO z cases are handled separately below */

        switch (CLPAIR(xc, yc)) {

        /*
         * Infinity handling
         */
        case CLPAIR(IEEE754_CLASS_INF, IEEE754_CLASS_ZERO):
        case CLPAIR(IEEE754_CLASS_ZERO, IEEE754_CLASS_INF):
                ieee754_setcx(IEEE754_INVALID_OPERATION);
                return ieee754dp_indef();

        case CLPAIR(IEEE754_CLASS_NORM, IEEE754_CLASS_INF):
        case CLPAIR(IEEE754_CLASS_DNORM, IEEE754_CLASS_INF):
        case CLPAIR(IEEE754_CLASS_INF, IEEE754_CLASS_NORM):
        case CLPAIR(IEEE754_CLASS_INF, IEEE754_CLASS_DNORM):
        case CLPAIR(IEEE754_CLASS_INF, IEEE754_CLASS_INF):
                if ((zc == IEEE754_CLASS_INF) && (zs != rs)) {
                        /*
                         * Cases of addition of infinities with opposite signs
                         * or subtraction of infinities with same signs.
                         */
                        ieee754_setcx(IEEE754_INVALID_OPERATION);
                        return ieee754dp_indef();
                }
                /*
                 * z is here either not an infinity, or an infinity having the
                 * same sign as product (x*y). The result must be an infinity,
                 * and its sign is determined only by the sign of product (x*y).
                 */
                return ieee754dp_inf(rs);

        case CLPAIR(IEEE754_CLASS_ZERO, IEEE754_CLASS_ZERO):
        case CLPAIR(IEEE754_CLASS_ZERO, IEEE754_CLASS_NORM):
        case CLPAIR(IEEE754_CLASS_ZERO, IEEE754_CLASS_DNORM):
        case CLPAIR(IEEE754_CLASS_NORM, IEEE754_CLASS_ZERO):
        case CLPAIR(IEEE754_CLASS_DNORM, IEEE754_CLASS_ZERO):
                if (zc == IEEE754_CLASS_INF)
                        return ieee754dp_inf(zs);
                if (zc == IEEE754_CLASS_ZERO) {
                        /* Handle cases +0 + (-0) and similar ones. */
                        if (zs == rs)
                                /*
                                 * Cases of addition of zeros of equal signs
                                 * or subtraction of zeroes of opposite signs.
                                 * The sign of the resulting zero is in any
                                 * such case determined only by the sign of z.
                                 */
                                return z;

                        return ieee754dp_zero(ieee754_csr.rm == FPU_CSR_RD);
                }
                /* x*y is here 0, and z is not 0, so just return z */
                return z;

        case CLPAIR(IEEE754_CLASS_DNORM, IEEE754_CLASS_DNORM):
                DPDNORMX;
                fallthrough;
        case CLPAIR(IEEE754_CLASS_NORM, IEEE754_CLASS_DNORM):
                if (zc == IEEE754_CLASS_INF)
                        return ieee754dp_inf(zs);
                DPDNORMY;
                break;

        case CLPAIR(IEEE754_CLASS_DNORM, IEEE754_CLASS_NORM):
                if (zc == IEEE754_CLASS_INF)
                        return ieee754dp_inf(zs);
                DPDNORMX;
                break;

        case CLPAIR(IEEE754_CLASS_NORM, IEEE754_CLASS_NORM):
                if (zc == IEEE754_CLASS_INF)
                        return ieee754dp_inf(zs);
                /* continue to real computations */
        }

        /* Finally get to do some computation */

        /*
         * Do the multiplication bit first
         *
         * rm = xm * ym, re = xe + ye basically
         *
         * At this point xm and ym should have been normalized.
         */
        assert(xm & DP_HIDDEN_BIT);
        assert(ym & DP_HIDDEN_BIT);

        re = xe + ye;

        /* shunt to top of word */
        xm <<= 64 - (DP_FBITS + 1);
        ym <<= 64 - (DP_FBITS + 1);

        /*
         * Multiply 64 bits xm and ym to give 128 bits result in hrm:lrm.
         */

        lxm = xm;
        hxm = xm >> 32;
        lym = ym;
        hym = ym >> 32;

        lrm = DPXMULT(lxm, lym);
        hrm = DPXMULT(hxm, hym);

        t = DPXMULT(lxm, hym);

        at = lrm + (t << 32);
        hrm += at < lrm;
        lrm = at;

        hrm = hrm + (t >> 32);

        t = DPXMULT(hxm, lym);

        at = lrm + (t << 32);
        hrm += at < lrm;
        lrm = at;

        hrm = hrm + (t >> 32);

        /* Put explicit bit at bit 126 if necessary */
        if ((int64_t)hrm < 0) {
                lrm = (hrm << 63) | (lrm >> 1);
                hrm = hrm >> 1;
                re++;
        }

        assert(hrm & (1 << 62));

        if (zc == IEEE754_CLASS_ZERO) {
                /*
                 * Move explicit bit from bit 126 to bit 55 since the
                 * ieee754dp_format code expects the mantissa to be
                 * 56 bits wide (53 + 3 rounding bits).
                 */
                srl128(&hrm, &lrm, (126 - 55));
                return ieee754dp_format(rs, re, lrm);
        }

        /* Move explicit bit from bit 52 to bit 126 */
        lzm = 0;
        hzm = zm << 10;
        assert(hzm & (1 << 62));

        /* Make the exponents the same */
        if (ze > re) {
                /*
                 * Have to shift y fraction right to align.
                 */
                s = ze - re;
                srl128(&hrm, &lrm, s);
                re += s;
        } else if (re > ze) {
                /*
                 * Have to shift x fraction right to align.
                 */
                s = re - ze;
                srl128(&hzm, &lzm, s);
                ze += s;
        }
        assert(ze == re);
        assert(ze <= DP_EMAX);

        /* Do the addition */
        if (zs == rs) {
                /*
                 * Generate 128 bit result by adding two 127 bit numbers
                 * leaving result in hzm:lzm, zs and ze.
                 */
                hzm = hzm + hrm + (lzm > (lzm + lrm));
                lzm = lzm + lrm;
                if ((int64_t)hzm < 0) {        /* carry out */
                        srl128(&hzm, &lzm, 1);
                        ze++;
                }
        } else {
                if (hzm > hrm || (hzm == hrm && lzm >= lrm)) {
                        hzm = hzm - hrm - (lzm < lrm);
                        lzm = lzm - lrm;
                } else {
                        hzm = hrm - hzm - (lrm < lzm);
                        lzm = lrm - lzm;
                        zs = rs;
                }
                if (lzm == 0 && hzm == 0)
                        return ieee754dp_zero(ieee754_csr.rm == FPU_CSR_RD);

                /*
                 * Put explicit bit at bit 126 if necessary.
                 */
                if (hzm == 0) {
                        /* left shift by 63 or 64 bits */
                        if ((int64_t)lzm < 0) {
                                /* MSB of lzm is the explicit bit */
                                hzm = lzm >> 1;
                                lzm = lzm << 63;
                                ze -= 63;
                        } else {
                                hzm = lzm;
                                lzm = 0;
                                ze -= 64;
                        }
                }

                t = 0;
                while ((hzm >> (62 - t)) == 0)
                        t++;

                assert(t <= 62);
                if (t) {
                        hzm = hzm << t | lzm >> (64 - t);
                        lzm = lzm << t;
                        ze -= t;
                }
        }

        /*
         * Move explicit bit from bit 126 to bit 55 since the
         * ieee754dp_format code expects the mantissa to be
         * 56 bits wide (53 + 3 rounding bits).
         */
        srl128(&hzm, &lzm, (126 - 55));

        return ieee754dp_format(zs, ze, lzm);
}

union ieee754dp ieee754dp_maddf(union ieee754dp z, union ieee754dp x,
                                union ieee754dp y)
{
        return _dp_maddf(z, x, y, 0);
}

union ieee754dp ieee754dp_msubf(union ieee754dp z, union ieee754dp x,
                                union ieee754dp y)
{
        return _dp_maddf(z, x, y, MADDF_NEGATE_PRODUCT);
}

union ieee754dp ieee754dp_madd(union ieee754dp z, union ieee754dp x,
                                union ieee754dp y)
{
        return _dp_maddf(z, x, y, 0);
}

union ieee754dp ieee754dp_msub(union ieee754dp z, union ieee754dp x,
                                union ieee754dp y)
{
        return _dp_maddf(z, x, y, MADDF_NEGATE_ADDITION);
}

union ieee754dp ieee754dp_nmadd(union ieee754dp z, union ieee754dp x,
                                union ieee754dp y)
{
        return _dp_maddf(z, x, y, MADDF_NEGATE_PRODUCT|MADDF_NEGATE_ADDITION);
}

union ieee754dp ieee754dp_nmsub(union ieee754dp z, union ieee754dp x,
                                union ieee754dp y)
{
        return _dp_maddf(z, x, y, MADDF_NEGATE_PRODUCT);
}