?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
Some modifications to files needed to succesfully compile ;)
[wrt350n-kernel.git] / arch / sh / kernel / cpu / sh2a / fpu.c
blobff99562456fbe50f80308f479c960d617fb662bb
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 }
WRT350N Kernel Patches