Merge branch 'release' of git://lm-sensors.org/kernel/mhoffman/hwmon-2.6
[linux-2.6] / arch / sh / kernel / cpu / sh4 / fpu.c
1 /* $Id: fpu.c,v 1.4 2004/01/13 05:52:11 kkojima Exp $
2  *
3  * linux/arch/sh/kernel/fpu.c
4  *
5  * Save/restore floating point context for signal handlers.
6  *
7  * This file is subject to the terms and conditions of the GNU General Public
8  * License.  See the file "COPYING" in the main directory of this archive
9  * for more details.
10  *
11  * Copyright (C) 1999, 2000  Kaz Kojima & Niibe Yutaka
12  *
13  * FIXME! These routines can be optimized in big endian case.
14  */
15
16 #include <linux/sched.h>
17 #include <linux/signal.h>
18 #include <asm/processor.h>
19 #include <asm/system.h>
20 #include <asm/io.h>
21
22 /* The PR (precision) bit in the FP Status Register must be clear when
23  * an frchg instruction is executed, otherwise the instruction is undefined.
24  * Executing frchg with PR set causes a trap on some SH4 implementations.
25  */
26
27 #define FPSCR_RCHG 0x00000000
28
29
30 /*
31  * Save FPU registers onto task structure.
32  * Assume called with FPU enabled (SR.FD=0).
33  */
34 void
35 save_fpu(struct task_struct *tsk, struct pt_regs *regs)
36 {
37         unsigned long dummy;
38
39         clear_tsk_thread_flag(tsk, TIF_USEDFPU);
40         enable_fpu();
41         asm volatile("sts.l     fpul, @-%0\n\t"
42                      "sts.l     fpscr, @-%0\n\t"
43                      "lds       %2, fpscr\n\t"
44                      "frchg\n\t"
45                      "fmov.s    fr15, @-%0\n\t"
46                      "fmov.s    fr14, @-%0\n\t"
47                      "fmov.s    fr13, @-%0\n\t"
48                      "fmov.s    fr12, @-%0\n\t"
49                      "fmov.s    fr11, @-%0\n\t"
50                      "fmov.s    fr10, @-%0\n\t"
51                      "fmov.s    fr9, @-%0\n\t"
52                      "fmov.s    fr8, @-%0\n\t"
53                      "fmov.s    fr7, @-%0\n\t"
54                      "fmov.s    fr6, @-%0\n\t"
55                      "fmov.s    fr5, @-%0\n\t"
56                      "fmov.s    fr4, @-%0\n\t"
57                      "fmov.s    fr3, @-%0\n\t"
58                      "fmov.s    fr2, @-%0\n\t"
59                      "fmov.s    fr1, @-%0\n\t"
60                      "fmov.s    fr0, @-%0\n\t"
61                      "frchg\n\t"
62                      "fmov.s    fr15, @-%0\n\t"
63                      "fmov.s    fr14, @-%0\n\t"
64                      "fmov.s    fr13, @-%0\n\t"
65                      "fmov.s    fr12, @-%0\n\t"
66                      "fmov.s    fr11, @-%0\n\t"
67                      "fmov.s    fr10, @-%0\n\t"
68                      "fmov.s    fr9, @-%0\n\t"
69                      "fmov.s    fr8, @-%0\n\t"
70                      "fmov.s    fr7, @-%0\n\t"
71                      "fmov.s    fr6, @-%0\n\t"
72                      "fmov.s    fr5, @-%0\n\t"
73                      "fmov.s    fr4, @-%0\n\t"
74                      "fmov.s    fr3, @-%0\n\t"
75                      "fmov.s    fr2, @-%0\n\t"
76                      "fmov.s    fr1, @-%0\n\t"
77                      "fmov.s    fr0, @-%0\n\t"
78                      "lds       %3, fpscr\n\t"
79                      : "=r" (dummy)
80                      : "0" ((char *)(&tsk->thread.fpu.hard.status)),
81                        "r" (FPSCR_RCHG),
82                        "r" (FPSCR_INIT)
83                      : "memory");
84
85         disable_fpu();
86         release_fpu(regs);
87 }
88
89 static void
90 restore_fpu(struct task_struct *tsk)
91 {
92         unsigned long dummy;
93
94         enable_fpu();
95         asm volatile("lds       %2, fpscr\n\t"
96                      "fmov.s    @%0+, fr0\n\t"
97                      "fmov.s    @%0+, fr1\n\t"
98                      "fmov.s    @%0+, fr2\n\t"
99                      "fmov.s    @%0+, fr3\n\t"
100                      "fmov.s    @%0+, fr4\n\t"
101                      "fmov.s    @%0+, fr5\n\t"
102                      "fmov.s    @%0+, fr6\n\t"
103                      "fmov.s    @%0+, fr7\n\t"
104                      "fmov.s    @%0+, fr8\n\t"
105                      "fmov.s    @%0+, fr9\n\t"
106                      "fmov.s    @%0+, fr10\n\t"
107                      "fmov.s    @%0+, fr11\n\t"
108                      "fmov.s    @%0+, fr12\n\t"
109                      "fmov.s    @%0+, fr13\n\t"
110                      "fmov.s    @%0+, fr14\n\t"
111                      "fmov.s    @%0+, fr15\n\t"
112                      "frchg\n\t"
113                      "fmov.s    @%0+, fr0\n\t"
114                      "fmov.s    @%0+, fr1\n\t"
115                      "fmov.s    @%0+, fr2\n\t"
116                      "fmov.s    @%0+, fr3\n\t"
117                      "fmov.s    @%0+, fr4\n\t"
118                      "fmov.s    @%0+, fr5\n\t"
119                      "fmov.s    @%0+, fr6\n\t"
120                      "fmov.s    @%0+, fr7\n\t"
121                      "fmov.s    @%0+, fr8\n\t"
122                      "fmov.s    @%0+, fr9\n\t"
123                      "fmov.s    @%0+, fr10\n\t"
124                      "fmov.s    @%0+, fr11\n\t"
125                      "fmov.s    @%0+, fr12\n\t"
126                      "fmov.s    @%0+, fr13\n\t"
127                      "fmov.s    @%0+, fr14\n\t"
128                      "fmov.s    @%0+, fr15\n\t"
129                      "frchg\n\t"
130                      "lds.l     @%0+, fpscr\n\t"
131                      "lds.l     @%0+, fpul\n\t"
132                      : "=r" (dummy)
133                      : "0" (&tsk->thread.fpu), "r" (FPSCR_RCHG)
134                      : "memory");
135         disable_fpu();
136 }
137
138 /*
139  * Load the FPU with signalling NANS.  This bit pattern we're using
140  * has the property that no matter wether considered as single or as
141  * double precision represents signaling NANS.  
142  */
143
144 static void
145 fpu_init(void)
146 {
147         enable_fpu();
148         asm volatile("lds       %0, fpul\n\t"
149                      "lds       %1, fpscr\n\t"
150                      "fsts      fpul, fr0\n\t"
151                      "fsts      fpul, fr1\n\t"
152                      "fsts      fpul, fr2\n\t"
153                      "fsts      fpul, fr3\n\t"
154                      "fsts      fpul, fr4\n\t"
155                      "fsts      fpul, fr5\n\t"
156                      "fsts      fpul, fr6\n\t"
157                      "fsts      fpul, fr7\n\t"
158                      "fsts      fpul, fr8\n\t"
159                      "fsts      fpul, fr9\n\t"
160                      "fsts      fpul, fr10\n\t"
161                      "fsts      fpul, fr11\n\t"
162                      "fsts      fpul, fr12\n\t"
163                      "fsts      fpul, fr13\n\t"
164                      "fsts      fpul, fr14\n\t"
165                      "fsts      fpul, fr15\n\t"
166                      "frchg\n\t"
167                      "fsts      fpul, fr0\n\t"
168                      "fsts      fpul, fr1\n\t"
169                      "fsts      fpul, fr2\n\t"
170                      "fsts      fpul, fr3\n\t"
171                      "fsts      fpul, fr4\n\t"
172                      "fsts      fpul, fr5\n\t"
173                      "fsts      fpul, fr6\n\t"
174                      "fsts      fpul, fr7\n\t"
175                      "fsts      fpul, fr8\n\t"
176                      "fsts      fpul, fr9\n\t"
177                      "fsts      fpul, fr10\n\t"
178                      "fsts      fpul, fr11\n\t"
179                      "fsts      fpul, fr12\n\t"
180                      "fsts      fpul, fr13\n\t"
181                      "fsts      fpul, fr14\n\t"
182                      "fsts      fpul, fr15\n\t"
183                      "frchg\n\t"
184                      "lds       %2, fpscr\n\t"
185                      : /* no output */
186                      : "r" (0), "r" (FPSCR_RCHG), "r" (FPSCR_INIT));
187         disable_fpu();
188 }
189
190 /**
191  *      denormal_to_double - Given denormalized float number,
192  *                           store double float
193  *
194  *      @fpu: Pointer to sh_fpu_hard structure
195  *      @n: Index to FP register
196  */
197 static void
198 denormal_to_double (struct sh_fpu_hard_struct *fpu, int n)
199 {
200         unsigned long du, dl;
201         unsigned long x = fpu->fpul;
202         int exp = 1023 - 126;
203
204         if (x != 0 && (x & 0x7f800000) == 0) {
205                 du = (x & 0x80000000);
206                 while ((x & 0x00800000) == 0) {
207                         x <<= 1;
208                         exp--;
209                 }
210                 x &= 0x007fffff;
211                 du |= (exp << 20) | (x >> 3);
212                 dl = x << 29;
213
214                 fpu->fp_regs[n] = du;
215                 fpu->fp_regs[n+1] = dl;
216         }
217 }
218
219 /**
220  *      ieee_fpe_handler - Handle denormalized number exception
221  *
222  *      @regs: Pointer to register structure
223  *
224  *      Returns 1 when it's handled (should not cause exception).
225  */
226 static int
227 ieee_fpe_handler (struct pt_regs *regs)
228 {
229         unsigned short insn = *(unsigned short *) regs->pc;
230         unsigned short finsn;
231         unsigned long nextpc;
232         int nib[4] = {
233                 (insn >> 12) & 0xf,
234                 (insn >> 8) & 0xf,
235                 (insn >> 4) & 0xf,
236                 insn & 0xf};
237
238         if (nib[0] == 0xb ||
239             (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb)) /* bsr & jsr */
240                 regs->pr = regs->pc + 4;
241   
242         if (nib[0] == 0xa || nib[0] == 0xb) { /* bra & bsr */
243                 nextpc = regs->pc + 4 + ((short) ((insn & 0xfff) << 4) >> 3);
244                 finsn = *(unsigned short *) (regs->pc + 2);
245         } else if (nib[0] == 0x8 && nib[1] == 0xd) { /* bt/s */
246                 if (regs->sr & 1)
247                         nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1);
248                 else
249                         nextpc = regs->pc + 4;
250                 finsn = *(unsigned short *) (regs->pc + 2);
251         } else if (nib[0] == 0x8 && nib[1] == 0xf) { /* bf/s */
252                 if (regs->sr & 1)
253                         nextpc = regs->pc + 4;
254                 else
255                         nextpc = regs->pc + 4 + ((char) (insn & 0xff) << 1);
256                 finsn = *(unsigned short *) (regs->pc + 2);
257         } else if (nib[0] == 0x4 && nib[3] == 0xb &&
258                  (nib[2] == 0x0 || nib[2] == 0x2)) { /* jmp & jsr */
259                 nextpc = regs->regs[nib[1]];
260                 finsn = *(unsigned short *) (regs->pc + 2);
261         } else if (nib[0] == 0x0 && nib[3] == 0x3 &&
262                  (nib[2] == 0x0 || nib[2] == 0x2)) { /* braf & bsrf */
263                 nextpc = regs->pc + 4 + regs->regs[nib[1]];
264                 finsn = *(unsigned short *) (regs->pc + 2);
265         } else if (insn == 0x000b) { /* rts */
266                 nextpc = regs->pr;
267                 finsn = *(unsigned short *) (regs->pc + 2);
268         } else {
269                 nextpc = regs->pc + instruction_size(insn);
270                 finsn = insn;
271         }
272
273         if ((finsn & 0xf1ff) == 0xf0ad) { /* fcnvsd */
274                 struct task_struct *tsk = current;
275
276                 save_fpu(tsk, regs);
277                 if ((tsk->thread.fpu.hard.fpscr & (1 << 17))) {
278                         /* FPU error */
279                         denormal_to_double (&tsk->thread.fpu.hard,
280                                             (finsn >> 8) & 0xf);
281                         tsk->thread.fpu.hard.fpscr &=
282                                 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK);
283                         grab_fpu(regs);
284                         restore_fpu(tsk);
285                         set_tsk_thread_flag(tsk, TIF_USEDFPU);
286                 } else
287                         force_sig(SIGFPE, tsk);
288
289                 regs->pc = nextpc;
290                 return 1;
291         }
292
293         return 0;
294 }
295
296 asmlinkage void
297 do_fpu_error(unsigned long r4, unsigned long r5, unsigned long r6,
298              unsigned long r7, struct pt_regs __regs)
299 {
300         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
301         struct task_struct *tsk = current;
302
303         if (ieee_fpe_handler(regs))
304                 return;
305
306         regs->pc += 2;
307         save_fpu(tsk, regs);
308         force_sig(SIGFPE, tsk);
309 }
310
311 asmlinkage void
312 do_fpu_state_restore(unsigned long r4, unsigned long r5, unsigned long r6,
313                      unsigned long r7, struct pt_regs __regs)
314 {
315         struct pt_regs *regs = RELOC_HIDE(&__regs, 0);
316         struct task_struct *tsk = current;
317
318         grab_fpu(regs);
319         if (!user_mode(regs)) {
320                 printk(KERN_ERR "BUG: FPU is used in kernel mode.\n");
321                 return;
322         }
323
324         if (used_math()) {
325                 /* Using the FPU again.  */
326                 restore_fpu(tsk);
327         } else  {
328                 /* First time FPU user.  */
329                 fpu_init();
330                 set_used_math();
331         }
332         set_tsk_thread_flag(tsk, TIF_USEDFPU);
333 }