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