1 /* 2 * Save/restore floating point context for signal handlers. 3 * 4 * Copyright (C) 1999, 2000 Kaz Kojima & Niibe Yutaka 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 * FIXME! These routines can be optimized in big endian case. 11 */ 12 #include <linux/sched.h> 13 #include <linux/signal.h> 14 #include <asm/processor.h> 15 #include <asm/io.h> 16 17 /* The PR (precision) bit in the FP Status Register must be clear when 18 * an frchg instruction is executed, otherwise the instruction is undefined. 19 * Executing frchg with PR set causes a trap on some SH4 implementations. 20 */ 21 22 #define FPSCR_RCHG 0x00000000 23 24 25 /* 26 * Save FPU registers onto task structure. 27 * Assume called with FPU enabled (SR.FD=0). 28 */ 29 void 30 save_fpu(struct task_struct *tsk, struct pt_regs *regs) 31 { 32 unsigned long dummy; 33 34 clear_tsk_thread_flag(tsk, TIF_USEDFPU); 35 enable_fpu(); 36 asm volatile("sts.l fpul, @-%0\n\t" 37 "sts.l fpscr, @-%0\n\t" 38 "fmov.s fr15, @-%0\n\t" 39 "fmov.s fr14, @-%0\n\t" 40 "fmov.s fr13, @-%0\n\t" 41 "fmov.s fr12, @-%0\n\t" 42 "fmov.s fr11, @-%0\n\t" 43 "fmov.s fr10, @-%0\n\t" 44 "fmov.s fr9, @-%0\n\t" 45 "fmov.s fr8, @-%0\n\t" 46 "fmov.s fr7, @-%0\n\t" 47 "fmov.s fr6, @-%0\n\t" 48 "fmov.s fr5, @-%0\n\t" 49 "fmov.s fr4, @-%0\n\t" 50 "fmov.s fr3, @-%0\n\t" 51 "fmov.s fr2, @-%0\n\t" 52 "fmov.s fr1, @-%0\n\t" 53 "fmov.s fr0, @-%0\n\t" 54 "lds %3, fpscr\n\t" 55 : "=r" (dummy) 56 : "0" ((char *)(&tsk->thread.fpu.hard.status)), 57 "r" (FPSCR_RCHG), 58 "r" (FPSCR_INIT) 59 : "memory"); 60 61 disable_fpu(); 62 release_fpu(regs); 63 } 64 65 static void 66 restore_fpu(struct task_struct *tsk) 67 { 68 unsigned long dummy; 69 70 enable_fpu(); 71 asm volatile("fmov.s @%0+, fr0\n\t" 72 "fmov.s @%0+, fr1\n\t" 73 "fmov.s @%0+, fr2\n\t" 74 "fmov.s @%0+, fr3\n\t" 75 "fmov.s @%0+, fr4\n\t" 76 "fmov.s @%0+, fr5\n\t" 77 "fmov.s @%0+, fr6\n\t" 78 "fmov.s @%0+, fr7\n\t" 79 "fmov.s @%0+, fr8\n\t" 80 "fmov.s @%0+, fr9\n\t" 81 "fmov.s @%0+, fr10\n\t" 82 "fmov.s @%0+, fr11\n\t" 83 "fmov.s @%0+, fr12\n\t" 84 "fmov.s @%0+, fr13\n\t" 85 "fmov.s @%0+, fr14\n\t" 86 "fmov.s @%0+, fr15\n\t" 87 "lds.l @%0+, fpscr\n\t" 88 "lds.l @%0+, fpul\n\t" 89 : "=r" (dummy) 90 : "0" (&tsk->thread.fpu), "r" (FPSCR_RCHG) 91 : "memory"); 92 disable_fpu(); 93 } 94 95 /* 96 * Load the FPU with signalling NANS. This bit pattern we're using 97 * has the property that no matter wether considered as single or as 98 * double precission represents signaling NANS. 99 */ 100 101 static void 102 fpu_init(void) 103 { 104 enable_fpu(); 105 asm volatile("lds %0, fpul\n\t" 106 "fsts fpul, fr0\n\t" 107 "fsts fpul, fr1\n\t" 108 "fsts fpul, fr2\n\t" 109 "fsts fpul, fr3\n\t" 110 "fsts fpul, fr4\n\t" 111 "fsts fpul, fr5\n\t" 112 "fsts fpul, fr6\n\t" 113 "fsts fpul, fr7\n\t" 114 "fsts fpul, fr8\n\t" 115 "fsts fpul, fr9\n\t" 116 "fsts fpul, fr10\n\t" 117 "fsts fpul, fr11\n\t" 118 "fsts fpul, fr12\n\t" 119 "fsts fpul, fr13\n\t" 120 "fsts fpul, fr14\n\t" 121 "fsts fpul, fr15\n\t" 122 "lds %2, fpscr\n\t" 123 : /* no output */ 124 : "r" (0), "r" (FPSCR_RCHG), "r" (FPSCR_INIT)); 125 disable_fpu(); 126 } 127 128 /* 129 * Emulate arithmetic ops on denormalized number for some FPU insns. 130 */ 131 132 /* denormalized float * float */ 133 static int denormal_mulf(int hx, int hy) 134 { 135 unsigned int ix, iy; 136 unsigned long long m, n; 137 int exp, w; 138 139 ix = hx & 0x7fffffff; 140 iy = hy & 0x7fffffff; 141 if (iy < 0x00800000 || ix == 0) 142 return ((hx ^ hy) & 0x80000000); 143 144 exp = (iy & 0x7f800000) >> 23; 145 ix &= 0x007fffff; 146 iy = (iy & 0x007fffff) | 0x00800000; 147 m = (unsigned long long)ix * iy; 148 n = m; 149 w = -1; 150 while (n) { n >>= 1; w++; } 151 152 /* FIXME: use guard bits */ 153 exp += w - 126 - 46; 154 if (exp > 0) 155 ix = ((int) (m >> (w - 23)) & 0x007fffff) | (exp << 23); 156 else if (exp + 22 >= 0) 157 ix = (int) (m >> (w - 22 - exp)) & 0x007fffff; 158 else 159 ix = 0; 160 161 ix |= (hx ^ hy) & 0x80000000; 162 return ix; 163 } 164 165 /* denormalized double * double */ 166 static void mult64(unsigned long long x, unsigned long long y, 167 unsigned long long *highp, unsigned long long *lowp) 168 { 169 unsigned long long sub0, sub1, sub2, sub3; 170 unsigned long long high, low; 171 172 sub0 = (x >> 32) * (unsigned long) (y >> 32); 173 sub1 = (x & 0xffffffffLL) * (unsigned long) (y >> 32); 174 sub2 = (x >> 32) * (unsigned long) (y & 0xffffffffLL); 175 sub3 = (x & 0xffffffffLL) * (unsigned long) (y & 0xffffffffLL); 176 low = sub3; 177 high = 0LL; 178 sub3 += (sub1 << 32); 179 if (low > sub3) 180 high++; 181 low = sub3; 182 sub3 += (sub2 << 32); 183 if (low > sub3) 184 high++; 185 low = sub3; 186 high += (sub1 >> 32) + (sub2 >> 32); 187 high += sub0; 188 *lowp = low; 189 *highp = high; 190 } 191 192 static inline long long rshift64(unsigned long long mh, 193 unsigned long long ml, int n) 194 { 195 if (n >= 64) 196 return mh >> (n - 64); 197 return (mh << (64 - n)) | (ml >> n); 198 } 199 200 static long long denormal_muld(long long hx, long long hy) 201 { 202 unsigned long long ix, iy; 203 unsigned long long mh, ml, nh, nl; 204 int exp, w; 205 206 ix = hx & 0x7fffffffffffffffLL; 207 iy = hy & 0x7fffffffffffffffLL; 208 if (iy < 0x0010000000000000LL || ix == 0) 209 return ((hx ^ hy) & 0x8000000000000000LL); 210 211 exp = (iy & 0x7ff0000000000000LL) >> 52; 212 ix &= 0x000fffffffffffffLL; 213 iy = (iy & 0x000fffffffffffffLL) | 0x0010000000000000LL; 214 mult64(ix, iy, &mh, &ml); 215 nh = mh; 216 nl = ml; 217 w = -1; 218 if (nh) { 219 while (nh) { nh >>= 1; w++;} 220 w += 64; 221 } else 222 while (nl) { nl >>= 1; w++;} 223 224 /* FIXME: use guard bits */ 225 exp += w - 1022 - 52 * 2; 226 if (exp > 0) 227 ix = (rshift64(mh, ml, w - 52) & 0x000fffffffffffffLL) 228 | ((long long)exp << 52); 229 else if (exp + 51 >= 0) 230 ix = rshift64(mh, ml, w - 51 - exp) & 0x000fffffffffffffLL; 231 else 232 ix = 0; 233 234 ix |= (hx ^ hy) & 0x8000000000000000LL; 235 return ix; 236 } 237 238 /* ix - iy where iy: denormal and ix, iy >= 0 */ 239 static int denormal_subf1(unsigned int ix, unsigned int iy) 240 { 241 int frac; 242 int exp; 243 244 if (ix < 0x00800000) 245 return ix - iy; 246 247 exp = (ix & 0x7f800000) >> 23; 248 if (exp - 1 > 31) 249 return ix; 250 iy >>= exp - 1; 251 if (iy == 0) 252 return ix; 253 254 frac = (ix & 0x007fffff) | 0x00800000; 255 frac -= iy; 256 while (frac < 0x00800000) { 257 if (--exp == 0) 258 return frac; 259 frac <<= 1; 260 } 261 262 return (exp << 23) | (frac & 0x007fffff); 263 } 264 265 /* ix + iy where iy: denormal and ix, iy >= 0 */ 266 static int denormal_addf1(unsigned int ix, unsigned int iy) 267 { 268 int frac; 269 int exp; 270 271 if (ix < 0x00800000) 272 return ix + iy; 273 274 exp = (ix & 0x7f800000) >> 23; 275 if (exp - 1 > 31) 276 return ix; 277 iy >>= exp - 1; 278 if (iy == 0) 279 return ix; 280 281 frac = (ix & 0x007fffff) | 0x00800000; 282 frac += iy; 283 if (frac >= 0x01000000) { 284 frac >>= 1; 285 ++exp; 286 } 287 288 return (exp << 23) | (frac & 0x007fffff); 289 } 290 291 static int denormal_addf(int hx, int hy) 292 { 293 unsigned int ix, iy; 294 int sign; 295 296 if ((hx ^ hy) & 0x80000000) { 297 sign = hx & 0x80000000; 298 ix = hx & 0x7fffffff; 299 iy = hy & 0x7fffffff; 300 if (iy < 0x00800000) { 301 ix = denormal_subf1(ix, iy); 302 if (ix < 0) { 303 ix = -ix; 304 sign ^= 0x80000000; 305 } 306 } else { 307 ix = denormal_subf1(iy, ix); 308 sign ^= 0x80000000; 309 } 310 } else { 311 sign = hx & 0x80000000; 312 ix = hx & 0x7fffffff; 313 iy = hy & 0x7fffffff; 314 if (iy < 0x00800000) 315 ix = denormal_addf1(ix, iy); 316 else 317 ix = denormal_addf1(iy, ix); 318 } 319 320 return sign | ix; 321 } 322 323 /* ix - iy where iy: denormal and ix, iy >= 0 */ 324 static long long denormal_subd1(unsigned long long ix, unsigned long long iy) 325 { 326 long long frac; 327 int exp; 328 329 if (ix < 0x0010000000000000LL) 330 return ix - iy; 331 332 exp = (ix & 0x7ff0000000000000LL) >> 52; 333 if (exp - 1 > 63) 334 return ix; 335 iy >>= exp - 1; 336 if (iy == 0) 337 return ix; 338 339 frac = (ix & 0x000fffffffffffffLL) | 0x0010000000000000LL; 340 frac -= iy; 341 while (frac < 0x0010000000000000LL) { 342 if (--exp == 0) 343 return frac; 344 frac <<= 1; 345 } 346 347 return ((long long)exp << 52) | (frac & 0x000fffffffffffffLL); 348 } 349 350 /* ix + iy where iy: denormal and ix, iy >= 0 */ 351 static long long denormal_addd1(unsigned long long ix, unsigned long long iy) 352 { 353 long long frac; 354 long long exp; 355 356 if (ix < 0x0010000000000000LL) 357 return ix + iy; 358 359 exp = (ix & 0x7ff0000000000000LL) >> 52; 360 if (exp - 1 > 63) 361 return ix; 362 iy >>= exp - 1; 363 if (iy == 0) 364 return ix; 365 366 frac = (ix & 0x000fffffffffffffLL) | 0x0010000000000000LL; 367 frac += iy; 368 if (frac >= 0x0020000000000000LL) { 369 frac >>= 1; 370 ++exp; 371 } 372 373 return (exp << 52) | (frac & 0x000fffffffffffffLL); 374 } 375 376 static long long denormal_addd(long long hx, long long hy) 377 { 378 unsigned long long ix, iy; 379 long long sign; 380 381 if ((hx ^ hy) & 0x8000000000000000LL) { 382 sign = hx & 0x8000000000000000LL; 383 ix = hx & 0x7fffffffffffffffLL; 384 iy = hy & 0x7fffffffffffffffLL; 385 if (iy < 0x0010000000000000LL) { 386 ix = denormal_subd1(ix, iy); 387 if (ix < 0) { 388 ix = -ix; 389 sign ^= 0x8000000000000000LL; 390 } 391 } else { 392 ix = denormal_subd1(iy, ix); 393 sign ^= 0x8000000000000000LL; 394 } 395 } else { 396 sign = hx & 0x8000000000000000LL; 397 ix = hx & 0x7fffffffffffffffLL; 398 iy = hy & 0x7fffffffffffffffLL; 399 if (iy < 0x0010000000000000LL) 400 ix = denormal_addd1(ix, iy); 401 else 402 ix = denormal_addd1(iy, ix); 403 } 404 405 return sign | ix; 406 } 407 408 /** 409 * denormal_to_double - Given denormalized float number, 410 * store double float 411 * 412 * @fpu: Pointer to sh_fpu_hard structure 413 * @n: Index to FP register 414 */ 415 static void 416 denormal_to_double (struct sh_fpu_hard_struct *fpu, int n) 417 { 418 unsigned long du, dl; 419 unsigned long x = fpu->fpul; 420 int exp = 1023 - 126; 421 422 if (x != 0 && (x & 0x7f800000) == 0) { 423 du = (x & 0x80000000); 424 while ((x & 0x00800000) == 0) { 425 x <<= 1; 426 exp--; 427 } 428 x &= 0x007fffff; 429 du |= (exp << 20) | (x >> 3); 430 dl = x << 29; 431 432 fpu->fp_regs[n] = du; 433 fpu->fp_regs[n+1] = dl; 434 } 435 } 436 437 /** 438 * ieee_fpe_handler - Handle denormalized number exception 439 * 440 * @regs: Pointer to register structure 441 * 442 * Returns 1 when it's handled (should not cause exception). 443 */ 444 static int 445 ieee_fpe_handler (struct pt_regs *regs) 446 { 447 unsigned short insn = *(unsigned short *) regs->pc; 448 unsigned short finsn; 449 unsigned long nextpc; 450 int nib[4] = { 451 (insn >> 12) & 0xf, 452 (insn >> 8) & 0xf, 453 (insn >> 4) & 0xf, 454 insn & 0xf}; 455 456 if (nib[0] == 0xb || 457 (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb)) /* bsr & jsr */ 458 regs->pr = regs->pc + 4; 459 if (nib[0] == 0xa || nib[0] == 0xb) { /* bra & bsr */ 460 nextpc = regs->pc + 4 + ((short) ((insn & 0xfff) << 4) >> 3); 461 finsn = *(unsigned short *) (regs->pc + 2); 462 } else if (nib[0] == 0x8 && nib[1] == 0xd) { /* bt/s */ 463 if (regs->sr & 1) 464 nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1); 465 else 466 nextpc = regs->pc + 4; 467 finsn = *(unsigned short *) (regs->pc + 2); 468 } else if (nib[0] == 0x8 && nib[1] == 0xf) { /* bf/s */ 469 if (regs->sr & 1) 470 nextpc = regs->pc + 4; 471 else 472 nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1); 473 finsn = *(unsigned short *) (regs->pc + 2); 474 } else if (nib[0] == 0x4 && nib[3] == 0xb && 475 (nib[2] == 0x0 || nib[2] == 0x2)) { /* jmp & jsr */ 476 nextpc = regs->regs[nib[1]]; 477 finsn = *(unsigned short *) (regs->pc + 2); 478 } else if (nib[0] == 0x0 && nib[3] == 0x3 && 479 (nib[2] == 0x0 || nib[2] == 0x2)) { /* braf & bsrf */ 480 nextpc = regs->pc + 4 + regs->regs[nib[1]]; 481 finsn = *(unsigned short *) (regs->pc + 2); 482 } else if (insn == 0x000b) { /* rts */ 483 nextpc = regs->pr; 484 finsn = *(unsigned short *) (regs->pc + 2); 485 } else { 486 nextpc = regs->pc + 2; 487 finsn = insn; 488 } 489 490 #define FPSCR_FPU_ERROR (1 << 17) 491 492 if ((finsn & 0xf1ff) == 0xf0ad) { /* fcnvsd */ 493 struct task_struct *tsk = current; 494 495 if ((tsk->thread.fpu.hard.fpscr & FPSCR_FPU_ERROR)) { 496 /* FPU error */ 497 denormal_to_double (&tsk->thread.fpu.hard, 498 (finsn >> 8) & 0xf); 499 } else 500 return 0; 501 502 regs->pc = nextpc; 503 return 1; 504 } else if ((finsn & 0xf00f) == 0xf002) { /* fmul */ 505 struct task_struct *tsk = current; 506 int fpscr; 507 int n, m, prec; 508 unsigned int hx, hy; 509 510 n = (finsn >> 8) & 0xf; 511 m = (finsn >> 4) & 0xf; 512 hx = tsk->thread.fpu.hard.fp_regs[n]; 513 hy = tsk->thread.fpu.hard.fp_regs[m]; 514 fpscr = tsk->thread.fpu.hard.fpscr; 515 prec = fpscr & (1 << 19); 516 517 if ((fpscr & FPSCR_FPU_ERROR) 518 && (prec && ((hx & 0x7fffffff) < 0x00100000 519 || (hy & 0x7fffffff) < 0x00100000))) { 520 long long llx, lly; 521 522 /* FPU error because of denormal */ 523 llx = ((long long) hx << 32) 524 | tsk->thread.fpu.hard.fp_regs[n+1]; 525 lly = ((long long) hy << 32) 526 | tsk->thread.fpu.hard.fp_regs[m+1]; 527 if ((hx & 0x7fffffff) >= 0x00100000) 528 llx = denormal_muld(lly, llx); 529 else 530 llx = denormal_muld(llx, lly); 531 tsk->thread.fpu.hard.fp_regs[n] = llx >> 32; 532 tsk->thread.fpu.hard.fp_regs[n+1] = llx & 0xffffffff; 533 } else if ((fpscr & FPSCR_FPU_ERROR) 534 && (!prec && ((hx & 0x7fffffff) < 0x00800000 535 || (hy & 0x7fffffff) < 0x00800000))) { 536 /* FPU error because of denormal */ 537 if ((hx & 0x7fffffff) >= 0x00800000) 538 hx = denormal_mulf(hy, hx); 539 else 540 hx = denormal_mulf(hx, hy); 541 tsk->thread.fpu.hard.fp_regs[n] = hx; 542 } else 543 return 0; 544 545 regs->pc = nextpc; 546 return 1; 547 } else if ((finsn & 0xf00e) == 0xf000) { /* fadd, fsub */ 548 struct task_struct *tsk = current; 549 int fpscr; 550 int n, m, prec; 551 unsigned int hx, hy; 552 553 n = (finsn >> 8) & 0xf; 554 m = (finsn >> 4) & 0xf; 555 hx = tsk->thread.fpu.hard.fp_regs[n]; 556 hy = tsk->thread.fpu.hard.fp_regs[m]; 557 fpscr = tsk->thread.fpu.hard.fpscr; 558 prec = fpscr & (1 << 19); 559 560 if ((fpscr & FPSCR_FPU_ERROR) 561 && (prec && ((hx & 0x7fffffff) < 0x00100000 562 || (hy & 0x7fffffff) < 0x00100000))) { 563 long long llx, lly; 564 565 /* FPU error because of denormal */ 566 llx = ((long long) hx << 32) 567 | tsk->thread.fpu.hard.fp_regs[n+1]; 568 lly = ((long long) hy << 32) 569 | tsk->thread.fpu.hard.fp_regs[m+1]; 570 if ((finsn & 0xf00f) == 0xf000) 571 llx = denormal_addd(llx, lly); 572 else 573 llx = denormal_addd(llx, lly ^ (1LL << 63)); 574 tsk->thread.fpu.hard.fp_regs[n] = llx >> 32; 575 tsk->thread.fpu.hard.fp_regs[n+1] = llx & 0xffffffff; 576 } else if ((fpscr & FPSCR_FPU_ERROR) 577 && (!prec && ((hx & 0x7fffffff) < 0x00800000 578 || (hy & 0x7fffffff) < 0x00800000))) { 579 /* FPU error because of denormal */ 580 if ((finsn & 0xf00f) == 0xf000) 581 hx = denormal_addf(hx, hy); 582 else 583 hx = denormal_addf(hx, hy ^ 0x80000000); 584 tsk->thread.fpu.hard.fp_regs[n] = hx; 585 } else 586 return 0; 587 588 regs->pc = nextpc; 589 return 1; 590 } 591 592 return 0; 593 } 594 595 BUILD_TRAP_HANDLER(fpu_error) 596 { 597 struct task_struct *tsk = current; 598 TRAP_HANDLER_DECL; 599 600 save_fpu(tsk, regs); 601 if (ieee_fpe_handler(regs)) { 602 tsk->thread.fpu.hard.fpscr &= 603 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK); 604 grab_fpu(regs); 605 restore_fpu(tsk); 606 set_tsk_thread_flag(tsk, TIF_USEDFPU); 607 return; 608 } 609 610 force_sig(SIGFPE, tsk); 611 } 612 613 BUILD_TRAP_HANDLER(fpu_state_restore) 614 { 615 struct task_struct *tsk = current; 616 TRAP_HANDLER_DECL; 617 618 grab_fpu(regs); 619 if (!user_mode(regs)) { 620 printk(KERN_ERR "BUG: FPU is used in kernel mode.\n"); 621 return; 622 } 623 624 if (used_math()) { 625 /* Using the FPU again. */ 626 restore_fpu(tsk); 627 } else { 628 /* First time FPU user. */ 629 fpu_init(); 630 set_used_math(); 631 } 632 set_tsk_thread_flag(tsk, TIF_USEDFPU); 633 } 634