arch/sh/math-emu/math.c

   1 /*
   2  * arch/sh/math-emu/math.c
   3  *
   4  * Copyright (C) 2006 Takashi YOSHII <takasi-y@ops.dti.ne.jp>
   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.h>
  14 #include <linux/signal.h>
  15
  16 #include <asm/system.h>
  17 #include <asm/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 WRITE(d,a)      ({if(put_user(d, (typeof (d)*)a)) return -EFAULT;})
  55 #define READ(d,a)       ({if(get_user(d, (typeof (d)*)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 extention (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                 READ(FRn, Rm + R0 + 4);
 161                 n++;
 162                 READ(FRn, Rm + R0);
 163         } else {
 164                 READ(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                 READ(FRn, Rm + 4);
 177                 n++;
 178                 READ(FRn, Rm);
 179         } else {
 180                 READ(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                 READ(FRn, Rm + 4);
 193                 n++;
 194                 READ(FRn, Rm);
 195                 Rm += 8;
 196         } else {
 197                 READ(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                 WRITE(FRm, Rn + R0 + 4);
 211                 m++;
 212                 WRITE(FRm, Rn + R0);
 213         } else {
 214                 WRITE(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                 WRITE(FRm, Rn + 4);
 227                 m++;
 228                 WRITE(FRm, Rn);
 229         } else {
 230                 WRITE(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                 WRITE(FRm, Rn + 4);
 244                 m++;
 245                 WRITE(FRm, Rn);
 246         } else {
 247                 Rn -= 4;
 248                 WRITE(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                 WRITE(*reg, Rn);
 449                 break;
 450         case 0x4056:
 451         case 0x4066:
 452                 READ(*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  *      denormal_to_double - Given denormalized float number,
 472  *                           store double float
 473  *
 474  *      @fpu: Pointer to sh_fpu_hard structure
 475  *      @n: Index to FP register
 476  */
 477 static void denormal_to_double(struct sh_fpu_hard_struct *fpu, int n)
 478 {
 479         unsigned long du, dl;
 480         unsigned long x = fpu->fpul;
 481         int exp = 1023 - 126;
 482
 483         if (x != 0 && (x & 0x7f800000) == 0) {
 484                 du = (x & 0x80000000);
 485                 while ((x & 0x00800000) == 0) {
 486                         x <<= 1;
 487                         exp--;
 488                 }
 489                 x &= 0x007fffff;
 490                 du |= (exp << 20) | (x >> 3);
 491                 dl = x << 29;
 492
 493                 fpu->fp_regs[n] = du;
 494                 fpu->fp_regs[n+1] = dl;
 495         }
 496 }
 497
 498 /**
 499  *      ieee_fpe_handler - Handle denormalized number exception
 500  *
 501  *      @regs: Pointer to register structure
 502  *
 503  *      Returns 1 when it's handled (should not cause exception).
 504  */
 505 static int ieee_fpe_handler(struct pt_regs *regs)
 506 {
 507         unsigned short insn = *(unsigned short *)regs->pc;
 508         unsigned short finsn;
 509         unsigned long nextpc;
 510         int nib[4] = {
 511                 (insn >> 12) & 0xf,
 512                 (insn >> 8) & 0xf,
 513                 (insn >> 4) & 0xf,
 514                 insn & 0xf};
 515
 516         if (nib[0] == 0xb ||
 517             (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb)) /* bsr & jsr */
 518                 regs->pr = regs->pc + 4;
 519
 520         if (nib[0] == 0xa || nib[0] == 0xb) { /* bra & bsr */
 521                 nextpc = regs->pc + 4 + ((short) ((insn & 0xfff) << 4) >> 3);
 522                 finsn = *(unsigned short *) (regs->pc + 2);
 523         } else if (nib[0] == 0x8 && nib[1] == 0xd) { /* bt/s */
 524                 if (regs->sr & 1)
 525                         nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1);
 526                 else
 527                         nextpc = regs->pc + 4;
 528                 finsn = *(unsigned short *) (regs->pc + 2);
 529         } else if (nib[0] == 0x8 && nib[1] == 0xf) { /* bf/s */
 530                 if (regs->sr & 1)
 531                         nextpc = regs->pc + 4;
 532                 else
 533                         nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1);
 534                 finsn = *(unsigned short *) (regs->pc + 2);
 535         } else if (nib[0] == 0x4 && nib[3] == 0xb &&
 536                  (nib[2] == 0x0 || nib[2] == 0x2)) { /* jmp & jsr */
 537                 nextpc = regs->regs[nib[1]];
 538                 finsn = *(unsigned short *) (regs->pc + 2);
 539         } else if (nib[0] == 0x0 && nib[3] == 0x3 &&
 540                  (nib[2] == 0x0 || nib[2] == 0x2)) { /* braf & bsrf */
 541                 nextpc = regs->pc + 4 + regs->regs[nib[1]];
 542                 finsn = *(unsigned short *) (regs->pc + 2);
 543         } else if (insn == 0x000b) { /* rts */
 544                 nextpc = regs->pr;
 545                 finsn = *(unsigned short *) (regs->pc + 2);
 546         } else {
 547                 nextpc = regs->pc + 2;
 548                 finsn = insn;
 549         }
 550
 551         if ((finsn & 0xf1ff) == 0xf0ad) { /* fcnvsd */
 552                 struct task_struct *tsk = current;
 553
 554                 if ((tsk->thread.fpu.hard.fpscr & (1 << 17))) {
 555                         /* FPU error */
 556                         denormal_to_double (&tsk->thread.fpu.hard,
 557                                             (finsn >> 8) & 0xf);
 558                         tsk->thread.fpu.hard.fpscr &=
 559                                 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK);
 560                         set_tsk_thread_flag(tsk, TIF_USEDFPU);
 561                 } else {
 562                         tsk->thread.trap_no = 11;
 563                         tsk->thread.error_code = 0;
 564                         force_sig(SIGFPE, tsk);
 565                 }
 566
 567                 regs->pc = nextpc;
 568                 return 1;
 569         }
 570
 571         return 0;
 572 }
 573
 574 asmlinkage void do_fpu_error(unsigned long r4, unsigned long r5,
 575                              unsigned long r6, unsigned long r7,
 576                              struct pt_regs regs)
 577 {
 578         struct task_struct *tsk = current;
 579
 580         if (ieee_fpe_handler (&regs))
 581                 return;
 582
 583         regs.pc += 2;
 584         tsk->thread.trap_no = 11;
 585         tsk->thread.error_code = 0;
 586         force_sig(SIGFPE, tsk);
 587 }
 588
 589 /**
 590  * fpu_init - Initialize FPU registers
 591  * @fpu: Pointer to software emulated FPU registers.
 592  */
 593 static void fpu_init(struct sh_fpu_soft_struct *fpu)
 594 {
 595         int i;
 596
 597         fpu->fpscr = FPSCR_INIT;
 598         fpu->fpul = 0;
 599
 600         for (i = 0; i < 16; i++) {
 601                 fpu->fp_regs[i] = 0;
 602                 fpu->xfp_regs[i]= 0;
 603         }
 604 }
 605
 606 /**
 607  * do_fpu_inst - Handle reserved instructions for FPU emulation
 608  * @inst: instruction code.
 609  * @regs: registers on stack.
 610  */
 611 int do_fpu_inst(unsigned short inst, struct pt_regs *regs)
 612 {
 613         struct task_struct *tsk = current;
 614         struct sh_fpu_soft_struct *fpu = &(tsk->thread.fpu.soft);
 615
 616         if (!test_tsk_thread_flag(tsk, TIF_USEDFPU)) {
 617                 /* initialize once. */
 618                 fpu_init(fpu);
 619                 set_tsk_thread_flag(tsk, TIF_USEDFPU);
 620         }
 621
 622         return fpu_emulate(inst, fpu, regs);
 623 }