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 #include <linux/perf_event.h> 16 17 #include <asm/system.h> 18 #include <asm/uaccess.h> 19 #include <asm/processor.h> 20 #include <asm/io.h> 21 22 #include "sfp-util.h" 23 #include <math-emu/soft-fp.h> 24 #include <math-emu/single.h> 25 #include <math-emu/double.h> 26 27 #define FPUL (fregs->fpul) 28 #define FPSCR (fregs->fpscr) 29 #define FPSCR_RM (FPSCR&3) 30 #define FPSCR_DN ((FPSCR>>18)&1) 31 #define FPSCR_PR ((FPSCR>>19)&1) 32 #define FPSCR_SZ ((FPSCR>>20)&1) 33 #define FPSCR_FR ((FPSCR>>21)&1) 34 #define FPSCR_MASK 0x003fffffUL 35 36 #define BANK(n) (n^(FPSCR_FR?16:0)) 37 #define FR ((unsigned long*)(fregs->fp_regs)) 38 #define FR0 (FR[BANK(0)]) 39 #define FRn (FR[BANK(n)]) 40 #define FRm (FR[BANK(m)]) 41 #define DR ((unsigned long long*)(fregs->fp_regs)) 42 #define DRn (DR[BANK(n)/2]) 43 #define DRm (DR[BANK(m)/2]) 44 45 #define XREG(n) (n^16) 46 #define XFn (FR[BANK(XREG(n))]) 47 #define XFm (FR[BANK(XREG(m))]) 48 #define XDn (DR[BANK(XREG(n))/2]) 49 #define XDm (DR[BANK(XREG(m))/2]) 50 51 #define R0 (regs->regs[0]) 52 #define Rn (regs->regs[n]) 53 #define Rm (regs->regs[m]) 54 55 #define WRITE(d,a) ({if(put_user(d, (typeof (d)*)a)) return -EFAULT;}) 56 #define READ(d,a) ({if(get_user(d, (typeof (d)*)a)) return -EFAULT;}) 57 58 #define PACK_S(r,f) FP_PACK_SP(&r,f) 59 #define UNPACK_S(f,r) FP_UNPACK_SP(f,&r) 60 #define PACK_D(r,f) \ 61 {u32 t[2]; FP_PACK_DP(t,f); ((u32*)&r)[0]=t[1]; ((u32*)&r)[1]=t[0];} 62 #define UNPACK_D(f,r) \ 63 {u32 t[2]; t[0]=((u32*)&r)[1]; t[1]=((u32*)&r)[0]; FP_UNPACK_DP(f,t);} 64 65 // 2 args instructions. 66 #define BOTH_PRmn(op,x) \ 67 FP_DECL_EX; if(FPSCR_PR) op(D,x,DRm,DRn); else op(S,x,FRm,FRn); 68 69 #define CMP_X(SZ,R,M,N) do{ \ 70 FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); \ 71 UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \ 72 FP_CMP_##SZ(R, Fn, Fm, 2); }while(0) 73 #define EQ_X(SZ,R,M,N) do{ \ 74 FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); \ 75 UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \ 76 FP_CMP_EQ_##SZ(R, Fn, Fm); }while(0) 77 #define CMP(OP) ({ int r; BOTH_PRmn(OP##_X,r); r; }) 78 79 static int 80 fcmp_gt(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n) 81 { 82 if (CMP(CMP) > 0) 83 regs->sr |= 1; 84 else 85 regs->sr &= ~1; 86 87 return 0; 88 } 89 90 static int 91 fcmp_eq(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n) 92 { 93 if (CMP(CMP /*EQ*/) == 0) 94 regs->sr |= 1; 95 else 96 regs->sr &= ~1; 97 return 0; 98 } 99 100 #define ARITH_X(SZ,OP,M,N) do{ \ 101 FP_DECL_##SZ(Fm); FP_DECL_##SZ(Fn); FP_DECL_##SZ(Fr); \ 102 UNPACK_##SZ(Fm, M); UNPACK_##SZ(Fn, N); \ 103 FP_##OP##_##SZ(Fr, Fn, Fm); \ 104 PACK_##SZ(N, Fr); }while(0) 105 106 static int 107 fadd(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n) 108 { 109 BOTH_PRmn(ARITH_X, ADD); 110 return 0; 111 } 112 113 static int 114 fsub(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n) 115 { 116 BOTH_PRmn(ARITH_X, SUB); 117 return 0; 118 } 119 120 static int 121 fmul(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n) 122 { 123 BOTH_PRmn(ARITH_X, MUL); 124 return 0; 125 } 126 127 static int 128 fdiv(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n) 129 { 130 BOTH_PRmn(ARITH_X, DIV); 131 return 0; 132 } 133 134 static int 135 fmac(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n) 136 { 137 FP_DECL_EX; 138 FP_DECL_S(Fr); 139 FP_DECL_S(Ft); 140 FP_DECL_S(F0); 141 FP_DECL_S(Fm); 142 FP_DECL_S(Fn); 143 UNPACK_S(F0, FR0); 144 UNPACK_S(Fm, FRm); 145 UNPACK_S(Fn, FRn); 146 FP_MUL_S(Ft, Fm, F0); 147 FP_ADD_S(Fr, Fn, Ft); 148 PACK_S(FRn, Fr); 149 return 0; 150 } 151 152 // to process fmov's extension (odd n for DR access XD). 153 #define FMOV_EXT(x) if(x&1) x+=16-1 154 155 static int 156 fmov_idx_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, 157 int n) 158 { 159 if (FPSCR_SZ) { 160 FMOV_EXT(n); 161 READ(FRn, Rm + R0 + 4); 162 n++; 163 READ(FRn, Rm + R0); 164 } else { 165 READ(FRn, Rm + R0); 166 } 167 168 return 0; 169 } 170 171 static int 172 fmov_mem_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, 173 int n) 174 { 175 if (FPSCR_SZ) { 176 FMOV_EXT(n); 177 READ(FRn, Rm + 4); 178 n++; 179 READ(FRn, Rm); 180 } else { 181 READ(FRn, Rm); 182 } 183 184 return 0; 185 } 186 187 static int 188 fmov_inc_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, 189 int n) 190 { 191 if (FPSCR_SZ) { 192 FMOV_EXT(n); 193 READ(FRn, Rm + 4); 194 n++; 195 READ(FRn, Rm); 196 Rm += 8; 197 } else { 198 READ(FRn, Rm); 199 Rm += 4; 200 } 201 202 return 0; 203 } 204 205 static int 206 fmov_reg_idx(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, 207 int n) 208 { 209 if (FPSCR_SZ) { 210 FMOV_EXT(m); 211 WRITE(FRm, Rn + R0 + 4); 212 m++; 213 WRITE(FRm, Rn + R0); 214 } else { 215 WRITE(FRm, Rn + R0); 216 } 217 218 return 0; 219 } 220 221 static int 222 fmov_reg_mem(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, 223 int n) 224 { 225 if (FPSCR_SZ) { 226 FMOV_EXT(m); 227 WRITE(FRm, Rn + 4); 228 m++; 229 WRITE(FRm, Rn); 230 } else { 231 WRITE(FRm, Rn); 232 } 233 234 return 0; 235 } 236 237 static int 238 fmov_reg_dec(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, 239 int n) 240 { 241 if (FPSCR_SZ) { 242 FMOV_EXT(m); 243 Rn -= 8; 244 WRITE(FRm, Rn + 4); 245 m++; 246 WRITE(FRm, Rn); 247 } else { 248 Rn -= 4; 249 WRITE(FRm, Rn); 250 } 251 252 return 0; 253 } 254 255 static int 256 fmov_reg_reg(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, 257 int n) 258 { 259 if (FPSCR_SZ) { 260 FMOV_EXT(m); 261 FMOV_EXT(n); 262 DRn = DRm; 263 } else { 264 FRn = FRm; 265 } 266 267 return 0; 268 } 269 270 static int 271 fnop_mn(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int m, int n) 272 { 273 return -EINVAL; 274 } 275 276 // 1 arg instructions. 277 #define NOTYETn(i) static int i(struct sh_fpu_soft_struct *fregs, int n) \ 278 { printk( #i " not yet done.\n"); return 0; } 279 280 NOTYETn(ftrv) 281 NOTYETn(fsqrt) 282 NOTYETn(fipr) 283 NOTYETn(fsca) 284 NOTYETn(fsrra) 285 286 #define EMU_FLOAT_X(SZ,N) do { \ 287 FP_DECL_##SZ(Fn); \ 288 FP_FROM_INT_##SZ(Fn, FPUL, 32, int); \ 289 PACK_##SZ(N, Fn); }while(0) 290 static int ffloat(struct sh_fpu_soft_struct *fregs, int n) 291 { 292 FP_DECL_EX; 293 294 if (FPSCR_PR) 295 EMU_FLOAT_X(D, DRn); 296 else 297 EMU_FLOAT_X(S, FRn); 298 299 return 0; 300 } 301 302 #define EMU_FTRC_X(SZ,N) do { \ 303 FP_DECL_##SZ(Fn); \ 304 UNPACK_##SZ(Fn, N); \ 305 FP_TO_INT_##SZ(FPUL, Fn, 32, 1); }while(0) 306 static int ftrc(struct sh_fpu_soft_struct *fregs, int n) 307 { 308 FP_DECL_EX; 309 310 if (FPSCR_PR) 311 EMU_FTRC_X(D, DRn); 312 else 313 EMU_FTRC_X(S, FRn); 314 315 return 0; 316 } 317 318 static int fcnvsd(struct sh_fpu_soft_struct *fregs, int n) 319 { 320 FP_DECL_EX; 321 FP_DECL_S(Fn); 322 FP_DECL_D(Fr); 323 UNPACK_S(Fn, FPUL); 324 FP_CONV(D, S, 2, 1, Fr, Fn); 325 PACK_D(DRn, Fr); 326 return 0; 327 } 328 329 static int fcnvds(struct sh_fpu_soft_struct *fregs, int n) 330 { 331 FP_DECL_EX; 332 FP_DECL_D(Fn); 333 FP_DECL_S(Fr); 334 UNPACK_D(Fn, DRn); 335 FP_CONV(S, D, 1, 2, Fr, Fn); 336 PACK_S(FPUL, Fr); 337 return 0; 338 } 339 340 static int fxchg(struct sh_fpu_soft_struct *fregs, int flag) 341 { 342 FPSCR ^= flag; 343 return 0; 344 } 345 346 static int fsts(struct sh_fpu_soft_struct *fregs, int n) 347 { 348 FRn = FPUL; 349 return 0; 350 } 351 352 static int flds(struct sh_fpu_soft_struct *fregs, int n) 353 { 354 FPUL = FRn; 355 return 0; 356 } 357 358 static int fneg(struct sh_fpu_soft_struct *fregs, int n) 359 { 360 FRn ^= (1 << (_FP_W_TYPE_SIZE - 1)); 361 return 0; 362 } 363 364 static int fabs(struct sh_fpu_soft_struct *fregs, int n) 365 { 366 FRn &= ~(1 << (_FP_W_TYPE_SIZE - 1)); 367 return 0; 368 } 369 370 static int fld0(struct sh_fpu_soft_struct *fregs, int n) 371 { 372 FRn = 0; 373 return 0; 374 } 375 376 static int fld1(struct sh_fpu_soft_struct *fregs, int n) 377 { 378 FRn = (_FP_EXPBIAS_S << (_FP_FRACBITS_S - 1)); 379 return 0; 380 } 381 382 static int fnop_n(struct sh_fpu_soft_struct *fregs, int n) 383 { 384 return -EINVAL; 385 } 386 387 /// Instruction decoders. 388 389 static int id_fxfd(struct sh_fpu_soft_struct *, int); 390 static int id_fnxd(struct sh_fpu_soft_struct *, struct pt_regs *, int, int); 391 392 static int (*fnxd[])(struct sh_fpu_soft_struct *, int) = { 393 fsts, flds, ffloat, ftrc, fneg, fabs, fsqrt, fsrra, 394 fld0, fld1, fcnvsd, fcnvds, fnop_n, fnop_n, fipr, id_fxfd 395 }; 396 397 static int (*fnmx[])(struct sh_fpu_soft_struct *, struct pt_regs *, int, int) = { 398 fadd, fsub, fmul, fdiv, fcmp_eq, fcmp_gt, fmov_idx_reg, fmov_reg_idx, 399 fmov_mem_reg, fmov_inc_reg, fmov_reg_mem, fmov_reg_dec, 400 fmov_reg_reg, id_fnxd, fmac, fnop_mn}; 401 402 static int id_fxfd(struct sh_fpu_soft_struct *fregs, int x) 403 { 404 const int flag[] = { FPSCR_SZ, FPSCR_PR, FPSCR_FR, 0 }; 405 switch (x & 3) { 406 case 3: 407 fxchg(fregs, flag[x >> 2]); 408 break; 409 case 1: 410 ftrv(fregs, x - 1); 411 break; 412 default: 413 fsca(fregs, x); 414 } 415 return 0; 416 } 417 418 static int 419 id_fnxd(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, int x, int n) 420 { 421 return (fnxd[x])(fregs, n); 422 } 423 424 static int 425 id_fnmx(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, u16 code) 426 { 427 int n = (code >> 8) & 0xf, m = (code >> 4) & 0xf, x = code & 0xf; 428 return (fnmx[x])(fregs, regs, m, n); 429 } 430 431 static int 432 id_sys(struct sh_fpu_soft_struct *fregs, struct pt_regs *regs, u16 code) 433 { 434 int n = ((code >> 8) & 0xf); 435 unsigned long *reg = (code & 0x0010) ? &FPUL : &FPSCR; 436 437 switch (code & 0xf0ff) { 438 case 0x005a: 439 case 0x006a: 440 Rn = *reg; 441 break; 442 case 0x405a: 443 case 0x406a: 444 *reg = Rn; 445 break; 446 case 0x4052: 447 case 0x4062: 448 Rn -= 4; 449 WRITE(*reg, Rn); 450 break; 451 case 0x4056: 452 case 0x4066: 453 READ(*reg, Rn); 454 Rn += 4; 455 break; 456 default: 457 return -EINVAL; 458 } 459 460 return 0; 461 } 462 463 static int fpu_emulate(u16 code, struct sh_fpu_soft_struct *fregs, struct pt_regs *regs) 464 { 465 if ((code & 0xf000) == 0xf000) 466 return id_fnmx(fregs, regs, code); 467 else 468 return id_sys(fregs, regs, code); 469 } 470 471 /** 472 * denormal_to_double - Given denormalized float number, 473 * store double float 474 * 475 * @fpu: Pointer to sh_fpu_soft structure 476 * @n: Index to FP register 477 */ 478 static void denormal_to_double(struct sh_fpu_soft_struct *fpu, int n) 479 { 480 unsigned long du, dl; 481 unsigned long x = fpu->fpul; 482 int exp = 1023 - 126; 483 484 if (x != 0 && (x & 0x7f800000) == 0) { 485 du = (x & 0x80000000); 486 while ((x & 0x00800000) == 0) { 487 x <<= 1; 488 exp--; 489 } 490 x &= 0x007fffff; 491 du |= (exp << 20) | (x >> 3); 492 dl = x << 29; 493 494 fpu->fp_regs[n] = du; 495 fpu->fp_regs[n+1] = dl; 496 } 497 } 498 499 /** 500 * ieee_fpe_handler - Handle denormalized number exception 501 * 502 * @regs: Pointer to register structure 503 * 504 * Returns 1 when it's handled (should not cause exception). 505 */ 506 static int ieee_fpe_handler(struct pt_regs *regs) 507 { 508 unsigned short insn = *(unsigned short *)regs->pc; 509 unsigned short finsn; 510 unsigned long nextpc; 511 siginfo_t info; 512 int nib[4] = { 513 (insn >> 12) & 0xf, 514 (insn >> 8) & 0xf, 515 (insn >> 4) & 0xf, 516 insn & 0xf}; 517 518 if (nib[0] == 0xb || 519 (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb)) /* bsr & jsr */ 520 regs->pr = regs->pc + 4; 521 522 if (nib[0] == 0xa || nib[0] == 0xb) { /* bra & bsr */ 523 nextpc = regs->pc + 4 + ((short) ((insn & 0xfff) << 4) >> 3); 524 finsn = *(unsigned short *) (regs->pc + 2); 525 } else if (nib[0] == 0x8 && nib[1] == 0xd) { /* bt/s */ 526 if (regs->sr & 1) 527 nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1); 528 else 529 nextpc = regs->pc + 4; 530 finsn = *(unsigned short *) (regs->pc + 2); 531 } else if (nib[0] == 0x8 && nib[1] == 0xf) { /* bf/s */ 532 if (regs->sr & 1) 533 nextpc = regs->pc + 4; 534 else 535 nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1); 536 finsn = *(unsigned short *) (regs->pc + 2); 537 } else if (nib[0] == 0x4 && nib[3] == 0xb && 538 (nib[2] == 0x0 || nib[2] == 0x2)) { /* jmp & jsr */ 539 nextpc = regs->regs[nib[1]]; 540 finsn = *(unsigned short *) (regs->pc + 2); 541 } else if (nib[0] == 0x0 && nib[3] == 0x3 && 542 (nib[2] == 0x0 || nib[2] == 0x2)) { /* braf & bsrf */ 543 nextpc = regs->pc + 4 + regs->regs[nib[1]]; 544 finsn = *(unsigned short *) (regs->pc + 2); 545 } else if (insn == 0x000b) { /* rts */ 546 nextpc = regs->pr; 547 finsn = *(unsigned short *) (regs->pc + 2); 548 } else { 549 nextpc = regs->pc + 2; 550 finsn = insn; 551 } 552 553 if ((finsn & 0xf1ff) == 0xf0ad) { /* fcnvsd */ 554 struct task_struct *tsk = current; 555 556 if ((tsk->thread.xstate->softfpu.fpscr & (1 << 17))) { 557 /* FPU error */ 558 denormal_to_double (&tsk->thread.xstate->softfpu, 559 (finsn >> 8) & 0xf); 560 tsk->thread.xstate->softfpu.fpscr &= 561 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK); 562 task_thread_info(tsk)->status |= TS_USEDFPU; 563 } else { 564 info.si_signo = SIGFPE; 565 info.si_errno = 0; 566 info.si_code = FPE_FLTINV; 567 info.si_addr = (void __user *)regs->pc; 568 force_sig_info(SIGFPE, &info, tsk); 569 } 570 571 regs->pc = nextpc; 572 return 1; 573 } 574 575 return 0; 576 } 577 578 asmlinkage void do_fpu_error(unsigned long r4, unsigned long r5, 579 unsigned long r6, unsigned long r7, 580 struct pt_regs regs) 581 { 582 struct task_struct *tsk = current; 583 siginfo_t info; 584 585 if (ieee_fpe_handler (®s)) 586 return; 587 588 regs.pc += 2; 589 info.si_signo = SIGFPE; 590 info.si_errno = 0; 591 info.si_code = FPE_FLTINV; 592 info.si_addr = (void __user *)regs.pc; 593 force_sig_info(SIGFPE, &info, tsk); 594 } 595 596 /** 597 * fpu_init - Initialize FPU registers 598 * @fpu: Pointer to software emulated FPU registers. 599 */ 600 static void fpu_init(struct sh_fpu_soft_struct *fpu) 601 { 602 int i; 603 604 fpu->fpscr = FPSCR_INIT; 605 fpu->fpul = 0; 606 607 for (i = 0; i < 16; i++) { 608 fpu->fp_regs[i] = 0; 609 fpu->xfp_regs[i]= 0; 610 } 611 } 612 613 /** 614 * do_fpu_inst - Handle reserved instructions for FPU emulation 615 * @inst: instruction code. 616 * @regs: registers on stack. 617 */ 618 int do_fpu_inst(unsigned short inst, struct pt_regs *regs) 619 { 620 struct task_struct *tsk = current; 621 struct sh_fpu_soft_struct *fpu = &(tsk->thread.xstate->softfpu); 622 623 perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS, 1, 0, regs, 0); 624 625 if (!(task_thread_info(tsk)->status & TS_USEDFPU)) { 626 /* initialize once. */ 627 fpu_init(fpu); 628 task_thread_info(tsk)->status |= TS_USEDFPU; 629 } 630 631 return fpu_emulate(inst, fpu, regs); 632 } 633