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;
 }