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