?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
spi-topcliff-pch: add recovery processing in case wait-event timeout
[zen-stable.git] / arch / sh / kernel / cpu / sh4 / fpu.c
blob447482d7f65e6bdf573c33dabe67317afbe19cdd
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 <cpu/fpu.h>
17 #include <asm/processor.h>
18 #include <asm/system.h>
19 #include <asm/fpu.h>
20
21 /* The PR (precision) bit in the FP Status Register must be clear when
22 * an frchg instruction is executed, otherwise the instruction is undefined.
23 * Executing frchg with PR set causes a trap on some SH4 implementations.
24 */
25
26 #define FPSCR_RCHG 0x00000000
27 extern unsigned long long float64_div(unsigned long long a,
28 unsigned long long b);
29 extern unsigned long int float32_div(unsigned long int a, unsigned long int b);
30 extern unsigned long long float64_mul(unsigned long long a,
31 unsigned long long b);
32 extern unsigned long int float32_mul(unsigned long int a, unsigned long int b);
33 extern unsigned long long float64_add(unsigned long long a,
34 unsigned long long b);
35 extern unsigned long int float32_add(unsigned long int a, unsigned long int b);
36 extern unsigned long long float64_sub(unsigned long long a,
37 unsigned long long b);
38 extern unsigned long int float32_sub(unsigned long int a, unsigned long int b);
39 extern unsigned long int float64_to_float32(unsigned long long a);
40 static unsigned int fpu_exception_flags;
41
42 /*
43 * Save FPU registers onto task structure.
44 */
45 void save_fpu(struct task_struct *tsk)
46 {
47 unsigned long dummy;
48
49 enable_fpu();
50 asm volatile ("sts.l fpul, @-%0\n\t"
51 "sts.l fpscr, @-%0\n\t"
52 "lds %2, fpscr\n\t"
53 "frchg\n\t"
54 "fmov.s fr15, @-%0\n\t"
55 "fmov.s fr14, @-%0\n\t"
56 "fmov.s fr13, @-%0\n\t"
57 "fmov.s fr12, @-%0\n\t"
58 "fmov.s fr11, @-%0\n\t"
59 "fmov.s fr10, @-%0\n\t"
60 "fmov.s fr9, @-%0\n\t"
61 "fmov.s fr8, @-%0\n\t"
62 "fmov.s fr7, @-%0\n\t"
63 "fmov.s fr6, @-%0\n\t"
64 "fmov.s fr5, @-%0\n\t"
65 "fmov.s fr4, @-%0\n\t"
66 "fmov.s fr3, @-%0\n\t"
67 "fmov.s fr2, @-%0\n\t"
68 "fmov.s fr1, @-%0\n\t"
69 "fmov.s fr0, @-%0\n\t"
70 "frchg\n\t"
71 "fmov.s fr15, @-%0\n\t"
72 "fmov.s fr14, @-%0\n\t"
73 "fmov.s fr13, @-%0\n\t"
74 "fmov.s fr12, @-%0\n\t"
75 "fmov.s fr11, @-%0\n\t"
76 "fmov.s fr10, @-%0\n\t"
77 "fmov.s fr9, @-%0\n\t"
78 "fmov.s fr8, @-%0\n\t"
79 "fmov.s fr7, @-%0\n\t"
80 "fmov.s fr6, @-%0\n\t"
81 "fmov.s fr5, @-%0\n\t"
82 "fmov.s fr4, @-%0\n\t"
83 "fmov.s fr3, @-%0\n\t"
84 "fmov.s fr2, @-%0\n\t"
85 "fmov.s fr1, @-%0\n\t"
86 "fmov.s fr0, @-%0\n\t"
87 "lds %3, fpscr\n\t":"=r" (dummy)
88 :"0"((char *)(&tsk->thread.xstate->hardfpu.status)),
89 "r"(FPSCR_RCHG), "r"(FPSCR_INIT)
90 :"memory");
91
92 disable_fpu();
93 }
94
95 void restore_fpu(struct task_struct *tsk)
96 {
97 unsigned long dummy;
98
99 enable_fpu();
100 asm volatile ("lds %2, fpscr\n\t"
101 "fmov.s @%0+, fr0\n\t"
102 "fmov.s @%0+, fr1\n\t"
103 "fmov.s @%0+, fr2\n\t"
104 "fmov.s @%0+, fr3\n\t"
105 "fmov.s @%0+, fr4\n\t"
106 "fmov.s @%0+, fr5\n\t"
107 "fmov.s @%0+, fr6\n\t"
108 "fmov.s @%0+, fr7\n\t"
109 "fmov.s @%0+, fr8\n\t"
110 "fmov.s @%0+, fr9\n\t"
111 "fmov.s @%0+, fr10\n\t"
112 "fmov.s @%0+, fr11\n\t"
113 "fmov.s @%0+, fr12\n\t"
114 "fmov.s @%0+, fr13\n\t"
115 "fmov.s @%0+, fr14\n\t"
116 "fmov.s @%0+, fr15\n\t"
117 "frchg\n\t"
118 "fmov.s @%0+, fr0\n\t"
119 "fmov.s @%0+, fr1\n\t"
120 "fmov.s @%0+, fr2\n\t"
121 "fmov.s @%0+, fr3\n\t"
122 "fmov.s @%0+, fr4\n\t"
123 "fmov.s @%0+, fr5\n\t"
124 "fmov.s @%0+, fr6\n\t"
125 "fmov.s @%0+, fr7\n\t"
126 "fmov.s @%0+, fr8\n\t"
127 "fmov.s @%0+, fr9\n\t"
128 "fmov.s @%0+, fr10\n\t"
129 "fmov.s @%0+, fr11\n\t"
130 "fmov.s @%0+, fr12\n\t"
131 "fmov.s @%0+, fr13\n\t"
132 "fmov.s @%0+, fr14\n\t"
133 "fmov.s @%0+, fr15\n\t"
134 "frchg\n\t"
135 "lds.l @%0+, fpscr\n\t"
136 "lds.l @%0+, fpul\n\t"
137 :"=r" (dummy)
138 :"0" (tsk->thread.xstate), "r" (FPSCR_RCHG)
139 :"memory");
140 disable_fpu();
141 }
142
143 /**
144 * denormal_to_double - Given denormalized float number,
145 * store double float
146 *
147 * @fpu: Pointer to sh_fpu_hard structure
148 * @n: Index to FP register
149 */
150 static void denormal_to_double(struct sh_fpu_hard_struct *fpu, int n)
151 {
152 unsigned long du, dl;
153 unsigned long x = fpu->fpul;
154 int exp = 1023 - 126;
155
156 if (x != 0 && (x & 0x7f800000) == 0) {
157 du = (x & 0x80000000);
158 while ((x & 0x00800000) == 0) {
159 x <<= 1;
160 exp--;
161 }
162 x &= 0x007fffff;
163 du |= (exp << 20) | (x >> 3);
164 dl = x << 29;
165
166 fpu->fp_regs[n] = du;
167 fpu->fp_regs[n + 1] = dl;
168 }
169 }
170
171 /**
172 * ieee_fpe_handler - Handle denormalized number exception
173 *
174 * @regs: Pointer to register structure
175 *
176 * Returns 1 when it's handled (should not cause exception).
177 */
178 static int ieee_fpe_handler(struct pt_regs *regs)
179 {
180 unsigned short insn = *(unsigned short *)regs->pc;
181 unsigned short finsn;
182 unsigned long nextpc;
183 int nib[4] = {
184 (insn >> 12) & 0xf,
185 (insn >> 8) & 0xf,
186 (insn >> 4) & 0xf,
187 insn & 0xf
188 };
189
190 if (nib[0] == 0xb || (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb))
191 regs->pr = regs->pc + 4; /* bsr & jsr */
192
193 if (nib[0] == 0xa || nib[0] == 0xb) {
194 /* bra & bsr */
195 nextpc = regs->pc + 4 + ((short)((insn & 0xfff) << 4) >> 3);
196 finsn = *(unsigned short *)(regs->pc + 2);
197 } else if (nib[0] == 0x8 && nib[1] == 0xd) {
198 /* bt/s */
199 if (regs->sr & 1)
200 nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1);
201 else
202 nextpc = regs->pc + 4;
203 finsn = *(unsigned short *)(regs->pc + 2);
204 } else if (nib[0] == 0x8 && nib[1] == 0xf) {
205 /* bf/s */
206 if (regs->sr & 1)
207 nextpc = regs->pc + 4;
208 else
209 nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1);
210 finsn = *(unsigned short *)(regs->pc + 2);
211 } else if (nib[0] == 0x4 && nib[3] == 0xb &&
212 (nib[2] == 0x0 || nib[2] == 0x2)) {
213 /* jmp & jsr */
214 nextpc = regs->regs[nib[1]];
215 finsn = *(unsigned short *)(regs->pc + 2);
216 } else if (nib[0] == 0x0 && nib[3] == 0x3 &&
217 (nib[2] == 0x0 || nib[2] == 0x2)) {
218 /* braf & bsrf */
219 nextpc = regs->pc + 4 + regs->regs[nib[1]];
220 finsn = *(unsigned short *)(regs->pc + 2);
221 } else if (insn == 0x000b) {
222 /* rts */
223 nextpc = regs->pr;
224 finsn = *(unsigned short *)(regs->pc + 2);
225 } else {
226 nextpc = regs->pc + instruction_size(insn);
227 finsn = insn;
228 }
229
230 if ((finsn & 0xf1ff) == 0xf0ad) {
231 /* fcnvsd */
232 struct task_struct *tsk = current;
233
234 if ((tsk->thread.xstate->hardfpu.fpscr & FPSCR_CAUSE_ERROR))
235 /* FPU error */
236 denormal_to_double(&tsk->thread.xstate->hardfpu,
237 (finsn >> 8) & 0xf);
238 else
239 return 0;
240
241 regs->pc = nextpc;
242 return 1;
243 } else if ((finsn & 0xf00f) == 0xf002) {
244 /* fmul */
245 struct task_struct *tsk = current;
246 int fpscr;
247 int n, m, prec;
248 unsigned int hx, hy;
249
250 n = (finsn >> 8) & 0xf;
251 m = (finsn >> 4) & 0xf;
252 hx = tsk->thread.xstate->hardfpu.fp_regs[n];
253 hy = tsk->thread.xstate->hardfpu.fp_regs[m];
254 fpscr = tsk->thread.xstate->hardfpu.fpscr;
255 prec = fpscr & FPSCR_DBL_PRECISION;
256
257 if ((fpscr & FPSCR_CAUSE_ERROR)
258 && (prec && ((hx & 0x7fffffff) < 0x00100000
259 || (hy & 0x7fffffff) < 0x00100000))) {
260 long long llx, lly;
261
262 /* FPU error because of denormal (doubles) */
263 llx = ((long long)hx << 32)
264 | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
265 lly = ((long long)hy << 32)
266 | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
267 llx = float64_mul(llx, lly);
268 tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
269 tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
270 } else if ((fpscr & FPSCR_CAUSE_ERROR)
271 && (!prec && ((hx & 0x7fffffff) < 0x00800000
272 || (hy & 0x7fffffff) < 0x00800000))) {
273 /* FPU error because of denormal (floats) */
274 hx = float32_mul(hx, hy);
275 tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
276 } else
277 return 0;
278
279 regs->pc = nextpc;
280 return 1;
281 } else if ((finsn & 0xf00e) == 0xf000) {
282 /* fadd, fsub */
283 struct task_struct *tsk = current;
284 int fpscr;
285 int n, m, prec;
286 unsigned int hx, hy;
287
288 n = (finsn >> 8) & 0xf;
289 m = (finsn >> 4) & 0xf;
290 hx = tsk->thread.xstate->hardfpu.fp_regs[n];
291 hy = tsk->thread.xstate->hardfpu.fp_regs[m];
292 fpscr = tsk->thread.xstate->hardfpu.fpscr;
293 prec = fpscr & FPSCR_DBL_PRECISION;
294
295 if ((fpscr & FPSCR_CAUSE_ERROR)
296 && (prec && ((hx & 0x7fffffff) < 0x00100000
297 || (hy & 0x7fffffff) < 0x00100000))) {
298 long long llx, lly;
299
300 /* FPU error because of denormal (doubles) */
301 llx = ((long long)hx << 32)
302 | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
303 lly = ((long long)hy << 32)
304 | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
305 if ((finsn & 0xf00f) == 0xf000)
306 llx = float64_add(llx, lly);
307 else
308 llx = float64_sub(llx, lly);
309 tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
310 tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
311 } else if ((fpscr & FPSCR_CAUSE_ERROR)
312 && (!prec && ((hx & 0x7fffffff) < 0x00800000
313 || (hy & 0x7fffffff) < 0x00800000))) {
314 /* FPU error because of denormal (floats) */
315 if ((finsn & 0xf00f) == 0xf000)
316 hx = float32_add(hx, hy);
317 else
318 hx = float32_sub(hx, hy);
319 tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
320 } else
321 return 0;
322
323 regs->pc = nextpc;
324 return 1;
325 } else if ((finsn & 0xf003) == 0xf003) {
326 /* fdiv */
327 struct task_struct *tsk = current;
328 int fpscr;
329 int n, m, prec;
330 unsigned int hx, hy;
331
332 n = (finsn >> 8) & 0xf;
333 m = (finsn >> 4) & 0xf;
334 hx = tsk->thread.xstate->hardfpu.fp_regs[n];
335 hy = tsk->thread.xstate->hardfpu.fp_regs[m];
336 fpscr = tsk->thread.xstate->hardfpu.fpscr;
337 prec = fpscr & FPSCR_DBL_PRECISION;
338
339 if ((fpscr & FPSCR_CAUSE_ERROR)
340 && (prec && ((hx & 0x7fffffff) < 0x00100000
341 || (hy & 0x7fffffff) < 0x00100000))) {
342 long long llx, lly;
343
344 /* FPU error because of denormal (doubles) */
345 llx = ((long long)hx << 32)
346 | tsk->thread.xstate->hardfpu.fp_regs[n + 1];
347 lly = ((long long)hy << 32)
348 | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
349
350 llx = float64_div(llx, lly);
351
352 tsk->thread.xstate->hardfpu.fp_regs[n] = llx >> 32;
353 tsk->thread.xstate->hardfpu.fp_regs[n + 1] = llx & 0xffffffff;
354 } else if ((fpscr & FPSCR_CAUSE_ERROR)
355 && (!prec && ((hx & 0x7fffffff) < 0x00800000
356 || (hy & 0x7fffffff) < 0x00800000))) {
357 /* FPU error because of denormal (floats) */
358 hx = float32_div(hx, hy);
359 tsk->thread.xstate->hardfpu.fp_regs[n] = hx;
360 } else
361 return 0;
362
363 regs->pc = nextpc;
364 return 1;
365 } else if ((finsn & 0xf0bd) == 0xf0bd) {
366 /* fcnvds - double to single precision convert */
367 struct task_struct *tsk = current;
368 int m;
369 unsigned int hx;
370
371 m = (finsn >> 8) & 0x7;
372 hx = tsk->thread.xstate->hardfpu.fp_regs[m];
373
374 if ((tsk->thread.xstate->hardfpu.fpscr & FPSCR_CAUSE_ERROR)
375 && ((hx & 0x7fffffff) < 0x00100000)) {
376 /* subnormal double to float conversion */
377 long long llx;
378
379 llx = ((long long)tsk->thread.xstate->hardfpu.fp_regs[m] << 32)
380 | tsk->thread.xstate->hardfpu.fp_regs[m + 1];
381
382 tsk->thread.xstate->hardfpu.fpul = float64_to_float32(llx);
383 } else
384 return 0;
385
386 regs->pc = nextpc;
387 return 1;
388 }
389
390 return 0;
391 }
392
393 void float_raise(unsigned int flags)
394 {
395 fpu_exception_flags |= flags;
396 }
397
398 int float_rounding_mode(void)
399 {
400 struct task_struct *tsk = current;
401 int roundingMode = FPSCR_ROUNDING_MODE(tsk->thread.xstate->hardfpu.fpscr);
402 return roundingMode;
403 }
404
405 BUILD_TRAP_HANDLER(fpu_error)
406 {
407 struct task_struct *tsk = current;
408 TRAP_HANDLER_DECL;
409
410 __unlazy_fpu(tsk, regs);
411 fpu_exception_flags = 0;
412 if (ieee_fpe_handler(regs)) {
413 tsk->thread.xstate->hardfpu.fpscr &=
414 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK);
415 tsk->thread.xstate->hardfpu.fpscr |= fpu_exception_flags;
416 /* Set the FPSCR flag as well as cause bits - simply
417 * replicate the cause */
418 tsk->thread.xstate->hardfpu.fpscr |= (fpu_exception_flags >> 10);
419 grab_fpu(regs);
420 restore_fpu(tsk);
421 task_thread_info(tsk)->status |= TS_USEDFPU;
422 if ((((tsk->thread.xstate->hardfpu.fpscr & FPSCR_ENABLE_MASK) >> 7) &
423 (fpu_exception_flags >> 2)) == 0) {
424 return;
425 }
426 }
427
428 force_sig(SIGFPE, tsk);
429 }