Merge branch 'cpus4096-for-linus-2' of git://git.kernel.org/pub/scm/linux/kernel...
[linux-2.6] / arch / powerpc / math-emu / math_efp.c
1 /*
2  * arch/powerpc/math-emu/math_efp.c
3  *
4  * Copyright (C) 2006-2008 Freescale Semiconductor, Inc. All rights reserved.
5  *
6  * Author: Ebony Zhu,   <ebony.zhu@freescale.com>
7  *         Yu Liu,      <yu.liu@freescale.com>
8  *
9  * Derived from arch/alpha/math-emu/math.c
10  *              arch/powerpc/math-emu/math.c
11  *
12  * Description:
13  * This file is the exception handler to make E500 SPE instructions
14  * fully comply with IEEE-754 floating point standard.
15  *
16  * This program is free software; you can redistribute it and/or
17  * modify it under the terms of the GNU General Public License
18  * as published by the Free Software Foundation; either version
19  * 2 of the License, or (at your option) any later version.
20  */
21
22 #include <linux/types.h>
23
24 #include <asm/uaccess.h>
25 #include <asm/reg.h>
26
27 #define FP_EX_BOOKE_E500_SPE
28 #include <asm/sfp-machine.h>
29
30 #include <math-emu/soft-fp.h>
31 #include <math-emu/single.h>
32 #include <math-emu/double.h>
33
34 #define EFAPU           0x4
35
36 #define VCT             0x4
37 #define SPFP            0x6
38 #define DPFP            0x7
39
40 #define EFSADD          0x2c0
41 #define EFSSUB          0x2c1
42 #define EFSABS          0x2c4
43 #define EFSNABS         0x2c5
44 #define EFSNEG          0x2c6
45 #define EFSMUL          0x2c8
46 #define EFSDIV          0x2c9
47 #define EFSCMPGT        0x2cc
48 #define EFSCMPLT        0x2cd
49 #define EFSCMPEQ        0x2ce
50 #define EFSCFD          0x2cf
51 #define EFSCFSI         0x2d1
52 #define EFSCTUI         0x2d4
53 #define EFSCTSI         0x2d5
54 #define EFSCTUF         0x2d6
55 #define EFSCTSF         0x2d7
56 #define EFSCTUIZ        0x2d8
57 #define EFSCTSIZ        0x2da
58
59 #define EVFSADD         0x280
60 #define EVFSSUB         0x281
61 #define EVFSABS         0x284
62 #define EVFSNABS        0x285
63 #define EVFSNEG         0x286
64 #define EVFSMUL         0x288
65 #define EVFSDIV         0x289
66 #define EVFSCMPGT       0x28c
67 #define EVFSCMPLT       0x28d
68 #define EVFSCMPEQ       0x28e
69 #define EVFSCTUI        0x294
70 #define EVFSCTSI        0x295
71 #define EVFSCTUF        0x296
72 #define EVFSCTSF        0x297
73 #define EVFSCTUIZ       0x298
74 #define EVFSCTSIZ       0x29a
75
76 #define EFDADD          0x2e0
77 #define EFDSUB          0x2e1
78 #define EFDABS          0x2e4
79 #define EFDNABS         0x2e5
80 #define EFDNEG          0x2e6
81 #define EFDMUL          0x2e8
82 #define EFDDIV          0x2e9
83 #define EFDCTUIDZ       0x2ea
84 #define EFDCTSIDZ       0x2eb
85 #define EFDCMPGT        0x2ec
86 #define EFDCMPLT        0x2ed
87 #define EFDCMPEQ        0x2ee
88 #define EFDCFS          0x2ef
89 #define EFDCTUI         0x2f4
90 #define EFDCTSI         0x2f5
91 #define EFDCTUF         0x2f6
92 #define EFDCTSF         0x2f7
93 #define EFDCTUIZ        0x2f8
94 #define EFDCTSIZ        0x2fa
95
96 #define AB      2
97 #define XA      3
98 #define XB      4
99 #define XCR     5
100 #define NOTYPE  0
101
102 #define SIGN_BIT_S      (1UL << 31)
103 #define SIGN_BIT_D      (1ULL << 63)
104 #define FP_EX_MASK      (FP_EX_INEXACT | FP_EX_INVALID | FP_EX_DIVZERO | \
105                         FP_EX_UNDERFLOW | FP_EX_OVERFLOW)
106
107 union dw_union {
108         u64 dp[1];
109         u32 wp[2];
110 };
111
112 static unsigned long insn_type(unsigned long speinsn)
113 {
114         unsigned long ret = NOTYPE;
115
116         switch (speinsn & 0x7ff) {
117         case EFSABS:    ret = XA;       break;
118         case EFSADD:    ret = AB;       break;
119         case EFSCFD:    ret = XB;       break;
120         case EFSCMPEQ:  ret = XCR;      break;
121         case EFSCMPGT:  ret = XCR;      break;
122         case EFSCMPLT:  ret = XCR;      break;
123         case EFSCTSF:   ret = XB;       break;
124         case EFSCTSI:   ret = XB;       break;
125         case EFSCTSIZ:  ret = XB;       break;
126         case EFSCTUF:   ret = XB;       break;
127         case EFSCTUI:   ret = XB;       break;
128         case EFSCTUIZ:  ret = XB;       break;
129         case EFSDIV:    ret = AB;       break;
130         case EFSMUL:    ret = AB;       break;
131         case EFSNABS:   ret = XA;       break;
132         case EFSNEG:    ret = XA;       break;
133         case EFSSUB:    ret = AB;       break;
134         case EFSCFSI:   ret = XB;       break;
135
136         case EVFSABS:   ret = XA;       break;
137         case EVFSADD:   ret = AB;       break;
138         case EVFSCMPEQ: ret = XCR;      break;
139         case EVFSCMPGT: ret = XCR;      break;
140         case EVFSCMPLT: ret = XCR;      break;
141         case EVFSCTSF:  ret = XB;       break;
142         case EVFSCTSI:  ret = XB;       break;
143         case EVFSCTSIZ: ret = XB;       break;
144         case EVFSCTUF:  ret = XB;       break;
145         case EVFSCTUI:  ret = XB;       break;
146         case EVFSCTUIZ: ret = XB;       break;
147         case EVFSDIV:   ret = AB;       break;
148         case EVFSMUL:   ret = AB;       break;
149         case EVFSNABS:  ret = XA;       break;
150         case EVFSNEG:   ret = XA;       break;
151         case EVFSSUB:   ret = AB;       break;
152
153         case EFDABS:    ret = XA;       break;
154         case EFDADD:    ret = AB;       break;
155         case EFDCFS:    ret = XB;       break;
156         case EFDCMPEQ:  ret = XCR;      break;
157         case EFDCMPGT:  ret = XCR;      break;
158         case EFDCMPLT:  ret = XCR;      break;
159         case EFDCTSF:   ret = XB;       break;
160         case EFDCTSI:   ret = XB;       break;
161         case EFDCTSIDZ: ret = XB;       break;
162         case EFDCTSIZ:  ret = XB;       break;
163         case EFDCTUF:   ret = XB;       break;
164         case EFDCTUI:   ret = XB;       break;
165         case EFDCTUIDZ: ret = XB;       break;
166         case EFDCTUIZ:  ret = XB;       break;
167         case EFDDIV:    ret = AB;       break;
168         case EFDMUL:    ret = AB;       break;
169         case EFDNABS:   ret = XA;       break;
170         case EFDNEG:    ret = XA;       break;
171         case EFDSUB:    ret = AB;       break;
172
173         default:
174                 printk(KERN_ERR "\nOoops! SPE instruction no type found.");
175                 printk(KERN_ERR "\ninst code: %08lx\n", speinsn);
176         }
177
178         return ret;
179 }
180
181 int do_spe_mathemu(struct pt_regs *regs)
182 {
183         FP_DECL_EX;
184         int IR, cmp;
185
186         unsigned long type, func, fc, fa, fb, src, speinsn;
187         union dw_union vc, va, vb;
188
189         if (get_user(speinsn, (unsigned int __user *) regs->nip))
190                 return -EFAULT;
191         if ((speinsn >> 26) != EFAPU)
192                 return -EINVAL;         /* not an spe instruction */
193
194         type = insn_type(speinsn);
195         if (type == NOTYPE)
196                 return -ENOSYS;
197
198         func = speinsn & 0x7ff;
199         fc = (speinsn >> 21) & 0x1f;
200         fa = (speinsn >> 16) & 0x1f;
201         fb = (speinsn >> 11) & 0x1f;
202         src = (speinsn >> 5) & 0x7;
203
204         vc.wp[0] = current->thread.evr[fc];
205         vc.wp[1] = regs->gpr[fc];
206         va.wp[0] = current->thread.evr[fa];
207         va.wp[1] = regs->gpr[fa];
208         vb.wp[0] = current->thread.evr[fb];
209         vb.wp[1] = regs->gpr[fb];
210
211         __FPU_FPSCR = mfspr(SPRN_SPEFSCR);
212
213 #ifdef DEBUG
214         printk("speinsn:%08lx spefscr:%08lx\n", speinsn, __FPU_FPSCR);
215         printk("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
216         printk("va: %08x  %08x\n", va.wp[0], va.wp[1]);
217         printk("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);
218 #endif
219
220         switch (src) {
221         case SPFP: {
222                 FP_DECL_S(SA); FP_DECL_S(SB); FP_DECL_S(SR);
223
224                 switch (type) {
225                 case AB:
226                 case XCR:
227                         FP_UNPACK_SP(SA, va.wp + 1);
228                 case XB:
229                         FP_UNPACK_SP(SB, vb.wp + 1);
230                         break;
231                 case XA:
232                         FP_UNPACK_SP(SA, va.wp + 1);
233                         break;
234                 }
235
236 #ifdef DEBUG
237                 printk("SA: %ld %08lx %ld (%ld)\n", SA_s, SA_f, SA_e, SA_c);
238                 printk("SB: %ld %08lx %ld (%ld)\n", SB_s, SB_f, SB_e, SB_c);
239 #endif
240
241                 switch (func) {
242                 case EFSABS:
243                         vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
244                         goto update_regs;
245
246                 case EFSNABS:
247                         vc.wp[1] = va.wp[1] | SIGN_BIT_S;
248                         goto update_regs;
249
250                 case EFSNEG:
251                         vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
252                         goto update_regs;
253
254                 case EFSADD:
255                         FP_ADD_S(SR, SA, SB);
256                         goto pack_s;
257
258                 case EFSSUB:
259                         FP_SUB_S(SR, SA, SB);
260                         goto pack_s;
261
262                 case EFSMUL:
263                         FP_MUL_S(SR, SA, SB);
264                         goto pack_s;
265
266                 case EFSDIV:
267                         FP_DIV_S(SR, SA, SB);
268                         goto pack_s;
269
270                 case EFSCMPEQ:
271                         cmp = 0;
272                         goto cmp_s;
273
274                 case EFSCMPGT:
275                         cmp = 1;
276                         goto cmp_s;
277
278                 case EFSCMPLT:
279                         cmp = -1;
280                         goto cmp_s;
281
282                 case EFSCTSF:
283                 case EFSCTUF:
284                         if (!((vb.wp[1] >> 23) == 0xff && ((vb.wp[1] & 0x7fffff) > 0))) {
285                                 /* NaN */
286                                 if (((vb.wp[1] >> 23) & 0xff) == 0) {
287                                         /* denorm */
288                                         vc.wp[1] = 0x0;
289                                 } else if ((vb.wp[1] >> 31) == 0) {
290                                         /* positive normal */
291                                         vc.wp[1] = (func == EFSCTSF) ?
292                                                 0x7fffffff : 0xffffffff;
293                                 } else { /* negative normal */
294                                         vc.wp[1] = (func == EFSCTSF) ?
295                                                 0x80000000 : 0x0;
296                                 }
297                         } else { /* rB is NaN */
298                                 vc.wp[1] = 0x0;
299                         }
300                         goto update_regs;
301
302                 case EFSCFD: {
303                         FP_DECL_D(DB);
304                         FP_CLEAR_EXCEPTIONS;
305                         FP_UNPACK_DP(DB, vb.dp);
306 #ifdef DEBUG
307                         printk("DB: %ld %08lx %08lx %ld (%ld)\n",
308                                         DB_s, DB_f1, DB_f0, DB_e, DB_c);
309 #endif
310                         FP_CONV(S, D, 1, 2, SR, DB);
311                         goto pack_s;
312                 }
313
314                 case EFSCTSI:
315                 case EFSCTSIZ:
316                 case EFSCTUI:
317                 case EFSCTUIZ:
318                         if (func & 0x4) {
319                                 _FP_ROUND(1, SB);
320                         } else {
321                                 _FP_ROUND_ZERO(1, SB);
322                         }
323                         FP_TO_INT_S(vc.wp[1], SB, 32, ((func & 0x3) != 0));
324                         goto update_regs;
325
326                 default:
327                         goto illegal;
328                 }
329                 break;
330
331 pack_s:
332 #ifdef DEBUG
333                 printk("SR: %ld %08lx %ld (%ld)\n", SR_s, SR_f, SR_e, SR_c);
334 #endif
335                 FP_PACK_SP(vc.wp + 1, SR);
336                 goto update_regs;
337
338 cmp_s:
339                 FP_CMP_S(IR, SA, SB, 3);
340                 if (IR == 3 && (FP_ISSIGNAN_S(SA) || FP_ISSIGNAN_S(SB)))
341                         FP_SET_EXCEPTION(FP_EX_INVALID);
342                 if (IR == cmp) {
343                         IR = 0x4;
344                 } else {
345                         IR = 0;
346                 }
347                 goto update_ccr;
348         }
349
350         case DPFP: {
351                 FP_DECL_D(DA); FP_DECL_D(DB); FP_DECL_D(DR);
352
353                 switch (type) {
354                 case AB:
355                 case XCR:
356                         FP_UNPACK_DP(DA, va.dp);
357                 case XB:
358                         FP_UNPACK_DP(DB, vb.dp);
359                         break;
360                 case XA:
361                         FP_UNPACK_DP(DA, va.dp);
362                         break;
363                 }
364
365 #ifdef DEBUG
366                 printk("DA: %ld %08lx %08lx %ld (%ld)\n",
367                                 DA_s, DA_f1, DA_f0, DA_e, DA_c);
368                 printk("DB: %ld %08lx %08lx %ld (%ld)\n",
369                                 DB_s, DB_f1, DB_f0, DB_e, DB_c);
370 #endif
371
372                 switch (func) {
373                 case EFDABS:
374                         vc.dp[0] = va.dp[0] & ~SIGN_BIT_D;
375                         goto update_regs;
376
377                 case EFDNABS:
378                         vc.dp[0] = va.dp[0] | SIGN_BIT_D;
379                         goto update_regs;
380
381                 case EFDNEG:
382                         vc.dp[0] = va.dp[0] ^ SIGN_BIT_D;
383                         goto update_regs;
384
385                 case EFDADD:
386                         FP_ADD_D(DR, DA, DB);
387                         goto pack_d;
388
389                 case EFDSUB:
390                         FP_SUB_D(DR, DA, DB);
391                         goto pack_d;
392
393                 case EFDMUL:
394                         FP_MUL_D(DR, DA, DB);
395                         goto pack_d;
396
397                 case EFDDIV:
398                         FP_DIV_D(DR, DA, DB);
399                         goto pack_d;
400
401                 case EFDCMPEQ:
402                         cmp = 0;
403                         goto cmp_d;
404
405                 case EFDCMPGT:
406                         cmp = 1;
407                         goto cmp_d;
408
409                 case EFDCMPLT:
410                         cmp = -1;
411                         goto cmp_d;
412
413                 case EFDCTSF:
414                 case EFDCTUF:
415                         if (!((vb.wp[0] >> 20) == 0x7ff &&
416                            ((vb.wp[0] & 0xfffff) > 0 || (vb.wp[1] > 0)))) {
417                                 /* not a NaN */
418                                 if (((vb.wp[0] >> 20) & 0x7ff) == 0) {
419                                         /* denorm */
420                                         vc.wp[1] = 0x0;
421                                 } else if ((vb.wp[0] >> 31) == 0) {
422                                         /* positive normal */
423                                         vc.wp[1] = (func == EFDCTSF) ?
424                                                 0x7fffffff : 0xffffffff;
425                                 } else { /* negative normal */
426                                         vc.wp[1] = (func == EFDCTSF) ?
427                                                 0x80000000 : 0x0;
428                                 }
429                         } else { /* NaN */
430                                 vc.wp[1] = 0x0;
431                         }
432                         goto update_regs;
433
434                 case EFDCFS: {
435                         FP_DECL_S(SB);
436                         FP_CLEAR_EXCEPTIONS;
437                         FP_UNPACK_SP(SB, vb.wp + 1);
438 #ifdef DEBUG
439                         printk("SB: %ld %08lx %ld (%ld)\n",
440                                         SB_s, SB_f, SB_e, SB_c);
441 #endif
442                         FP_CONV(D, S, 2, 1, DR, SB);
443                         goto pack_d;
444                 }
445
446                 case EFDCTUIDZ:
447                 case EFDCTSIDZ:
448                         _FP_ROUND_ZERO(2, DB);
449                         FP_TO_INT_D(vc.dp[0], DB, 64, ((func & 0x1) == 0));
450                         goto update_regs;
451
452                 case EFDCTUI:
453                 case EFDCTSI:
454                 case EFDCTUIZ:
455                 case EFDCTSIZ:
456                         if (func & 0x4) {
457                                 _FP_ROUND(2, DB);
458                         } else {
459                                 _FP_ROUND_ZERO(2, DB);
460                         }
461                         FP_TO_INT_D(vc.wp[1], DB, 32, ((func & 0x3) != 0));
462                         goto update_regs;
463
464                 default:
465                         goto illegal;
466                 }
467                 break;
468
469 pack_d:
470 #ifdef DEBUG
471                 printk("DR: %ld %08lx %08lx %ld (%ld)\n",
472                                 DR_s, DR_f1, DR_f0, DR_e, DR_c);
473 #endif
474                 FP_PACK_DP(vc.dp, DR);
475                 goto update_regs;
476
477 cmp_d:
478                 FP_CMP_D(IR, DA, DB, 3);
479                 if (IR == 3 && (FP_ISSIGNAN_D(DA) || FP_ISSIGNAN_D(DB)))
480                         FP_SET_EXCEPTION(FP_EX_INVALID);
481                 if (IR == cmp) {
482                         IR = 0x4;
483                 } else {
484                         IR = 0;
485                 }
486                 goto update_ccr;
487
488         }
489
490         case VCT: {
491                 FP_DECL_S(SA0); FP_DECL_S(SB0); FP_DECL_S(SR0);
492                 FP_DECL_S(SA1); FP_DECL_S(SB1); FP_DECL_S(SR1);
493                 int IR0, IR1;
494
495                 switch (type) {
496                 case AB:
497                 case XCR:
498                         FP_UNPACK_SP(SA0, va.wp);
499                         FP_UNPACK_SP(SA1, va.wp + 1);
500                 case XB:
501                         FP_UNPACK_SP(SB0, vb.wp);
502                         FP_UNPACK_SP(SB1, vb.wp + 1);
503                         break;
504                 case XA:
505                         FP_UNPACK_SP(SA0, va.wp);
506                         FP_UNPACK_SP(SA1, va.wp + 1);
507                         break;
508                 }
509
510 #ifdef DEBUG
511                 printk("SA0: %ld %08lx %ld (%ld)\n", SA0_s, SA0_f, SA0_e, SA0_c);
512                 printk("SA1: %ld %08lx %ld (%ld)\n", SA1_s, SA1_f, SA1_e, SA1_c);
513                 printk("SB0: %ld %08lx %ld (%ld)\n", SB0_s, SB0_f, SB0_e, SB0_c);
514                 printk("SB1: %ld %08lx %ld (%ld)\n", SB1_s, SB1_f, SB1_e, SB1_c);
515 #endif
516
517                 switch (func) {
518                 case EVFSABS:
519                         vc.wp[0] = va.wp[0] & ~SIGN_BIT_S;
520                         vc.wp[1] = va.wp[1] & ~SIGN_BIT_S;
521                         goto update_regs;
522
523                 case EVFSNABS:
524                         vc.wp[0] = va.wp[0] | SIGN_BIT_S;
525                         vc.wp[1] = va.wp[1] | SIGN_BIT_S;
526                         goto update_regs;
527
528                 case EVFSNEG:
529                         vc.wp[0] = va.wp[0] ^ SIGN_BIT_S;
530                         vc.wp[1] = va.wp[1] ^ SIGN_BIT_S;
531                         goto update_regs;
532
533                 case EVFSADD:
534                         FP_ADD_S(SR0, SA0, SB0);
535                         FP_ADD_S(SR1, SA1, SB1);
536                         goto pack_vs;
537
538                 case EVFSSUB:
539                         FP_SUB_S(SR0, SA0, SB0);
540                         FP_SUB_S(SR1, SA1, SB1);
541                         goto pack_vs;
542
543                 case EVFSMUL:
544                         FP_MUL_S(SR0, SA0, SB0);
545                         FP_MUL_S(SR1, SA1, SB1);
546                         goto pack_vs;
547
548                 case EVFSDIV:
549                         FP_DIV_S(SR0, SA0, SB0);
550                         FP_DIV_S(SR1, SA1, SB1);
551                         goto pack_vs;
552
553                 case EVFSCMPEQ:
554                         cmp = 0;
555                         goto cmp_vs;
556
557                 case EVFSCMPGT:
558                         cmp = 1;
559                         goto cmp_vs;
560
561                 case EVFSCMPLT:
562                         cmp = -1;
563                         goto cmp_vs;
564
565                 case EVFSCTSF:
566                         __asm__ __volatile__ ("mtspr 512, %4\n"
567                                 "efsctsf %0, %2\n"
568                                 "efsctsf %1, %3\n"
569                                 : "=r" (vc.wp[0]), "=r" (vc.wp[1])
570                                 : "r" (vb.wp[0]), "r" (vb.wp[1]), "r" (0));
571                         goto update_regs;
572
573                 case EVFSCTUF:
574                         __asm__ __volatile__ ("mtspr 512, %4\n"
575                                 "efsctuf %0, %2\n"
576                                 "efsctuf %1, %3\n"
577                                 : "=r" (vc.wp[0]), "=r" (vc.wp[1])
578                                 : "r" (vb.wp[0]), "r" (vb.wp[1]), "r" (0));
579                         goto update_regs;
580
581                 case EVFSCTUI:
582                 case EVFSCTSI:
583                 case EVFSCTUIZ:
584                 case EVFSCTSIZ:
585                         if (func & 0x4) {
586                                 _FP_ROUND(1, SB0);
587                                 _FP_ROUND(1, SB1);
588                         } else {
589                                 _FP_ROUND_ZERO(1, SB0);
590                                 _FP_ROUND_ZERO(1, SB1);
591                         }
592                         FP_TO_INT_S(vc.wp[0], SB0, 32, ((func & 0x3) != 0));
593                         FP_TO_INT_S(vc.wp[1], SB1, 32, ((func & 0x3) != 0));
594                         goto update_regs;
595
596                 default:
597                         goto illegal;
598                 }
599                 break;
600
601 pack_vs:
602 #ifdef DEBUG
603                 printk("SR0: %ld %08lx %ld (%ld)\n", SR0_s, SR0_f, SR0_e, SR0_c);
604                 printk("SR1: %ld %08lx %ld (%ld)\n", SR1_s, SR1_f, SR1_e, SR1_c);
605 #endif
606                 FP_PACK_SP(vc.wp, SR0);
607                 FP_PACK_SP(vc.wp + 1, SR1);
608                 goto update_regs;
609
610 cmp_vs:
611                 {
612                         int ch, cl;
613
614                         FP_CMP_S(IR0, SA0, SB0, 3);
615                         FP_CMP_S(IR1, SA1, SB1, 3);
616                         if (IR0 == 3 && (FP_ISSIGNAN_S(SA0) || FP_ISSIGNAN_S(SB0)))
617                                 FP_SET_EXCEPTION(FP_EX_INVALID);
618                         if (IR1 == 3 && (FP_ISSIGNAN_S(SA1) || FP_ISSIGNAN_S(SB1)))
619                                 FP_SET_EXCEPTION(FP_EX_INVALID);
620                         ch = (IR0 == cmp) ? 1 : 0;
621                         cl = (IR1 == cmp) ? 1 : 0;
622                         IR = (ch << 3) | (cl << 2) | ((ch | cl) << 1) |
623                                 ((ch & cl) << 0);
624                         goto update_ccr;
625                 }
626         }
627         default:
628                 return -EINVAL;
629         }
630
631 update_ccr:
632         regs->ccr &= ~(15 << ((7 - ((speinsn >> 23) & 0x7)) << 2));
633         regs->ccr |= (IR << ((7 - ((speinsn >> 23) & 0x7)) << 2));
634
635 update_regs:
636         __FPU_FPSCR &= ~FP_EX_MASK;
637         __FPU_FPSCR |= (FP_CUR_EXCEPTIONS & FP_EX_MASK);
638         mtspr(SPRN_SPEFSCR, __FPU_FPSCR);
639
640         current->thread.evr[fc] = vc.wp[0];
641         regs->gpr[fc] = vc.wp[1];
642
643 #ifdef DEBUG
644         printk("ccr = %08lx\n", regs->ccr);
645         printk("cur exceptions = %08x spefscr = %08lx\n",
646                         FP_CUR_EXCEPTIONS, __FPU_FPSCR);
647         printk("vc: %08x  %08x\n", vc.wp[0], vc.wp[1]);
648         printk("va: %08x  %08x\n", va.wp[0], va.wp[1]);
649         printk("vb: %08x  %08x\n", vb.wp[0], vb.wp[1]);
650 #endif
651
652         return 0;
653
654 illegal:
655         printk(KERN_ERR "\nOoops! IEEE-754 compliance handler encountered un-supported instruction.\ninst code: %08lx\n", speinsn);
656         return -ENOSYS;
657 }
658
659 int speround_handler(struct pt_regs *regs)
660 {
661         union dw_union fgpr;
662         int s_lo, s_hi;
663         unsigned long speinsn, type, fc;
664
665         if (get_user(speinsn, (unsigned int __user *) regs->nip))
666                 return -EFAULT;
667         if ((speinsn >> 26) != 4)
668                 return -EINVAL;         /* not an spe instruction */
669
670         type = insn_type(speinsn & 0x7ff);
671         if (type == XCR) return -ENOSYS;
672
673         fc = (speinsn >> 21) & 0x1f;
674         s_lo = regs->gpr[fc] & SIGN_BIT_S;
675         s_hi = current->thread.evr[fc] & SIGN_BIT_S;
676         fgpr.wp[0] = current->thread.evr[fc];
677         fgpr.wp[1] = regs->gpr[fc];
678
679         __FPU_FPSCR = mfspr(SPRN_SPEFSCR);
680
681         switch ((speinsn >> 5) & 0x7) {
682         /* Since SPE instructions on E500 core can handle round to nearest
683          * and round toward zero with IEEE-754 complied, we just need
684          * to handle round toward +Inf and round toward -Inf by software.
685          */
686         case SPFP:
687                 if ((FP_ROUNDMODE) == FP_RND_PINF) {
688                         if (!s_lo) fgpr.wp[1]++; /* Z > 0, choose Z1 */
689                 } else { /* round to -Inf */
690                         if (s_lo) fgpr.wp[1]++; /* Z < 0, choose Z2 */
691                 }
692                 break;
693
694         case DPFP:
695                 if (FP_ROUNDMODE == FP_RND_PINF) {
696                         if (!s_hi) fgpr.dp[0]++; /* Z > 0, choose Z1 */
697                 } else { /* round to -Inf */
698                         if (s_hi) fgpr.dp[0]++; /* Z < 0, choose Z2 */
699                 }
700                 break;
701
702         case VCT:
703                 if (FP_ROUNDMODE == FP_RND_PINF) {
704                         if (!s_lo) fgpr.wp[1]++; /* Z_low > 0, choose Z1 */
705                         if (!s_hi) fgpr.wp[0]++; /* Z_high word > 0, choose Z1 */
706                 } else { /* round to -Inf */
707                         if (s_lo) fgpr.wp[1]++; /* Z_low < 0, choose Z2 */
708                         if (s_hi) fgpr.wp[0]++; /* Z_high < 0, choose Z2 */
709                 }
710                 break;
711
712         default:
713                 return -EINVAL;
714         }
715
716         current->thread.evr[fc] = fgpr.wp[0];
717         regs->gpr[fc] = fgpr.wp[1];
718
719         return 0;
720 }