3 files changed, 24 insertions, 20 deletions

diff --git a/arch/arm/vfp/vfpdouble.c b/arch/arm/vfp/vfpdouble.c
index 9b367a65cb4d..febd115dba28 100644
--- a/arch/arm/vfp/vfpdouble.c
+++ b/arch/arm/vfp/vfpdouble.c
@@ -588,6 +588,7 @@ static u32 vfp_double_ftosi(int sd, int unused, int dm, u32 fpscr)
         struct vfp_double vdm;
         u32 d, exceptions = 0;
         int rmode = fpscr & FPSCR_RMODE_MASK;
+        int tm;
 
         vfp_double_unpack(&vdm, vfp_get_double(dm));
         vfp_double_dump("VDM", &vdm);
@@ -595,10 +596,14 @@ static u32 vfp_double_ftosi(int sd, int unused, int dm, u32 fpscr)
         /*
          * Do we have denormalised number?
          */
-        if (vfp_double_type(&vdm) & VFP_DENORMAL)
+        tm = vfp_double_type(&vdm);
+        if (tm & VFP_DENORMAL)
                 exceptions |= FPSCR_IDC;
 
-        if (vdm.exponent >= 1023 + 32) {
+        if (tm & VFP_NAN) {
+                d = 0;
+                exceptions |= FPSCR_IOC;
+        } else if (vdm.exponent >= 1023 + 32) {
                 d = 0x7fffffff;
                 if (vdm.sign)
                         d = ~d;
@@ -1122,9 +1127,9 @@ u32 vfp_double_cpdo(u32 inst, u32 fpscr)
 {
         u32 op = inst & FOP_MASK;
         u32 exceptions = 0;
-        unsigned int dd = vfp_get_sd(inst);
-        unsigned int dn = vfp_get_sn(inst);
-        unsigned int dm = vfp_get_sm(inst);
+        unsigned int dd = vfp_get_dd(inst);
+        unsigned int dn = vfp_get_dn(inst);
+        unsigned int dm = vfp_get_dm(inst);
         unsigned int vecitr, veclen, vecstride;
         u32 (*fop)(int, int, s32, u32);
 
@@ -1141,7 +1146,7 @@ u32 vfp_double_cpdo(u32 inst, u32 fpscr)
         pr_debug("VFP: vecstride=%u veclen=%u\n", vecstride,
                  (veclen >> FPSCR_LENGTH_BIT) + 1);
 
-        fop = (op == FOP_EXT) ? fop_extfns[dn] : fop_fns[FOP_TO_IDX(op)];
+        fop = (op == FOP_EXT) ? fop_extfns[FEXT_TO_IDX(inst)] : fop_fns[FOP_TO_IDX(op)];
         if (!fop)
                 goto invalid;
 
@@ -1149,17 +1154,13 @@ u32 vfp_double_cpdo(u32 inst, u32 fpscr)
                 u32 except;
 
                 if (op == FOP_EXT)
-                        pr_debug("VFP: itr%d (d%u.%u) = op[%u] (d%u.%u)\n",
+                        pr_debug("VFP: itr%d (d%u) = op[%u] (d%u)\n",
                                  vecitr >> FPSCR_LENGTH_BIT,
-                                 dd >> 1, dd & 1, dn,
-                                 dm >> 1, dm & 1);
+                                 dd, dn, dm);
                 else
-                        pr_debug("VFP: itr%d (d%u.%u) = (d%u.%u) op[%u] (d%u.%u)\n",
+                        pr_debug("VFP: itr%d (d%u) = (d%u) op[%u] (d%u)\n",
                                  vecitr >> FPSCR_LENGTH_BIT,
-                                 dd >> 1, dd & 1,
-                                 dn >> 1, dn & 1,
-                                 FOP_TO_IDX(op),
-                                 dm >> 1, dm & 1);
+                                 dd, dn, FOP_TO_IDX(op), dm);
 
                 except = fop(dd, dn, dm, fpscr);
                 pr_debug("VFP: itr%d: exceptions=%08x\n",
diff --git a/arch/arm/vfp/vfphw.S b/arch/arm/vfp/vfphw.S
index b7ed57e00cd4..a3f65b47aea9 100644
--- a/arch/arm/vfp/vfphw.S
+++ b/arch/arm/vfp/vfphw.S
@@ -189,11 +189,10 @@ vfp_put_float:
 
         .globl  vfp_get_double
 vfp_get_double:
-        mov     r0, r0, lsr #1
         add     pc, pc, r0, lsl #3
         mov     r0, r0
         .irp    dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
-        mrrc    p10, 1, r0, r1, c\dr    @ fmrrd r0, r1, d\dr
+        mrrc    p11, 1, r0, r1, c\dr    @ fmrrd r0, r1, d\dr
         mov     pc, lr
         .endr
 
@@ -204,10 +203,9 @@ vfp_get_double:
 
         .globl  vfp_put_double
 vfp_put_double:
-        mov     r0, r0, lsr #1
         add     pc, pc, r0, lsl #3
         mov     r0, r0
         .irp    dr,0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15
-        mcrr    p10, 1, r1, r2, c\dr    @ fmrrd r1, r2, d\dr
+        mcrr    p11, 1, r1, r2, c\dr    @ fmdrr r1, r2, d\dr
         mov     pc, lr
         .endr
diff --git a/arch/arm/vfp/vfpsingle.c b/arch/arm/vfp/vfpsingle.c
index 14dd696ddeb1..4ac27f193934 100644
--- a/arch/arm/vfp/vfpsingle.c
+++ b/arch/arm/vfp/vfpsingle.c
@@ -632,6 +632,7 @@ static u32 vfp_single_ftosi(int sd, int unused, s32 m, u32 fpscr)
         struct vfp_single vsm;
         u32 d, exceptions = 0;
         int rmode = fpscr & FPSCR_RMODE_MASK;
+        int tm;
 
         vfp_single_unpack(&vsm, m);
         vfp_single_dump("VSM", &vsm);
@@ -639,10 +640,14 @@ static u32 vfp_single_ftosi(int sd, int unused, s32 m, u32 fpscr)
         /*
          * Do we have a denormalised number?
          */
+        tm = vfp_single_type(&vsm);
         if (vfp_single_type(&vsm) & VFP_DENORMAL)
                 exceptions |= FPSCR_IDC;
 
-        if (vsm.exponent >= 127 + 32) {
+        if (tm & VFP_NAN) {
+                d = 0;
+                exceptions |= FPSCR_IOC;
+        } else if (vsm.exponent >= 127 + 32) {
                 /*
                  * m >= 2^31-2^7: invalid
                  */
@@ -1188,7 +1193,7 @@ u32 vfp_single_cpdo(u32 inst, u32 fpscr)
         pr_debug("VFP: vecstride=%u veclen=%u\n", vecstride,
                  (veclen >> FPSCR_LENGTH_BIT) + 1);
 
-        fop = (op == FOP_EXT) ? fop_extfns[sn] : fop_fns[FOP_TO_IDX(op)];
+        fop = (op == FOP_EXT) ? fop_extfns[FEXT_TO_IDX(inst)] : fop_fns[FOP_TO_IDX(op)];
         if (!fop)
                 goto invalid;