Merge branch 'for-linus' of master.kernel.org:/home/rmk/linux-2.6-arm

* 'for-linus' of master.kernel.org:/home/rmk/linux-2.6-arm:
  [ARM] 4117/1: S3C2412: Fix writel() usage in selection code
  [ARM] 4111/1: Allow VFP to work with thread migration on SMP
  [ARM] 4112/1: Only ioremap to supersections if DOMAIN_IO is zero
  [ARM] 4106/1: S3C2410: typo fixes in register definitions
  [ARM] 4102/1: Allow for PHYS_OFFSET on any valid 2MiB address
  [ARM] Fix AMBA serial drivers for non-first serial ports
  [ARM] 4100/1: iop3xx: fix cpu mask for iop333
  [ARM] Update mach-types
  [ARM] Fix show_mem() for discontigmem
  [ARM] 4096/1: S3C24XX: change return code form s3c2410_gpio_getcfg()
  [ARM] 4095/1: S3C24XX: Fix GPIO set for Bank A
  [ARM] 4092/1: i.MX/MX1 CPU Frequency scaling latency definition
  [ARM] 4089/1: AT91: GPIO wake IRQ cleanup
  [ARM] 4088/1: AT91: Unbalanced IRQ in serial driver suspend/resume
  [ARM] 4087/1: AT91: CPU reset for SAM9x processors
  [ARM] 4086/1: AT91: Whitespace cleanup
  [ARM] 4085/1: AT91: Header fixes.
  [ARM] 4084/1: Remove CONFIG_DEBUG_WAITQ

19 files changed, 139 insertions, 56 deletions

diff --git a/arch/arm/configs/at91sam9260ek_defconfig b/arch/arm/configs/at91sam9260ek_defconfig
index 79049206dfa5..46b0c734aeb9 100644
--- a/arch/arm/configs/at91sam9260ek_defconfig
+++ b/arch/arm/configs/at91sam9260ek_defconfig
@@ -923,7 +923,6 @@ CONFIG_FORCED_INLINING=y
 # CONFIG_HEADERS_CHECK is not set
 # CONFIG_RCU_TORTURE_TEST is not set
 CONFIG_DEBUG_USER=y
-# CONFIG_DEBUG_WAITQ is not set
 # CONFIG_DEBUG_ERRORS is not set
 CONFIG_DEBUG_LL=y
 # CONFIG_DEBUG_ICEDCC is not set
diff --git a/arch/arm/configs/at91sam9261ek_defconfig b/arch/arm/configs/at91sam9261ek_defconfig
index 784ad7c0186d..fcd8fa091e9d 100644
--- a/arch/arm/configs/at91sam9261ek_defconfig
+++ b/arch/arm/configs/at91sam9261ek_defconfig
@@ -1079,7 +1079,6 @@ CONFIG_FORCED_INLINING=y
 # CONFIG_HEADERS_CHECK is not set
 # CONFIG_RCU_TORTURE_TEST is not set
 CONFIG_DEBUG_USER=y
-# CONFIG_DEBUG_WAITQ is not set
 # CONFIG_DEBUG_ERRORS is not set
 CONFIG_DEBUG_LL=y
 # CONFIG_DEBUG_ICEDCC is not set
diff --git a/arch/arm/kernel/head.S b/arch/arm/kernel/head.S
index d994561816a1..cf495a3084b3 100644
--- a/arch/arm/kernel/head.S
+++ b/arch/arm/kernel/head.S
@@ -22,6 +22,10 @@
 #include <asm/thread_info.h>
 #include <asm/system.h>
 
+#if (PHYS_OFFSET & 0x001fffff)
+#error "PHYS_OFFSET must be at an even 2MiB boundary!"
+#endif
+
 #define KERNEL_RAM_VADDR        (PAGE_OFFSET + TEXT_OFFSET)
 #define KERNEL_RAM_PADDR        (PHYS_OFFSET + TEXT_OFFSET)
 
@@ -251,7 +255,8 @@ __create_page_tables:
          * Then map first 1MB of ram in case it contains our boot params.
          */
         add     r0, r4, #PAGE_OFFSET >> 18
-        orr     r6, r7, #PHYS_OFFSET
+        orr     r6, r7, #(PHYS_OFFSET & 0xff000000)
+        orr     r6, r6, #(PHYS_OFFSET & 0x00e00000)
         str     r6, [r0]
 
 #ifdef CONFIG_XIP_KERNEL
diff --git a/arch/arm/mach-at91rm9200/at91rm9200_devices.c b/arch/arm/mach-at91rm9200/at91rm9200_devices.c
index 4641b99db0ee..57fac7203fe4 100644
--- a/arch/arm/mach-at91rm9200/at91rm9200_devices.c
+++ b/arch/arm/mach-at91rm9200/at91rm9200_devices.c
@@ -272,7 +272,7 @@ void __init at91_add_device_cf(struct at91_cf_data *data)
         at91_set_A_periph(AT91_PIN_PC12, 0);    /* NCS6/CFCE2 */
 
         /* nWAIT is _not_ a default setting */
-        at91_set_A_periph(AT91_PIN_PC6, 1);     /*  nWAIT */
+        at91_set_A_periph(AT91_PIN_PC6, 1);     /* nWAIT */
 
         cf_data = *data;
         platform_device_register(&at91rm9200_cf_device);
diff --git a/arch/arm/mach-at91rm9200/at91sam9260.c b/arch/arm/mach-at91rm9200/at91sam9260.c
index 203f073a53e6..b14871adc300 100644
--- a/arch/arm/mach-at91rm9200/at91sam9260.c
+++ b/arch/arm/mach-at91rm9200/at91sam9260.c
@@ -16,6 +16,7 @@
 #include <asm/mach/map.h>
 #include <asm/arch/at91sam9260.h>
 #include <asm/arch/at91_pmc.h>
+#include <asm/arch/at91_rstc.h>
 
 #include "generic.h"
 #include "clock.h"
@@ -212,7 +213,7 @@ static struct at91_gpio_bank at91sam9260_gpio[] = {
 
 static void at91sam9260_reset(void)
 {
-#warning "Implement CPU reset"
+        at91_sys_write(AT91_RSTC_CR, (0xA5 << 24) | AT91_RSTC_PROCRST | AT91_RSTC_PERRST);
 }
 
 
diff --git a/arch/arm/mach-at91rm9200/at91sam9261.c b/arch/arm/mach-at91rm9200/at91sam9261.c
index 5a82f35da2e9..d242bb885c6d 100644
--- a/arch/arm/mach-at91rm9200/at91sam9261.c
+++ b/arch/arm/mach-at91rm9200/at91sam9261.c
@@ -16,6 +16,7 @@
 #include <asm/mach/map.h>
 #include <asm/arch/at91sam9261.h>
 #include <asm/arch/at91_pmc.h>
+#include <asm/arch/at91_rstc.h>
 
 #include "generic.h"
 #include "clock.h"
@@ -207,7 +208,7 @@ static struct at91_gpio_bank at91sam9261_gpio[] = {
 
 static void at91sam9261_reset(void)
 {
-#warning "Implement CPU reset"
+        at91_sys_write(AT91_RSTC_CR, (0xA5 << 24) | AT91_RSTC_PROCRST | AT91_RSTC_PERRST);
 }
 
 
diff --git a/arch/arm/mach-at91rm9200/gpio.c b/arch/arm/mach-at91rm9200/gpio.c
index 3f188508c391..af22659c8a28 100644
--- a/arch/arm/mach-at91rm9200/gpio.c
+++ b/arch/arm/mach-at91rm9200/gpio.c
@@ -20,7 +20,6 @@
 #include <asm/io.h>
 #include <asm/hardware.h>
 #include <asm/arch/at91_pio.h>
-#include <asm/arch/at91_pmc.h>
 #include <asm/arch/gpio.h>
 
 #include "generic.h"
@@ -224,17 +223,17 @@ static u32 backups[MAX_GPIO_BANKS];
 static int gpio_irq_set_wake(unsigned pin, unsigned state)
 {
         unsigned        mask = pin_to_mask(pin);
+        unsigned        bank = (pin - PIN_BASE) / 32;
 
-        pin -= PIN_BASE;
-        pin /= 32;
-
-        if (unlikely(pin >= MAX_GPIO_BANKS))
+        if (unlikely(bank >= MAX_GPIO_BANKS))
                 return -EINVAL;
 
         if (state)
-                wakeups[pin] |= mask;
+                wakeups[bank] |= mask;
         else
-                wakeups[pin] &= ~mask;
+                wakeups[bank] &= ~mask;
+
+        set_irq_wake(gpio[bank].id, state);
 
         return 0;
 }
@@ -246,29 +245,15 @@ void at91_gpio_suspend(void)
         for (i = 0; i < gpio_banks; i++) {
                 u32 pio = gpio[i].offset;
 
-                /*
-                 * Note: drivers should have disabled GPIO interrupts that
-                 * aren't supposed to be wakeup sources.
-                 * But that is not much good on ARM.....  disable_irq() does
-                 * not update the hardware immediately, so the hardware mask
-                 * (IMR) has the wrong value (not current, too much is
-                 * permitted).
-                 *
-                 * Our workaround is to disable all non-wakeup IRQs ...
-                 * which is exactly what correct drivers asked for in the
-                 * first place!
-                 */
                 backups[i] = at91_sys_read(pio + PIO_IMR);
                 at91_sys_write(pio + PIO_IDR, backups[i]);
                 at91_sys_write(pio + PIO_IER, wakeups[i]);
 
-                if (!wakeups[i]) {
-                        disable_irq_wake(gpio[i].id);
-                        at91_sys_write(AT91_PMC_PCDR, 1 << gpio[i].id);
-                } else {
-                        enable_irq_wake(gpio[i].id);
+                if (!wakeups[i])
+                        clk_disable(gpio[i].clock);
+                else {
 #ifdef CONFIG_PM_DEBUG
-                        printk(KERN_DEBUG "GPIO-%c may wake for %08x\n", "ABCD"[i], wakeups[i]);
+                        printk(KERN_DEBUG "GPIO-%c may wake for %08x\n", 'A'+i, wakeups[i]);
 #endif
                 }
         }
@@ -281,9 +266,11 @@ void at91_gpio_resume(void)
         for (i = 0; i < gpio_banks; i++) {
                 u32 pio = gpio[i].offset;
 
+                if (!wakeups[i])
+                        clk_enable(gpio[i].clock);
+
                 at91_sys_write(pio + PIO_IDR, wakeups[i]);
                 at91_sys_write(pio + PIO_IER, backups[i]);
-                at91_sys_write(AT91_PMC_PCER, 1 << gpio[i].id);
         }
 }
 
diff --git a/arch/arm/mach-imx/cpufreq.c b/arch/arm/mach-imx/cpufreq.c
index ac5f99895660..4f66e90db74f 100644
--- a/arch/arm/mach-imx/cpufreq.c
+++ b/arch/arm/mach-imx/cpufreq.c
@@ -184,6 +184,17 @@ static int imx_set_target(struct cpufreq_policy *policy,
         long sysclk;
         unsigned int bclk_div = 1;
 
+        /*
+         * Some governors do not respects CPU and policy lower limits
+         * which leads to bad things (division by zero etc), ensure
+         * that such things do not happen.
+         */
+        if(target_freq < policy->cpuinfo.min_freq)
+                target_freq = policy->cpuinfo.min_freq;
+
+        if(target_freq < policy->min)
+                target_freq = policy->min;
+
         freq = target_freq * 1000;
 
         pr_debug(KERN_DEBUG "imx: requested frequency %ld Hz, mpctl0 at boot 0x%08x\n",
@@ -258,7 +269,8 @@ static int __init imx_cpufreq_driver_init(struct cpufreq_policy *policy)
         policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
         policy->cpuinfo.min_freq = 8000;
         policy->cpuinfo.max_freq = 200000;
-        policy->cpuinfo.transition_latency = CPUFREQ_ETERNAL;
+         /* Manual states, that PLL stabilizes in two CLK32 periods */
+        policy->cpuinfo.transition_latency = 4 * 1000000000LL / CLK32;
         return 0;
 }
 
diff --git a/arch/arm/mach-s3c2410/gpio.c b/arch/arm/mach-s3c2410/gpio.c
index ba346546150b..f6fb215bb48c 100644
--- a/arch/arm/mach-s3c2410/gpio.c
+++ b/arch/arm/mach-s3c2410/gpio.c
@@ -57,6 +57,7 @@ void s3c2410_gpio_cfgpin(unsigned int pin, unsigned int function)
         case S3C2410_GPIO_SFN2:
         case S3C2410_GPIO_SFN3:
                 if (pin < S3C2410_GPIO_BANKB) {
+                        function -= 1;
                         function &= 1;
                         function <<= S3C2410_GPIO_OFFSET(pin);
                 } else {
@@ -83,15 +84,18 @@ EXPORT_SYMBOL(s3c2410_gpio_cfgpin);
 unsigned int s3c2410_gpio_getcfg(unsigned int pin)
 {
         void __iomem *base = S3C24XX_GPIO_BASE(pin);
-        unsigned long mask;
+        unsigned long val = __raw_readl(base);
 
         if (pin < S3C2410_GPIO_BANKB) {
-                mask = 1 << S3C2410_GPIO_OFFSET(pin);
+                val >>= S3C2410_GPIO_OFFSET(pin);
+                val &= 1;
+                val += 1;
         } else {
-                mask = 3 << S3C2410_GPIO_OFFSET(pin)*2;
+                val >>= S3C2410_GPIO_OFFSET(pin)*2;
+                val &= 3;
         }
 
-        return __raw_readl(base) & mask;
+        return val | S3C2410_GPIO_INPUT;
 }
 
 EXPORT_SYMBOL(s3c2410_gpio_getcfg);
diff --git a/arch/arm/mach-s3c2410/pm.c b/arch/arm/mach-s3c2410/pm.c
index 00834097eb82..ebf294dd31da 100644
--- a/arch/arm/mach-s3c2410/pm.c
+++ b/arch/arm/mach-s3c2410/pm.c
@@ -451,15 +451,14 @@ static void s3c2410_pm_check_resume_pin(unsigned int pin, unsigned int irqoffs)
                 irqstate = s3c_irqwake_eintmask & (1L<<irqoffs);
 
         pinstate = s3c2410_gpio_getcfg(pin);
-        pinstate >>= S3C2410_GPIO_OFFSET(pin)*2;
 
         if (!irqstate) {
-                if (pinstate == 0x02)
+                if (pinstate == S3C2410_GPIO_IRQ)
                         DBG("Leaving IRQ %d (pin %d) enabled\n", irq, pin);
         } else {
-                if (pinstate == 0x02) {
+                if (pinstate == S3C2410_GPIO_IRQ) {
                         DBG("Disabling IRQ %d (pin %d)\n", irq, pin);
-                        s3c2410_gpio_cfgpin(pin, 0x00);
+                        s3c2410_gpio_cfgpin(pin, S3C2410_GPIO_INPUT);
                 }
         }
 }
diff --git a/arch/arm/mach-s3c2410/s3c2412-dma.c b/arch/arm/mach-s3c2410/s3c2412-dma.c
index fe71a8fdb87c..138f726ac6bf 100644
--- a/arch/arm/mach-s3c2410/s3c2412-dma.c
+++ b/arch/arm/mach-s3c2410/s3c2412-dma.c
@@ -133,8 +133,8 @@ static struct s3c24xx_dma_map __initdata s3c2412_dma_mappings[] = {
 static void s3c2412_dma_select(struct s3c2410_dma_chan *chan,
                                struct s3c24xx_dma_map *map)
 {
-        writel(chan->regs + S3C2412_DMA_DMAREQSEL,
-               map->channels[0] | S3C2412_DMAREQSEL_HW);
+        writel(map->channels[0] | S3C2412_DMAREQSEL_HW,
+               chan->regs + S3C2412_DMA_DMAREQSEL);
 }
 
 static struct s3c24xx_dma_selection __initdata s3c2412_dma_sel = {
diff --git a/arch/arm/mm/init.c b/arch/arm/mm/init.c
index b5814b4b6f35..7760193e74cc 100644
--- a/arch/arm/mm/init.c
+++ b/arch/arm/mm/init.c
@@ -52,15 +52,18 @@ void show_mem(void)
         printk("Free swap:       %6ldkB\n", nr_swap_pages<<(PAGE_SHIFT-10));
 
         for_each_online_node(node) {
+                pg_data_t *n = NODE_DATA(node);
+                struct page *map = n->node_mem_map - n->node_start_pfn;
+
                 for_each_nodebank (i,mi,node) {
                         unsigned int pfn1, pfn2;
                         struct page *page, *end;
 
-                        pfn1 = mi->bank[i].start >> PAGE_SHIFT;
-                        pfn2 = (mi->bank[i].size + mi->bank[i].start) >> PAGE_SHIFT;
+                        pfn1 = __phys_to_pfn(mi->bank[i].start);
+                        pfn2 = __phys_to_pfn(mi->bank[i].size + mi->bank[i].start);
 
-                        page = NODE_MEM_MAP(node) + pfn1;
-                        end  = NODE_MEM_MAP(node) + pfn2;
+                        page = map + pfn1;
+                        end  = map + pfn2;
 
                         do {
                                 total++;
diff --git a/arch/arm/mm/ioremap.c b/arch/arm/mm/ioremap.c
index 251685fe73a8..0ac615c0f798 100644
--- a/arch/arm/mm/ioremap.c
+++ b/arch/arm/mm/ioremap.c
@@ -300,7 +300,8 @@ __ioremap_pfn(unsigned long pfn, unsigned long offset, size_t size,
         addr = (unsigned long)area->addr;
 
 #ifndef CONFIG_SMP
-        if ((((cpu_architecture() >= CPU_ARCH_ARMv6) && (get_cr() & CR_XP)) ||
+        if (DOMAIN_IO == 0 &&
+            (((cpu_architecture() >= CPU_ARCH_ARMv6) && (get_cr() & CR_XP)) ||
                cpu_is_xsc3()) &&
                !((__pfn_to_phys(pfn) | size | addr) & ~SUPERSECTION_MASK)) {
                 area->flags |= VM_ARM_SECTION_MAPPING;
diff --git a/arch/arm/mm/proc-xscale.S b/arch/arm/mm/proc-xscale.S
index 490e11b34231..d29fe927ee9e 100644
--- a/arch/arm/mm/proc-xscale.S
+++ b/arch/arm/mm/proc-xscale.S
@@ -708,7 +708,7 @@ __8032x_proc_info:
         .type   __8033x_proc_info,#object
 __8033x_proc_info:
         .long   0x69054010
-        .long   0xffffff30
+        .long   0xfffffd30
         .long   PMD_TYPE_SECT | \
                 PMD_SECT_BUFFERABLE | \
                 PMD_SECT_CACHEABLE | \
diff --git a/arch/arm/tools/mach-types b/arch/arm/tools/mach-types
index 8bcb838e5444..bd78058b7178 100644
--- a/arch/arm/tools/mach-types
+++ b/arch/arm/tools/mach-types
@@ -12,7 +12,7 @@
 #
 #   http://www.arm.linux.org.uk/developer/machines/?action=new
 #
-# Last update: Thu Dec 7 17:19:20 2006
+# Last update: Tue Jan 16 16:52:56 2007
 #
 # machine_is_xxx        CONFIG_xxxx             MACH_TYPE_xxx           number
 #
@@ -1219,3 +1219,26 @@ zevio_1020		MACH_ZEVIO_1020		ZEVIO_1020		1207
 hitrack                 MACH_HITRACK            HITRACK                 1208
 syme1                   MACH_SYME1              SYME1                   1209
 syhl1                   MACH_SYHL1              SYHL1                   1210
+empca400                MACH_EMPCA400           EMPCA400                1211
+em7210                  MACH_EM7210             EM7210                  1212
+htchermes               MACH_HTCHERMES          HTCHERMES               1213
+eti_c1                  MACH_ETI_C1             ETI_C1                  1214
+mach_dep2410            MACH_MACH_DEP2410       MACH_DEP2410            1215
+ac100                   MACH_AC100              AC100                   1216
+sneetch                 MACH_SNEETCH            SNEETCH                 1217
+studentmate             MACH_STUDENTMATE        STUDENTMATE             1218
+zir2410                 MACH_ZIR2410            ZIR2410                 1219
+zir2413                 MACH_ZIR2413            ZIR2413                 1220
+dlonip3                 MACH_DLONIP3            DLONIP3                 1221
+instream                MACH_INSTREAM           INSTREAM                1222
+ambarella               MACH_AMBARELLA          AMBARELLA               1223
+nevis                   MACH_NEVIS              NEVIS                   1224
+htc_trinity             MACH_HTC_TRINITY        HTC_TRINITY             1225
+ql202b                  MACH_QL202B             QL202B                  1226
+vpac270                 MACH_VPAC270            VPAC270                 1227
+rd129                   MACH_RD129              RD129                   1228
+htcwizard               MACH_HTCWIZARD          HTCWIZARD               1229
+xscale_treo680          MACH_XSCALE_TREO680     XSCALE_TREO680          1230
+tecon_tmezon            MACH_TECON_TMEZON       TECON_TMEZON            1231
+zylonite                MACH_ZYLONITE           ZYLONITE                1233
+gene1270                MACH_GENE1270           GENE1270                1234
diff --git a/arch/arm/vfp/entry.S b/arch/arm/vfp/entry.S
index 7b595547c1c8..ca2a5ad19ea6 100644
--- a/arch/arm/vfp/entry.S
+++ b/arch/arm/vfp/entry.S
@@ -25,6 +25,7 @@
 do_vfp:
         enable_irq
         ldr     r4, .LCvfp
+        ldr     r11, [r10, #TI_CPU]     @ CPU number
         add     r10, r10, #TI_VFPSTATE  @ r10 = workspace
         ldr     pc, [r4]                @ call VFP entry point
 
diff --git a/arch/arm/vfp/vfp.h b/arch/arm/vfp/vfp.h
index f2797896e6d5..54a2ad6d9ca2 100644
--- a/arch/arm/vfp/vfp.h
+++ b/arch/arm/vfp/vfp.h
@@ -370,3 +370,7 @@ struct op {
         u32 (* const fn)(int dd, int dn, int dm, u32 fpscr);
         u32 flags;
 };
+
+#ifdef CONFIG_SMP
+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 e51e6679c402..d4b7b229631d 100644
--- a/arch/arm/vfp/vfphw.S
+++ b/arch/arm/vfp/vfphw.S
@@ -65,6 +65,7 @@
 @  r2  = faulted PC+4
 @  r9  = successful return
 @  r10 = vfp_state union
+@  r11 = CPU number
 @  lr  = failure return
 
         .globl  vfp_support_entry
@@ -79,7 +80,7 @@ vfp_support_entry:
         DBGSTR1 "enable %x", r10
         ldr     r3, last_VFP_context_address
         orr     r1, r1, #FPEXC_ENABLE   @ user FPEXC has the enable bit set
-        ldr     r4, [r3]                @ last_VFP_context pointer
+        ldr     r4, [r3, r11, lsl #2]   @ last_VFP_context pointer
         bic     r5, r1, #FPEXC_EXCEPTION @ make sure exceptions are disabled
         cmp     r4, r10
         beq     check_for_exception     @ we are returning to the same
@@ -91,7 +92,9 @@ vfp_support_entry:
                                         @ 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
@@ -105,10 +108,11 @@ vfp_support_entry:
         stmia   r4, {r1, r5, r6, r8}    @ save FPEXC, FPSCR, FPINST, FPINST2
                                         @ 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]               @ update the last_VFP_context pointer
+        str     r10, [r3, r11, lsl #2]  @ update the last_VFP_context pointer
                                         @ Load the saved state back into the VFP
         VFPFLDMIA r10                   @ reload the working registers while
                                         @ FPEXC is in a safe state
@@ -162,6 +166,24 @@ process_exception:
                                         @ required. If not, the user code will
                                         @ retry the faulted instruction
 
+#ifdef CONFIG_SMP
+        .globl  vfp_save_state
+        .type   vfp_save_state, %function
+vfp_save_state:
+        @ Save the current VFP state
+        @ r0 - save location
+        @ r1 - FPEXC
+        DBGSTR1 "save VFP state %p", r0
+        VFPFMRX r2, FPSCR               @ current status
+        VFPFMRX r3, FPINST              @ FPINST (always there, rev0 onwards)
+        tst     r1, #FPEXC_FPV2         @ is there an FPINST2 to read?
+        VFPFMRX r12, FPINST2, NE        @ FPINST2 if needed - avoids reading
+                                        @ nonexistant reg on rev0
+        VFPFSTMIA r0                    @ save the working registers
+        stmia   r0, {r1, r2, r3, r12}   @ save FPEXC, FPSCR, FPINST, FPINST2
+        mov     pc, lr
+#endif
+
 last_VFP_context_address:
         .word   last_VFP_context
 
diff --git a/arch/arm/vfp/vfpmodule.c b/arch/arm/vfp/vfpmodule.c
index 490d9d18a7d1..f1e5951dc721 100644
--- a/arch/arm/vfp/vfpmodule.c
+++ b/arch/arm/vfp/vfpmodule.c
@@ -28,7 +28,7 @@ void vfp_testing_entry(void);
 void vfp_support_entry(void);
 
 void (*vfp_vector)(void) = vfp_testing_entry;
-union vfp_state *last_VFP_context;
+union vfp_state *last_VFP_context[NR_CPUS];
 
 /*
  * Dual-use variable.
@@ -41,13 +41,35 @@ static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
 {
         struct thread_info *thread = v;
         union vfp_state *vfp;
+        __u32 cpu = thread->cpu;
 
         if (likely(cmd == THREAD_NOTIFY_SWITCH)) {
+                u32 fpexc = fmrx(FPEXC);
+
+#ifdef CONFIG_SMP
+                /*
+                 * On SMP, if VFP is enabled, save the old state in
+                 * case the thread migrates to a different CPU. The
+                 * restoring is done lazily.
+                 */
+                if ((fpexc & FPEXC_ENABLE) && last_VFP_context[cpu]) {
+                        vfp_save_state(last_VFP_context[cpu], fpexc);
+                        last_VFP_context[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)
+                        last_VFP_context[cpu] = NULL;
+#endif
+
                 /*
                  * Always disable VFP so we can lazily save/restore the
                  * old state.
                  */
-                fmxr(FPEXC, fmrx(FPEXC) & ~FPEXC_ENABLE);
+                fmxr(FPEXC, fpexc & ~FPEXC_ENABLE);
                 return NOTIFY_DONE;
         }
 
@@ -68,8 +90,8 @@ static int vfp_notifier(struct notifier_block *self, unsigned long cmd, void *v)
         }
 
         /* flush and release case: Per-thread VFP cleanup. */
-        if (last_VFP_context == vfp)
-                last_VFP_context = NULL;
+        if (last_VFP_context[cpu] == vfp)
+                last_VFP_context[cpu] = NULL;
 
         return NOTIFY_DONE;
 }