3 files changed, 62 insertions, 25 deletions

diff --git a/arch/arm/vfp/Makefile b/arch/arm/vfp/Makefile
index 39f6d8e1af73..6de73aab0195 100644
--- a/arch/arm/vfp/Makefile
+++ b/arch/arm/vfp/Makefile
@@ -4,8 +4,8 @@
 # Copyright (C) 2001 ARM Limited
 #
 
-# EXTRA_CFLAGS := -DDEBUG
-# EXTRA_AFLAGS := -DDEBUG
+# ccflags-y := -DDEBUG
+# asflags-y := -DDEBUG
 
 KBUILD_AFLAGS   :=$(KBUILD_AFLAGS:-msoft-float=-Wa,-mfpu=softvfp+vfp)
 LDFLAGS         +=--no-warn-mismatch
diff --git a/arch/arm/vfp/vfphw.S b/arch/arm/vfp/vfphw.S
index d66cead97d28..9897dcfc16d6 100644
--- a/arch/arm/vfp/vfphw.S
+++ b/arch/arm/vfp/vfphw.S
@@ -206,6 +206,7 @@ ENTRY(vfp_save_state)
         mov     pc, lr
 ENDPROC(vfp_save_state)
 
+        .align
 last_VFP_context_address:
         .word   last_VFP_context
 
diff --git a/arch/arm/vfp/vfpmodule.c b/arch/arm/vfp/vfpmodule.c
index 8063a322c790..f25e7ec89416 100644
--- a/arch/arm/vfp/vfpmodule.c
+++ b/arch/arm/vfp/vfpmodule.c
@@ -10,9 +10,12 @@
  */
 #include <linux/module.h>
 #include <linux/types.h>
+#include <linux/cpu.h>
 #include <linux/kernel.h>
+#include <linux/notifier.h>
 #include <linux/signal.h>
 #include <linux/sched.h>
+#include <linux/smp.h>
 #include <linux/init.h>
 
 #include <asm/cputype.h>
@@ -75,6 +78,14 @@ static void vfp_thread_exit(struct thread_info *thread)
         put_cpu();
 }
 
+static void vfp_thread_copy(struct thread_info *thread)
+{
+        struct thread_info *parent = current_thread_info();
+
+        vfp_sync_hwstate(parent);
+        thread->vfpstate = parent->vfpstate;
+}
+
 /*
  * When this function is called with the following 'cmd's, the following
  * is true while this function is being run:
@@ -101,12 +112,17 @@ static void vfp_thread_exit(struct thread_info *thread)
 static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
 {
         struct thread_info *thread = v;
+        u32 fpexc;
+#ifdef CONFIG_SMP
+        unsigned int cpu;
+#endif
 
-        if (likely(cmd == THREAD_NOTIFY_SWITCH)) {
-                u32 fpexc = fmrx(FPEXC);
+        switch (cmd) {
+        case THREAD_NOTIFY_SWITCH:
+                fpexc = fmrx(FPEXC);
 
 #ifdef CONFIG_SMP
-                unsigned int cpu = thread->cpu;
+                cpu = thread->cpu;
 
                 /*
                  * On SMP, if VFP is enabled, save the old state in
@@ -131,13 +147,20 @@ static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
                  * old state.
                  */
                 fmxr(FPEXC, fpexc & ~FPEXC_EN);
-                return NOTIFY_DONE;
-        }
+                break;
 
-        if (cmd == THREAD_NOTIFY_FLUSH)
+        case THREAD_NOTIFY_FLUSH:
                 vfp_thread_flush(thread);
-        else
+                break;
+
+        case THREAD_NOTIFY_EXIT:
                 vfp_thread_exit(thread);
+                break;
+
+        case THREAD_NOTIFY_COPY:
+                vfp_thread_copy(thread);
+                break;
+        }
 
         return NOTIFY_DONE;
 }
@@ -150,7 +173,7 @@ static struct notifier_block vfp_notifier_block = {
  * Raise a SIGFPE for the current process.
  * sicode describes the signal being raised.
  */
-void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs)
+static void vfp_raise_sigfpe(unsigned int sicode, struct pt_regs *regs)
 {
         siginfo_t info;
 
@@ -375,9 +398,9 @@ static void vfp_enable(void *unused)
 }
 
 #ifdef CONFIG_PM
-#include <linux/sysdev.h>
+#include <linux/syscore_ops.h>
 
-static int vfp_pm_suspend(struct sys_device *dev, pm_message_t state)
+static int vfp_pm_suspend(void)
 {
         struct thread_info *ti = current_thread_info();
         u32 fpexc = fmrx(FPEXC);
@@ -397,34 +420,25 @@ static int vfp_pm_suspend(struct sys_device *dev, pm_message_t state)
         return 0;
 }
 
-static int vfp_pm_resume(struct sys_device *dev)
+static void vfp_pm_resume(void)
 {
         /* 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",
+static struct syscore_ops vfp_pm_syscore_ops = {
         .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);
+        register_syscore_ops(&vfp_pm_syscore_ops);
 }
 
-
 #else
 static inline void vfp_pm_init(void) { }
 #endif /* CONFIG_PM */
@@ -484,7 +498,27 @@ void vfp_flush_hwstate(struct thread_info *thread)
         put_cpu();
 }
 
-#include <linux/smp.h>
+/*
+ * VFP hardware can lose all context when a CPU goes offline.
+ * As we will be running in SMP mode with CPU hotplug, we will save the
+ * hardware state at every thread switch.  We clear our held state when
+ * a CPU has been killed, indicating that the VFP hardware doesn't contain
+ * a threads VFP state.  When a CPU starts up, we re-enable access to the
+ * VFP hardware.
+ *
+ * Both CPU_DYING and CPU_STARTING are called on the CPU which
+ * is being offlined/onlined.
+ */
+static int vfp_hotplug(struct notifier_block *b, unsigned long action,
+        void *hcpu)
+{
+        if (action == CPU_DYING || action == CPU_DYING_FROZEN) {
+                unsigned int cpu = (long)hcpu;
+                last_VFP_context[cpu] = NULL;
+        } else if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
+                vfp_enable(NULL);
+        return NOTIFY_OK;
+}
 
 /*
  * VFP support code initialisation.
@@ -514,6 +548,8 @@ static int __init vfp_init(void)
         else if (vfpsid & FPSID_NODOUBLE) {
                 printk("no double precision support\n");
         } else {
+                hotcpu_notifier(vfp_hotplug, 0);
+
                 smp_call_function(vfp_enable, NULL, 1);
 
                 VFP_arch = (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT;  /* Extract the architecture version */