1
2
3
4
5
6
7
8
9
10
11
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
22
23
24
25
26#define FPSCR_RCHG 0x00000000
27extern unsigned long long float64_div(unsigned long long a,
28 unsigned long long b);
29extern unsigned long int float32_div(unsigned long int a, unsigned long int b);
30extern unsigned long long float64_mul(unsigned long long a,
31 unsigned long long b);
32extern unsigned long int float32_mul(unsigned long int a, unsigned long int b);
33extern unsigned long long float64_add(unsigned long long a,
34 unsigned long long b);
35extern unsigned long int float32_add(unsigned long int a, unsigned long int b);
36extern unsigned long long float64_sub(unsigned long long a,
37 unsigned long long b);
38extern unsigned long int float32_sub(unsigned long int a, unsigned long int b);
39extern unsigned long int float64_to_float32(unsigned long long a);
40static unsigned int fpu_exception_flags;
41
42
43
44
45
46void save_fpu(struct task_struct *tsk, struct pt_regs *regs)
47{
48 unsigned long dummy;
49
50 clear_tsk_thread_flag(tsk, TIF_USEDFPU);
51 enable_fpu();
52 asm volatile ("sts.l fpul, @-%0\n\t"
53 "sts.l fpscr, @-%0\n\t"
54 "lds %2, fpscr\n\t"
55 "frchg\n\t"
56 "fmov.s fr15, @-%0\n\t"
57 "fmov.s fr14, @-%0\n\t"
58 "fmov.s fr13, @-%0\n\t"
59 "fmov.s fr12, @-%0\n\t"
60 "fmov.s fr11, @-%0\n\t"
61 "fmov.s fr10, @-%0\n\t"
62 "fmov.s fr9, @-%0\n\t"
63 "fmov.s fr8, @-%0\n\t"
64 "fmov.s fr7, @-%0\n\t"
65 "fmov.s fr6, @-%0\n\t"
66 "fmov.s fr5, @-%0\n\t"
67 "fmov.s fr4, @-%0\n\t"
68 "fmov.s fr3, @-%0\n\t"
69 "fmov.s fr2, @-%0\n\t"
70 "fmov.s fr1, @-%0\n\t"
71 "fmov.s fr0, @-%0\n\t"
72 "frchg\n\t"
73 "fmov.s fr15, @-%0\n\t"
74 "fmov.s fr14, @-%0\n\t"
75 "fmov.s fr13, @-%0\n\t"
76 "fmov.s fr12, @-%0\n\t"
77 "fmov.s fr11, @-%0\n\t"
78 "fmov.s fr10, @-%0\n\t"
79 "fmov.s fr9, @-%0\n\t"
80 "fmov.s fr8, @-%0\n\t"
81 "fmov.s fr7, @-%0\n\t"
82 "fmov.s fr6, @-%0\n\t"
83 "fmov.s fr5, @-%0\n\t"
84 "fmov.s fr4, @-%0\n\t"
85 "fmov.s fr3, @-%0\n\t"
86 "fmov.s fr2, @-%0\n\t"
87 "fmov.s fr1, @-%0\n\t"
88 "fmov.s fr0, @-%0\n\t"
89 "lds %3, fpscr\n\t":"=r" (dummy)
90 :"0"((char *)(&tsk->thread.fpu.hard.status)),
91 "r"(FPSCR_RCHG), "r"(FPSCR_INIT)
92 :"memory");
93
94 disable_fpu();
95 release_fpu(regs);
96}
97
98static void restore_fpu(struct task_struct *tsk)
99{
100 unsigned long dummy;
101
102 enable_fpu();
103 asm volatile ("lds %2, fpscr\n\t"
104 "fmov.s @%0+, fr0\n\t"
105 "fmov.s @%0+, fr1\n\t"
106 "fmov.s @%0+, fr2\n\t"
107 "fmov.s @%0+, fr3\n\t"
108 "fmov.s @%0+, fr4\n\t"
109 "fmov.s @%0+, fr5\n\t"
110 "fmov.s @%0+, fr6\n\t"
111 "fmov.s @%0+, fr7\n\t"
112 "fmov.s @%0+, fr8\n\t"
113 "fmov.s @%0+, fr9\n\t"
114 "fmov.s @%0+, fr10\n\t"
115 "fmov.s @%0+, fr11\n\t"
116 "fmov.s @%0+, fr12\n\t"
117 "fmov.s @%0+, fr13\n\t"
118 "fmov.s @%0+, fr14\n\t"
119 "fmov.s @%0+, fr15\n\t"
120 "frchg\n\t"
121 "fmov.s @%0+, fr0\n\t"
122 "fmov.s @%0+, fr1\n\t"
123 "fmov.s @%0+, fr2\n\t"
124 "fmov.s @%0+, fr3\n\t"
125 "fmov.s @%0+, fr4\n\t"
126 "fmov.s @%0+, fr5\n\t"
127 "fmov.s @%0+, fr6\n\t"
128 "fmov.s @%0+, fr7\n\t"
129 "fmov.s @%0+, fr8\n\t"
130 "fmov.s @%0+, fr9\n\t"
131 "fmov.s @%0+, fr10\n\t"
132 "fmov.s @%0+, fr11\n\t"
133 "fmov.s @%0+, fr12\n\t"
134 "fmov.s @%0+, fr13\n\t"
135 "fmov.s @%0+, fr14\n\t"
136 "fmov.s @%0+, fr15\n\t"
137 "frchg\n\t"
138 "lds.l @%0+, fpscr\n\t"
139 "lds.l @%0+, fpul\n\t"
140 :"=r" (dummy)
141 :"0"(&tsk->thread.fpu), "r"(FPSCR_RCHG)
142 :"memory");
143 disable_fpu();
144}
145
146
147
148
149
150
151
152static void fpu_init(void)
153{
154 enable_fpu();
155 asm volatile ( "lds %0, fpul\n\t"
156 "lds %1, fpscr\n\t"
157 "fsts fpul, fr0\n\t"
158 "fsts fpul, fr1\n\t"
159 "fsts fpul, fr2\n\t"
160 "fsts fpul, fr3\n\t"
161 "fsts fpul, fr4\n\t"
162 "fsts fpul, fr5\n\t"
163 "fsts fpul, fr6\n\t"
164 "fsts fpul, fr7\n\t"
165 "fsts fpul, fr8\n\t"
166 "fsts fpul, fr9\n\t"
167 "fsts fpul, fr10\n\t"
168 "fsts fpul, fr11\n\t"
169 "fsts fpul, fr12\n\t"
170 "fsts fpul, fr13\n\t"
171 "fsts fpul, fr14\n\t"
172 "fsts fpul, fr15\n\t"
173 "frchg\n\t"
174 "fsts fpul, fr0\n\t"
175 "fsts fpul, fr1\n\t"
176 "fsts fpul, fr2\n\t"
177 "fsts fpul, fr3\n\t"
178 "fsts fpul, fr4\n\t"
179 "fsts fpul, fr5\n\t"
180 "fsts fpul, fr6\n\t"
181 "fsts fpul, fr7\n\t"
182 "fsts fpul, fr8\n\t"
183 "fsts fpul, fr9\n\t"
184 "fsts fpul, fr10\n\t"
185 "fsts fpul, fr11\n\t"
186 "fsts fpul, fr12\n\t"
187 "fsts fpul, fr13\n\t"
188 "fsts fpul, fr14\n\t"
189 "fsts fpul, fr15\n\t"
190 "frchg\n\t"
191 "lds %2, fpscr\n\t"
192 :
193 :"r" (0), "r"(FPSCR_RCHG), "r"(FPSCR_INIT));
194 disable_fpu();
195}
196
197
198
199
200
201
202
203
204static void denormal_to_double(struct sh_fpu_hard_struct *fpu, int n)
205{
206 unsigned long du, dl;
207 unsigned long x = fpu->fpul;
208 int exp = 1023 - 126;
209
210 if (x != 0 && (x & 0x7f800000) == 0) {
211 du = (x & 0x80000000);
212 while ((x & 0x00800000) == 0) {
213 x <<= 1;
214 exp--;
215 }
216 x &= 0x007fffff;
217 du |= (exp << 20) | (x >> 3);
218 dl = x << 29;
219
220 fpu->fp_regs[n] = du;
221 fpu->fp_regs[n + 1] = dl;
222 }
223}
224
225
226
227
228
229
230
231
232static int ieee_fpe_handler(struct pt_regs *regs)
233{
234 unsigned short insn = *(unsigned short *)regs->pc;
235 unsigned short finsn;
236 unsigned long nextpc;
237 int nib[4] = {
238 (insn >> 12) & 0xf,
239 (insn >> 8) & 0xf,
240 (insn >> 4) & 0xf,
241 insn & 0xf
242 };
243
244 if (nib[0] == 0xb || (nib[0] == 0x4 && nib[2] == 0x0 && nib[3] == 0xb))
245 regs->pr = regs->pc + 4;
246
247 if (nib[0] == 0xa || nib[0] == 0xb) {
248
249 nextpc = regs->pc + 4 + ((short)((insn & 0xfff) << 4) >> 3);
250 finsn = *(unsigned short *)(regs->pc + 2);
251 } else if (nib[0] == 0x8 && nib[1] == 0xd) {
252
253 if (regs->sr & 1)
254 nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1);
255 else
256 nextpc = regs->pc + 4;
257 finsn = *(unsigned short *)(regs->pc + 2);
258 } else if (nib[0] == 0x8 && nib[1] == 0xf) {
259
260 if (regs->sr & 1)
261 nextpc = regs->pc + 4;
262 else
263 nextpc = regs->pc + 4 + ((char)(insn & 0xff) << 1);
264 finsn = *(unsigned short *)(regs->pc + 2);
265 } else if (nib[0] == 0x4 && nib[3] == 0xb &&
266 (nib[2] == 0x0 || nib[2] == 0x2)) {
267
268 nextpc = regs->regs[nib[1]];
269 finsn = *(unsigned short *)(regs->pc + 2);
270 } else if (nib[0] == 0x0 && nib[3] == 0x3 &&
271 (nib[2] == 0x0 || nib[2] == 0x2)) {
272
273 nextpc = regs->pc + 4 + regs->regs[nib[1]];
274 finsn = *(unsigned short *)(regs->pc + 2);
275 } else if (insn == 0x000b) {
276
277 nextpc = regs->pr;
278 finsn = *(unsigned short *)(regs->pc + 2);
279 } else {
280 nextpc = regs->pc + instruction_size(insn);
281 finsn = insn;
282 }
283
284 if ((finsn & 0xf1ff) == 0xf0ad) {
285
286 struct task_struct *tsk = current;
287
288 save_fpu(tsk, regs);
289 if ((tsk->thread.fpu.hard.fpscr & FPSCR_CAUSE_ERROR))
290
291 denormal_to_double(&tsk->thread.fpu.hard,
292 (finsn >> 8) & 0xf);
293 else
294 return 0;
295
296 regs->pc = nextpc;
297 return 1;
298 } else if ((finsn & 0xf00f) == 0xf002) {
299
300 struct task_struct *tsk = current;
301 int fpscr;
302 int n, m, prec;
303 unsigned int hx, hy;
304
305 n = (finsn >> 8) & 0xf;
306 m = (finsn >> 4) & 0xf;
307 hx = tsk->thread.fpu.hard.fp_regs[n];
308 hy = tsk->thread.fpu.hard.fp_regs[m];
309 fpscr = tsk->thread.fpu.hard.fpscr;
310 prec = fpscr & FPSCR_DBL_PRECISION;
311
312 if ((fpscr & FPSCR_CAUSE_ERROR)
313 && (prec && ((hx & 0x7fffffff) < 0x00100000
314 || (hy & 0x7fffffff) < 0x00100000))) {
315 long long llx, lly;
316
317
318 llx = ((long long)hx << 32)
319 | tsk->thread.fpu.hard.fp_regs[n + 1];
320 lly = ((long long)hy << 32)
321 | tsk->thread.fpu.hard.fp_regs[m + 1];
322 llx = float64_mul(llx, lly);
323 tsk->thread.fpu.hard.fp_regs[n] = llx >> 32;
324 tsk->thread.fpu.hard.fp_regs[n + 1] = llx & 0xffffffff;
325 } else if ((fpscr & FPSCR_CAUSE_ERROR)
326 && (!prec && ((hx & 0x7fffffff) < 0x00800000
327 || (hy & 0x7fffffff) < 0x00800000))) {
328
329 hx = float32_mul(hx, hy);
330 tsk->thread.fpu.hard.fp_regs[n] = hx;
331 } else
332 return 0;
333
334 regs->pc = nextpc;
335 return 1;
336 } else if ((finsn & 0xf00e) == 0xf000) {
337
338 struct task_struct *tsk = current;
339 int fpscr;
340 int n, m, prec;
341 unsigned int hx, hy;
342
343 n = (finsn >> 8) & 0xf;
344 m = (finsn >> 4) & 0xf;
345 hx = tsk->thread.fpu.hard.fp_regs[n];
346 hy = tsk->thread.fpu.hard.fp_regs[m];
347 fpscr = tsk->thread.fpu.hard.fpscr;
348 prec = fpscr & FPSCR_DBL_PRECISION;
349
350 if ((fpscr & FPSCR_CAUSE_ERROR)
351 && (prec && ((hx & 0x7fffffff) < 0x00100000
352 || (hy & 0x7fffffff) < 0x00100000))) {
353 long long llx, lly;
354
355
356 llx = ((long long)hx << 32)
357 | tsk->thread.fpu.hard.fp_regs[n + 1];
358 lly = ((long long)hy << 32)
359 | tsk->thread.fpu.hard.fp_regs[m + 1];
360 if ((finsn & 0xf00f) == 0xf000)
361 llx = float64_add(llx, lly);
362 else
363 llx = float64_sub(llx, lly);
364 tsk->thread.fpu.hard.fp_regs[n] = llx >> 32;
365 tsk->thread.fpu.hard.fp_regs[n + 1] = llx & 0xffffffff;
366 } else if ((fpscr & FPSCR_CAUSE_ERROR)
367 && (!prec && ((hx & 0x7fffffff) < 0x00800000
368 || (hy & 0x7fffffff) < 0x00800000))) {
369
370 if ((finsn & 0xf00f) == 0xf000)
371 hx = float32_add(hx, hy);
372 else
373 hx = float32_sub(hx, hy);
374 tsk->thread.fpu.hard.fp_regs[n] = hx;
375 } else
376 return 0;
377
378 regs->pc = nextpc;
379 return 1;
380 } else if ((finsn & 0xf003) == 0xf003) {
381
382 struct task_struct *tsk = current;
383 int fpscr;
384 int n, m, prec;
385 unsigned int hx, hy;
386
387 n = (finsn >> 8) & 0xf;
388 m = (finsn >> 4) & 0xf;
389 hx = tsk->thread.fpu.hard.fp_regs[n];
390 hy = tsk->thread.fpu.hard.fp_regs[m];
391 fpscr = tsk->thread.fpu.hard.fpscr;
392 prec = fpscr & FPSCR_DBL_PRECISION;
393
394 if ((fpscr & FPSCR_CAUSE_ERROR)
395 && (prec && ((hx & 0x7fffffff) < 0x00100000
396 || (hy & 0x7fffffff) < 0x00100000))) {
397 long long llx, lly;
398
399
400 llx = ((long long)hx << 32)
401 | tsk->thread.fpu.hard.fp_regs[n + 1];
402 lly = ((long long)hy << 32)
403 | tsk->thread.fpu.hard.fp_regs[m + 1];
404
405 llx = float64_div(llx, lly);
406
407 tsk->thread.fpu.hard.fp_regs[n] = llx >> 32;
408 tsk->thread.fpu.hard.fp_regs[n + 1] = llx & 0xffffffff;
409 } else if ((fpscr & FPSCR_CAUSE_ERROR)
410 && (!prec && ((hx & 0x7fffffff) < 0x00800000
411 || (hy & 0x7fffffff) < 0x00800000))) {
412
413 hx = float32_div(hx, hy);
414 tsk->thread.fpu.hard.fp_regs[n] = hx;
415 } else
416 return 0;
417
418 regs->pc = nextpc;
419 return 1;
420 } else if ((finsn & 0xf0bd) == 0xf0bd) {
421
422 struct task_struct *tsk = current;
423 int m;
424 unsigned int hx;
425
426 m = (finsn >> 8) & 0x7;
427 hx = tsk->thread.fpu.hard.fp_regs[m];
428
429 if ((tsk->thread.fpu.hard.fpscr & FPSCR_CAUSE_ERROR)
430 && ((hx & 0x7fffffff) < 0x00100000)) {
431
432 long long llx;
433
434 llx = ((long long)tsk->thread.fpu.hard.fp_regs[m] << 32)
435 | tsk->thread.fpu.hard.fp_regs[m + 1];
436
437 tsk->thread.fpu.hard.fpul = float64_to_float32(llx);
438 } else
439 return 0;
440
441 regs->pc = nextpc;
442 return 1;
443 }
444
445 return 0;
446}
447
448void float_raise(unsigned int flags)
449{
450 fpu_exception_flags |= flags;
451}
452
453int float_rounding_mode(void)
454{
455 struct task_struct *tsk = current;
456 int roundingMode = FPSCR_ROUNDING_MODE(tsk->thread.fpu.hard.fpscr);
457 return roundingMode;
458}
459
460BUILD_TRAP_HANDLER(fpu_error)
461{
462 struct task_struct *tsk = current;
463 TRAP_HANDLER_DECL;
464
465 save_fpu(tsk, regs);
466 fpu_exception_flags = 0;
467 if (ieee_fpe_handler(regs)) {
468 tsk->thread.fpu.hard.fpscr &=
469 ~(FPSCR_CAUSE_MASK | FPSCR_FLAG_MASK);
470 tsk->thread.fpu.hard.fpscr |= fpu_exception_flags;
471
472
473 tsk->thread.fpu.hard.fpscr |= (fpu_exception_flags >> 10);
474 grab_fpu(regs);
475 restore_fpu(tsk);
476 set_tsk_thread_flag(tsk, TIF_USEDFPU);
477 if ((((tsk->thread.fpu.hard.fpscr & FPSCR_ENABLE_MASK) >> 7) &
478 (fpu_exception_flags >> 2)) == 0) {
479 return;
480 }
481 }
482
483 force_sig(SIGFPE, tsk);
484}
485
486BUILD_TRAP_HANDLER(fpu_state_restore)
487{
488 struct task_struct *tsk = current;
489 TRAP_HANDLER_DECL;
490
491 grab_fpu(regs);
492 if (!user_mode(regs)) {
493 printk(KERN_ERR "BUG: FPU is used in kernel mode.\n");
494 return;
495 }
496
497 if (used_math()) {
498
499 restore_fpu(tsk);
500 } else {
501
502 fpu_init();
503 set_used_math();
504 }
505 set_tsk_thread_flag(tsk, TIF_USEDFPU);
506}
507
