3 files changed, 85 insertions, 11 deletions

diff --git a/arch/arm/vfp/vfp.h b/arch/arm/vfp/vfp.h
index c85860bad585..8de86e4feada 100644
--- a/arch/arm/vfp/vfp.h
+++ b/arch/arm/vfp/vfp.h
@@ -377,6 +377,6 @@ struct op {
         u32 flags;
 };
 
-#ifdef CONFIG_SMP
+#if defined(CONFIG_SMP) || defined(CONFIG_PM)
 extern void vfp_save_state(void *location, u32 fpexc);
 #endif
diff --git a/arch/arm/vfp/vfphw.S b/arch/arm/vfp/vfphw.S
index a62dcf7098ba..c92a08bd6a86 100644
--- a/arch/arm/vfp/vfphw.S
+++ b/arch/arm/vfp/vfphw.S
@@ -101,9 +101,12 @@ ENTRY(vfp_support_entry)
         VFPFSTMIA r4, r5                @ save the working registers
         VFPFMRX r5, FPSCR               @ current status
         tst     r1, #FPEXC_EX           @ is there additional state to save?
-        VFPFMRX r6, FPINST, NE          @ FPINST (only if FPEXC.EX is set)
-        tstne   r1, #FPEXC_FP2V         @ is there an FPINST2 to read?
-        VFPFMRX r8, FPINST2, NE         @ FPINST2 if needed (and present)
+        beq     1f
+        VFPFMRX r6, FPINST              @ FPINST (only if FPEXC.EX is set)
+        tst     r1, #FPEXC_FP2V         @ is there an FPINST2 to read?
+        beq     1f
+        VFPFMRX r8, FPINST2             @ FPINST2 if needed (and present)
+1:
         stmia   r4, {r1, r5, r6, r8}    @ save FPEXC, FPSCR, FPINST, FPINST2
                                         @ and point r4 at the word at the
                                         @ start of the register dump
@@ -117,9 +120,12 @@ no_old_VFP_process:
                                         @ FPEXC is in a safe state
         ldmia   r10, {r1, r5, r6, r8}   @ load FPEXC, FPSCR, FPINST, FPINST2
         tst     r1, #FPEXC_EX           @ is there additional state to restore?
-        VFPFMXR FPINST, r6, NE          @ restore FPINST (only if FPEXC.EX is set)
-        tstne   r1, #FPEXC_FP2V         @ is there an FPINST2 to write?
-        VFPFMXR FPINST2, r8, NE         @ FPINST2 if needed (and present)
+        beq     1f
+        VFPFMXR FPINST, r6              @ restore FPINST (only if FPEXC.EX is set)
+        tst     r1, #FPEXC_FP2V         @ is there an FPINST2 to write?
+        beq     1f
+        VFPFMXR FPINST2, r8             @ FPINST2 if needed (and present)
+1:
         VFPFMXR FPSCR, r5               @ restore status
 
 check_for_exception:
@@ -166,7 +172,7 @@ process_exception:
                                         @ retry the faulted instruction
 ENDPROC(vfp_support_entry)
 
-#ifdef CONFIG_SMP
+#if defined(CONFIG_SMP) || defined(CONFIG_PM)
 ENTRY(vfp_save_state)
         @ Save the current VFP state
         @ r0 - save location
@@ -175,9 +181,12 @@ ENTRY(vfp_save_state)
         VFPFSTMIA r0, r2                @ save the working registers
         VFPFMRX r2, FPSCR               @ current status
         tst     r1, #FPEXC_EX           @ is there additional state to save?
-        VFPFMRX r3, FPINST, NE          @ FPINST (only if FPEXC.EX is set)
-        tstne   r1, #FPEXC_FP2V         @ is there an FPINST2 to read?
-        VFPFMRX r12, FPINST2, NE        @ FPINST2 if needed (and present)
+        beq     1f
+        VFPFMRX r3, FPINST              @ FPINST (only if FPEXC.EX is set)
+        tst     r1, #FPEXC_FP2V         @ is there an FPINST2 to read?
+        beq     1f
+        VFPFMRX r12, FPINST2            @ FPINST2 if needed (and present)
+1:
         stmia   r0, {r1, r2, r3, r12}   @ save FPEXC, FPSCR, FPINST, FPINST2
         mov     pc, lr
 ENDPROC(vfp_save_state)
diff --git a/arch/arm/vfp/vfpmodule.c b/arch/arm/vfp/vfpmodule.c
index c0d2c9bb952b..9f476a1be2ca 100644
--- a/arch/arm/vfp/vfpmodule.c
+++ b/arch/arm/vfp/vfpmodule.c
@@ -322,6 +322,61 @@ static void vfp_enable(void *unused)
         set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11));
 }
 
+#ifdef CONFIG_PM
+#include <linux/sysdev.h>
+
+static int vfp_pm_suspend(struct sys_device *dev, pm_message_t state)
+{
+        struct thread_info *ti = current_thread_info();
+        u32 fpexc = fmrx(FPEXC);
+
+        /* if vfp is on, then save state for resumption */
+        if (fpexc & FPEXC_EN) {
+                printk(KERN_DEBUG "%s: saving vfp state\n", __func__);
+                vfp_save_state(&ti->vfpstate, fpexc);
+
+                /* disable, just in case */
+                fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
+        }
+
+        /* clear any information we had about last context state */
+        memset(last_VFP_context, 0, sizeof(last_VFP_context));
+
+        return 0;
+}
+
+static int vfp_pm_resume(struct sys_device *dev)
+{
+        /* ensure we have access to the vfp */
+        vfp_enable(NULL);
+
+        /* and disable it to ensure the next usage restores the state */
+        fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
+
+        return 0;
+}
+
+static struct sysdev_class vfp_pm_sysclass = {
+        .name           = "vfp",
+        .suspend        = vfp_pm_suspend,
+        .resume         = vfp_pm_resume,
+};
+
+static struct sys_device vfp_pm_sysdev = {
+        .cls    = &vfp_pm_sysclass,
+};
+
+static void vfp_pm_init(void)
+{
+        sysdev_class_register(&vfp_pm_sysclass);
+        sysdev_register(&vfp_pm_sysdev);
+}
+
+
+#else
+static inline void vfp_pm_init(void) { }
+#endif /* CONFIG_PM */
+
 #include <linux/smp.h>
 
 /*
@@ -365,12 +420,22 @@ static int __init vfp_init(void)
                 vfp_vector = vfp_support_entry;
 
                 thread_register_notifier(&vfp_notifier_block);
+                vfp_pm_init();
 
                 /*
                  * We detected VFP, and the support code is
                  * in place; report VFP support to userspace.
                  */
                 elf_hwcap |= HWCAP_VFP;
+#ifdef CONFIG_NEON
+                /*
+                 * Check for the presence of the Advanced SIMD
+                 * load/store instructions, integer and single
+                 * precision floating point operations.
+                 */
+                if ((fmrx(MVFR1) & 0x000fff00) == 0x00011100)
+                        elf_hwcap |= HWCAP_NEON;
+#endif
         }
         return 0;
 }