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/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 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 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.xstate->softfpu.fpscr & (1 << 17))) { 555 /* FPU error */ 556 denormal_to_double (&tsk->thread.xstate->softfpu, 557 (finsn >> 8) & 0xf); 558 tsk->thread.xstate->softfpu.fpscr &= 559 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK); 560 task_thread_info(tsk)->status |= TS_USEDFPU; 561 } else { 562 force_sig_fault(SIGFPE, FPE_FLTINV, 563 (void __user *)regs->pc, tsk); 564 } 565 566 regs->pc = nextpc; 567 return 1; 568 } 569 570 return 0; 571 } 572 573 /** 574 * fpu_init - Initialize FPU registers 575 * @fpu: Pointer to software emulated FPU registers. 576 */ 577 static void fpu_init(struct sh_fpu_soft_struct *fpu) 578 { 579 int i; 580 581 fpu->fpscr = FPSCR_INIT; 582 fpu->fpul = 0; 583 584 for (i = 0; i < 16; i++) { 585 fpu->fp_regs[i] = 0; 586 fpu->xfp_regs[i]= 0; 587 } 588 } 589 590 /** 591 * do_fpu_inst - Handle reserved instructions for FPU emulation 592 * @inst: instruction code. 593 * @regs: registers on stack. 594 */ 595 int do_fpu_inst(unsigned short inst, struct pt_regs *regs) 596 { 597 struct task_struct *tsk = current; 598 struct sh_fpu_soft_struct *fpu = &(tsk->thread.xstate->softfpu); 599 600 perf_sw_event(PERF_COUNT_SW_EMULATION_FAULTS, 1, regs, 0); 601 602 if (!(task_thread_info(tsk)->status & TS_USEDFPU)) { 603 /* initialize once. */ 604 fpu_init(fpu); 605 task_thread_info(tsk)->status |= TS_USEDFPU; 606 } 607 608 return fpu_emulate(inst, fpu, regs); 609 } 610