Quelle  math.c   Sprache: C

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (C) 1999  Eddie C. Dost  (ecd@atecom.com)
 */

#include <linux/types.h>
#include <linux/sched.h>

#include <linux/uaccess.h>
#include <asm/reg.h>
#include <asm/switch_to.h>

#include <asm/sfp-machine.h>
#include <math-emu/double.h>

#define FLOATFUNC(x) extern int x(void *, void *, void *, void *)

/* The instructions list which may be not implemented by a hardware FPU */
FLOATFUNC(fre);
FLOATFUNC(frsqrtes);
FLOATFUNC(fsqrt);
FLOATFUNC(fsqrts);
FLOATFUNC(mtfsf);
FLOATFUNC(mtfsfi);

#ifdef CONFIG_MATH_EMULATION_HW_UNIMPLEMENTED
#undef FLOATFUNC
#define FLOATFUNC(x) static inline int x(void *op1, void *op2, void *op3, \
       void *op4) { return 0; }
#endif

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(lfd);
FLOATFUNC(lfs);

FLOATFUNC(stfd);
FLOATFUNC(stfs);
FLOATFUNC(stfiwx);

FLOATFUNC(fabs);
FLOATFUNC(fmr);
FLOATFUNC(fnabs);
FLOATFUNC(fneg);

/* Optional */
FLOATFUNC(fres);
FLOATFUNC(frsqrte);
FLOATFUNC(fsel);


#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 FRSQRTES 0x01a  /*   26 */
#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 FRE  0x018  /*   24 */
#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

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;
  if (eflag & EFLAG_INVALID)
   fpscr |= FPSCR_VX;
  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;
 }

// fpscr &= ~(FPSCR_VX);
 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;
}

int
do_mathemu(struct pt_regs *regs)
{
 void *op0 = NULL, *op1 = NULL, *op2 = NULL, *op3 = NULL;
 unsigned long pc = regs->nip;
 signed short sdisp;
 u32 insn = 0;
 int idx = 0;
 int (*func)(void *, void *, void *, void *);
 int type = 0;
 int eflag, trap;

 if (get_user(insn, (u32 __user *)pc))
  return -EFAULT;

 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 = XB; break;
  case FRES: func = fres; type = XB; break;
  case FMULS: func = fmuls; type = AC; break;
  case FRSQRTES: func = frsqrtes;type = XB; 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 = XB; break;
   case FRE: func = fre; type = XB; break;
   case FSEL: func = fsel; type = ABC; break;
   case FMUL: func = fmul; type = AC; break;
   case FRSQRTE: func = frsqrte; type = XB; 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:
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f);
  op2 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f);
  break;

 case AC:
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f);
  op2 = (void *)¤t->thread.TS_FPR((insn >>  6) & 0x1f);
  break;

 case ABC:
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f);
  op2 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f);
  op3 = (void *)¤t->thread.TS_FPR((insn >>  6) & 0x1f);
  break;

 case D:
  idx = (insn >> 16) & 0x1f;
  sdisp = (insn & 0xffff);
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp);
  break;

 case DU:
  idx = (insn >> 16) & 0x1f;
  if (!idx)
   goto illegal;

  sdisp = (insn & 0xffff);
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)(regs->gpr[idx] + sdisp);
  break;

 case X:
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  break;

 case XA:
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f);
  break;

 case XB:
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f);
  break;

 case XE:
  idx = (insn >> 16) & 0x1f;
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)((idx ? regs->gpr[idx] : 0)
    + regs->gpr[(insn >> 11) & 0x1f]);
  break;

 case XEU:
  idx = (insn >> 16) & 0x1f;
  if (!idx)
   goto illegal;
  op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f);
  op1 = (void *)(regs->gpr[idx]
    + regs->gpr[(insn >> 11) & 0x1f]);
  break;

 case XCR:
  op0 = (void *)®s->ccr;
  op1 = (void *)(long)((insn >> 23) & 0x7);
  op2 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f);
  op3 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f);
  break;

 case XCRL:
  op0 = (void *)®s->ccr;
  op1 = (void *)(long)((insn >> 23) & 0x7);
  op2 = (void *)(long)((insn >> 18) & 0x7);
  break;

 case XCRB:
  op0 = (void *)(long)((insn >> 21) & 0x1f);
  break;

 case XCRI:
  op0 = (void *)(long)((insn >> 23) & 0x7);
  op1 = (void *)(long)((insn >> 12) & 0xf);
  break;

 case XFLB:
  op0 = (void *)(long)((insn >> 17) & 0xff);
  op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f);
  break;

 default:
  goto illegal;
 }

 /*
 * If we support a HW FPU, we need to ensure the FP state
 * is flushed into the thread_struct before attempting
 * emulation
 */
 flush_fp_to_thread(current);

 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;
 }

 regs_add_return_ip(regs, 4);
 return 0;

illegal:
 return -ENOSYS;
}