Commit | Line | Data |
---|---|---|
1da177e4 | 1 | /* |
1da177e4 LT |
2 | * Copyright (C) 1999 Eddie C. Dost (ecd@atecom.com) |
3 | */ | |
4 | ||
1da177e4 LT |
5 | #include <linux/types.h> |
6 | #include <linux/sched.h> | |
7 | ||
8 | #include <asm/uaccess.h> | |
9 | #include <asm/reg.h> | |
10 | ||
d2b194ed KG |
11 | #include <asm/sfp-machine.h> |
12 | #include <math-emu/double.h> | |
1da177e4 LT |
13 | |
14 | #define FLOATFUNC(x) extern int x(void *, void *, void *, void *) | |
15 | ||
16 | FLOATFUNC(fadd); | |
17 | FLOATFUNC(fadds); | |
18 | FLOATFUNC(fdiv); | |
19 | FLOATFUNC(fdivs); | |
20 | FLOATFUNC(fmul); | |
21 | FLOATFUNC(fmuls); | |
22 | FLOATFUNC(fsub); | |
23 | FLOATFUNC(fsubs); | |
24 | ||
25 | FLOATFUNC(fmadd); | |
26 | FLOATFUNC(fmadds); | |
27 | FLOATFUNC(fmsub); | |
28 | FLOATFUNC(fmsubs); | |
29 | FLOATFUNC(fnmadd); | |
30 | FLOATFUNC(fnmadds); | |
31 | FLOATFUNC(fnmsub); | |
32 | FLOATFUNC(fnmsubs); | |
33 | ||
34 | FLOATFUNC(fctiw); | |
35 | FLOATFUNC(fctiwz); | |
36 | FLOATFUNC(frsp); | |
37 | ||
38 | FLOATFUNC(fcmpo); | |
39 | FLOATFUNC(fcmpu); | |
40 | ||
41 | FLOATFUNC(mcrfs); | |
42 | FLOATFUNC(mffs); | |
43 | FLOATFUNC(mtfsb0); | |
44 | FLOATFUNC(mtfsb1); | |
45 | FLOATFUNC(mtfsf); | |
46 | FLOATFUNC(mtfsfi); | |
47 | ||
48 | FLOATFUNC(lfd); | |
49 | FLOATFUNC(lfs); | |
50 | ||
51 | FLOATFUNC(stfd); | |
52 | FLOATFUNC(stfs); | |
53 | FLOATFUNC(stfiwx); | |
54 | ||
55 | FLOATFUNC(fabs); | |
56 | FLOATFUNC(fmr); | |
57 | FLOATFUNC(fnabs); | |
58 | FLOATFUNC(fneg); | |
59 | ||
60 | /* Optional */ | |
61 | FLOATFUNC(fres); | |
62 | FLOATFUNC(frsqrte); | |
63 | FLOATFUNC(fsel); | |
64 | FLOATFUNC(fsqrt); | |
65 | FLOATFUNC(fsqrts); | |
66 | ||
67 | ||
68 | #define OP31 0x1f /* 31 */ | |
69 | #define LFS 0x30 /* 48 */ | |
70 | #define LFSU 0x31 /* 49 */ | |
71 | #define LFD 0x32 /* 50 */ | |
72 | #define LFDU 0x33 /* 51 */ | |
73 | #define STFS 0x34 /* 52 */ | |
74 | #define STFSU 0x35 /* 53 */ | |
75 | #define STFD 0x36 /* 54 */ | |
76 | #define STFDU 0x37 /* 55 */ | |
77 | #define OP59 0x3b /* 59 */ | |
78 | #define OP63 0x3f /* 63 */ | |
79 | ||
80 | /* Opcode 31: */ | |
81 | /* X-Form: */ | |
82 | #define LFSX 0x217 /* 535 */ | |
83 | #define LFSUX 0x237 /* 567 */ | |
84 | #define LFDX 0x257 /* 599 */ | |
85 | #define LFDUX 0x277 /* 631 */ | |
86 | #define STFSX 0x297 /* 663 */ | |
87 | #define STFSUX 0x2b7 /* 695 */ | |
88 | #define STFDX 0x2d7 /* 727 */ | |
89 | #define STFDUX 0x2f7 /* 759 */ | |
90 | #define STFIWX 0x3d7 /* 983 */ | |
91 | ||
92 | /* Opcode 59: */ | |
93 | /* A-Form: */ | |
94 | #define FDIVS 0x012 /* 18 */ | |
95 | #define FSUBS 0x014 /* 20 */ | |
96 | #define FADDS 0x015 /* 21 */ | |
97 | #define FSQRTS 0x016 /* 22 */ | |
98 | #define FRES 0x018 /* 24 */ | |
99 | #define FMULS 0x019 /* 25 */ | |
100 | #define FMSUBS 0x01c /* 28 */ | |
101 | #define FMADDS 0x01d /* 29 */ | |
102 | #define FNMSUBS 0x01e /* 30 */ | |
103 | #define FNMADDS 0x01f /* 31 */ | |
104 | ||
105 | /* Opcode 63: */ | |
106 | /* A-Form: */ | |
107 | #define FDIV 0x012 /* 18 */ | |
108 | #define FSUB 0x014 /* 20 */ | |
109 | #define FADD 0x015 /* 21 */ | |
110 | #define FSQRT 0x016 /* 22 */ | |
111 | #define FSEL 0x017 /* 23 */ | |
112 | #define FMUL 0x019 /* 25 */ | |
113 | #define FRSQRTE 0x01a /* 26 */ | |
114 | #define FMSUB 0x01c /* 28 */ | |
115 | #define FMADD 0x01d /* 29 */ | |
116 | #define FNMSUB 0x01e /* 30 */ | |
117 | #define FNMADD 0x01f /* 31 */ | |
118 | ||
119 | /* X-Form: */ | |
120 | #define FCMPU 0x000 /* 0 */ | |
121 | #define FRSP 0x00c /* 12 */ | |
122 | #define FCTIW 0x00e /* 14 */ | |
123 | #define FCTIWZ 0x00f /* 15 */ | |
124 | #define FCMPO 0x020 /* 32 */ | |
125 | #define MTFSB1 0x026 /* 38 */ | |
126 | #define FNEG 0x028 /* 40 */ | |
127 | #define MCRFS 0x040 /* 64 */ | |
128 | #define MTFSB0 0x046 /* 70 */ | |
129 | #define FMR 0x048 /* 72 */ | |
130 | #define MTFSFI 0x086 /* 134 */ | |
131 | #define FNABS 0x088 /* 136 */ | |
132 | #define FABS 0x108 /* 264 */ | |
133 | #define MFFS 0x247 /* 583 */ | |
134 | #define MTFSF 0x2c7 /* 711 */ | |
135 | ||
136 | ||
137 | #define AB 2 | |
138 | #define AC 3 | |
139 | #define ABC 4 | |
140 | #define D 5 | |
141 | #define DU 6 | |
142 | #define X 7 | |
143 | #define XA 8 | |
144 | #define XB 9 | |
145 | #define XCR 11 | |
146 | #define XCRB 12 | |
147 | #define XCRI 13 | |
148 | #define XCRL 16 | |
149 | #define XE 14 | |
150 | #define XEU 15 | |
151 | #define XFLB 10 | |
152 | ||
153 | #ifdef CONFIG_MATH_EMULATION | |
154 | static int | |
155 | record_exception(struct pt_regs *regs, int eflag) | |
156 | { | |
157 | u32 fpscr; | |
158 | ||
159 | fpscr = __FPU_FPSCR; | |
160 | ||
161 | if (eflag) { | |
162 | fpscr |= FPSCR_FX; | |
163 | if (eflag & EFLAG_OVERFLOW) | |
164 | fpscr |= FPSCR_OX; | |
165 | if (eflag & EFLAG_UNDERFLOW) | |
166 | fpscr |= FPSCR_UX; | |
167 | if (eflag & EFLAG_DIVZERO) | |
168 | fpscr |= FPSCR_ZX; | |
169 | if (eflag & EFLAG_INEXACT) | |
170 | fpscr |= FPSCR_XX; | |
d2b194ed KG |
171 | if (eflag & EFLAG_INVALID) |
172 | fpscr |= FPSCR_VX; | |
1da177e4 LT |
173 | if (eflag & EFLAG_VXSNAN) |
174 | fpscr |= FPSCR_VXSNAN; | |
175 | if (eflag & EFLAG_VXISI) | |
176 | fpscr |= FPSCR_VXISI; | |
177 | if (eflag & EFLAG_VXIDI) | |
178 | fpscr |= FPSCR_VXIDI; | |
179 | if (eflag & EFLAG_VXZDZ) | |
180 | fpscr |= FPSCR_VXZDZ; | |
181 | if (eflag & EFLAG_VXIMZ) | |
182 | fpscr |= FPSCR_VXIMZ; | |
183 | if (eflag & EFLAG_VXVC) | |
184 | fpscr |= FPSCR_VXVC; | |
185 | if (eflag & EFLAG_VXSOFT) | |
186 | fpscr |= FPSCR_VXSOFT; | |
187 | if (eflag & EFLAG_VXSQRT) | |
188 | fpscr |= FPSCR_VXSQRT; | |
189 | if (eflag & EFLAG_VXCVI) | |
190 | fpscr |= FPSCR_VXCVI; | |
191 | } | |
192 | ||
d2b194ed | 193 | // fpscr &= ~(FPSCR_VX); |
1da177e4 LT |
194 | if (fpscr & (FPSCR_VXSNAN | FPSCR_VXISI | FPSCR_VXIDI | |
195 | FPSCR_VXZDZ | FPSCR_VXIMZ | FPSCR_VXVC | | |
196 | FPSCR_VXSOFT | FPSCR_VXSQRT | FPSCR_VXCVI)) | |
197 | fpscr |= FPSCR_VX; | |
198 | ||
199 | fpscr &= ~(FPSCR_FEX); | |
200 | if (((fpscr & FPSCR_VX) && (fpscr & FPSCR_VE)) || | |
201 | ((fpscr & FPSCR_OX) && (fpscr & FPSCR_OE)) || | |
202 | ((fpscr & FPSCR_UX) && (fpscr & FPSCR_UE)) || | |
203 | ((fpscr & FPSCR_ZX) && (fpscr & FPSCR_ZE)) || | |
204 | ((fpscr & FPSCR_XX) && (fpscr & FPSCR_XE))) | |
205 | fpscr |= FPSCR_FEX; | |
206 | ||
207 | __FPU_FPSCR = fpscr; | |
208 | ||
209 | return (fpscr & FPSCR_FEX) ? 1 : 0; | |
210 | } | |
211 | #endif /* CONFIG_MATH_EMULATION */ | |
212 | ||
213 | int | |
214 | do_mathemu(struct pt_regs *regs) | |
215 | { | |
216 | void *op0 = 0, *op1 = 0, *op2 = 0, *op3 = 0; | |
217 | unsigned long pc = regs->nip; | |
218 | signed short sdisp; | |
219 | u32 insn = 0; | |
220 | int idx = 0; | |
221 | #ifdef CONFIG_MATH_EMULATION | |
222 | int (*func)(void *, void *, void *, void *); | |
223 | int type = 0; | |
224 | int eflag, trap; | |
225 | #endif | |
226 | ||
227 | if (get_user(insn, (u32 *)pc)) | |
228 | return -EFAULT; | |
229 | ||
230 | #ifndef CONFIG_MATH_EMULATION | |
231 | switch (insn >> 26) { | |
232 | case LFD: | |
233 | idx = (insn >> 16) & 0x1f; | |
234 | sdisp = (insn & 0xffff); | |
9c75a31c | 235 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
1da177e4 LT |
236 | op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); |
237 | lfd(op0, op1, op2, op3); | |
238 | break; | |
239 | case LFDU: | |
240 | idx = (insn >> 16) & 0x1f; | |
241 | sdisp = (insn & 0xffff); | |
9c75a31c | 242 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
1da177e4 LT |
243 | op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); |
244 | lfd(op0, op1, op2, op3); | |
245 | regs->gpr[idx] = (unsigned long)op1; | |
246 | break; | |
247 | case STFD: | |
248 | idx = (insn >> 16) & 0x1f; | |
249 | sdisp = (insn & 0xffff); | |
9c75a31c | 250 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
1da177e4 LT |
251 | op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); |
252 | stfd(op0, op1, op2, op3); | |
253 | break; | |
254 | case STFDU: | |
255 | idx = (insn >> 16) & 0x1f; | |
256 | sdisp = (insn & 0xffff); | |
9c75a31c | 257 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
1da177e4 LT |
258 | op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); |
259 | stfd(op0, op1, op2, op3); | |
260 | regs->gpr[idx] = (unsigned long)op1; | |
261 | break; | |
262 | case OP63: | |
9c75a31c MN |
263 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
264 | op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); | |
1da177e4 LT |
265 | fmr(op0, op1, op2, op3); |
266 | break; | |
267 | default: | |
268 | goto illegal; | |
269 | } | |
270 | #else /* CONFIG_MATH_EMULATION */ | |
271 | switch (insn >> 26) { | |
272 | case LFS: func = lfs; type = D; break; | |
273 | case LFSU: func = lfs; type = DU; break; | |
274 | case LFD: func = lfd; type = D; break; | |
275 | case LFDU: func = lfd; type = DU; break; | |
276 | case STFS: func = stfs; type = D; break; | |
277 | case STFSU: func = stfs; type = DU; break; | |
278 | case STFD: func = stfd; type = D; break; | |
279 | case STFDU: func = stfd; type = DU; break; | |
280 | ||
281 | case OP31: | |
282 | switch ((insn >> 1) & 0x3ff) { | |
283 | case LFSX: func = lfs; type = XE; break; | |
284 | case LFSUX: func = lfs; type = XEU; break; | |
285 | case LFDX: func = lfd; type = XE; break; | |
286 | case LFDUX: func = lfd; type = XEU; break; | |
287 | case STFSX: func = stfs; type = XE; break; | |
288 | case STFSUX: func = stfs; type = XEU; break; | |
289 | case STFDX: func = stfd; type = XE; break; | |
290 | case STFDUX: func = stfd; type = XEU; break; | |
291 | case STFIWX: func = stfiwx; type = XE; break; | |
292 | default: | |
293 | goto illegal; | |
294 | } | |
295 | break; | |
296 | ||
297 | case OP59: | |
298 | switch ((insn >> 1) & 0x1f) { | |
299 | case FDIVS: func = fdivs; type = AB; break; | |
300 | case FSUBS: func = fsubs; type = AB; break; | |
301 | case FADDS: func = fadds; type = AB; break; | |
302 | case FSQRTS: func = fsqrts; type = AB; break; | |
303 | case FRES: func = fres; type = AB; break; | |
304 | case FMULS: func = fmuls; type = AC; break; | |
305 | case FMSUBS: func = fmsubs; type = ABC; break; | |
306 | case FMADDS: func = fmadds; type = ABC; break; | |
307 | case FNMSUBS: func = fnmsubs; type = ABC; break; | |
308 | case FNMADDS: func = fnmadds; type = ABC; break; | |
309 | default: | |
310 | goto illegal; | |
311 | } | |
312 | break; | |
313 | ||
314 | case OP63: | |
315 | if (insn & 0x20) { | |
316 | switch ((insn >> 1) & 0x1f) { | |
317 | case FDIV: func = fdiv; type = AB; break; | |
318 | case FSUB: func = fsub; type = AB; break; | |
319 | case FADD: func = fadd; type = AB; break; | |
320 | case FSQRT: func = fsqrt; type = AB; break; | |
321 | case FSEL: func = fsel; type = ABC; break; | |
322 | case FMUL: func = fmul; type = AC; break; | |
323 | case FRSQRTE: func = frsqrte; type = AB; break; | |
324 | case FMSUB: func = fmsub; type = ABC; break; | |
325 | case FMADD: func = fmadd; type = ABC; break; | |
326 | case FNMSUB: func = fnmsub; type = ABC; break; | |
327 | case FNMADD: func = fnmadd; type = ABC; break; | |
328 | default: | |
329 | goto illegal; | |
330 | } | |
331 | break; | |
332 | } | |
333 | ||
334 | switch ((insn >> 1) & 0x3ff) { | |
335 | case FCMPU: func = fcmpu; type = XCR; break; | |
336 | case FRSP: func = frsp; type = XB; break; | |
337 | case FCTIW: func = fctiw; type = XB; break; | |
338 | case FCTIWZ: func = fctiwz; type = XB; break; | |
339 | case FCMPO: func = fcmpo; type = XCR; break; | |
340 | case MTFSB1: func = mtfsb1; type = XCRB; break; | |
341 | case FNEG: func = fneg; type = XB; break; | |
342 | case MCRFS: func = mcrfs; type = XCRL; break; | |
343 | case MTFSB0: func = mtfsb0; type = XCRB; break; | |
344 | case FMR: func = fmr; type = XB; break; | |
345 | case MTFSFI: func = mtfsfi; type = XCRI; break; | |
346 | case FNABS: func = fnabs; type = XB; break; | |
347 | case FABS: func = fabs; type = XB; break; | |
348 | case MFFS: func = mffs; type = X; break; | |
349 | case MTFSF: func = mtfsf; type = XFLB; break; | |
350 | default: | |
351 | goto illegal; | |
352 | } | |
353 | break; | |
354 | ||
355 | default: | |
356 | goto illegal; | |
357 | } | |
358 | ||
359 | switch (type) { | |
360 | case AB: | |
9c75a31c MN |
361 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
362 | op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); | |
363 | op2 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); | |
1da177e4 LT |
364 | break; |
365 | ||
366 | case AC: | |
9c75a31c MN |
367 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
368 | op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); | |
369 | op2 = (void *)¤t->thread.TS_FPR((insn >> 6) & 0x1f); | |
1da177e4 LT |
370 | break; |
371 | ||
372 | case ABC: | |
9c75a31c MN |
373 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
374 | op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); | |
375 | op2 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); | |
376 | op3 = (void *)¤t->thread.TS_FPR((insn >> 6) & 0x1f); | |
1da177e4 LT |
377 | break; |
378 | ||
379 | case D: | |
380 | idx = (insn >> 16) & 0x1f; | |
381 | sdisp = (insn & 0xffff); | |
9c75a31c | 382 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
1da177e4 LT |
383 | op1 = (void *)((idx ? regs->gpr[idx] : 0) + sdisp); |
384 | break; | |
385 | ||
386 | case DU: | |
387 | idx = (insn >> 16) & 0x1f; | |
388 | if (!idx) | |
389 | goto illegal; | |
390 | ||
391 | sdisp = (insn & 0xffff); | |
9c75a31c | 392 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
1da177e4 LT |
393 | op1 = (void *)(regs->gpr[idx] + sdisp); |
394 | break; | |
395 | ||
396 | case X: | |
9c75a31c | 397 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
1da177e4 LT |
398 | break; |
399 | ||
400 | case XA: | |
9c75a31c MN |
401 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
402 | op1 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); | |
1da177e4 LT |
403 | break; |
404 | ||
405 | case XB: | |
9c75a31c MN |
406 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
407 | op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); | |
1da177e4 LT |
408 | break; |
409 | ||
410 | case XE: | |
411 | idx = (insn >> 16) & 0x1f; | |
9c75a31c | 412 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
ba02946a KG |
413 | if (!idx) { |
414 | if (((insn >> 1) & 0x3ff) == STFIWX) | |
415 | op1 = (void *)(regs->gpr[(insn >> 11) & 0x1f]); | |
416 | else | |
417 | goto illegal; | |
418 | } else { | |
419 | op1 = (void *)(regs->gpr[idx] + regs->gpr[(insn >> 11) & 0x1f]); | |
420 | } | |
421 | ||
1da177e4 LT |
422 | break; |
423 | ||
424 | case XEU: | |
425 | idx = (insn >> 16) & 0x1f; | |
9c75a31c | 426 | op0 = (void *)¤t->thread.TS_FPR((insn >> 21) & 0x1f); |
1da177e4 LT |
427 | op1 = (void *)((idx ? regs->gpr[idx] : 0) |
428 | + regs->gpr[(insn >> 11) & 0x1f]); | |
429 | break; | |
430 | ||
431 | case XCR: | |
432 | op0 = (void *)®s->ccr; | |
433 | op1 = (void *)((insn >> 23) & 0x7); | |
9c75a31c MN |
434 | op2 = (void *)¤t->thread.TS_FPR((insn >> 16) & 0x1f); |
435 | op3 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); | |
1da177e4 LT |
436 | break; |
437 | ||
438 | case XCRL: | |
439 | op0 = (void *)®s->ccr; | |
440 | op1 = (void *)((insn >> 23) & 0x7); | |
441 | op2 = (void *)((insn >> 18) & 0x7); | |
442 | break; | |
443 | ||
444 | case XCRB: | |
445 | op0 = (void *)((insn >> 21) & 0x1f); | |
446 | break; | |
447 | ||
448 | case XCRI: | |
449 | op0 = (void *)((insn >> 23) & 0x7); | |
450 | op1 = (void *)((insn >> 12) & 0xf); | |
451 | break; | |
452 | ||
453 | case XFLB: | |
454 | op0 = (void *)((insn >> 17) & 0xff); | |
9c75a31c | 455 | op1 = (void *)¤t->thread.TS_FPR((insn >> 11) & 0x1f); |
1da177e4 LT |
456 | break; |
457 | ||
458 | default: | |
459 | goto illegal; | |
460 | } | |
461 | ||
462 | eflag = func(op0, op1, op2, op3); | |
463 | ||
464 | if (insn & 1) { | |
465 | regs->ccr &= ~(0x0f000000); | |
466 | regs->ccr |= (__FPU_FPSCR >> 4) & 0x0f000000; | |
467 | } | |
468 | ||
469 | trap = record_exception(regs, eflag); | |
470 | if (trap) | |
471 | return 1; | |
472 | ||
473 | switch (type) { | |
474 | case DU: | |
475 | case XEU: | |
476 | regs->gpr[idx] = (unsigned long)op1; | |
477 | break; | |
478 | ||
479 | default: | |
480 | break; | |
481 | } | |
482 | #endif /* CONFIG_MATH_EMULATION */ | |
483 | ||
484 | regs->nip += 4; | |
485 | return 0; | |
486 | ||
487 | illegal: | |
488 | return -ENOSYS; | |
489 | } |