Patched in Tegra support.

4 files changed, 139 insertions, 253 deletions

diff --git a/arch/arm/vfp/Makefile b/arch/arm/vfp/Makefile
index a81404c09d5..6de73aab019 100644
--- a/arch/arm/vfp/Makefile
+++ b/arch/arm/vfp/Makefile
@@ -7,7 +7,7 @@
 # ccflags-y := -DDEBUG
 # asflags-y := -DDEBUG
 
-KBUILD_AFLAGS   :=$(KBUILD_AFLAGS:-msoft-float=-Wa,-mfpu=softvfp+vfp -mfloat-abi=soft)
+KBUILD_AFLAGS   :=$(KBUILD_AFLAGS:-msoft-float=-Wa,-mfpu=softvfp+vfp)
 LDFLAGS         +=--no-warn-mismatch
 
 obj-y                   += vfp.o
diff --git a/arch/arm/vfp/entry.S b/arch/arm/vfp/entry.S
index cc926c98598..c1a97840258 100644
--- a/arch/arm/vfp/entry.S
+++ b/arch/arm/vfp/entry.S
@@ -7,20 +7,18 @@
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
+ *
+ * Basic entry code, called from the kernel's undefined instruction trap.
+ *  r0  = faulted instruction
+ *  r2  = faulted PC+4
+ *  r9  = successful return
+ *  r10 = thread_info structure
+ *  lr  = failure return
  */
 #include <asm/thread_info.h>
 #include <asm/vfpmacros.h>
 #include "../kernel/entry-header.S"
 
-@ VFP entry point.
-@
-@  r0  = instruction opcode (32-bit ARM or two 16-bit Thumb)
-@  r2  = PC value to resume execution after successful emulation
-@  r9  = normal "successful" return address
-@  r10 = this threads thread_info structure
-@  lr  = unrecognised instruction return address
-@  IRQs disabled.
-@
 ENTRY(do_vfp)
 #ifdef CONFIG_PREEMPT
         ldr     r4, [r10, #TI_PREEMPT]  @ get preempt count
@@ -28,6 +26,7 @@ ENTRY(do_vfp)
         str     r11, [r10, #TI_PREEMPT]
 #endif
         enable_irq
+        str     r2, [sp, #S_PC]         @ update regs->ARM_pc for Thumb 2 case
         ldr     r4, .LCvfp
         ldr     r11, [r10, #TI_CPU]     @ CPU number
         add     r10, r10, #TI_VFPSTATE  @ r10 = workspace
diff --git a/arch/arm/vfp/vfphw.S b/arch/arm/vfp/vfphw.S
index ea0349f6358..404538ae591 100644
--- a/arch/arm/vfp/vfphw.S
+++ b/arch/arm/vfp/vfphw.S
@@ -16,7 +16,6 @@
  */
 #include <asm/thread_info.h>
 #include <asm/vfpmacros.h>
-#include <linux/kern_levels.h>
 #include "../kernel/entry-header.S"
 
         .macro  DBGSTR, str
@@ -25,7 +24,7 @@
         add     r0, pc, #4
         bl      printk
         b       1f
-        .asciz  KERN_DEBUG "VFP: \str\n"
+        .asciz  "<7>VFP: \str\n"
         .balign 4
 1:      ldmfd   sp!, {r0-r3, ip, lr}
 #endif
@@ -38,7 +37,7 @@
         add     r0, pc, #4
         bl      printk
         b       1f
-        .asciz  KERN_DEBUG "VFP: \str\n"
+        .asciz  "<7>VFP: \str\n"
         .balign 4
 1:      ldmfd   sp!, {r0-r3, ip, lr}
 #endif
@@ -53,7 +52,7 @@
         add     r0, pc, #4
         bl      printk
         b       1f
-        .asciz  KERN_DEBUG "VFP: \str\n"
+        .asciz  "<7>VFP: \str\n"
         .balign 4
 1:      ldmfd   sp!, {r0-r3, ip, lr}
 #endif
@@ -62,13 +61,13 @@
 
 @ VFP hardware support entry point.
 @
-@  r0  = instruction opcode (32-bit ARM or two 16-bit Thumb)
-@  r2  = PC value to resume execution after successful emulation
-@  r9  = normal "successful" return address
+@  r0  = faulted instruction
+@  r2  = faulted PC+4
+@  r9  = successful return
 @  r10 = vfp_state union
 @  r11 = CPU number
-@  lr  = unrecognised instruction return address
-@  IRQs enabled.
+@  lr  = failure return
+
 ENTRY(vfp_support_entry)
         DBGSTR3 "instr %08x pc %08x state %p", r0, r2, r10
 
@@ -83,22 +82,19 @@ ENTRY(vfp_support_entry)
         ldr     r4, [r3, r11, lsl #2]   @ vfp_current_hw_state pointer
         bic     r5, r1, #FPEXC_EX       @ make sure exceptions are disabled
         cmp     r4, r10                 @ this thread owns the hw context?
-#ifndef CONFIG_SMP
-        @ For UP, checking that this thread owns the hw context is
-        @ sufficient to determine that the hardware state is valid.
         beq     vfp_hw_state_valid
 
-        @ On UP, we lazily save the VFP context.  As a different
-        @ thread wants ownership of the VFP hardware, save the old
-        @ state if there was a previous (valid) owner.
-
         VFPFMXR FPEXC, r5               @ enable VFP, disable any pending
                                         @ exceptions, so we can get at the
                                         @ rest of it
 
+#ifndef CONFIG_SMP
+        @ Save out the current registers to the old thread state
+        @ No need for SMP since this is not done lazily
+
         DBGSTR1 "save old state %p", r4
-        cmp     r4, #0                  @ if the vfp_current_hw_state is NULL
-        beq     vfp_reload_hw           @ then the hw state needs reloading
+        cmp     r4, #0
+        beq     no_old_VFP_process
         VFPFSTMIA r4, r5                @ save the working registers
         VFPFMRX r5, FPSCR               @ current status
 #ifndef CONFIG_CPU_FEROCEON
@@ -111,33 +107,11 @@ ENTRY(vfp_support_entry)
 1:
 #endif
         stmia   r4, {r1, r5, r6, r8}    @ save FPEXC, FPSCR, FPINST, FPINST2
-vfp_reload_hw:
-
-#else
-        @ For SMP, if this thread does not own the hw context, then we
-        @ need to reload it.  No need to save the old state as on SMP,
-        @ we always save the state when we switch away from a thread.
-        bne     vfp_reload_hw
-
-        @ This thread has ownership of the current hardware context.
-        @ However, it may have been migrated to another CPU, in which
-        @ case the saved state is newer than the hardware context.
-        @ Check this by looking at the CPU number which the state was
-        @ last loaded onto.
-        ldr     ip, [r10, #VFP_CPU]
-        teq     ip, r11
-        beq     vfp_hw_state_valid
-
-vfp_reload_hw:
-        @ We're loading this threads state into the VFP hardware. Update
-        @ the CPU number which contains the most up to date VFP context.
-        str     r11, [r10, #VFP_CPU]
-
-        VFPFMXR FPEXC, r5               @ enable VFP, disable any pending
-                                        @ exceptions, so we can get at the
-                                        @ rest of it
+                                        @ and point r4 at the word at the
+                                        @ start of the register dump
 #endif
 
+no_old_VFP_process:
         DBGSTR1 "load state %p", r10
         str     r10, [r3, r11, lsl #2]  @ update the vfp_current_hw_state pointer
                                         @ Load the saved state back into the VFP
@@ -162,12 +136,9 @@ vfp_hw_state_valid:
                                         @ exception before retrying branch
                                         @ out before setting an FPEXC that
                                         @ stops us reading stuff
-        VFPFMXR FPEXC, r1               @ Restore FPEXC last
-        sub     r2, r2, #4              @ Retry current instruction - if Thumb
-        str     r2, [sp, #S_PC]         @ mode it's two 16-bit instructions,
-                                        @ else it's one 32-bit instruction, so
-                                        @ always subtract 4 from the following
-                                        @ instruction address.
+        VFPFMXR FPEXC, r1               @ restore FPEXC last
+        sub     r2, r2, #4
+        str     r2, [sp, #S_PC]         @ retry the instruction
 #ifdef CONFIG_PREEMPT
         get_thread_info r10
         ldr     r4, [r10, #TI_PREEMPT]  @ get preempt count
diff --git a/arch/arm/vfp/vfpmodule.c b/arch/arm/vfp/vfpmodule.c
index 3b44e0dd0a9..e381dc68505 100644
--- a/arch/arm/vfp/vfpmodule.c
+++ b/arch/arm/vfp/vfpmodule.c
@@ -8,24 +8,20 @@
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  */
+#include <linux/module.h>
 #include <linux/types.h>
 #include <linux/cpu.h>
-#include <linux/cpu_pm.h>
-#include <linux/hardirq.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 <linux/uaccess.h>
-#include <linux/user.h>
 
-#include <asm/cp15.h>
 #include <asm/cputype.h>
-#include <asm/system_info.h>
 #include <asm/thread_notify.h>
 #include <asm/vfp.h>
+#include <asm/cpu_pm.h>
 
 #include "vfpinstr.h"
 #include "vfp.h"
@@ -40,51 +36,18 @@ void vfp_null_entry(void);
 void (*vfp_vector)(void) = vfp_null_entry;
 
 /*
- * Dual-use variable.
- * Used in startup: set to non-zero if VFP checks fail
- * After startup, holds VFP architecture
- */
-unsigned int VFP_arch;
-
-/*
  * The pointer to the vfpstate structure of the thread which currently
  * owns the context held in the VFP hardware, or NULL if the hardware
  * context is invalid.
- *
- * For UP, this is sufficient to tell which thread owns the VFP context.
- * However, for SMP, we also need to check the CPU number stored in the
- * saved state too to catch migrations.
  */
 union vfp_state *vfp_current_hw_state[NR_CPUS];
 
 /*
- * Is 'thread's most up to date state stored in this CPUs hardware?
- * Must be called from non-preemptible context.
- */
-static bool vfp_state_in_hw(unsigned int cpu, struct thread_info *thread)
-{
-#ifdef CONFIG_SMP
-        if (thread->vfpstate.hard.cpu != cpu)
-                return false;
-#endif
-        return vfp_current_hw_state[cpu] == &thread->vfpstate;
-}
-
-/*
- * Force a reload of the VFP context from the thread structure.  We do
- * this by ensuring that access to the VFP hardware is disabled, and
- * clear vfp_current_hw_state.  Must be called from non-preemptible context.
+ * Dual-use variable.
+ * Used in startup: set to non-zero if VFP checks fail
+ * After startup, holds VFP architecture
  */
-static void vfp_force_reload(unsigned int cpu, struct thread_info *thread)
-{
-        if (vfp_state_in_hw(cpu, thread)) {
-                fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
-                vfp_current_hw_state[cpu] = NULL;
-        }
-#ifdef CONFIG_SMP
-        thread->vfpstate.hard.cpu = NR_CPUS;
-#endif
-}
+unsigned int VFP_arch;
 
 /*
  * Per-thread VFP initialization.
@@ -94,27 +57,21 @@ static void vfp_thread_flush(struct thread_info *thread)
         union vfp_state *vfp = &thread->vfpstate;
         unsigned int cpu;
 
+        memset(vfp, 0, sizeof(union vfp_state));
+
+        vfp->hard.fpexc = FPEXC_EN;
+        vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
+
         /*
          * Disable VFP to ensure we initialize it first.  We must ensure
-         * that the modification of vfp_current_hw_state[] and hardware
-         * disable are done for the same CPU and without preemption.
-         *
-         * Do this first to ensure that preemption won't overwrite our
-         * state saving should access to the VFP be enabled at this point.
+         * that the modification of vfp_current_hw_state[] and hardware disable
+         * are done for the same CPU and without preemption.
          */
         cpu = get_cpu();
         if (vfp_current_hw_state[cpu] == vfp)
                 vfp_current_hw_state[cpu] = NULL;
         fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
         put_cpu();
-
-        memset(vfp, 0, sizeof(union vfp_state));
-
-        vfp->hard.fpexc = FPEXC_EN;
-        vfp->hard.fpscr = FPSCR_ROUND_NEAREST;
-#ifdef CONFIG_SMP
-        vfp->hard.cpu = NR_CPUS;
-#endif
 }
 
 static void vfp_thread_exit(struct thread_info *thread)
@@ -134,9 +91,6 @@ static void vfp_thread_copy(struct thread_info *thread)
 
         vfp_sync_hwstate(parent);
         thread->vfpstate = parent->vfpstate;
-#ifdef CONFIG_SMP
-        thread->vfpstate.hard.cpu = NR_CPUS;
-#endif
 }
 
 /*
@@ -182,8 +136,17 @@ static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
                  * case the thread migrates to a different CPU. The
                  * restoring is done lazily.
                  */
-                if ((fpexc & FPEXC_EN) && vfp_current_hw_state[cpu])
+                if ((fpexc & FPEXC_EN) && vfp_current_hw_state[cpu]) {
                         vfp_save_state(vfp_current_hw_state[cpu], fpexc);
+                        vfp_current_hw_state[cpu]->hard.cpu = cpu;
+                }
+                /*
+                 * Thread migration, just force the reloading of the
+                 * state on the new CPU in case the VFP registers
+                 * contain stale data.
+                 */
+                if (thread->vfpstate.hard.cpu != cpu)
+                        vfp_current_hw_state[cpu] = NULL;
 #endif
 
                 /*
@@ -213,6 +176,35 @@ static struct notifier_block vfp_notifier_block = {
         .notifier_call  = vfp_notifier,
 };
 
+static int vfp_cpu_pm_notifier(struct notifier_block *self, unsigned long cmd,
+        void *v)
+{
+        u32 fpexc = fmrx(FPEXC);
+        unsigned int cpu = smp_processor_id();
+
+        switch (cmd) {
+        case CPU_PM_ENTER:
+                if (vfp_current_hw_state[cpu]) {
+                        fmxr(FPEXC, fpexc | FPEXC_EN);
+                        vfp_save_state(vfp_current_hw_state[cpu], fpexc);
+                        /* force a reload when coming back from idle */
+                        vfp_current_hw_state[cpu] = NULL;
+                        fmxr(FPEXC, fpexc & ~FPEXC_EN);
+                }
+                break;
+        case CPU_PM_ENTER_FAILED:
+        case CPU_PM_EXIT:
+                /* make sure VFP is disabled when leaving idle */
+                fmxr(FPEXC, fpexc & ~FPEXC_EN);
+                break;
+        }
+        return NOTIFY_OK;
+}
+
+static struct notifier_block vfp_cpu_pm_notifier_block = {
+        .notifier_call = vfp_cpu_pm_notifier,
+};
+
 /*
  * Raise a SIGFPE for the current process.
  * sicode describes the signal being raised.
@@ -241,11 +233,11 @@ static void vfp_panic(char *reason, u32 inst)
 {
         int i;
 
-        pr_err("VFP: Error: %s\n", reason);
-        pr_err("VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n",
+        printk(KERN_ERR "VFP: Error: %s\n", reason);
+        printk(KERN_ERR "VFP: EXC 0x%08x SCR 0x%08x INST 0x%08x\n",
                 fmrx(FPEXC), fmrx(FPSCR), inst);
         for (i = 0; i < 32; i += 2)
-                pr_err("VFP: s%2u: 0x%08x s%2u: 0x%08x\n",
+                printk(KERN_ERR "VFP: s%2u: 0x%08x s%2u: 0x%08x\n",
                        i, vfp_get_float(i), i+1, vfp_get_float(i+1));
 }
 
@@ -433,10 +425,7 @@ void VFP_bounce(u32 trigger, u32 fpexc, struct pt_regs *regs)
 
 static void vfp_enable(void *unused)
 {
-        u32 access;
-
-        BUG_ON(preemptible());
-        access = get_copro_access();
+        u32 access = get_copro_access();
 
         /*
          * Enable full access to VFP (cp10 and cp11)
@@ -444,29 +433,35 @@ static void vfp_enable(void *unused)
         set_copro_access(access | CPACC_FULL(10) | CPACC_FULL(11));
 }
 
-#ifdef CONFIG_CPU_PM
+#ifdef CONFIG_PM
+#include <linux/syscore_ops.h>
+
 static int vfp_pm_suspend(void)
 {
         struct thread_info *ti = current_thread_info();
         u32 fpexc = fmrx(FPEXC);
 
+        /* If lazy disable, re-enable the VFP ready for it to be saved */
+        if (vfp_current_hw_state[ti->cpu] != &ti->vfpstate) {
+                fpexc |= FPEXC_EN;
+                fmxr(FPEXC, fpexc);
+        }
+
         /* if vfp is on, then save state for resumption */
         if (fpexc & FPEXC_EN) {
-                pr_debug("%s: saving vfp state\n", __func__);
+                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);
         } else if (vfp_current_hw_state[ti->cpu]) {
-#ifndef CONFIG_SMP
                 fmxr(FPEXC, fpexc | FPEXC_EN);
                 vfp_save_state(vfp_current_hw_state[ti->cpu], fpexc);
                 fmxr(FPEXC, fpexc);
-#endif
         }
 
         /* clear any information we had about last context state */
-        vfp_current_hw_state[ti->cpu] = NULL;
+        memset(vfp_current_hw_state, 0, sizeof(vfp_current_hw_state));
 
         return 0;
 }
@@ -480,43 +475,29 @@ static void vfp_pm_resume(void)
         fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_EN);
 }
 
-static int vfp_cpu_pm_notifier(struct notifier_block *self, unsigned long cmd,
-        void *v)
-{
-        switch (cmd) {
-        case CPU_PM_ENTER:
-                vfp_pm_suspend();
-                break;
-        case CPU_PM_ENTER_FAILED:
-        case CPU_PM_EXIT:
-                vfp_pm_resume();
-                break;
-        }
-        return NOTIFY_OK;
-}
-
-static struct notifier_block vfp_cpu_pm_notifier_block = {
-        .notifier_call = vfp_cpu_pm_notifier,
+static struct syscore_ops vfp_pm_syscore_ops = {
+        .suspend        = vfp_pm_suspend,
+        .resume         = vfp_pm_resume,
 };
 
 static void vfp_pm_init(void)
 {
-        cpu_pm_register_notifier(&vfp_cpu_pm_notifier_block);
+        register_syscore_ops(&vfp_pm_syscore_ops);
 }
 
 #else
 static inline void vfp_pm_init(void) { }
-#endif /* CONFIG_CPU_PM */
+#endif /* CONFIG_PM */
 
-/*
- * Ensure that the VFP state stored in 'thread->vfpstate' is up to date
- * with the hardware state.
- */
 void vfp_sync_hwstate(struct thread_info *thread)
 {
         unsigned int cpu = get_cpu();
 
-        if (vfp_state_in_hw(cpu, thread)) {
+        /*
+         * If the thread we're interested in is the current owner of the
+         * hardware VFP state, then we need to save its state.
+         */
+        if (vfp_current_hw_state[cpu] == &thread->vfpstate) {
                 u32 fpexc = fmrx(FPEXC);
 
                 /*
@@ -530,101 +511,37 @@ void vfp_sync_hwstate(struct thread_info *thread)
         put_cpu();
 }
 
-/* Ensure that the thread reloads the hardware VFP state on the next use. */
 void vfp_flush_hwstate(struct thread_info *thread)
 {
         unsigned int cpu = get_cpu();
 
-        vfp_force_reload(cpu, thread);
-
-        put_cpu();
-}
-
-/*
- * Save the current VFP state into the provided structures and prepare
- * for entry into a new function (signal handler).
- */
-int vfp_preserve_user_clear_hwstate(struct user_vfp __user *ufp,
-                                    struct user_vfp_exc __user *ufp_exc)
-{
-        struct thread_info *thread = current_thread_info();
-        struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;
-        int err = 0;
-
-        /* Ensure that the saved hwstate is up-to-date. */
-        vfp_sync_hwstate(thread);
-
-        /*
-         * Copy the floating point registers. There can be unused
-         * registers see asm/hwcap.h for details.
-         */
-        err |= __copy_to_user(&ufp->fpregs, &hwstate->fpregs,
-                              sizeof(hwstate->fpregs));
         /*
-         * Copy the status and control register.
+         * If the thread we're interested in is the current owner of the
+         * hardware VFP state, then we need to save its state.
          */
-        __put_user_error(hwstate->fpscr, &ufp->fpscr, err);
-
-        /*
-         * Copy the exception registers.
-         */
-        __put_user_error(hwstate->fpexc, &ufp_exc->fpexc, err);
-        __put_user_error(hwstate->fpinst, &ufp_exc->fpinst, err);
-        __put_user_error(hwstate->fpinst2, &ufp_exc->fpinst2, err);
-
-        if (err)
-                return -EFAULT;
-
-        /* Ensure that VFP is disabled. */
-        vfp_flush_hwstate(thread);
-
-        /*
-         * As per the PCS, clear the length and stride bits for function
-         * entry.
-         */
-        hwstate->fpscr &= ~(FPSCR_LENGTH_MASK | FPSCR_STRIDE_MASK);
-        return 0;
-}
-
-/* Sanitise and restore the current VFP state from the provided structures. */
-int vfp_restore_user_hwstate(struct user_vfp __user *ufp,
-                             struct user_vfp_exc __user *ufp_exc)
-{
-        struct thread_info *thread = current_thread_info();
-        struct vfp_hard_struct *hwstate = &thread->vfpstate.hard;
-        unsigned long fpexc;
-        int err = 0;
+        if (vfp_current_hw_state[cpu] == &thread->vfpstate) {
+                u32 fpexc = fmrx(FPEXC);
 
-        /* Disable VFP to avoid corrupting the new thread state. */
-        vfp_flush_hwstate(thread);
+                fmxr(FPEXC, fpexc & ~FPEXC_EN);
 
-        /*
-         * Copy the floating point registers. There can be unused
-         * registers see asm/hwcap.h for details.
-         */
-        err |= __copy_from_user(&hwstate->fpregs, &ufp->fpregs,
-                                sizeof(hwstate->fpregs));
-        /*
-         * Copy the status and control register.
-         */
-        __get_user_error(hwstate->fpscr, &ufp->fpscr, err);
+                /*
+                 * Set the context to NULL to force a reload the next time
+                 * the thread uses the VFP.
+                 */
+                vfp_current_hw_state[cpu] = NULL;
+        }
 
+#ifdef CONFIG_SMP
         /*
-         * Sanitise and restore the exception registers.
+         * For SMP we still have to take care of the case where the thread
+         * migrates to another CPU and then back to the original CPU on which
+         * the last VFP user is still the same thread. Mark the thread VFP
+         * state as belonging to a non-existent CPU so that the saved one will
+         * be reloaded in the above case.
          */
-        __get_user_error(fpexc, &ufp_exc->fpexc, err);
-
-        /* Ensure the VFP is enabled. */
-        fpexc |= FPEXC_EN;
-
-        /* Ensure FPINST2 is invalid and the exception flag is cleared. */
-        fpexc &= ~(FPEXC_EX | FPEXC_FP2V);
-        hwstate->fpexc = fpexc;
-
-        __get_user_error(hwstate->fpinst, &ufp_exc->fpinst, err);
-        __get_user_error(hwstate->fpinst2, &ufp_exc->fpinst2, err);
-
-        return err ? -EFAULT : 0;
+        thread->vfpstate.hard.cpu = NR_CPUS;
+#endif
+        put_cpu();
 }
 
 /*
@@ -642,7 +559,8 @@ static int vfp_hotplug(struct notifier_block *b, unsigned long action,
         void *hcpu)
 {
         if (action == CPU_DYING || action == CPU_DYING_FROZEN) {
-                vfp_force_reload((long)hcpu, current_thread_info());
+                unsigned int cpu = (long)hcpu;
+                vfp_current_hw_state[cpu] = NULL;
         } else if (action == CPU_STARTING || action == CPU_STARTING_FROZEN)
                 vfp_enable(NULL);
         return NOTIFY_OK;
@@ -657,7 +575,7 @@ static int __init vfp_init(void)
         unsigned int cpu_arch = cpu_architecture();
 
         if (cpu_arch >= CPU_ARCH_ARMv6)
-                on_each_cpu(vfp_enable, NULL, 1);
+                vfp_enable(NULL);
 
         /*
          * First check that there is a VFP that we can use.
@@ -670,16 +588,18 @@ static int __init vfp_init(void)
         barrier();
         vfp_vector = vfp_null_entry;
 
-        pr_info("VFP support v0.3: ");
+        printk(KERN_INFO "VFP support v0.3: ");
         if (VFP_arch)
-                pr_cont("not present\n");
+                printk("not present\n");
         else if (vfpsid & FPSID_NODOUBLE) {
-                pr_cont("no double precision support\n");
+                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 */
-                pr_cont("implementor %02x architecture %d part %02x variant %x rev %x\n",
+                printk("implementor %02x architecture %d part %02x variant %x rev %x\n",
                         (vfpsid & FPSID_IMPLEMENTER_MASK) >> FPSID_IMPLEMENTER_BIT,
                         (vfpsid & FPSID_ARCH_MASK) >> FPSID_ARCH_BIT,
                         (vfpsid & FPSID_PART_MASK) >> FPSID_PART_BIT,
@@ -689,6 +609,7 @@ static int __init vfp_init(void)
                 vfp_vector = vfp_support_entry;
 
                 thread_register_notifier(&vfp_notifier_block);
+                cpu_pm_register_notifier(&vfp_cpu_pm_notifier_block);
                 vfp_pm_init();
 
                 /*
@@ -701,14 +622,11 @@ static int __init vfp_init(void)
                         elf_hwcap |= HWCAP_VFPv3;
 
                         /*
-                         * Check for VFPv3 D16 and VFPv4 D16.  CPUs in
-                         * this configuration only have 16 x 64bit
-                         * registers.
+                         * Check for VFPv3 D16. CPUs in this configuration
+                         * only have 16 x 64bit registers.
                          */
                         if (((fmrx(MVFR0) & MVFR0_A_SIMD_MASK)) == 1)
-                                elf_hwcap |= HWCAP_VFPv3D16; /* also v4-D16 */
-                        else
-                                elf_hwcap |= HWCAP_VFPD32;
+                                elf_hwcap |= HWCAP_VFPv3D16;
                 }
 #endif
                 /*
@@ -722,10 +640,8 @@ static int __init vfp_init(void)
                         if ((fmrx(MVFR1) & 0x000fff00) == 0x00011100)
                                 elf_hwcap |= HWCAP_NEON;
 #endif
-#ifdef CONFIG_VFPv3
                         if ((fmrx(MVFR1) & 0xf0000000) == 0x10000000)
                                 elf_hwcap |= HWCAP_VFPv4;
-#endif
                 }
         }
         return 0;