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