2005-04-17 02:20:36 +04:00
/*
* Copyright ( C ) 1999 Eddie C . Dost ( ecd @ atecom . com )
*/
# include <linux/types.h>
# include <linux/sched.h>
# include <asm/uaccess.h>
# include <asm/reg.h>
2008-06-04 11:59:29 +04:00
# include <asm/sfp-machine.h>
# include <math-emu/double.h>
2005-04-17 02:20:36 +04:00
# define FLOATFUNC(x) extern int x(void *, void *, void *, void *)
FLOATFUNC ( fadd ) ;
FLOATFUNC ( fadds ) ;
FLOATFUNC ( fdiv ) ;
FLOATFUNC ( fdivs ) ;
FLOATFUNC ( fmul ) ;
FLOATFUNC ( fmuls ) ;
FLOATFUNC ( fsub ) ;
FLOATFUNC ( fsubs ) ;
FLOATFUNC ( fmadd ) ;
FLOATFUNC ( fmadds ) ;
FLOATFUNC ( fmsub ) ;
FLOATFUNC ( fmsubs ) ;
FLOATFUNC ( fnmadd ) ;
FLOATFUNC ( fnmadds ) ;
FLOATFUNC ( fnmsub ) ;
FLOATFUNC ( fnmsubs ) ;
FLOATFUNC ( fctiw ) ;
FLOATFUNC ( fctiwz ) ;
FLOATFUNC ( frsp ) ;
FLOATFUNC ( fcmpo ) ;
FLOATFUNC ( fcmpu ) ;
FLOATFUNC ( mcrfs ) ;
FLOATFUNC ( mffs ) ;
FLOATFUNC ( mtfsb0 ) ;
FLOATFUNC ( mtfsb1 ) ;
FLOATFUNC ( mtfsf ) ;
FLOATFUNC ( mtfsfi ) ;
FLOATFUNC ( lfd ) ;
FLOATFUNC ( lfs ) ;
FLOATFUNC ( stfd ) ;
FLOATFUNC ( stfs ) ;
FLOATFUNC ( stfiwx ) ;
FLOATFUNC ( fabs ) ;
FLOATFUNC ( fmr ) ;
FLOATFUNC ( fnabs ) ;
FLOATFUNC ( fneg ) ;
/* Optional */
FLOATFUNC ( fres ) ;
FLOATFUNC ( frsqrte ) ;
FLOATFUNC ( fsel ) ;
FLOATFUNC ( fsqrt ) ;
FLOATFUNC ( fsqrts ) ;
# define OP31 0x1f /* 31 */
# define LFS 0x30 /* 48 */
# define LFSU 0x31 /* 49 */
# define LFD 0x32 /* 50 */
# define LFDU 0x33 /* 51 */
# define STFS 0x34 /* 52 */
# define STFSU 0x35 /* 53 */
# define STFD 0x36 /* 54 */
# define STFDU 0x37 /* 55 */
# define OP59 0x3b /* 59 */
# define OP63 0x3f /* 63 */
/* Opcode 31: */
/* X-Form: */
# define LFSX 0x217 /* 535 */
# define LFSUX 0x237 /* 567 */
# define LFDX 0x257 /* 599 */
# define LFDUX 0x277 /* 631 */
# define STFSX 0x297 /* 663 */
# define STFSUX 0x2b7 /* 695 */
# define STFDX 0x2d7 /* 727 */
# define STFDUX 0x2f7 /* 759 */
# define STFIWX 0x3d7 /* 983 */
/* Opcode 59: */
/* A-Form: */
# define FDIVS 0x012 /* 18 */
# define FSUBS 0x014 /* 20 */
# define FADDS 0x015 /* 21 */
# define FSQRTS 0x016 /* 22 */
# define FRES 0x018 /* 24 */
# define FMULS 0x019 /* 25 */
# define FMSUBS 0x01c /* 28 */
# define FMADDS 0x01d /* 29 */
# define FNMSUBS 0x01e /* 30 */
# define FNMADDS 0x01f /* 31 */
/* Opcode 63: */
/* A-Form: */
# define FDIV 0x012 /* 18 */
# define FSUB 0x014 /* 20 */
# define FADD 0x015 /* 21 */
# define FSQRT 0x016 /* 22 */
# define FSEL 0x017 /* 23 */
# define FMUL 0x019 /* 25 */
# define FRSQRTE 0x01a /* 26 */
# define FMSUB 0x01c /* 28 */
# define FMADD 0x01d /* 29 */
# define FNMSUB 0x01e /* 30 */
# define FNMADD 0x01f /* 31 */
/* X-Form: */
# define FCMPU 0x000 /* 0 */
# define FRSP 0x00c /* 12 */
# define FCTIW 0x00e /* 14 */
# define FCTIWZ 0x00f /* 15 */
# define FCMPO 0x020 /* 32 */
# define MTFSB1 0x026 /* 38 */
# define FNEG 0x028 /* 40 */
# define MCRFS 0x040 /* 64 */
# define MTFSB0 0x046 /* 70 */
# define FMR 0x048 /* 72 */
# define MTFSFI 0x086 /* 134 */
# define FNABS 0x088 /* 136 */
# define FABS 0x108 /* 264 */
# define MFFS 0x247 /* 583 */
# define MTFSF 0x2c7 /* 711 */
# define AB 2
# define AC 3
# define ABC 4
# define D 5
# define DU 6
# define X 7
# define XA 8
# define XB 9
# define XCR 11
# define XCRB 12
# define XCRI 13
# define XCRL 16
# define XE 14
# define XEU 15
# define XFLB 10
# ifdef CONFIG_MATH_EMULATION
static int
record_exception ( struct pt_regs * regs , int eflag )
{
u32 fpscr ;
fpscr = __FPU_FPSCR ;
if ( eflag ) {
fpscr | = FPSCR_FX ;
if ( eflag & EFLAG_OVERFLOW )
fpscr | = FPSCR_OX ;
if ( eflag & EFLAG_UNDERFLOW )
fpscr | = FPSCR_UX ;
if ( eflag & EFLAG_DIVZERO )
fpscr | = FPSCR_ZX ;
if ( eflag & EFLAG_INEXACT )
fpscr | = FPSCR_XX ;
2008-06-04 11:59:29 +04:00
if ( eflag & EFLAG_INVALID )
fpscr | = FPSCR_VX ;
2005-04-17 02:20:36 +04:00
if ( eflag & EFLAG_VXSNAN )
fpscr | = FPSCR_VXSNAN ;
if ( eflag & EFLAG_VXISI )
fpscr | = FPSCR_VXISI ;
if ( eflag & EFLAG_VXIDI )
fpscr | = FPSCR_VXIDI ;
if ( eflag & EFLAG_VXZDZ )
fpscr | = FPSCR_VXZDZ ;
if ( eflag & EFLAG_VXIMZ )
fpscr | = FPSCR_VXIMZ ;
if ( eflag & EFLAG_VXVC )
fpscr | = FPSCR_VXVC ;
if ( eflag & EFLAG_VXSOFT )
fpscr | = FPSCR_VXSOFT ;
if ( eflag & EFLAG_VXSQRT )
fpscr | = FPSCR_VXSQRT ;
if ( eflag & EFLAG_VXCVI )
fpscr | = FPSCR_VXCVI ;
}
2008-06-04 11:59:29 +04:00
// fpscr &= ~(FPSCR_VX);
2005-04-17 02:20:36 +04:00
if ( fpscr & ( FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI |
FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC |
FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI ) )
fpscr | = FPSCR_VX ;
fpscr & = ~ ( FPSCR_FEX ) ;
if ( ( ( fpscr & FPSCR_VX ) & & ( fpscr & FPSCR_VE ) ) | |
( ( fpscr & FPSCR_OX ) & & ( fpscr & FPSCR_OE ) ) | |
( ( fpscr & FPSCR_UX ) & & ( fpscr & FPSCR_UE ) ) | |
( ( fpscr & FPSCR_ZX ) & & ( fpscr & FPSCR_ZE ) ) | |
( ( fpscr & FPSCR_XX ) & & ( fpscr & FPSCR_XE ) ) )
fpscr | = FPSCR_FEX ;
__FPU_FPSCR = fpscr ;
return ( fpscr & FPSCR_FEX ) ? 1 : 0 ;
}
# endif /* CONFIG_MATH_EMULATION */
int
do_mathemu ( struct pt_regs * regs )
{
void * op0 = 0 , * op1 = 0 , * op2 = 0 , * op3 = 0 ;
unsigned long pc = regs - > nip ;
signed short sdisp ;
u32 insn = 0 ;
int idx = 0 ;
# ifdef CONFIG_MATH_EMULATION
int ( * func ) ( void * , void * , void * , void * ) ;
int type = 0 ;
int eflag , trap ;
# endif
if ( get_user ( insn , ( u32 * ) pc ) )
return - EFAULT ;
# ifndef CONFIG_MATH_EMULATION
switch ( insn > > 26 ) {
case LFD :
idx = ( insn > > 16 ) & 0x1f ;
sdisp = ( insn & 0xffff ) ;
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
op1 = ( void * ) ( ( idx ? regs - > gpr [ idx ] : 0 ) + sdisp ) ;
lfd ( op0 , op1 , op2 , op3 ) ;
break ;
case LFDU :
idx = ( insn > > 16 ) & 0x1f ;
sdisp = ( insn & 0xffff ) ;
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
op1 = ( void * ) ( ( idx ? regs - > gpr [ idx ] : 0 ) + sdisp ) ;
lfd ( op0 , op1 , op2 , op3 ) ;
regs - > gpr [ idx ] = ( unsigned long ) op1 ;
break ;
case STFD :
idx = ( insn > > 16 ) & 0x1f ;
sdisp = ( insn & 0xffff ) ;
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
op1 = ( void * ) ( ( idx ? regs - > gpr [ idx ] : 0 ) + sdisp ) ;
stfd ( op0 , op1 , op2 , op3 ) ;
break ;
case STFDU :
idx = ( insn > > 16 ) & 0x1f ;
sdisp = ( insn & 0xffff ) ;
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
op1 = ( void * ) ( ( idx ? regs - > gpr [ idx ] : 0 ) + sdisp ) ;
stfd ( op0 , op1 , op2 , op3 ) ;
regs - > gpr [ idx ] = ( unsigned long ) op1 ;
break ;
case OP63 :
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
op1 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 11 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
fmr ( op0 , op1 , op2 , op3 ) ;
break ;
default :
goto illegal ;
}
# else /* CONFIG_MATH_EMULATION */
switch ( insn > > 26 ) {
case LFS : func = lfs ; type = D ; break ;
case LFSU : func = lfs ; type = DU ; break ;
case LFD : func = lfd ; type = D ; break ;
case LFDU : func = lfd ; type = DU ; break ;
case STFS : func = stfs ; type = D ; break ;
case STFSU : func = stfs ; type = DU ; break ;
case STFD : func = stfd ; type = D ; break ;
case STFDU : func = stfd ; type = DU ; break ;
case OP31 :
switch ( ( insn > > 1 ) & 0x3ff ) {
case LFSX : func = lfs ; type = XE ; break ;
case LFSUX : func = lfs ; type = XEU ; break ;
case LFDX : func = lfd ; type = XE ; break ;
case LFDUX : func = lfd ; type = XEU ; break ;
case STFSX : func = stfs ; type = XE ; break ;
case STFSUX : func = stfs ; type = XEU ; break ;
case STFDX : func = stfd ; type = XE ; break ;
case STFDUX : func = stfd ; type = XEU ; break ;
case STFIWX : func = stfiwx ; type = XE ; break ;
default :
goto illegal ;
}
break ;
case OP59 :
switch ( ( insn > > 1 ) & 0x1f ) {
case FDIVS : func = fdivs ; type = AB ; break ;
case FSUBS : func = fsubs ; type = AB ; break ;
case FADDS : func = fadds ; type = AB ; break ;
case FSQRTS : func = fsqrts ; type = AB ; break ;
case FRES : func = fres ; type = AB ; break ;
case FMULS : func = fmuls ; type = AC ; break ;
case FMSUBS : func = fmsubs ; type = ABC ; break ;
case FMADDS : func = fmadds ; type = ABC ; break ;
case FNMSUBS : func = fnmsubs ; type = ABC ; break ;
case FNMADDS : func = fnmadds ; type = ABC ; break ;
default :
goto illegal ;
}
break ;
case OP63 :
if ( insn & 0x20 ) {
switch ( ( insn > > 1 ) & 0x1f ) {
case FDIV : func = fdiv ; type = AB ; break ;
case FSUB : func = fsub ; type = AB ; break ;
case FADD : func = fadd ; type = AB ; break ;
case FSQRT : func = fsqrt ; type = AB ; break ;
case FSEL : func = fsel ; type = ABC ; break ;
case FMUL : func = fmul ; type = AC ; break ;
case FRSQRTE : func = frsqrte ; type = AB ; break ;
case FMSUB : func = fmsub ; type = ABC ; break ;
case FMADD : func = fmadd ; type = ABC ; break ;
case FNMSUB : func = fnmsub ; type = ABC ; break ;
case FNMADD : func = fnmadd ; type = ABC ; break ;
default :
goto illegal ;
}
break ;
}
switch ( ( insn > > 1 ) & 0x3ff ) {
case FCMPU : func = fcmpu ; type = XCR ; break ;
case FRSP : func = frsp ; type = XB ; break ;
case FCTIW : func = fctiw ; type = XB ; break ;
case FCTIWZ : func = fctiwz ; type = XB ; break ;
case FCMPO : func = fcmpo ; type = XCR ; break ;
case MTFSB1 : func = mtfsb1 ; type = XCRB ; break ;
case FNEG : func = fneg ; type = XB ; break ;
case MCRFS : func = mcrfs ; type = XCRL ; break ;
case MTFSB0 : func = mtfsb0 ; type = XCRB ; break ;
case FMR : func = fmr ; type = XB ; break ;
case MTFSFI : func = mtfsfi ; type = XCRI ; break ;
case FNABS : func = fnabs ; type = XB ; break ;
case FABS : func = fabs ; type = XB ; break ;
case MFFS : func = mffs ; type = X ; break ;
case MTFSF : func = mtfsf ; type = XFLB ; break ;
default :
goto illegal ;
}
break ;
default :
goto illegal ;
}
switch ( type ) {
case AB :
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
op1 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 16 ) & 0x1f ) ;
op2 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 11 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
break ;
case AC :
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
op1 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 16 ) & 0x1f ) ;
op2 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 6 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
break ;
case ABC :
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
op1 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 16 ) & 0x1f ) ;
op2 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 11 ) & 0x1f ) ;
op3 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 6 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
break ;
case D :
idx = ( insn > > 16 ) & 0x1f ;
sdisp = ( insn & 0xffff ) ;
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
op1 = ( void * ) ( ( idx ? regs - > gpr [ idx ] : 0 ) + sdisp ) ;
break ;
case DU :
idx = ( insn > > 16 ) & 0x1f ;
if ( ! idx )
goto illegal ;
sdisp = ( insn & 0xffff ) ;
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
op1 = ( void * ) ( regs - > gpr [ idx ] + sdisp ) ;
break ;
case X :
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
break ;
case XA :
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
op1 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 16 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
break ;
case XB :
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
op1 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 11 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
break ;
case XE :
idx = ( insn > > 16 ) & 0x1f ;
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2007-10-12 02:07:34 +04:00
if ( ! idx ) {
if ( ( ( insn > > 1 ) & 0x3ff ) = = STFIWX )
op1 = ( void * ) ( regs - > gpr [ ( insn > > 11 ) & 0x1f ] ) ;
else
goto illegal ;
} else {
op1 = ( void * ) ( regs - > gpr [ idx ] + regs - > gpr [ ( insn > > 11 ) & 0x1f ] ) ;
}
2005-04-17 02:20:36 +04:00
break ;
case XEU :
idx = ( insn > > 16 ) & 0x1f ;
2008-06-26 11:07:48 +04:00
op0 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 21 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
op1 = ( void * ) ( ( idx ? regs - > gpr [ idx ] : 0 )
+ regs - > gpr [ ( insn > > 11 ) & 0x1f ] ) ;
break ;
case XCR :
op0 = ( void * ) & regs - > ccr ;
op1 = ( void * ) ( ( insn > > 23 ) & 0x7 ) ;
2008-06-26 11:07:48 +04:00
op2 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 16 ) & 0x1f ) ;
op3 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 11 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
break ;
case XCRL :
op0 = ( void * ) & regs - > ccr ;
op1 = ( void * ) ( ( insn > > 23 ) & 0x7 ) ;
op2 = ( void * ) ( ( insn > > 18 ) & 0x7 ) ;
break ;
case XCRB :
op0 = ( void * ) ( ( insn > > 21 ) & 0x1f ) ;
break ;
case XCRI :
op0 = ( void * ) ( ( insn > > 23 ) & 0x7 ) ;
op1 = ( void * ) ( ( insn > > 12 ) & 0xf ) ;
break ;
case XFLB :
op0 = ( void * ) ( ( insn > > 17 ) & 0xff ) ;
2008-06-26 11:07:48 +04:00
op1 = ( void * ) & current - > thread . TS_FPR ( ( insn > > 11 ) & 0x1f ) ;
2005-04-17 02:20:36 +04:00
break ;
default :
goto illegal ;
}
eflag = func ( op0 , op1 , op2 , op3 ) ;
if ( insn & 1 ) {
regs - > ccr & = ~ ( 0x0f000000 ) ;
regs - > ccr | = ( __FPU_FPSCR > > 4 ) & 0x0f000000 ;
}
trap = record_exception ( regs , eflag ) ;
if ( trap )
return 1 ;
switch ( type ) {
case DU :
case XEU :
regs - > gpr [ idx ] = ( unsigned long ) op1 ;
break ;
default :
break ;
}
# endif /* CONFIG_MATH_EMULATION */
regs - > nip + = 4 ;
return 0 ;
illegal :
return - ENOSYS ;
}