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/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 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 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_soft structure 475 * @n: Index to FP register 476 */ 477 static void denormal_to_double(struct sh_fpu_soft_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 siginfo_t info; 511 int nib[4] = { 512 (insn >> 12) & 0xf, 513 (insn >> 8) & 0xf, 514 (insn >> 4) & 0xf, 515 insn & 0xf}; 516 517 if (nib[0] == 0xb || 518 (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb)) /* bsr & jsr */ 519 regs->pr = regs->pc + 4; 520 521 if (nib[0] == 0xa || nib[0] == 0xb) { /* bra & bsr */ 522 nextpc = regs->pc + 4 + ((short) ((insn & 0xfff) << 4) >> 3); 523 finsn = *(unsigned short *) (regs->pc + 2); 524 } else if (nib[0] == 0x8 && nib[1] == 0xd) { /* bt/s */ 525 if (regs->sr & 1) 526 nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1); 527 else 528 nextpc = regs->pc + 4; 529 finsn = *(unsigned short *) (regs->pc + 2); 530 } else if (nib[0] == 0x8 && nib[1] == 0xf) { /* bf/s */ 531 if (regs->sr & 1) 532 nextpc = regs->pc + 4; 533 else 534 nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1); 535 finsn = *(unsigned short *) (regs->pc + 2); 536 } else if (nib[0] == 0x4 && nib[3] == 0xb && 537 (nib[2] == 0x0 || nib[2] == 0x2)) { /* jmp & jsr */ 538 nextpc = regs->regs[nib[1]]; 539 finsn = *(unsigned short *) (regs->pc + 2); 540 } else if (nib[0] == 0x0 && nib[3] == 0x3 && 541 (nib[2] == 0x0 || nib[2] == 0x2)) { /* braf & bsrf */ 542 nextpc = regs->pc + 4 + regs->regs[nib[1]]; 543 finsn = *(unsigned short *) (regs->pc + 2); 544 } else if (insn == 0x000b) { /* rts */ 545 nextpc = regs->pr; 546 finsn = *(unsigned short *) (regs->pc + 2); 547 } else { 548 nextpc = regs->pc + 2; 549 finsn = insn; 550 } 551 552 if ((finsn & 0xf1ff) == 0xf0ad) { /* fcnvsd */ 553 struct task_struct *tsk = current; 554 555 if ((tsk->thread.xstate->softfpu.fpscr & (1 << 17))) { 556 /* FPU error */ 557 denormal_to_double (&tsk->thread.xstate->softfpu, 558 (finsn >> 8) & 0xf); 559 tsk->thread.xstate->softfpu.fpscr &= 560 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK); 561 task_thread_info(tsk)->status |= TS_USEDFPU; 562 } else { 563 info.si_signo = SIGFPE; 564 info.si_errno = 0; 565 info.si_code = FPE_FLTINV; 566 info.si_addr = (void __user *)regs->pc; 567 force_sig_info(SIGFPE, &info, tsk); 568 } 569 570 regs->pc = nextpc; 571 return 1; 572 } 573 574 return 0; 575 } 576 577 /** 578 * fpu_init - Initialize FPU registers 579 * @fpu: Pointer to software emulated FPU registers. 580 */ 581 static void fpu_init(struct sh_fpu_soft_struct *fpu) 582 { 583 int i; 584 585 fpu->fpscr = FPSCR_INIT; 586 fpu->fpul = 0; 587 588 for (i = 0; i < 16; i++) { 589 fpu->fp_regs[i] = 0; 590 fpu->xfp_regs[i]= 0; 591 } 592 } 593 594 /** 595 * do_fpu_inst - Handle reserved instructions for FPU emulation 596 * @inst: instruction code. 597 * @regs: registers on stack. 598 */ 599 int do_fpu_inst(unsigned short inst, struct pt_regs *regs) 600 { 601 struct task_struct *tsk = current; 602 struct sh_fpu_soft_struct *fpu = &(tsk->thread.xstate->softfpu); 603 604 perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS, 1, regs, 0); 605 606 if (!(task_thread_info(tsk)->status & TS_USEDFPU)) { 607 /* initialize once. */ 608 fpu_init(fpu); 609 task_thread_info(tsk)->status |= TS_USEDFPU; 610 } 611 612 return fpu_emulate(inst, fpu, regs); 613 } 614