1 files changed, 16 insertions, 2 deletions

diff --git a/include/asm-arm/vfpmacros.h b/include/asm-arm/vfpmacros.h
index 27fe028b4e72..cccb3892e73c 100644
--- a/include/asm-arm/vfpmacros.h
+++ b/include/asm-arm/vfpmacros.h
@@ -15,19 +15,33 @@
         .endm
 
         @ read all the working registers back into the VFP
-        .macro  VFPFLDMIA, base
+        .macro  VFPFLDMIA, base, tmp
 #if __LINUX_ARM_ARCH__ < 6
         LDC     p11, cr0, [\base],#33*4             @ FLDMIAX \base!, {d0-d15}
 #else
         LDC     p11, cr0, [\base],#32*4             @ FLDMIAD \base!, {d0-d15}
 #endif
+#ifdef CONFIG_VFPv3
+        VFPFMRX \tmp, MVFR0                         @ Media and VFP Feature Register 0
+        and     \tmp, \tmp, #MVFR0_A_SIMD_MASK      @ A_SIMD field
+        cmp     \tmp, #2                            @ 32 x 64bit registers?
+        ldceql  p11, cr0, [\base],#32*4             @ FLDMIAD \base!, {d16-d31}
+        addne   \base, \base, #32*4                 @ step over unused register space
+#endif
         .endm
 
         @ write all the working registers out of the VFP
-        .macro  VFPFSTMIA, base
+        .macro  VFPFSTMIA, base, tmp
 #if __LINUX_ARM_ARCH__ < 6
         STC     p11, cr0, [\base],#33*4             @ FSTMIAX \base!, {d0-d15}
 #else
         STC     p11, cr0, [\base],#32*4             @ FSTMIAD \base!, {d0-d15}
 #endif
+#ifdef CONFIG_VFPv3
+        VFPFMRX \tmp, MVFR0                         @ Media and VFP Feature Register 0
+        and     \tmp, \tmp, #MVFR0_A_SIMD_MASK      @ A_SIMD field
+        cmp     \tmp, #2                            @ 32 x 64bit registers?
+        stceql  p11, cr0, [\base],#32*4             @ FSTMIAD \base!, {d16-d31}
+        addne   \base, \base, #32*4                 @ step over unused register space
+#endif
         .endm