Merge git://git.kernel.org/pub/scm/linux/kernel/git/davem/sparc-2.6
[linux-2.6] / arch / sh / kernel / cpu / sh2a / fpu.c
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 }