/*
 * cp1emu.c: a MIPS coprocessor 1 (fpu) instruction emulator
 *
 * MIPS floating point support
 * Copyright (C) 1994-2000 Algorithmics Ltd.
 *
 * Kevin D. Kissell, kevink@mips.com and Carsten Langgaard, carstenl@mips.com
 * Copyright (C) 2000  MIPS Technologies, Inc.
 *
 *  This program is free software; you can distribute it and/or modify it
 *  under the terms of the GNU General Public License (Version 2) as
 *  published by the Free Software Foundation.
 *
 *  This program is distributed in the hope it will be useful, but WITHOUT
 *  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
 *  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
 *  for more details.
 *
 *  You should have received a copy of the GNU General Public License along
 *  with this program; if not, write to the Free Software Foundation, Inc.,
 *  59 Temple Place - Suite 330, Boston MA 02111-1307, USA.
 *
 * A complete emulator for MIPS coprocessor 1 instructions.  This is
 * required for #float(switch) or #float(trap), where it catches all
 * COP1 instructions via the "CoProcessor Unusable" exception.
 *
 * More surprisingly it is also required for #float(ieee), to help out
 * the hardware fpu at the boundaries of the IEEE-754 representation
 * (denormalised values, infinities, underflow, etc).  It is made
 * quite nasty because emulation of some non-COP1 instructions is
 * required, e.g. in branch delay slots.
 *
 * Note if you know that you won't have an fpu, then you'll get much
 * better performance by compiling with -msoft-float!
 */
#include <linux/sched.h>
#include <linux/module.h>
#include <linux/debugfs.h>
#include <linux/perf_event.h>

#include <asm/inst.h>
#include <asm/bootinfo.h>
#include <asm/processor.h>
#include <asm/ptrace.h>
#include <asm/signal.h>
#include <asm/mipsregs.h>
#include <asm/fpu_emulator.h>
#include <asm/uaccess.h>
#include <asm/branch.h>

#include "ieee754.h"

/* Strap kernel emulator for full MIPS IV emulation */

#ifdef __mips
#undef __mips
#endif
#define __mips 4

/* Function which emulates a floating point instruction. */

static int fpu_emu(struct pt_regs *, struct mips_fpu_struct *,
        mips_instruction);

#if __mips >= 4 && __mips != 32
static int fpux_emu(struct pt_regs *,
        struct mips_fpu_struct *, mips_instruction, void *__user *);
#endif

/* Further private data for which no space exists in mips_fpu_struct */

#ifdef CONFIG_DEBUG_FS
DEFINE_PER_CPU(struct mips_fpu_emulator_stats, fpuemustats);
#endif

/* Control registers */

#define FPCREG_RID      0       /* $0  = revision id */
#define FPCREG_CSR      31      /* $31 = csr */

/* Determine rounding mode from the RM bits of the FCSR */
#define modeindex(v) ((v) & FPU_CSR_RM)

/* Convert Mips rounding mode (0..3) to IEEE library modes. */
static const unsigned char ieee_rm[4] = {
        [FPU_CSR_RN] = IEEE754_RN,
        [FPU_CSR_RZ] = IEEE754_RZ,
        [FPU_CSR_RU] = IEEE754_RU,
        [FPU_CSR_RD] = IEEE754_RD,
};
/* Convert IEEE library modes to Mips rounding mode (0..3). */
static const unsigned char mips_rm[4] = {
        [IEEE754_RN] = FPU_CSR_RN,
        [IEEE754_RZ] = FPU_CSR_RZ,
        [IEEE754_RD] = FPU_CSR_RD,
        [IEEE754_RU] = FPU_CSR_RU,
};

#if __mips >= 4
/* convert condition code register number to csr bit */
static const unsigned int fpucondbit[8] = {
        FPU_CSR_COND0,
        FPU_CSR_COND1,
        FPU_CSR_COND2,
        FPU_CSR_COND3,
        FPU_CSR_COND4,
        FPU_CSR_COND5,
        FPU_CSR_COND6,
        FPU_CSR_COND7
};
#endif


/*
 * Redundant with logic already in kernel/branch.c,
 * embedded in compute_return_epc.  At some point,
 * a single subroutine should be used across both
 * modules.
 */
static int isBranchInstr(mips_instruction * i)
{
        switch (MIPSInst_OPCODE(*i)) {
        case spec_op:
                switch (MIPSInst_FUNC(*i)) {
                case jalr_op:
                case jr_op:
                        return 1;
                }
                break;

        case bcond_op:
                switch (MIPSInst_RT(*i)) {
                case bltz_op:
                case bgez_op:
                case bltzl_op:
                case bgezl_op:
                case bltzal_op:
                case bgezal_op:
                case bltzall_op:
                case bgezall_op:
                        return 1;
                }
                break;

        case j_op:
        case jal_op:
        case jalx_op:
        case beq_op:
        case bne_op:
        case blez_op:
        case bgtz_op:
        case beql_op:
        case bnel_op:
        case blezl_op:
        case bgtzl_op:
                return 1;

        case cop0_op:
        case cop1_op:
        case cop2_op:
        case cop1x_op:
                if (MIPSInst_RS(*i) == bc_op)
                        return 1;
                break;
        }

        return 0;
}

/*
 * In the Linux kernel, we support selection of FPR format on the
 * basis of the Status.FR bit.  If an FPU is not present, the FR bit
 * is hardwired to zero, which would imply a 32-bit FPU even for
 * 64-bit CPUs.  For 64-bit kernels with no FPU we use TIF_32BIT_REGS
 * as a proxy for the FR bit so that a 64-bit FPU is emulated.  In any
 * case, for a 32-bit kernel which uses the O32 MIPS ABI, only the
 * even FPRs are used (Status.FR = 0).
 */
static inline int cop1_64bit(struct pt_regs *xcp)
{
        if (cpu_has_fpu)
                return xcp->cp0_status & ST0_FR;
#ifdef CONFIG_64BIT
        return !test_thread_flag(TIF_32BIT_REGS);
#else
        return 0;
#endif
}

#define SIFROMREG(si, x) ((si) = cop1_64bit(xcp) || !(x & 1) ? \
                        (int)ctx->fpr[x] : (int)(ctx->fpr[x & ~1] >> 32))

#define SITOREG(si, x)  (ctx->fpr[x & ~(cop1_64bit(xcp) == 0)] = \
                        cop1_64bit(xcp) || !(x & 1) ? \
                        ctx->fpr[x & ~1] >> 32 << 32 | (u32)(si) : \
                        ctx->fpr[x & ~1] << 32 >> 32 | (u64)(si) << 32)

#define DIFROMREG(di, x) ((di) = ctx->fpr[x & ~(cop1_64bit(xcp) == 0)])
#define DITOREG(di, x)  (ctx->fpr[x & ~(cop1_64bit(xcp) == 0)] = (di))

#define SPFROMREG(sp, x) SIFROMREG((sp).bits, x)
#define SPTOREG(sp, x)  SITOREG((sp).bits, x)
#define DPFROMREG(dp, x)        DIFROMREG((dp).bits, x)
#define DPTOREG(dp, x)  DITOREG((dp).bits, x)

/*
 * Emulate the single floating point instruction pointed at by EPC.
 * Two instructions if the instruction is in a branch delay slot.
 */

static int cop1Emulate(struct pt_regs *xcp, struct mips_fpu_struct *ctx,
                       void *__user *fault_addr)
{
        mips_instruction ir;
        unsigned long emulpc, contpc;
        unsigned int cond;

        if (!access_ok(VERIFY_READ, xcp->cp0_epc, sizeof(mips_instruction))) {
                MIPS_FPU_EMU_INC_STATS(errors);
                *fault_addr = (mips_instruction __user *)xcp->cp0_epc;
                return SIGBUS;
        }
        if (__get_user(ir, (mips_instruction __user *) xcp->cp0_epc)) {
                MIPS_FPU_EMU_INC_STATS(errors);
                *fault_addr = (mips_instruction __user *)xcp->cp0_epc;
                return SIGSEGV;
        }

        /* XXX NEC Vr54xx bug workaround */
        if ((xcp->cp0_cause & CAUSEF_BD) && !isBranchInstr(&ir))
                xcp->cp0_cause &= ~CAUSEF_BD;

        if (xcp->cp0_cause & CAUSEF_BD) {
                /*
                 * The instruction to be emulated is in a branch delay slot
                 * which means that we have to  emulate the branch instruction
                 * BEFORE we do the cop1 instruction.
                 *
                 * This branch could be a COP1 branch, but in that case we
                 * would have had a trap for that instruction, and would not
                 * come through this route.
                 *
                 * Linux MIPS branch emulator operates on context, updating the
                 * cp0_epc.
                 */
                emulpc = xcp->cp0_epc + 4;      /* Snapshot emulation target */

                if (__compute_return_epc(xcp)) {
#ifdef CP1DBG
                        printk("failed to emulate branch at %p\n",
                                (void *) (xcp->cp0_epc));
#endif
                        return SIGILL;
                }
                if (!access_ok(VERIFY_READ, emulpc, sizeof(mips_instruction))) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = (mips_instruction __user *)emulpc;
                        return SIGBUS;
                }
                if (__get_user(ir, (mips_instruction __user *) emulpc)) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = (mips_instruction __user *)emulpc;
                        return SIGSEGV;
                }
                /* __compute_return_epc() will have updated cp0_epc */
                contpc = xcp->cp0_epc;
                /* In order not to confuse ptrace() et al, tweak context */
                xcp->cp0_epc = emulpc - 4;
        } else {
                emulpc = xcp->cp0_epc;
                contpc = xcp->cp0_epc + 4;
        }

      emul:
        perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS,
                        1, 0, xcp, 0);
        MIPS_FPU_EMU_INC_STATS(emulated);
        switch (MIPSInst_OPCODE(ir)) {
        case ldc1_op:{
                u64 __user *va = (u64 __user *) (xcp->regs[MIPSInst_RS(ir)] +
                        MIPSInst_SIMM(ir));
                u64 val;

                MIPS_FPU_EMU_INC_STATS(loads);

                if (!access_ok(VERIFY_READ, va, sizeof(u64))) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = va;
                        return SIGBUS;
                }
                if (__get_user(val, va)) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = va;
                        return SIGSEGV;
                }
                DITOREG(val, MIPSInst_RT(ir));
                break;
        }

        case sdc1_op:{
                u64 __user *va = (u64 __user *) (xcp->regs[MIPSInst_RS(ir)] +
                        MIPSInst_SIMM(ir));
                u64 val;

                MIPS_FPU_EMU_INC_STATS(stores);
                DIFROMREG(val, MIPSInst_RT(ir));
                if (!access_ok(VERIFY_WRITE, va, sizeof(u64))) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = va;
                        return SIGBUS;
                }
                if (__put_user(val, va)) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = va;
                        return SIGSEGV;
                }
                break;
        }

        case lwc1_op:{
                u32 __user *va = (u32 __user *) (xcp->regs[MIPSInst_RS(ir)] +
                        MIPSInst_SIMM(ir));
                u32 val;

                MIPS_FPU_EMU_INC_STATS(loads);
                if (!access_ok(VERIFY_READ, va, sizeof(u32))) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = va;
                        return SIGBUS;
                }
                if (__get_user(val, va)) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = va;
                        return SIGSEGV;
                }
                SITOREG(val, MIPSInst_RT(ir));
                break;
        }

        case swc1_op:{
                u32 __user *va = (u32 __user *) (xcp->regs[MIPSInst_RS(ir)] +
                        MIPSInst_SIMM(ir));
                u32 val;

                MIPS_FPU_EMU_INC_STATS(stores);
                SIFROMREG(val, MIPSInst_RT(ir));
                if (!access_ok(VERIFY_WRITE, va, sizeof(u32))) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = va;
                        return SIGBUS;
                }
                if (__put_user(val, va)) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = va;
                        return SIGSEGV;
                }
                break;
        }

        case cop1_op:
                switch (MIPSInst_RS(ir)) {

#if defined(__mips64)
                case dmfc_op:
                        /* copregister fs -> gpr[rt] */
                        if (MIPSInst_RT(ir) != 0) {
                                DIFROMREG(xcp->regs[MIPSInst_RT(ir)],
                                        MIPSInst_RD(ir));
                        }
                        break;

                case dmtc_op:
                        /* copregister fs <- rt */
                        DITOREG(xcp->regs[MIPSInst_RT(ir)], MIPSInst_RD(ir));
                        break;
#endif

                case mfc_op:
                        /* copregister rd -> gpr[rt] */
                        if (MIPSInst_RT(ir) != 0) {
                                SIFROMREG(xcp->regs[MIPSInst_RT(ir)],
                                        MIPSInst_RD(ir));
                        }
                        break;

                case mtc_op:
                        /* copregister rd <- rt */
                        SITOREG(xcp->regs[MIPSInst_RT(ir)], MIPSInst_RD(ir));
                        break;

                case cfc_op:{
                        /* cop control register rd -> gpr[rt] */
                        u32 value;

                        if (MIPSInst_RD(ir) == FPCREG_CSR) {
                                value = ctx->fcr31;
                                value = (value & ~FPU_CSR_RM) |
                                        mips_rm[modeindex(value)];
#ifdef CSRTRACE
                                printk("%p gpr[%d]<-csr=%08x\n",
                                        (void *) (xcp->cp0_epc),
                                        MIPSInst_RT(ir), value);
#endif
                        }
                        else if (MIPSInst_RD(ir) == FPCREG_RID)
                                value = 0;
                        else
                                value = 0;
                        if (MIPSInst_RT(ir))
                                xcp->regs[MIPSInst_RT(ir)] = value;
                        break;
                }

                case ctc_op:{
                        /* copregister rd <- rt */
                        u32 value;

                        if (MIPSInst_RT(ir) == 0)
                                value = 0;
                        else
                                value = xcp->regs[MIPSInst_RT(ir)];

                        /* we only have one writable control reg
                         */
                        if (MIPSInst_RD(ir) == FPCREG_CSR) {
#ifdef CSRTRACE
                                printk("%p gpr[%d]->csr=%08x\n",
                                        (void *) (xcp->cp0_epc),
                                        MIPSInst_RT(ir), value);
#endif

                                /*
                                 * Don't write reserved bits,
                                 * and convert to ieee library modes
                                 */
                                ctx->fcr31 = (value &
                                                ~(FPU_CSR_RSVD | FPU_CSR_RM)) |
                                                ieee_rm[modeindex(value)];
                        }
                        if ((ctx->fcr31 >> 5) & ctx->fcr31 & FPU_CSR_ALL_E) {
                                return SIGFPE;
                        }
                        break;
                }

                case bc_op:{
                        int likely = 0;

                        if (xcp->cp0_cause & CAUSEF_BD)
                                return SIGILL;

#if __mips >= 4
                        cond = ctx->fcr31 & fpucondbit[MIPSInst_RT(ir) >> 2];
#else
                        cond = ctx->fcr31 & FPU_CSR_COND;
#endif
                        switch (MIPSInst_RT(ir) & 3) {
                        case bcfl_op:
                                likely = 1;
                        case bcf_op:
                                cond = !cond;
                                break;
                        case bctl_op:
                                likely = 1;
                        case bct_op:
                                break;
                        default:
                                /* thats an illegal instruction */
                                return SIGILL;
                        }

                        xcp->cp0_cause |= CAUSEF_BD;
                        if (cond) {
                                /* branch taken: emulate dslot
                                 * instruction
                                 */
                                xcp->cp0_epc += 4;
                                contpc = (xcp->cp0_epc +
                                        (MIPSInst_SIMM(ir) << 2));

                                if (!access_ok(VERIFY_READ, xcp->cp0_epc,
                                               sizeof(mips_instruction))) {
                                        MIPS_FPU_EMU_INC_STATS(errors);
                                        *fault_addr = (mips_instruction __user *)xcp->cp0_epc;
                                        return SIGBUS;
                                }
                                if (__get_user(ir,
                                    (mips_instruction __user *) xcp->cp0_epc)) {
                                        MIPS_FPU_EMU_INC_STATS(errors);
                                        *fault_addr = (mips_instruction __user *)xcp->cp0_epc;
                                        return SIGSEGV;
                                }

                                switch (MIPSInst_OPCODE(ir)) {
                                case lwc1_op:
                                case swc1_op:
#if (__mips >= 2 || defined(__mips64))
                                case ldc1_op:
                                case sdc1_op:
#endif
                                case cop1_op:
#if __mips >= 4 && __mips != 32
                                case cop1x_op:
#endif
                                        /* its one of ours */
                                        goto emul;
#if __mips >= 4
                                case spec_op:
                                        if (MIPSInst_FUNC(ir) == movc_op)
                                                goto emul;
                                        break;
#endif
                                }

                                /*
                                 * Single step the non-cp1
                                 * instruction in the dslot
                                 */
                                return mips_dsemul(xcp, ir, contpc);
                        }
                        else {
                                /* branch not taken */
                                if (likely) {
                                        /*
                                         * branch likely nullifies
                                         * dslot if not taken
                                         */
                                        xcp->cp0_epc += 4;
                                        contpc += 4;
                                        /*
                                         * else continue & execute
                                         * dslot as normal insn
                                         */
                                }
                        }
                        break;
                }

                default:
                        if (!(MIPSInst_RS(ir) & 0x10))
                                return SIGILL;
                        {
                                int sig;

                                /* a real fpu computation instruction */
                                if ((sig = fpu_emu(xcp, ctx, ir)))
                                        return sig;
                        }
                }
                break;

#if __mips >= 4 && __mips != 32
        case cop1x_op:{
                int sig = fpux_emu(xcp, ctx, ir, fault_addr);
                if (sig)
                        return sig;
                break;
        }
#endif

#if __mips >= 4
        case spec_op:
                if (MIPSInst_FUNC(ir) != movc_op)
                        return SIGILL;
                cond = fpucondbit[MIPSInst_RT(ir) >> 2];
                if (((ctx->fcr31 & cond) != 0) == ((MIPSInst_RT(ir) & 1) != 0))
                        xcp->regs[MIPSInst_RD(ir)] =
                                xcp->regs[MIPSInst_RS(ir)];
                break;
#endif

        default:
                return SIGILL;
        }

        /* we did it !! */
        xcp->cp0_epc = contpc;
        xcp->cp0_cause &= ~CAUSEF_BD;

        return 0;
}

/*
 * Conversion table from MIPS compare ops 48-63
 * cond = ieee754dp_cmp(x,y,IEEE754_UN,sig);
 */
static const unsigned char cmptab[8] = {
        0,                      /* cmp_0 (sig) cmp_sf */
        IEEE754_CUN,            /* cmp_un (sig) cmp_ngle */
        IEEE754_CEQ,            /* cmp_eq (sig) cmp_seq */
        IEEE754_CEQ | IEEE754_CUN,      /* cmp_ueq (sig) cmp_ngl  */
        IEEE754_CLT,            /* cmp_olt (sig) cmp_lt */
        IEEE754_CLT | IEEE754_CUN,      /* cmp_ult (sig) cmp_nge */
        IEEE754_CLT | IEEE754_CEQ,      /* cmp_ole (sig) cmp_le */
        IEEE754_CLT | IEEE754_CEQ | IEEE754_CUN,        /* cmp_ule (sig) cmp_ngt */
};


#if __mips >= 4 && __mips != 32

/*
 * Additional MIPS4 instructions
 */

#define DEF3OP(name, p, f1, f2, f3) \
static ieee754##p fpemu_##p##_##name(ieee754##p r, ieee754##p s, \
    ieee754##p t) \
{ \
        struct _ieee754_csr ieee754_csr_save; \
        s = f1(s, t); \
        ieee754_csr_save = ieee754_csr; \
        s = f2(s, r); \
        ieee754_csr_save.cx |= ieee754_csr.cx; \
        ieee754_csr_save.sx |= ieee754_csr.sx; \
        s = f3(s); \
        ieee754_csr.cx |= ieee754_csr_save.cx; \
        ieee754_csr.sx |= ieee754_csr_save.sx; \
        return s; \
}

static ieee754dp fpemu_dp_recip(ieee754dp d)
{
        return ieee754dp_div(ieee754dp_one(0), d);
}

static ieee754dp fpemu_dp_rsqrt(ieee754dp d)
{
        return ieee754dp_div(ieee754dp_one(0), ieee754dp_sqrt(d));
}

static ieee754sp fpemu_sp_recip(ieee754sp s)
{
        return ieee754sp_div(ieee754sp_one(0), s);
}

static ieee754sp fpemu_sp_rsqrt(ieee754sp s)
{
        return ieee754sp_div(ieee754sp_one(0), ieee754sp_sqrt(s));
}

DEF3OP(madd, sp, ieee754sp_mul, ieee754sp_add, );
DEF3OP(msub, sp, ieee754sp_mul, ieee754sp_sub, );
DEF3OP(nmadd, sp, ieee754sp_mul, ieee754sp_add, ieee754sp_neg);
DEF3OP(nmsub, sp, ieee754sp_mul, ieee754sp_sub, ieee754sp_neg);
DEF3OP(madd, dp, ieee754dp_mul, ieee754dp_add, );
DEF3OP(msub, dp, ieee754dp_mul, ieee754dp_sub, );
DEF3OP(nmadd, dp, ieee754dp_mul, ieee754dp_add, ieee754dp_neg);
DEF3OP(nmsub, dp, ieee754dp_mul, ieee754dp_sub, ieee754dp_neg);

static int fpux_emu(struct pt_regs *xcp, struct mips_fpu_struct *ctx,
        mips_instruction ir, void *__user *fault_addr)
{
        unsigned rcsr = 0;      /* resulting csr */

        MIPS_FPU_EMU_INC_STATS(cp1xops);

        switch (MIPSInst_FMA_FFMT(ir)) {
        case s_fmt:{            /* 0 */

                ieee754sp(*handler) (ieee754sp, ieee754sp, ieee754sp);
                ieee754sp fd, fr, fs, ft;
                u32 __user *va;
                u32 val;

                switch (MIPSInst_FUNC(ir)) {
                case lwxc1_op:
                        va = (void __user *) (xcp->regs[MIPSInst_FR(ir)] +
                                xcp->regs[MIPSInst_FT(ir)]);

                        MIPS_FPU_EMU_INC_STATS(loads);
                        if (!access_ok(VERIFY_READ, va, sizeof(u32))) {
                                MIPS_FPU_EMU_INC_STATS(errors);
                                *fault_addr = va;
                                return SIGBUS;
                        }
                        if (__get_user(val, va)) {
                                MIPS_FPU_EMU_INC_STATS(errors);
                                *fault_addr = va;
                                return SIGSEGV;
                        }
                        SITOREG(val, MIPSInst_FD(ir));
                        break;

                case swxc1_op:
                        va = (void __user *) (xcp->regs[MIPSInst_FR(ir)] +
                                xcp->regs[MIPSInst_FT(ir)]);

                        MIPS_FPU_EMU_INC_STATS(stores);

                        SIFROMREG(val, MIPSInst_FS(ir));
                        if (!access_ok(VERIFY_WRITE, va, sizeof(u32))) {
                                MIPS_FPU_EMU_INC_STATS(errors);
                                *fault_addr = va;
                                return SIGBUS;
                        }
                        if (put_user(val, va)) {
                                MIPS_FPU_EMU_INC_STATS(errors);
                                *fault_addr = va;
                                return SIGSEGV;
                        }
                        break;

                case madd_s_op:
                        handler = fpemu_sp_madd;
                        goto scoptop;
                case msub_s_op:
                        handler = fpemu_sp_msub;
                        goto scoptop;
                case nmadd_s_op:
                        handler = fpemu_sp_nmadd;
                        goto scoptop;
                case nmsub_s_op:
                        handler = fpemu_sp_nmsub;
                        goto scoptop;

                      scoptop:
                        SPFROMREG(fr, MIPSInst_FR(ir));
                        SPFROMREG(fs, MIPSInst_FS(ir));
                        SPFROMREG(ft, MIPSInst_FT(ir));
                        fd = (*handler) (fr, fs, ft);
                        SPTOREG(fd, MIPSInst_FD(ir));

                      copcsr:
                        if (ieee754_cxtest(IEEE754_INEXACT))
                                rcsr |= FPU_CSR_INE_X | FPU_CSR_INE_S;
                        if (ieee754_cxtest(IEEE754_UNDERFLOW))
                                rcsr |= FPU_CSR_UDF_X | FPU_CSR_UDF_S;
                        if (ieee754_cxtest(IEEE754_OVERFLOW))
                                rcsr |= FPU_CSR_OVF_X | FPU_CSR_OVF_S;
                        if (ieee754_cxtest(IEEE754_INVALID_OPERATION))
                                rcsr |= FPU_CSR_INV_X | FPU_CSR_INV_S;

                        ctx->fcr31 = (ctx->fcr31 & ~FPU_CSR_ALL_X) | rcsr;
                        if ((ctx->fcr31 >> 5) & ctx->fcr31 & FPU_CSR_ALL_E) {
                                /*printk ("SIGFPE: fpu csr = %08x\n",
                                   ctx->fcr31); */
                                return SIGFPE;
                        }

                        break;

                default:
                        return SIGILL;
                }
                break;
        }

        case d_fmt:{            /* 1 */
                ieee754dp(*handler) (ieee754dp, ieee754dp, ieee754dp);
                ieee754dp fd, fr, fs, ft;
                u64 __user *va;
                u64 val;

                switch (MIPSInst_FUNC(ir)) {
                case ldxc1_op:
                        va = (void __user *) (xcp->regs[MIPSInst_FR(ir)] +
                                xcp->regs[MIPSInst_FT(ir)]);

                        MIPS_FPU_EMU_INC_STATS(loads);
                        if (!access_ok(VERIFY_READ, va, sizeof(u64))) {
                                MIPS_FPU_EMU_INC_STATS(errors);
                                *fault_addr = va;
                                return SIGBUS;
                        }
                        if (__get_user(val, va)) {
                                MIPS_FPU_EMU_INC_STATS(errors);
                                *fault_addr = va;
                                return SIGSEGV;
                        }
                        DITOREG(val, MIPSInst_FD(ir));
                        break;

                case sdxc1_op:
                        va = (void __user *) (xcp->regs[MIPSInst_FR(ir)] +
                                xcp->regs[MIPSInst_FT(ir)]);

                        MIPS_FPU_EMU_INC_STATS(stores);
                        DIFROMREG(val, MIPSInst_FS(ir));
                        if (!access_ok(VERIFY_WRITE, va, sizeof(u64))) {
                                MIPS_FPU_EMU_INC_STATS(errors);
                                *fault_addr = va;
                                return SIGBUS;
                        }
                        if (__put_user(val, va)) {
                                MIPS_FPU_EMU_INC_STATS(errors);
                                *fault_addr = va;
                                return SIGSEGV;
                        }
                        break;

                case madd_d_op:
                        handler = fpemu_dp_madd;
                        goto dcoptop;
                case msub_d_op:
                        handler = fpemu_dp_msub;
                        goto dcoptop;
                case nmadd_d_op:
                        handler = fpemu_dp_nmadd;
                        goto dcoptop;
                case nmsub_d_op:
                        handler = fpemu_dp_nmsub;
                        goto dcoptop;

                      dcoptop:
                        DPFROMREG(fr, MIPSInst_FR(ir));
                        DPFROMREG(fs, MIPSInst_FS(ir));
                        DPFROMREG(ft, MIPSInst_FT(ir));
                        fd = (*handler) (fr, fs, ft);
                        DPTOREG(fd, MIPSInst_FD(ir));
                        goto copcsr;

                default:
                        return SIGILL;
                }
                break;
        }

        case 0x7:               /* 7 */
                if (MIPSInst_FUNC(ir) != pfetch_op) {
                        return SIGILL;
                }
                /* ignore prefx operation */
                break;

        default:
                return SIGILL;
        }

        return 0;
}
#endif



/*
 * Emulate a single COP1 arithmetic instruction.
 */
static int fpu_emu(struct pt_regs *xcp, struct mips_fpu_struct *ctx,
        mips_instruction ir)
{
        int rfmt;               /* resulting format */
        unsigned rcsr = 0;      /* resulting csr */
        unsigned cond;
        union {
                ieee754dp d;
                ieee754sp s;
                int w;
#ifdef __mips64
                s64 l;
#endif
        } rv;                   /* resulting value */

        MIPS_FPU_EMU_INC_STATS(cp1ops);
        switch (rfmt = (MIPSInst_FFMT(ir) & 0xf)) {
        case s_fmt:{            /* 0 */
                union {
                        ieee754sp(*b) (ieee754sp, ieee754sp);
                        ieee754sp(*u) (ieee754sp);
                } handler;

                switch (MIPSInst_FUNC(ir)) {
                        /* binary ops */
                case fadd_op:
                        handler.b = ieee754sp_add;
                        goto scopbop;
                case fsub_op:
                        handler.b = ieee754sp_sub;
                        goto scopbop;
                case fmul_op:
                        handler.b = ieee754sp_mul;
                        goto scopbop;
                case fdiv_op:
                        handler.b = ieee754sp_div;
                        goto scopbop;

                        /* unary  ops */
#if __mips >= 2 || defined(__mips64)
                case fsqrt_op:
                        handler.u = ieee754sp_sqrt;
                        goto scopuop;
#endif
#if __mips >= 4 && __mips != 32
                case frsqrt_op:
                        handler.u = fpemu_sp_rsqrt;
                        goto scopuop;
                case frecip_op:
                        handler.u = fpemu_sp_recip;
                        goto scopuop;
#endif
#if __mips >= 4
                case fmovc_op:
                        cond = fpucondbit[MIPSInst_FT(ir) >> 2];
                        if (((ctx->fcr31 & cond) != 0) !=
                                ((MIPSInst_FT(ir) & 1) != 0))
                                return 0;
                        SPFROMREG(rv.s, MIPSInst_FS(ir));
                        break;
                case fmovz_op:
                        if (xcp->regs[MIPSInst_FT(ir)] != 0)
                                return 0;
                        SPFROMREG(rv.s, MIPSInst_FS(ir));
                        break;
                case fmovn_op:
                        if (xcp->regs[MIPSInst_FT(ir)] == 0)
                                return 0;
                        SPFROMREG(rv.s, MIPSInst_FS(ir));
                        break;
#endif
                case fabs_op:
                        handler.u = ieee754sp_abs;
                        goto scopuop;
                case fneg_op:
                        handler.u = ieee754sp_neg;
                        goto scopuop;
                case fmov_op:
                        /* an easy one */
                        SPFROMREG(rv.s, MIPSInst_FS(ir));
                        goto copcsr;

                        /* binary op on handler */
                      scopbop:
                        {
                                ieee754sp fs, ft;

                                SPFROMREG(fs, MIPSInst_FS(ir));
                                SPFROMREG(ft, MIPSInst_FT(ir));

                                rv.s = (*handler.b) (fs, ft);
                                goto copcsr;
                        }
                      scopuop:
                        {
                                ieee754sp fs;

                                SPFROMREG(fs, MIPSInst_FS(ir));
                                rv.s = (*handler.u) (fs);
                                goto copcsr;
                        }
                      copcsr:
                        if (ieee754_cxtest(IEEE754_INEXACT))
                                rcsr |= FPU_CSR_INE_X | FPU_CSR_INE_S;
                        if (ieee754_cxtest(IEEE754_UNDERFLOW))
                                rcsr |= FPU_CSR_UDF_X | FPU_CSR_UDF_S;
                        if (ieee754_cxtest(IEEE754_OVERFLOW))
                                rcsr |= FPU_CSR_OVF_X | FPU_CSR_OVF_S;
                        if (ieee754_cxtest(IEEE754_ZERO_DIVIDE))
                                rcsr |= FPU_CSR_DIV_X | FPU_CSR_DIV_S;
                        if (ieee754_cxtest(IEEE754_INVALID_OPERATION))
                                rcsr |= FPU_CSR_INV_X | FPU_CSR_INV_S;
                        break;

                        /* unary conv ops */
                case fcvts_op:
                        return SIGILL;  /* not defined */
                case fcvtd_op:{
                        ieee754sp fs;

                        SPFROMREG(fs, MIPSInst_FS(ir));
                        rv.d = ieee754dp_fsp(fs);
                        rfmt = d_fmt;
                        goto copcsr;
                }
                case fcvtw_op:{
                        ieee754sp fs;

                        SPFROMREG(fs, MIPSInst_FS(ir));
                        rv.w = ieee754sp_tint(fs);
                        rfmt = w_fmt;
                        goto copcsr;
                }

#if __mips >= 2 || defined(__mips64)
                case fround_op:
                case ftrunc_op:
                case fceil_op:
                case ffloor_op:{
                        unsigned int oldrm = ieee754_csr.rm;
                        ieee754sp fs;

                        SPFROMREG(fs, MIPSInst_FS(ir));
                        ieee754_csr.rm = ieee_rm[modeindex(MIPSInst_FUNC(ir))];
                        rv.w = ieee754sp_tint(fs);
                        ieee754_csr.rm = oldrm;
                        rfmt = w_fmt;
                        goto copcsr;
                }
#endif /* __mips >= 2 */

#if defined(__mips64)
                case fcvtl_op:{
                        ieee754sp fs;

                        SPFROMREG(fs, MIPSInst_FS(ir));
                        rv.l = ieee754sp_tlong(fs);
                        rfmt = l_fmt;
                        goto copcsr;
                }

                case froundl_op:
                case ftruncl_op:

                case fceill_op:
                case ffloorl_op:{
                        unsigned int oldrm = ieee754_csr.rm;
                        ieee754sp fs;

                        SPFROMREG(fs, MIPSInst_FS(ir));
                        ieee754_csr.rm = ieee_rm[modeindex(MIPSInst_FUNC(ir))];
                        rv.l = ieee754sp_tlong(fs);
                        ieee754_csr.rm = oldrm;
                        rfmt = l_fmt;
                        goto copcsr;
                }
#endif /* defined(__mips64) */

                default:
                        if (MIPSInst_FUNC(ir) >= fcmp_op) {
                                unsigned cmpop = MIPSInst_FUNC(ir) - fcmp_op;
                                ieee754sp fs, ft;

                                SPFROMREG(fs, MIPSInst_FS(ir));
                                SPFROMREG(ft, MIPSInst_FT(ir));
                                rv.w = ieee754sp_cmp(fs, ft,
                                        cmptab[cmpop & 0x7], cmpop & 0x8);
                                rfmt = -1;
                                if ((cmpop & 0x8) && ieee754_cxtest
                                        (IEEE754_INVALID_OPERATION))
                                        rcsr = FPU_CSR_INV_X | FPU_CSR_INV_S;
                                else
                                        goto copcsr;

                        }
                        else {
                                return SIGILL;
                        }
                        break;
                }
                break;
        }

        case d_fmt:{
                union {
                        ieee754dp(*b) (ieee754dp, ieee754dp);
                        ieee754dp(*u) (ieee754dp);
                } handler;

                switch (MIPSInst_FUNC(ir)) {
                        /* binary ops */
                case fadd_op:
                        handler.b = ieee754dp_add;
                        goto dcopbop;
                case fsub_op:
                        handler.b = ieee754dp_sub;
                        goto dcopbop;
                case fmul_op:
                        handler.b = ieee754dp_mul;
                        goto dcopbop;
                case fdiv_op:
                        handler.b = ieee754dp_div;
                        goto dcopbop;

                        /* unary  ops */
#if __mips >= 2 || defined(__mips64)
                case fsqrt_op:
                        handler.u = ieee754dp_sqrt;
                        goto dcopuop;
#endif
#if __mips >= 4 && __mips != 32
                case frsqrt_op:
                        handler.u = fpemu_dp_rsqrt;
                        goto dcopuop;
                case frecip_op:
                        handler.u = fpemu_dp_recip;
                        goto dcopuop;
#endif
#if __mips >= 4
                case fmovc_op:
                        cond = fpucondbit[MIPSInst_FT(ir) >> 2];
                        if (((ctx->fcr31 & cond) != 0) !=
                                ((MIPSInst_FT(ir) & 1) != 0))
                                return 0;
                        DPFROMREG(rv.d, MIPSInst_FS(ir));
                        break;
                case fmovz_op:
                        if (xcp->regs[MIPSInst_FT(ir)] != 0)
                                return 0;
                        DPFROMREG(rv.d, MIPSInst_FS(ir));
                        break;
                case fmovn_op:
                        if (xcp->regs[MIPSInst_FT(ir)] == 0)
                                return 0;
                        DPFROMREG(rv.d, MIPSInst_FS(ir));
                        break;
#endif
                case fabs_op:
                        handler.u = ieee754dp_abs;
                        goto dcopuop;

                case fneg_op:
                        handler.u = ieee754dp_neg;
                        goto dcopuop;

                case fmov_op:
                        /* an easy one */
                        DPFROMREG(rv.d, MIPSInst_FS(ir));
                        goto copcsr;

                        /* binary op on handler */
                      dcopbop:{
                                ieee754dp fs, ft;

                                DPFROMREG(fs, MIPSInst_FS(ir));
                                DPFROMREG(ft, MIPSInst_FT(ir));

                                rv.d = (*handler.b) (fs, ft);
                                goto copcsr;
                        }
                      dcopuop:{
                                ieee754dp fs;

                                DPFROMREG(fs, MIPSInst_FS(ir));
                                rv.d = (*handler.u) (fs);
                                goto copcsr;
                        }

                        /* unary conv ops */
                case fcvts_op:{
                        ieee754dp fs;

                        DPFROMREG(fs, MIPSInst_FS(ir));
                        rv.s = ieee754sp_fdp(fs);
                        rfmt = s_fmt;
                        goto copcsr;
                }
                case fcvtd_op:
                        return SIGILL;  /* not defined */

                case fcvtw_op:{
                        ieee754dp fs;

                        DPFROMREG(fs, MIPSInst_FS(ir));
                        rv.w = ieee754dp_tint(fs);      /* wrong */
                        rfmt = w_fmt;
                        goto copcsr;
                }

#if __mips >= 2 || defined(__mips64)
                case fround_op:
                case ftrunc_op:
                case fceil_op:
                case ffloor_op:{
                        unsigned int oldrm = ieee754_csr.rm;
                        ieee754dp fs;

                        DPFROMREG(fs, MIPSInst_FS(ir));
                        ieee754_csr.rm = ieee_rm[modeindex(MIPSInst_FUNC(ir))];
                        rv.w = ieee754dp_tint(fs);
                        ieee754_csr.rm = oldrm;
                        rfmt = w_fmt;
                        goto copcsr;
                }
#endif

#if defined(__mips64)
                case fcvtl_op:{
                        ieee754dp fs;

                        DPFROMREG(fs, MIPSInst_FS(ir));
                        rv.l = ieee754dp_tlong(fs);
                        rfmt = l_fmt;
                        goto copcsr;
                }

                case froundl_op:
                case ftruncl_op:
                case fceill_op:
                case ffloorl_op:{
                        unsigned int oldrm = ieee754_csr.rm;
                        ieee754dp fs;

                        DPFROMREG(fs, MIPSInst_FS(ir));
                        ieee754_csr.rm = ieee_rm[modeindex(MIPSInst_FUNC(ir))];
                        rv.l = ieee754dp_tlong(fs);
                        ieee754_csr.rm = oldrm;
                        rfmt = l_fmt;
                        goto copcsr;
                }
#endif /* __mips >= 3 */

                default:
                        if (MIPSInst_FUNC(ir) >= fcmp_op) {
                                unsigned cmpop = MIPSInst_FUNC(ir) - fcmp_op;
                                ieee754dp fs, ft;

                                DPFROMREG(fs, MIPSInst_FS(ir));
                                DPFROMREG(ft, MIPSInst_FT(ir));
                                rv.w = ieee754dp_cmp(fs, ft,
                                        cmptab[cmpop & 0x7], cmpop & 0x8);
                                rfmt = -1;
                                if ((cmpop & 0x8)
                                        &&
                                        ieee754_cxtest
                                        (IEEE754_INVALID_OPERATION))
                                        rcsr = FPU_CSR_INV_X | FPU_CSR_INV_S;
                                else
                                        goto copcsr;

                        }
                        else {
                                return SIGILL;
                        }
                        break;
                }
                break;
        }

        case w_fmt:{
                ieee754sp fs;

                switch (MIPSInst_FUNC(ir)) {
                case fcvts_op:
                        /* convert word to single precision real */
                        SPFROMREG(fs, MIPSInst_FS(ir));
                        rv.s = ieee754sp_fint(fs.bits);
                        rfmt = s_fmt;
                        goto copcsr;
                case fcvtd_op:
                        /* convert word to double precision real */
                        SPFROMREG(fs, MIPSInst_FS(ir));
                        rv.d = ieee754dp_fint(fs.bits);
                        rfmt = d_fmt;
                        goto copcsr;
                default:
                        return SIGILL;
                }
                break;
        }

#if defined(__mips64)
        case l_fmt:{
                switch (MIPSInst_FUNC(ir)) {
                case fcvts_op:
                        /* convert long to single precision real */
                        rv.s = ieee754sp_flong(ctx->fpr[MIPSInst_FS(ir)]);
                        rfmt = s_fmt;
                        goto copcsr;
                case fcvtd_op:
                        /* convert long to double precision real */
                        rv.d = ieee754dp_flong(ctx->fpr[MIPSInst_FS(ir)]);
                        rfmt = d_fmt;
                        goto copcsr;
                default:
                        return SIGILL;
                }
                break;
        }
#endif

        default:
                return SIGILL;
        }

        /*
         * Update the fpu CSR register for this operation.
         * If an exception is required, generate a tidy SIGFPE exception,
         * without updating the result register.
         * Note: cause exception bits do not accumulate, they are rewritten
         * for each op; only the flag/sticky bits accumulate.
         */
        ctx->fcr31 = (ctx->fcr31 & ~FPU_CSR_ALL_X) | rcsr;
        if ((ctx->fcr31 >> 5) & ctx->fcr31 & FPU_CSR_ALL_E) {
                /*printk ("SIGFPE: fpu csr = %08x\n",ctx->fcr31); */
                return SIGFPE;
        }

        /*
         * Now we can safely write the result back to the register file.
         */
        switch (rfmt) {
        case -1:{
#if __mips >= 4
                cond = fpucondbit[MIPSInst_FD(ir) >> 2];
#else
                cond = FPU_CSR_COND;
#endif
                if (rv.w)
                        ctx->fcr31 |= cond;
                else
                        ctx->fcr31 &= ~cond;
                break;
        }
        case d_fmt:
                DPTOREG(rv.d, MIPSInst_FD(ir));
                break;
        case s_fmt:
                SPTOREG(rv.s, MIPSInst_FD(ir));
                break;
        case w_fmt:
                SITOREG(rv.w, MIPSInst_FD(ir));
                break;
#if defined(__mips64)
        case l_fmt:
                DITOREG(rv.l, MIPSInst_FD(ir));
                break;
#endif
        default:
                return SIGILL;
        }

        return 0;
}

int fpu_emulator_cop1Handler(struct pt_regs *xcp, struct mips_fpu_struct *ctx,
        int has_fpu, void *__user *fault_addr)
{
        unsigned long oldepc, prevepc;
        mips_instruction insn;
        int sig = 0;

        oldepc = xcp->cp0_epc;
        do {
                prevepc = xcp->cp0_epc;

                if (!access_ok(VERIFY_READ, xcp->cp0_epc, sizeof(mips_instruction))) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = (mips_instruction __user *)xcp->cp0_epc;
                        return SIGBUS;
                }
                if (__get_user(insn, (mips_instruction __user *) xcp->cp0_epc)) {
                        MIPS_FPU_EMU_INC_STATS(errors);
                        *fault_addr = (mips_instruction __user *)xcp->cp0_epc;
                        return SIGSEGV;
                }
                if (insn == 0)
                        xcp->cp0_epc += 4;      /* skip nops */
                else {
                        /*
                         * The 'ieee754_csr' is an alias of
                         * ctx->fcr31.  No need to copy ctx->fcr31 to
                         * ieee754_csr.  But ieee754_csr.rm is ieee
                         * library modes. (not mips rounding mode)
                         */
                        /* convert to ieee library modes */
                        ieee754_csr.rm = ieee_rm[ieee754_csr.rm];
                        sig = cop1Emulate(xcp, ctx, fault_addr);
                        /* revert to mips rounding mode */
                        ieee754_csr.rm = mips_rm[ieee754_csr.rm];
                }

                if (has_fpu)
                        break;
                if (sig)
                        break;

                cond_resched();
        } while (xcp->cp0_epc > prevepc);

        /* SIGILL indicates a non-fpu instruction */
        if (sig == SIGILL && xcp->cp0_epc != oldepc)
                /* but if epc has advanced, then ignore it */
                sig = 0;

        return sig;
}

#ifdef CONFIG_DEBUG_FS

static int fpuemu_stat_get(void *data, u64 *val)
{
        int cpu;
        unsigned long sum = 0;
        for_each_online_cpu(cpu) {
                struct mips_fpu_emulator_stats *ps;
                local_t *pv;
                ps = &per_cpu(fpuemustats, cpu);
                pv = (void *)ps + (unsigned long)data;
                sum += local_read(pv);
        }
        *val = sum;
        return 0;
}
DEFINE_SIMPLE_ATTRIBUTE(fops_fpuemu_stat, fpuemu_stat_get, NULL, "%llu\n");

extern struct dentry *mips_debugfs_dir;
static int __init debugfs_fpuemu(void)
{
        struct dentry *d, *dir;

        if (!mips_debugfs_dir)
                return -ENODEV;
        dir = debugfs_create_dir("fpuemustats", mips_debugfs_dir);
        if (!dir)
                return -ENOMEM;

#define FPU_STAT_CREATE(M)                                              \
        do {                                                            \
                d = debugfs_create_file(#M , S_IRUGO, dir,              \
                        (void *)offsetof(struct mips_fpu_emulator_stats, M), \
                        &fops_fpuemu_stat);                             \
                if (!d)                                                 \
                        return -ENOMEM;                                 \
        } while (0)

        FPU_STAT_CREATE(emulated);
        FPU_STAT_CREATE(loads);
        FPU_STAT_CREATE(stores);
        FPU_STAT_CREATE(cp1ops);
        FPU_STAT_CREATE(cp1xops);
        FPU_STAT_CREATE(errors);

        return 0;
}
__initcall(debugfs_fpuemu);
#endif