arch/sh/math-emu/math.c

   1 /*
   2  * arch/sh/math-emu/math.c
   3  *
   4  * Copyright (C) 2006 Takashi YOSHII <[email protected]>
   5  *
   6  * This file is subject to the terms and conditions of the GNU General Public
   7  * License.  See the file "COPYING" in the main directory of this archive
   8  * for more details.
   9  */
  10 #include <linux/kernel.h>
  11 #include <linux/errno.h>
  12 #include <linux/types.h>
  13 #include <linux/sched/signal.h>
  14 #include <linux/signal.h>
  15 #include <linux/perf_event.h>
  16
  17 #include <linux/uaccess.h>
  18 #include <asm/processor.h>
  19 #include <asm/io.h>
  20
  21 #include "sfp-util.h"
  22 #include <math-emu/soft-fp.h>
  23 #include <math-emu/single.h>
  24 #include <math-emu/double.h>
  25
  26 #define FPUL            (fregs->fpul)
  27 #define FPSCR           (fregs->fpscr)
  28 #define FPSCR_RM        (FPSCR&3)
  29 #define FPSCR_DN        ((FPSCR>>18)&1)
  30 #define FPSCR_PR        ((FPSCR>>19)&1)
  31 #define FPSCR_SZ        ((FPSCR>>20)&1)
  32 #define FPSCR_FR        ((FPSCR>>21)&1)
  33 #define FPSCR_MASK      0x003fffffUL
  34
  35 #define BANK(n) (n^(FPSCR_FR?16:0))
  36 #define FR      ((unsigned long*)(fregs->fp_regs))
  37 #define FR0     (FR[BANK(0)])
  38 #define FRn     (FR[BANK(n)])
  39 #define FRm     (FR[BANK(m)])
  40 #define DR      ((unsigned long long*)(fregs->fp_regs))
  41 #define DRn     (DR[BANK(n)/2])
  42 #define DRm     (DR[BANK(m)/2])
  43
  44 #define XREG(n) (n^16)
  45 #define XFn     (FR[BANK(XREG(n))])
  46 #define XFm     (FR[BANK(XREG(m))])
  47 #define XDn     (DR[BANK(XREG(n))/2])
  48 #define XDm     (DR[BANK(XREG(m))/2])
  49
  50 #define R0      (regs->regs[0])
  51 #define Rn      (regs->regs[n])
  52 #define Rm      (regs->regs[m])
  53
  54 #define MWRITE(d,a)     ({if(put_user(d, (typeof (d) __user *)a)) return -EFAULT;})
  55 #define MREAD(d,a)      ({if(get_user(d, (typeof (d) __user *)a)) return -EFAULT;})
  56
  57 #define PACK_S(r,f)     FP_PACK_SP(&r,f)
  58 #define UNPACK_S(f,r)   FP_UNPACK_SP(f,&r)
  59 #define PACK_D(r,f) \
  60         {u32 t[2]; FP_PACK_DP(t,f); ((u32*)&r)[0]=t[1]; ((u32*)&r)[1]=t[0];}
  61 #define UNPACK_D(f,r) \
  62         {u32 t[2]; t[0]=((u32*)&r)[1]; t[1]=((u32*)&r)[0]; FP_UNPACK_DP(f,t);}
  63
  64 // 2 args instructions.
  65 #define BOTH_PRmn(op,x) \
  66         FP_DECL_EX; if(FPSCR_PR) op(D,x,DRm,DRn); else op(S,x,FRm,FRn);
  67
  68 #define CMP_X(SZ,R,M,N) do{ \
  69         FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); \
  70         UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \
  71         FP_CMP_##SZ(R, Fn, Fm, 2); }while(0)
  72 #define EQ_X(SZ,R,M,N) do{ \
  73         FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); \
  74         UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \
  75         FP_CMP_EQ_##SZ(R, Fn, Fm); }while(0)
  76 #define CMP(OP) ({ int r; BOTH_PRmn(OP##_X,r); r; })
  77
  78 static int
  79 fcmp_gt(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  80 {
  81         if (CMP(CMP) > 0)
  82                 regs->sr |= 1;
  83         else
  84                 regs->sr &= ~1;
  85
  86         return 0;
  87 }
  88
  89 static int
  90 fcmp_eq(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
  91 {
  92         if (CMP(CMP /*EQ*/) == 0)
  93                 regs->sr |= 1;
  94         else
  95                 regs->sr &= ~1;
  96         return 0;
  97 }
  98
  99 #define ARITH_X(SZ,OP,M,N) do{ \
 100         FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); FP_DECL_##SZ(Fr); \
 101         UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \
 102         FP_##OP##_##SZ(Fr, Fn, Fm); \
 103         PACK_##SZ(N, Fr); }while(0)
 104
 105 static int
 106 fadd(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
 107 {
 108         BOTH_PRmn(ARITH_X, ADD);
 109         return 0;
 110 }
 111
 112 static int
 113 fsub(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
 114 {
 115         BOTH_PRmn(ARITH_X, SUB);
 116         return 0;
 117 }
 118
 119 static int
 120 fmul(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
 121 {
 122         BOTH_PRmn(ARITH_X, MUL);
 123         return 0;
 124 }
 125
 126 static int
 127 fdiv(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
 128 {
 129         BOTH_PRmn(ARITH_X, DIV);
 130         return 0;
 131 }
 132
 133 static int
 134 fmac(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
 135 {
 136         FP_DECL_EX;
 137         FP_DECL_S(Fr);
 138         FP_DECL_S(Ft);
 139         FP_DECL_S(F0);
 140         FP_DECL_S(Fm);
 141         FP_DECL_S(Fn);
 142         UNPACK_S(F0, FR0);
 143         UNPACK_S(Fm, FRm);
 144         UNPACK_S(Fn, FRn);
 145         FP_MUL_S(Ft, Fm, F0);
 146         FP_ADD_S(Fr, Fn, Ft);
 147         PACK_S(FRn, Fr);
 148         return 0;
 149 }
 150
 151 // to process fmov's extension (odd n for DR access XD).
 152 #define FMOV_EXT(x) if(x&1) x+=16-1
 153
 154 static int
 155 fmov_idx_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
 156              int n)
 157 {
 158         if (FPSCR_SZ) {
 159                 FMOV_EXT(n);
 160                 MREAD(FRn, Rm + R0 + 4);
 161                 n++;
 162                 MREAD(FRn, Rm + R0);
 163         } else {
 164                 MREAD(FRn, Rm + R0);
 165         }
 166
 167         return 0;
 168 }
 169
 170 static int
 171 fmov_mem_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
 172              int n)
 173 {
 174         if (FPSCR_SZ) {
 175                 FMOV_EXT(n);
 176                 MREAD(FRn, Rm + 4);
 177                 n++;
 178                 MREAD(FRn, Rm);
 179         } else {
 180                 MREAD(FRn, Rm);
 181         }
 182
 183         return 0;
 184 }
 185
 186 static int
 187 fmov_inc_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
 188              int n)
 189 {
 190         if (FPSCR_SZ) {
 191                 FMOV_EXT(n);
 192                 MREAD(FRn, Rm + 4);
 193                 n++;
 194                 MREAD(FRn, Rm);
 195                 Rm += 8;
 196         } else {
 197                 MREAD(FRn, Rm);
 198                 Rm += 4;
 199         }
 200
 201         return 0;
 202 }
 203
 204 static int
 205 fmov_reg_idx(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
 206              int n)
 207 {
 208         if (FPSCR_SZ) {
 209                 FMOV_EXT(m);
 210                 MWRITE(FRm, Rn + R0 + 4);
 211                 m++;
 212                 MWRITE(FRm, Rn + R0);
 213         } else {
 214                 MWRITE(FRm, Rn + R0);
 215         }
 216
 217         return 0;
 218 }
 219
 220 static int
 221 fmov_reg_mem(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
 222              int n)
 223 {
 224         if (FPSCR_SZ) {
 225                 FMOV_EXT(m);
 226                 MWRITE(FRm, Rn + 4);
 227                 m++;
 228                 MWRITE(FRm, Rn);
 229         } else {
 230                 MWRITE(FRm, Rn);
 231         }
 232
 233         return 0;
 234 }
 235
 236 static int
 237 fmov_reg_dec(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
 238              int n)
 239 {
 240         if (FPSCR_SZ) {
 241                 FMOV_EXT(m);
 242                 Rn -= 8;
 243                 MWRITE(FRm, Rn + 4);
 244                 m++;
 245                 MWRITE(FRm, Rn);
 246         } else {
 247                 Rn -= 4;
 248                 MWRITE(FRm, Rn);
 249         }
 250
 251         return 0;
 252 }
 253
 254 static int
 255 fmov_reg_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m,
 256              int n)
 257 {
 258         if (FPSCR_SZ) {
 259                 FMOV_EXT(m);
 260                 FMOV_EXT(n);
 261                 DRn = DRm;
 262         } else {
 263                 FRn = FRm;
 264         }
 265
 266         return 0;
 267 }
 268
 269 static int
 270 fnop_mn(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n)
 271 {
 272         return -EINVAL;
 273 }
 274
 275 // 1 arg instructions.
 276 #define NOTYETn(i) static int i(struct sh_fpu_soft_struct *fregs, int n) \
 277         { printk( #i " not yet done.\n"); return 0; }
 278
 279 NOTYETn(ftrv)
 280 NOTYETn(fsqrt)
 281 NOTYETn(fipr)
 282 NOTYETn(fsca)
 283 NOTYETn(fsrra)
 284
 285 #define EMU_FLOAT_X(SZ,N) do { \
 286         FP_DECL_##SZ(Fn); \
 287         FP_FROM_INT_##SZ(Fn, FPUL, 32, int); \
 288         PACK_##SZ(N, Fn); }while(0)
 289 static int ffloat(struct sh_fpu_soft_struct *fregs, int n)
 290 {
 291         FP_DECL_EX;
 292
 293         if (FPSCR_PR)
 294                 EMU_FLOAT_X(D, DRn);
 295         else
 296                 EMU_FLOAT_X(S, FRn);
 297
 298         return 0;
 299 }
 300
 301 #define EMU_FTRC_X(SZ,N) do { \
 302         FP_DECL_##SZ(Fn); \
 303         UNPACK_##SZ(Fn, N); \
 304         FP_TO_INT_##SZ(FPUL, Fn, 32, 1); }while(0)
 305 static int ftrc(struct sh_fpu_soft_struct *fregs, int n)
 306 {
 307         FP_DECL_EX;
 308
 309         if (FPSCR_PR)
 310                 EMU_FTRC_X(D, DRn);
 311         else
 312                 EMU_FTRC_X(S, FRn);
 313
 314         return 0;
 315 }
 316
 317 static int fcnvsd(struct sh_fpu_soft_struct *fregs, int n)
 318 {
 319         FP_DECL_EX;
 320         FP_DECL_S(Fn);
 321         FP_DECL_D(Fr);
 322         UNPACK_S(Fn, FPUL);
 323         FP_CONV(D, S, 2, 1, Fr, Fn);
 324         PACK_D(DRn, Fr);
 325         return 0;
 326 }
 327
 328 static int fcnvds(struct sh_fpu_soft_struct *fregs, int n)
 329 {
 330         FP_DECL_EX;
 331         FP_DECL_D(Fn);
 332         FP_DECL_S(Fr);
 333         UNPACK_D(Fn, DRn);
 334         FP_CONV(S, D, 1, 2, Fr, Fn);
 335         PACK_S(FPUL, Fr);
 336         return 0;
 337 }
 338
 339 static int fxchg(struct sh_fpu_soft_struct *fregs, int flag)
 340 {
 341         FPSCR ^= flag;
 342         return 0;
 343 }
 344
 345 static int fsts(struct sh_fpu_soft_struct *fregs, int n)
 346 {
 347         FRn = FPUL;
 348         return 0;
 349 }
 350
 351 static int flds(struct sh_fpu_soft_struct *fregs, int n)
 352 {
 353         FPUL = FRn;
 354         return 0;
 355 }
 356
 357 static int fneg(struct sh_fpu_soft_struct *fregs, int n)
 358 {
 359         FRn ^= (1 << (_FP_W_TYPE_SIZE - 1));
 360         return 0;
 361 }
 362
 363 static int fabs(struct sh_fpu_soft_struct *fregs, int n)
 364 {
 365         FRn &= ~(1 << (_FP_W_TYPE_SIZE - 1));
 366         return 0;
 367 }
 368
 369 static int fld0(struct sh_fpu_soft_struct *fregs, int n)
 370 {
 371         FRn = 0;
 372         return 0;
 373 }
 374
 375 static int fld1(struct sh_fpu_soft_struct *fregs, int n)
 376 {
 377         FRn = (_FP_EXPBIAS_S << (_FP_FRACBITS_S - 1));
 378         return 0;
 379 }
 380
 381 static int fnop_n(struct sh_fpu_soft_struct *fregs, int n)
 382 {
 383         return -EINVAL;
 384 }
 385
 386 /// Instruction decoders.
 387
 388 static int id_fxfd(struct sh_fpu_soft_struct *, int);
 389 static int id_fnxd(struct sh_fpu_soft_struct *, struct pt_regs *, int, int);
 390
 391 static int (*fnxd[])(struct sh_fpu_soft_struct *, int) = {
 392         fsts, flds, ffloat, ftrc, fneg, fabs, fsqrt, fsrra,
 393         fld0, fld1, fcnvsd, fcnvds, fnop_n, fnop_n, fipr, id_fxfd
 394 };
 395
 396 static int (*fnmx[])(struct sh_fpu_soft_struct *, struct pt_regs *, int, int) = {
 397         fadd, fsub, fmul, fdiv, fcmp_eq, fcmp_gt, fmov_idx_reg, fmov_reg_idx,
 398         fmov_mem_reg, fmov_inc_reg, fmov_reg_mem, fmov_reg_dec,
 399         fmov_reg_reg, id_fnxd, fmac, fnop_mn};
 400
 401 static int id_fxfd(struct sh_fpu_soft_struct *fregs, int x)
 402 {
 403         const int flag[] = { FPSCR_SZ, FPSCR_PR, FPSCR_FR, 0 };
 404         switch (x & 3) {
 405         case 3:
 406                 fxchg(fregs, flag[x >> 2]);
 407                 break;
 408         case 1:
 409                 ftrv(fregs, x - 1);
 410                 break;
 411         default:
 412                 fsca(fregs, x);
 413         }
 414         return 0;
 415 }
 416
 417 static int
 418 id_fnxd(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int x, int n)
 419 {
 420         return (fnxd[x])(fregs, n);
 421 }
 422
 423 static int
 424 id_fnmx(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, u16 code)
 425 {
 426         int n = (code >> 8) & 0xf, m = (code >> 4) & 0xf, x = code & 0xf;
 427         return (fnmx[x])(fregs, regs, m, n);
 428 }
 429
 430 static int
 431 id_sys(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, u16 code)
 432 {
 433         int n = ((code >> 8) & 0xf);
 434         unsigned long *reg = (code & 0x0010) ? &FPUL : &FPSCR;
 435
 436         switch (code & 0xf0ff) {
 437         case 0x005a:
 438         case 0x006a:
 439                 Rn = *reg;
 440                 break;
 441         case 0x405a:
 442         case 0x406a:
 443                 *reg = Rn;
 444                 break;
 445         case 0x4052:
 446         case 0x4062:
 447                 Rn -= 4;
 448                 MWRITE(*reg, Rn);
 449                 break;
 450         case 0x4056:
 451         case 0x4066:
 452                 MREAD(*reg, Rn);
 453                 Rn += 4;
 454                 break;
 455         default:
 456                 return -EINVAL;
 457         }
 458
 459         return 0;
 460 }
 461
 462 static int fpu_emulate(u16 code, struct sh_fpu_soft_struct *fregs, struct pt_regs *regs)
 463 {
 464         if ((code & 0xf000) == 0xf000)
 465                 return id_fnmx(fregs, regs, code);
 466         else
 467                 return id_sys(fregs, regs, code);
 468 }
 469
 470 /**
 471  * fpu_init - Initialize FPU registers
 472  * @fpu: Pointer to software emulated FPU registers.
 473  */
 474 static void fpu_init(struct sh_fpu_soft_struct *fpu)
 475 {
 476         int i;
 477
 478         fpu->fpscr = FPSCR_INIT;
 479         fpu->fpul = 0;
 480
 481         for (i = 0; i < 16; i++) {
 482                 fpu->fp_regs[i] = 0;
 483                 fpu->xfp_regs[i]= 0;
 484         }
 485 }
 486
 487 /**
 488  * do_fpu_inst - Handle reserved instructions for FPU emulation
 489  * @inst: instruction code.
 490  * @regs: registers on stack.
 491  */
 492 int do_fpu_inst(unsigned short inst, struct pt_regs *regs)
 493 {
 494         struct task_struct *tsk = current;
 495         struct sh_fpu_soft_struct *fpu = &(tsk->thread.xstate->softfpu);
 496
 497         perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS, 1, regs, 0);
 498
 499         if (!(task_thread_info(tsk)->status & TS_USEDFPU)) {
 500                 /* initialize once. */
 501                 fpu_init(fpu);
 502                 task_thread_info(tsk)->status |= TS_USEDFPU;
 503         }
 504
 505         return fpu_emulate(inst, fpu, regs);
 506 }