1 /* 2 * Save/restore floating point context for signal handlers. 3 * 4 * This file is subject to the terms and conditions of the GNU General Public 5 * License. See the file "COPYING" in the main directory of this archive 6 * for more details. 7 * 8 * Copyright (C) 1999, 2000 Kaz Kojima & Niibe Yutaka 9 * Copyright (C) 2006 ST Microelectronics Ltd. (denorm support) 10 * 11 * FIXME! These routines have not been tested for big endian case. 12 */ 13 #include <linux/sched.h> 14 #include <linux/signal.h> 15 #include <linux/io.h> 16 #include <asm/cpu/fpu.h> 17 #include <asm/processor.h> 18 #include <asm/system.h> 19 20 /* The PR (precision) bit in the FP Status Register must be clear when 21 * an frchg instruction is executed, otherwise the instruction is undefined. 22 * Executing frchg with PR set causes a trap on some SH4 implementations. 23 */ 24 25 #define FPSCR_RCHG 0x00000000 26 extern unsigned long long float64_div(unsigned long long a, 27 unsigned long long b); 28 extern unsigned long int float32_div(unsigned long int a, unsigned long int b); 29 extern unsigned long long float64_mul(unsigned long long a, 30 unsigned long long b); 31 extern unsigned long int float32_mul(unsigned long int a, unsigned long int b); 32 extern unsigned long long float64_add(unsigned long long a, 33 unsigned long long b); 34 extern unsigned long int float32_add(unsigned long int a, unsigned long int b); 35 extern unsigned long long float64_sub(unsigned long long a, 36 unsigned long long b); 37 extern unsigned long int float32_sub(unsigned long int a, unsigned long int b); 38 39 static unsigned int fpu_exception_flags; 40 41 /* 42 * Save FPU registers onto task structure. 43 * Assume called with FPU enabled (SR.FD=0). 44 */ 45 void save_fpu(struct task_struct *tsk, struct pt_regs *regs) 46 { 47 unsigned long dummy; 48 49 clear_tsk_thread_flag(tsk, TIF_USEDFPU); 50 enable_fpu(); 51 asm volatile ("sts.l fpul, @-%0\n\t" 52 "sts.l fpscr, @-%0\n\t" 53 "lds %2, fpscr\n\t" 54 "frchg\n\t" 55 "fmov.s fr15, @-%0\n\t" 56 "fmov.s fr14, @-%0\n\t" 57 "fmov.s fr13, @-%0\n\t" 58 "fmov.s fr12, @-%0\n\t" 59 "fmov.s fr11, @-%0\n\t" 60 "fmov.s fr10, @-%0\n\t" 61 "fmov.s fr9, @-%0\n\t" 62 "fmov.s fr8, @-%0\n\t" 63 "fmov.s fr7, @-%0\n\t" 64 "fmov.s fr6, @-%0\n\t" 65 "fmov.s fr5, @-%0\n\t" 66 "fmov.s fr4, @-%0\n\t" 67 "fmov.s fr3, @-%0\n\t" 68 "fmov.s fr2, @-%0\n\t" 69 "fmov.s fr1, @-%0\n\t" 70 "fmov.s fr0, @-%0\n\t" 71 "frchg\n\t" 72 "fmov.s fr15, @-%0\n\t" 73 "fmov.s fr14, @-%0\n\t" 74 "fmov.s fr13, @-%0\n\t" 75 "fmov.s fr12, @-%0\n\t" 76 "fmov.s fr11, @-%0\n\t" 77 "fmov.s fr10, @-%0\n\t" 78 "fmov.s fr9, @-%0\n\t" 79 "fmov.s fr8, @-%0\n\t" 80 "fmov.s fr7, @-%0\n\t" 81 "fmov.s fr6, @-%0\n\t" 82 "fmov.s fr5, @-%0\n\t" 83 "fmov.s fr4, @-%0\n\t" 84 "fmov.s fr3, @-%0\n\t" 85 "fmov.s fr2, @-%0\n\t" 86 "fmov.s fr1, @-%0\n\t" 87 "fmov.s fr0, @-%0\n\t" 88 "lds %3, fpscr\n\t":"=r" (dummy) 89 :"0"((char *)(&tsk->thread.fpu.hard.status)), 90 "r"(FPSCR_RCHG), "r"(FPSCR_INIT) 91 :"memory"); 92 93 disable_fpu(); 94 release_fpu(regs); 95 } 96 97 static void restore_fpu(struct task_struct *tsk) 98 { 99 unsigned long dummy; 100 101 enable_fpu(); 102 asm volatile ("lds %2, fpscr\n\t" 103 "fmov.s @%0+, fr0\n\t" 104 "fmov.s @%0+, fr1\n\t" 105 "fmov.s @%0+, fr2\n\t" 106 "fmov.s @%0+, fr3\n\t" 107 "fmov.s @%0+, fr4\n\t" 108 "fmov.s @%0+, fr5\n\t" 109 "fmov.s @%0+, fr6\n\t" 110 "fmov.s @%0+, fr7\n\t" 111 "fmov.s @%0+, fr8\n\t" 112 "fmov.s @%0+, fr9\n\t" 113 "fmov.s @%0+, fr10\n\t" 114 "fmov.s @%0+, fr11\n\t" 115 "fmov.s @%0+, fr12\n\t" 116 "fmov.s @%0+, fr13\n\t" 117 "fmov.s @%0+, fr14\n\t" 118 "fmov.s @%0+, fr15\n\t" 119 "frchg\n\t" 120 "fmov.s @%0+, fr0\n\t" 121 "fmov.s @%0+, fr1\n\t" 122 "fmov.s @%0+, fr2\n\t" 123 "fmov.s @%0+, fr3\n\t" 124 "fmov.s @%0+, fr4\n\t" 125 "fmov.s @%0+, fr5\n\t" 126 "fmov.s @%0+, fr6\n\t" 127 "fmov.s @%0+, fr7\n\t" 128 "fmov.s @%0+, fr8\n\t" 129 "fmov.s @%0+, fr9\n\t" 130 "fmov.s @%0+, fr10\n\t" 131 "fmov.s @%0+, fr11\n\t" 132 "fmov.s @%0+, fr12\n\t" 133 "fmov.s @%0+, fr13\n\t" 134 "fmov.s @%0+, fr14\n\t" 135 "fmov.s @%0+, fr15\n\t" 136 "frchg\n\t" 137 "lds.l @%0+, fpscr\n\t" 138 "lds.l @%0+, fpul\n\t" 139 :"=r" (dummy) 140 :"0"(&tsk->thread.fpu), "r"(FPSCR_RCHG) 141 :"memory"); 142 disable_fpu(); 143 } 144 145 /* 146 * Load the FPU with signalling NANS. This bit pattern we're using 147 * has the property that no matter wether considered as single or as 148 * double precision represents signaling NANS. 149 */ 150 151 static void fpu_init(void) 152 { 153 enable_fpu(); 154 asm volatile ( "lds %0, fpul\n\t" 155 "lds %1, fpscr\n\t" 156 "fsts fpul, fr0\n\t" 157 "fsts fpul, fr1\n\t" 158 "fsts fpul, fr2\n\t" 159 "fsts fpul, fr3\n\t" 160 "fsts fpul, fr4\n\t" 161 "fsts fpul, fr5\n\t" 162 "fsts fpul, fr6\n\t" 163 "fsts fpul, fr7\n\t" 164 "fsts fpul, fr8\n\t" 165 "fsts fpul, fr9\n\t" 166 "fsts fpul, fr10\n\t" 167 "fsts fpul, fr11\n\t" 168 "fsts fpul, fr12\n\t" 169 "fsts fpul, fr13\n\t" 170 "fsts fpul, fr14\n\t" 171 "fsts fpul, fr15\n\t" 172 "frchg\n\t" 173 "fsts fpul, fr0\n\t" 174 "fsts fpul, fr1\n\t" 175 "fsts fpul, fr2\n\t" 176 "fsts fpul, fr3\n\t" 177 "fsts fpul, fr4\n\t" 178 "fsts fpul, fr5\n\t" 179 "fsts fpul, fr6\n\t" 180 "fsts fpul, fr7\n\t" 181 "fsts fpul, fr8\n\t" 182 "fsts fpul, fr9\n\t" 183 "fsts fpul, fr10\n\t" 184 "fsts fpul, fr11\n\t" 185 "fsts fpul, fr12\n\t" 186 "fsts fpul, fr13\n\t" 187 "fsts fpul, fr14\n\t" 188 "fsts fpul, fr15\n\t" 189 "frchg\n\t" 190 "lds %2, fpscr\n\t" 191 : /* no output */ 192 :"r" (0), "r"(FPSCR_RCHG), "r"(FPSCR_INIT)); 193 disable_fpu(); 194 } 195 196 /** 197 * denormal_to_double - Given denormalized float number, 198 * store double float 199 * 200 * @fpu: Pointer to sh_fpu_hard structure 201 * @n: Index to FP register 202 */ 203 static void denormal_to_double(struct sh_fpu_hard_struct *fpu, int n) 204 { 205 unsigned long du, dl; 206 unsigned long x = fpu->fpul; 207 int exp = 1023 - 126; 208 209 if (x != 0 && (x & 0x7f800000) == 0) { 210 du = (x & 0x80000000); 211 while ((x & 0x00800000) == 0) { 212 x <<= 1; 213 exp--; 214 } 215 x &= 0x007fffff; 216 du |= (exp << 20) | (x >> 3); 217 dl = x << 29; 218 219 fpu->fp_regs[n] = du; 220 fpu->fp_regs[n + 1] = dl; 221 } 222 } 223 224 /** 225 * ieee_fpe_handler - Handle denormalized number exception 226 * 227 * @regs: Pointer to register structure 228 * 229 * Returns 1 when it's handled (should not cause exception). 230 */ 231 static int ieee_fpe_handler(struct pt_regs *regs) 232 { 233 unsigned short insn = *(unsigned short *)regs->pc; 234 unsigned short finsn; 235 unsigned long nextpc; 236 int nib[4] = { 237 (insn >> 12) & 0xf, 238 (insn >> 8) & 0xf, 239 (insn >> 4) & 0xf, 240 insn & 0xf 241 }; 242 243 if (nib[0] == 0xb || (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb)) 244 regs->pr = regs->pc + 4; /* bsr & jsr */ 245 246 if (nib[0] == 0xa || nib[0] == 0xb) { 247 /* bra & bsr */ 248 nextpc = regs->pc + 4 + ((short)((insn & 0xfff) << 4) >> 3); 249 finsn = *(unsigned short *)(regs->pc + 2); 250 } else if (nib[0] == 0x8 && nib[1] == 0xd) { 251 /* bt/s */ 252 if (regs->sr & 1) 253 nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1); 254 else 255 nextpc = regs->pc + 4; 256 finsn = *(unsigned short *)(regs->pc + 2); 257 } else if (nib[0] == 0x8 && nib[1] == 0xf) { 258 /* bf/s */ 259 if (regs->sr & 1) 260 nextpc = regs->pc + 4; 261 else 262 nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1); 263 finsn = *(unsigned short *)(regs->pc + 2); 264 } else if (nib[0] == 0x4 && nib[3] == 0xb && 265 (nib[2] == 0x0 || nib[2] == 0x2)) { 266 /* jmp & jsr */ 267 nextpc = regs->regs[nib[1]]; 268 finsn = *(unsigned short *)(regs->pc + 2); 269 } else if (nib[0] == 0x0 && nib[3] == 0x3 && 270 (nib[2] == 0x0 || nib[2] == 0x2)) { 271 /* braf & bsrf */ 272 nextpc = regs->pc + 4 + regs->regs[nib[1]]; 273 finsn = *(unsigned short *)(regs->pc + 2); 274 } else if (insn == 0x000b) { 275 /* rts */ 276 nextpc = regs->pr; 277 finsn = *(unsigned short *)(regs->pc + 2); 278 } else { 279 nextpc = regs->pc + instruction_size(insn); 280 finsn = insn; 281 } 282 283 if ((finsn & 0xf1ff) == 0xf0ad) { 284 /* fcnvsd */ 285 struct task_struct *tsk = current; 286 287 save_fpu(tsk, regs); 288 if ((tsk->thread.fpu.hard.fpscr & FPSCR_CAUSE_ERROR)) 289 /* FPU error */ 290 denormal_to_double(&tsk->thread.fpu.hard, 291 (finsn >> 8) & 0xf); 292 else 293 return 0; 294 295 regs->pc = nextpc; 296 return 1; 297 } else if ((finsn & 0xf00f) == 0xf002) { 298 /* fmul */ 299 struct task_struct *tsk = current; 300 int fpscr; 301 int n, m, prec; 302 unsigned int hx, hy; 303 304 n = (finsn >> 8) & 0xf; 305 m = (finsn >> 4) & 0xf; 306 hx = tsk->thread.fpu.hard.fp_regs[n]; 307 hy = tsk->thread.fpu.hard.fp_regs[m]; 308 fpscr = tsk->thread.fpu.hard.fpscr; 309 prec = fpscr & FPSCR_DBL_PRECISION; 310 311 if ((fpscr & FPSCR_CAUSE_ERROR) 312 && (prec && ((hx & 0x7fffffff) < 0x00100000 313 || (hy & 0x7fffffff) < 0x00100000))) { 314 long long llx, lly; 315 316 /* FPU error because of denormal (doubles) */ 317 llx = ((long long)hx << 32) 318 | tsk->thread.fpu.hard.fp_regs[n + 1]; 319 lly = ((long long)hy << 32) 320 | tsk->thread.fpu.hard.fp_regs[m + 1]; 321 llx = float64_mul(llx, lly); 322 tsk->thread.fpu.hard.fp_regs[n] = llx >> 32; 323 tsk->thread.fpu.hard.fp_regs[n + 1] = llx & 0xffffffff; 324 } else if ((fpscr & FPSCR_CAUSE_ERROR) 325 && (!prec && ((hx & 0x7fffffff) < 0x00800000 326 || (hy & 0x7fffffff) < 0x00800000))) { 327 /* FPU error because of denormal (floats) */ 328 hx = float32_mul(hx, hy); 329 tsk->thread.fpu.hard.fp_regs[n] = hx; 330 } else 331 return 0; 332 333 regs->pc = nextpc; 334 return 1; 335 } else if ((finsn & 0xf00e) == 0xf000) { 336 /* fadd, fsub */ 337 struct task_struct *tsk = current; 338 int fpscr; 339 int n, m, prec; 340 unsigned int hx, hy; 341 342 n = (finsn >> 8) & 0xf; 343 m = (finsn >> 4) & 0xf; 344 hx = tsk->thread.fpu.hard.fp_regs[n]; 345 hy = tsk->thread.fpu.hard.fp_regs[m]; 346 fpscr = tsk->thread.fpu.hard.fpscr; 347 prec = fpscr & FPSCR_DBL_PRECISION; 348 349 if ((fpscr & FPSCR_CAUSE_ERROR) 350 && (prec && ((hx & 0x7fffffff) < 0x00100000 351 || (hy & 0x7fffffff) < 0x00100000))) { 352 long long llx, lly; 353 354 /* FPU error because of denormal (doubles) */ 355 llx = ((long long)hx << 32) 356 | tsk->thread.fpu.hard.fp_regs[n + 1]; 357 lly = ((long long)hy << 32) 358 | tsk->thread.fpu.hard.fp_regs[m + 1]; 359 if ((finsn & 0xf00f) == 0xf000) 360 llx = float64_add(llx, lly); 361 else 362 llx = float64_sub(llx, lly); 363 tsk->thread.fpu.hard.fp_regs[n] = llx >> 32; 364 tsk->thread.fpu.hard.fp_regs[n + 1] = llx & 0xffffffff; 365 } else if ((fpscr & FPSCR_CAUSE_ERROR) 366 && (!prec && ((hx & 0x7fffffff) < 0x00800000 367 || (hy & 0x7fffffff) < 0x00800000))) { 368 /* FPU error because of denormal (floats) */ 369 if ((finsn & 0xf00f) == 0xf000) 370 hx = float32_add(hx, hy); 371 else 372 hx = float32_sub(hx, hy); 373 tsk->thread.fpu.hard.fp_regs[n] = hx; 374 } else 375 return 0; 376 377 regs->pc = nextpc; 378 return 1; 379 } else if ((finsn & 0xf003) == 0xf003) { 380 /* fdiv */ 381 struct task_struct *tsk = current; 382 int fpscr; 383 int n, m, prec; 384 unsigned int hx, hy; 385 386 n = (finsn >> 8) & 0xf; 387 m = (finsn >> 4) & 0xf; 388 hx = tsk->thread.fpu.hard.fp_regs[n]; 389 hy = tsk->thread.fpu.hard.fp_regs[m]; 390 fpscr = tsk->thread.fpu.hard.fpscr; 391 prec = fpscr & FPSCR_DBL_PRECISION; 392 393 if ((fpscr & FPSCR_CAUSE_ERROR) 394 && (prec && ((hx & 0x7fffffff) < 0x00100000 395 || (hy & 0x7fffffff) < 0x00100000))) { 396 long long llx, lly; 397 398 /* FPU error because of denormal (doubles) */ 399 llx = ((long long)hx << 32) 400 | tsk->thread.fpu.hard.fp_regs[n + 1]; 401 lly = ((long long)hy << 32) 402 | tsk->thread.fpu.hard.fp_regs[m + 1]; 403 404 llx = float64_div(llx, lly); 405 406 tsk->thread.fpu.hard.fp_regs[n] = llx >> 32; 407 tsk->thread.fpu.hard.fp_regs[n + 1] = llx & 0xffffffff; 408 } else if ((fpscr & FPSCR_CAUSE_ERROR) 409 && (!prec && ((hx & 0x7fffffff) < 0x00800000 410 || (hy & 0x7fffffff) < 0x00800000))) { 411 /* FPU error because of denormal (floats) */ 412 hx = float32_div(hx, hy); 413 tsk->thread.fpu.hard.fp_regs[n] = hx; 414 } else 415 return 0; 416 417 regs->pc = nextpc; 418 return 1; 419 } 420 421 return 0; 422 } 423 424 void float_raise(unsigned int flags) 425 { 426 fpu_exception_flags |= flags; 427 } 428 429 int float_rounding_mode(void) 430 { 431 struct task_struct *tsk = current; 432 int roundingMode = FPSCR_ROUNDING_MODE(tsk->thread.fpu.hard.fpscr); 433 return roundingMode; 434 } 435 436 BUILD_TRAP_HANDLER(fpu_error) 437 { 438 struct task_struct *tsk = current; 439 TRAP_HANDLER_DECL; 440 441 save_fpu(tsk, regs); 442 fpu_exception_flags = 0; 443 if (ieee_fpe_handler(regs)) { 444 tsk->thread.fpu.hard.fpscr &= 445 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK); 446 tsk->thread.fpu.hard.fpscr |= fpu_exception_flags; 447 /* Set the FPSCR flag as well as cause bits - simply 448 * replicate the cause */ 449 tsk->thread.fpu.hard.fpscr |= (fpu_exception_flags >> 10); 450 grab_fpu(regs); 451 restore_fpu(tsk); 452 set_tsk_thread_flag(tsk, TIF_USEDFPU); 453 if ((((tsk->thread.fpu.hard.fpscr & FPSCR_ENABLE_MASK) >> 7) & 454 (fpu_exception_flags >> 2)) == 0) { 455 return; 456 } 457 } 458 459 force_sig(SIGFPE, tsk); 460 } 461 462 BUILD_TRAP_HANDLER(fpu_state_restore) 463 { 464 struct task_struct *tsk = current; 465 TRAP_HANDLER_DECL; 466 467 grab_fpu(regs); 468 if (!user_mode(regs)) { 469 printk(KERN_ERR "BUG: FPU is used in kernel mode.\n"); 470 return; 471 } 472 473 if (used_math()) { 474 /* Using the FPU again. */ 475 restore_fpu(tsk); 476 } else { 477 /* First time FPU user. */ 478 fpu_init(); 479 set_used_math(); 480 } 481 set_tsk_thread_flag(tsk, TIF_USEDFPU); 482 } 483