#include <linux/types.h>
#include <asm/uaccess.h>
#include <asm/reg.h>
#define FP_EX_BOOKE_E500_SPE
#include <asm/sfp-machine.h>
#include <math-emu/soft-fp.h>
#include <math-emu/single.h>
#include <math-emu/double.h>
#define EFAPU 0x4
#define VCT 0x4
#define SPFP 0x6
#define DPFP 0x7
#define EFSADD 0x2c0
#define EFSSUB 0x2c1
#define EFSABS 0x2c4
#define EFSNABS 0x2c5
#define EFSNEG 0x2c6
#define EFSMUL 0x2c8
#define EFSDIV 0x2c9
#define EFSCMPGT 0x2cc
#define EFSCMPLT 0x2cd
#define EFSCMPEQ 0x2ce
#define EFSCFD 0x2cf
#define EFSCFSI 0x2d1
#define EFSCTUI 0x2d4
#define EFSCTSI 0x2d5
#define EFSCTUF 0x2d6
#define EFSCTSF 0x2d7
#define EFSCTUIZ 0x2d8
#define EFSCTSIZ 0x2da
#define EVFSADD 0x280
#define EVFSSUB 0x281
#define EVFSABS 0x284
#define EVFSNABS 0x285
#define EVFSNEG 0x286
#define EVFSMUL 0x288
#define EVFSDIV 0x289
#define EVFSCMPGT 0x28c
#define EVFSCMPLT 0x28d
#define EVFSCMPEQ 0x28e
#define EVFSCTUI 0x294
#define EVFSCTSI 0x295
#define EVFSCTUF 0x296
#define EVFSCTSF 0x297
#define EVFSCTUIZ 0x298
#define EVFSCTSIZ 0x29a
#define EFDADD 0x2e0
#define EFDSUB 0x2e1
#define EFDABS 0x2e4
#define EFDNABS 0x2e5
#define EFDNEG 0x2e6
#define EFDMUL 0x2e8
#define EFDDIV 0x2e9
#define EFDCTUIDZ 0x2ea
#define EFDCTSIDZ 0x2eb
#define EFDCMPGT 0x2ec
#define EFDCMPLT 0x2ed
#define EFDCMPEQ 0x2ee
#define EFDCFS 0x2ef
#define EFDCTUI 0x2f4
#define EFDCTSI 0x2f5
#define EFDCTUF 0x2f6
#define EFDCTSF 0x2f7
#define EFDCTUIZ 0x2f8
#define EFDCTSIZ 0x2fa
#define AB 2
#define XA 3
#define XB 4
#define XCR 5
#define NOTYPE 0
#define SIGN_BIT_S (1UL << 31)
#define SIGN_BIT_D (1ULL << 63)
#define FP_EX_MASK (FP_EX_INEXACT | FP_EX_INVALID | FP_EX_DIVZERO | \
FP_EX_UNDERFLOW | FP_EX_OVERFLOW)
static int have_e500_cpu_a005_erratum;
union dw_union {
u64 dp[1];
u32 wp[2];
};
static unsigned long insn_type(unsigned long speinsn)
{
unsigned long ret = NOTYPE;
switch (speinsn & 0x7ff) {
case EFSABS: ret = XA; break;
case EFSADD: ret = AB; break;
case EFSCFD: ret = XB; break;
case EFSCMPEQ: ret = XCR; break;
case EFSCMPGT: ret = XCR; break;
case EFSCMPLT: ret = XCR; break;
case EFSCTSF: ret = XB; break;
case EFSCTSI: ret = XB; break;
case EFSCTSIZ: ret = XB; break;
case EFSCTUF: ret = XB; break;
case EFSCTUI: ret = XB; break;
case EFSCTUIZ: ret = XB; break;
case EFSDIV: ret = AB; break;
case EFSMUL: ret = AB; break;
case EFSNABS: ret = XA; break;
case EFSNEG: ret = XA; break;
case EFSSUB: ret = AB; break;
case EFSCFSI: ret = XB; break;
case EVFSABS: ret = XA; break;
case EVFSADD: ret = AB; break;
case EVFSCMPEQ: ret = XCR; break;
case EVFSCMPGT: ret = XCR; break;
case EVFSCMPLT: ret = XCR; break;
case EVFSCTSF: ret = XB; break;
case EVFSCTSI: ret = XB; break;
case EVFSCTSIZ: ret = XB; break;
case EVFSCTUF: ret = XB; break;
case EVFSCTUI: ret = XB; break;
case EVFSCTUIZ: ret = XB; break;
case EVFSDIV: ret = AB; break;
case EVFSMUL: ret = AB; break;
case EVFSNABS: ret = XA; break;
case EVFSNEG: ret = XA; break;
case EVFSSUB: ret = AB; break;
case EFDABS: ret = XA; break;
case EFDADD: ret = AB; break;
case EFDCFS: ret = XB; break;
case EFDCMPEQ: ret = XCR; break;
case EFDCMPGT: ret = XCR; break;
case EFDCMPLT: ret = XCR; break;
case EFDCTSF: ret = XB; break;
case EFDCTSI: ret = XB; break;
case EFDCTSIDZ: ret = XB; break;
case EFDCTSIZ: ret = XB; break;
case EFDCTUF: ret = XB; break;
case EFDCTUI: ret = XB; break;
case EFDCTUIDZ: ret = XB; break;
case EFDCTUIZ: ret = XB; break;
case EFDDIV: ret = AB; break;
case EFDMUL: ret = AB; break;
case EFDNABS: ret = XA; break;
case EFDNEG: ret = XA; break;
case EFDSUB: ret = AB; break;
}
return ret;
}
int do_spe_mathemu(struct pt_regs *regs)
{
FP_DECL_EX;
int IR, cmp;
unsigned long type, func, fc, fa, fb, src, speinsn;
union dw_union vc, va, vb;
if (get_user(speinsn, (unsigned int __user *) regs->nip))
return -EFAULT;
if ((speinsn >> 26) != EFAPU)
return -EINVAL; /* not an spe instruction */
type = insn_type(speinsn);
if (type == NOTYPE)
goto illegal;
func = speinsn & 0x7ff;
fc = (speinsn >> 21) & 0x1f;
fa = (speinsn >> 16) & 0x1f;
fb = (speinsn >> 11) & 0x1f;
src = (speinsn >> 5) & 0x7;
vc.wp[0] = current->thread.evr[fc];
vc.wp[1] = regs->gpr[fc];
va.wp[0] = current->thread.evr[fa];
va.wp[1] = regs->gpr[fa];
vb.wp[0] = current->thread.evr[fb];
vb.wp[1] = regs->gpr[fb];
__FPU_FPSCR = mfspr(SPRN_SPEFSCR);
pr_debug("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
pr_debug("vc: %08x %08x\n", vc.wp[0], vc.wp[1]);
pr_debug("va: %08x %08x\n", va.wp[0], va.wp[1]);
pr_debug("vb: %08x %08x\n", vb.wp[0], vb.wp[1]);
switch (src) {
case SPFP: {
FP_DECL_S(SA); FP_DECL_S(SB); FP_DECL_S(SR);
switch (type) {
case AB:
case XCR:
FP_UNPACK_SP(SA, va.wp + 1);
case XB:
FP_UNPACK_SP(SB, vb.wp + 1);
break;
case XA:
FP_UNPACK_SP(SA, va.wp + 1);
break;
}
pr_debug("SA: %ld %08lx %ld (%ld)\n", SA_s, SA_f, SA_e, SA_c);
pr_debug("SB: %ld %08lx %ld (%ld)\n", SB_s, SB_f, SB_e, SB_c);
switch (func) {
case EFSABS:
vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
goto update_regs;
case EFSNABS:
vc.wp[1] = va.wp[1] | SIGN_BIT_S;
goto update_regs;
case EFSNEG:
vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
goto update_regs;
case EFSADD:
FP_ADD_S(SR, SA, SB);
goto pack_s;
case EFSSUB:
FP_SUB_S(SR, SA, SB);
goto pack_s;
case EFSMUL:
FP_MUL_S(SR, SA, SB);
goto pack_s;
case EFSDIV:
FP_DIV_S(SR, SA, SB);
goto pack_s;
case EFSCMPEQ:
cmp = 0;
goto cmp_s;
case EFSCMPGT:
cmp = 1;
goto cmp_s;
case EFSCMPLT:
cmp = -1;
goto cmp_s;
case EFSCTSF:
case EFSCTUF:
if (!((vb.wp[1] >> 23) == 0xff && ((vb.wp[1] & 0x7fffff) > 0))) {
/* NaN */
if (((vb.wp[1] >> 23) & 0xff) == 0) {
/* denorm */
vc.wp[1] = 0x0;
} else if ((vb.wp[1] >> 31) == 0) {
/* positive normal */
vc.wp[1] = (func == EFSCTSF) ?
0x7fffffff : 0xffffffff;
} else { /* negative normal */
vc.wp[1] = (func == EFSCTSF) ?
0x80000000 : 0x0;
}
} else { /* rB is NaN */
vc.wp[1] = 0x0;
}
goto update_regs;
case EFSCFD: {
FP_DECL_D(DB);
FP_CLEAR_EXCEPTIONS;
FP_UNPACK_DP(DB, vb.dp);
pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
DB_s, DB_f1, DB_f0, DB_e, DB_c);
FP_CONV(S, D, 1, 2, SR, DB);
goto pack_s;
}
case EFSCTSI:
case EFSCTSIZ:
case EFSCTUI:
case EFSCTUIZ:
if (func & 0x4) {
_FP_ROUND(1, SB);
} else {
_FP_ROUND_ZERO(1, SB);
}
FP_TO_INT_S(vc.wp[1], SB, 32,
(((func & 0x3) != 0) || SB_s));
goto update_regs;
default:
goto illegal;
}
break;
pack_s:
pr_debug("SR: %ld %08lx %ld (%ld)\n", SR_s, SR_f, SR_e, SR_c);
FP_PACK_SP(vc.wp + 1, SR);
goto update_regs;
cmp_s:
FP_CMP_S(IR, SA, SB, 3);
if (IR == 3 && (FP_ISSIGNAN_S(SA) || FP_ISSIGNAN_S(SB)))
FP_SET_EXCEPTION(FP_EX_INVALID);
if (IR == cmp) {
IR = 0x4;
} else {
IR = 0;
}
goto update_ccr;
}
case DPFP: {
FP_DECL_D(DA); FP_DECL_D(DB); FP_DECL_D(DR);
switch (type) {
case AB:
case XCR:
FP_UNPACK_DP(DA, va.dp);
case XB:
FP_UNPACK_DP(DB, vb.dp);
break;
case XA:
FP_UNPACK_DP(DA, va.dp);
break;
}
pr_debug("DA: %ld %08lx %08lx %ld (%ld)\n",
DA_s, DA_f1, DA_f0, DA_e, DA_c);
pr_debug("DB: %ld %08lx %08lx %ld (%ld)\n",
DB_s, DB_f1, DB_f0, DB_e, DB_c);
switch (func) {
case EFDABS:
vc.dp[0] = va.dp[0] & ~SIGN_BIT_D;
goto update_regs;
case EFDNABS:
vc.dp[0] = va.dp[0] | SIGN_BIT_D;
goto update_regs;
case EFDNEG:
vc.dp[0] = va.dp[0] ^ SIGN_BIT_D;
goto update_regs;
case EFDADD:
FP_ADD_D(DR, DA, DB);
goto pack_d;
case EFDSUB:
FP_SUB_D(DR, DA, DB);
goto pack_d;
case EFDMUL:
FP_MUL_D(DR, DA, DB);
goto pack_d;
case EFDDIV:
FP_DIV_D(DR, DA, DB);
goto pack_d;
case EFDCMPEQ:
cmp = 0;
goto cmp_d;
case EFDCMPGT:
cmp = 1;
goto cmp_d;
case EFDCMPL
