Merge branches 'fixes' and 'devel-stable' into for-linus

88 files changed, 1114 insertions, 616 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 93d595a7477..4f8f3a4e4e0 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -365,6 +365,7 @@ config ARCH_MXC
 	select GENERIC_CLOCKEVENTS
 	select ARCH_REQUIRE_GPIOLIB
 	select CLKDEV_LOOKUP
+	select HAVE_SCHED_CLOCK
 	help
 	  Support for Freescale MXC/iMX-based family of processors
 
diff --git a/arch/arm/include/asm/mach/udc_pxa2xx.h b/arch/arm/include/asm/mach/udc_pxa2xx.h
index 833306ee9e7..ea297ac70bc 100644
--- a/arch/arm/include/asm/mach/udc_pxa2xx.h
+++ b/arch/arm/include/asm/mach/udc_pxa2xx.h
@@ -20,8 +20,6 @@ struct pxa2xx_udc_mach_info {
 	 * VBUS IRQ and omit the methods above.  Store the GPIO number
 	 * here.  Note that sometimes the signals go through inverters...
 	 */
-	bool	gpio_vbus_inverted;
-	int	gpio_vbus;			/* high == vbus present */
 	bool	gpio_pullup_inverted;
 	int	gpio_pullup;			/* high == pullup activated */
 };
diff --git a/arch/arm/mach-imx/Kconfig b/arch/arm/mach-imx/Kconfig
index 5eec099e0c7..56b930a1344 100644
--- a/arch/arm/mach-imx/Kconfig
+++ b/arch/arm/mach-imx/Kconfig
@@ -255,6 +255,7 @@ config MACH_IMX27_VISSTRIM_M10
 	bool "Vista Silicon i.MX27 Visstrim_m10"
 	select SOC_IMX27
 	select IMX_HAVE_PLATFORM_IMX_I2C
+	select IMX_HAVE_PLATFORM_IMX_SSI
 	select IMX_HAVE_PLATFORM_IMX_UART
 	select IMX_HAVE_PLATFORM_MXC_MMC
 	select IMX_HAVE_PLATFORM_MXC_EHCI
diff --git a/arch/arm/mach-imx/eukrea_mbimxsd25-baseboard.c b/arch/arm/mach-imx/eukrea_mbimxsd25-baseboard.c
index cb705c28de0..6269053505f 100644
--- a/arch/arm/mach-imx/eukrea_mbimxsd25-baseboard.c
+++ b/arch/arm/mach-imx/eukrea_mbimxsd25-baseboard.c
@@ -34,6 +34,7 @@
 #include <mach/mx25.h>
 #include <mach/imx-uart.h>
 #include <mach/audmux.h>
+#include <mach/esdhc.h>
 
 #include "devices-imx25.h"
 
@@ -242,6 +243,11 @@ struct imx_ssi_platform_data eukrea_mbimxsd_ssi_pdata __initconst = {
 	.flags = IMX_SSI_SYN | IMX_SSI_NET | IMX_SSI_USE_I2S_SLAVE,
 };
 
+static struct esdhc_platform_data sd1_pdata = {
+	.cd_gpio = GPIO_SD1CD,
+	.wp_gpio = -EINVAL,
+};
+
 /*
  * system init for baseboard usage. Will be called by cpuimx25 init.
  *
@@ -275,7 +281,7 @@ void __init eukrea_mbimxsd25_baseboard_init(void)
 	imx25_add_imx_ssi(0, &eukrea_mbimxsd_ssi_pdata);
 
 	imx25_add_flexcan1(NULL);
-	imx25_add_sdhci_esdhc_imx(0, NULL);
+	imx25_add_sdhci_esdhc_imx(0, &sd1_pdata);
 
 	gpio_request(GPIO_LED1, "LED1");
 	gpio_direction_output(GPIO_LED1, 1);
diff --git a/arch/arm/mach-mx3/eukrea_mbimxsd-baseboard.c b/arch/arm/mach-mx3/eukrea_mbimxsd-baseboard.c
index 80761474c0f..2e288b38b4a 100644
--- a/arch/arm/mach-mx3/eukrea_mbimxsd-baseboard.c
+++ b/arch/arm/mach-mx3/eukrea_mbimxsd-baseboard.c
@@ -43,6 +43,7 @@
 #include <mach/ipu.h>
 #include <mach/mx3fb.h>
 #include <mach/audmux.h>
+#include <mach/esdhc.h>
 
 #include "devices-imx35.h"
 #include "devices.h"
@@ -163,11 +164,14 @@ static iomux_v3_cfg_t eukrea_mbimxsd_pads[] = {
 	MX35_PAD_SD1_DATA1__ESDHC1_DAT1,
 	MX35_PAD_SD1_DATA2__ESDHC1_DAT2,
 	MX35_PAD_SD1_DATA3__ESDHC1_DAT3,
+	/* SD1 CD */
+	MX35_PAD_LD18__GPIO3_24,
 };
 
 #define GPIO_LED1	IMX_GPIO_NR(3, 29)
 #define GPIO_SWITCH1	IMX_GPIO_NR(3, 25)
-#define GPIO_LCDPWR	(4)
+#define GPIO_LCDPWR	IMX_GPIO_NR(1, 4)
+#define GPIO_SD1CD	IMX_GPIO_NR(3, 24)
 
 static void eukrea_mbimxsd_lcd_power_set(struct plat_lcd_data *pd,
 				   unsigned int power)
@@ -254,6 +258,11 @@ struct imx_ssi_platform_data eukrea_mbimxsd_ssi_pdata __initconst = {
 	.flags = IMX_SSI_SYN | IMX_SSI_NET | IMX_SSI_USE_I2S_SLAVE,
 };
 
+static struct esdhc_platform_data sd1_pdata = {
+	.cd_gpio = GPIO_SD1CD,
+	.wp_gpio = -EINVAL,
+};
+
 /*
  * system init for baseboard usage. Will be called by cpuimx35 init.
  *
@@ -289,7 +298,7 @@ void __init eukrea_mbimxsd35_baseboard_init(void)
 	imx35_add_imx_ssi(0, &eukrea_mbimxsd_ssi_pdata);
 
 	imx35_add_flexcan1(NULL);
-	imx35_add_sdhci_esdhc_imx(0, NULL);
+	imx35_add_sdhci_esdhc_imx(0, &sd1_pdata);
 
 	gpio_request(GPIO_LED1, "LED1");
 	gpio_direction_output(GPIO_LED1, 1);
@@ -301,7 +310,6 @@ void __init eukrea_mbimxsd35_baseboard_init(void)
 
 	gpio_request(GPIO_LCDPWR, "LCDPWR");
 	gpio_direction_output(GPIO_LCDPWR, 1);
-	gpio_free(GPIO_LCDPWR);
 
 	i2c_register_board_info(0, eukrea_mbimxsd_i2c_devices,
 				ARRAY_SIZE(eukrea_mbimxsd_i2c_devices));
diff --git a/arch/arm/mach-mx3/mach-pcm043.c b/arch/arm/mach-mx3/mach-pcm043.c
index b3ecfb22d24..036ba1a4704 100644
--- a/arch/arm/mach-mx3/mach-pcm043.c
+++ b/arch/arm/mach-mx3/mach-pcm043.c
@@ -40,6 +40,7 @@
 #include <mach/mx3fb.h>
 #include <mach/ulpi.h>
 #include <mach/audmux.h>
+#include <mach/esdhc.h>
 
 #include "devices-imx35.h"
 #include "devices.h"
@@ -217,11 +218,15 @@ static iomux_v3_cfg_t pcm043_pads[] = {
 	MX35_PAD_SD1_DATA1__ESDHC1_DAT1,
 	MX35_PAD_SD1_DATA2__ESDHC1_DAT2,
 	MX35_PAD_SD1_DATA3__ESDHC1_DAT3,
+	MX35_PAD_ATA_DATA10__GPIO2_23, /* WriteProtect */
+	MX35_PAD_ATA_DATA11__GPIO2_24, /* CardDetect */
 };
 
 #define AC97_GPIO_TXFS	IMX_GPIO_NR(2, 31)
 #define AC97_GPIO_TXD	IMX_GPIO_NR(2, 28)
 #define AC97_GPIO_RESET	IMX_GPIO_NR(2, 0)
+#define SD1_GPIO_WP	IMX_GPIO_NR(2, 23)
+#define SD1_GPIO_CD	IMX_GPIO_NR(2, 24)
 
 static void pcm043_ac97_warm_reset(struct snd_ac97 *ac97)
 {
@@ -346,6 +351,11 @@ static int __init pcm043_otg_mode(char *options)
 }
 __setup("otg_mode=", pcm043_otg_mode);
 
+static struct esdhc_platform_data sd1_pdata = {
+	.wp_gpio = SD1_GPIO_WP,
+	.cd_gpio = SD1_GPIO_CD,
+};
+
 /*
  * Board specific initialization.
  */
@@ -395,7 +405,7 @@ static void __init pcm043_init(void)
 		imx35_add_fsl_usb2_udc(&otg_device_pdata);
 
 	imx35_add_flexcan1(NULL);
-	imx35_add_sdhci_esdhc_imx(0, NULL);
+	imx35_add_sdhci_esdhc_imx(0, &sd1_pdata);
 }
 
 static void __init pcm043_timer_init(void)
diff --git a/arch/arm/mach-mx5/Kconfig b/arch/arm/mach-mx5/Kconfig
index 83ee08847d4..159340da919 100644
--- a/arch/arm/mach-mx5/Kconfig
+++ b/arch/arm/mach-mx5/Kconfig
@@ -165,6 +165,7 @@ config MACH_MX53_LOCO
 	select IMX_HAVE_PLATFORM_IMX_I2C
 	select IMX_HAVE_PLATFORM_IMX_UART
 	select IMX_HAVE_PLATFORM_SDHCI_ESDHC_IMX
+	select IMX_HAVE_PLATFORM_GPIO_KEYS
 	help
 	  Include support for MX53 LOCO platform. This includes specific
 	  configurations for the board and its peripherals.
diff --git a/arch/arm/mach-mx5/Makefile b/arch/arm/mach-mx5/Makefile
index 4f63048be3c..0b9338cec51 100644
--- a/arch/arm/mach-mx5/Makefile
+++ b/arch/arm/mach-mx5/Makefile
@@ -3,7 +3,7 @@
 #
 
 # Object file lists.
-obj-y   := cpu.o mm.o clock-mx51-mx53.o devices.o ehci.o
+obj-y   := cpu.o mm.o clock-mx51-mx53.o devices.o ehci.o system.o
 obj-$(CONFIG_SOC_IMX50) += mm-mx50.o
 
 obj-$(CONFIG_CPU_FREQ_IMX)    += cpu_op-mx51.o
diff --git a/arch/arm/mach-mx5/board-mx51_babbage.c b/arch/arm/mach-mx5/board-mx51_babbage.c
index b2ecd194e76..bea4e4135f9 100644
--- a/arch/arm/mach-mx5/board-mx51_babbage.c
+++ b/arch/arm/mach-mx5/board-mx51_babbage.c
@@ -228,13 +228,12 @@ static inline void babbage_fec_reset(void)
 	int ret;
 
 	/* reset FEC PHY */
-	ret = gpio_request(BABBAGE_FEC_PHY_RESET, "fec-phy-reset");
+	ret = gpio_request_one(BABBAGE_FEC_PHY_RESET,
+					GPIOF_OUT_INIT_LOW, "fec-phy-reset");
 	if (ret) {
 		printk(KERN_ERR"failed to get GPIO_FEC_PHY_RESET: %d\n", ret);
 		return;
 	}
-	gpio_direction_output(BABBAGE_FEC_PHY_RESET, 0);
-	gpio_set_value(BABBAGE_FEC_PHY_RESET, 0);
 	msleep(1);
 	gpio_set_value(BABBAGE_FEC_PHY_RESET, 1);
 }
diff --git a/arch/arm/mach-mx5/board-mx53_evk.c b/arch/arm/mach-mx5/board-mx53_evk.c
index 7b5735c5ea5..2af3f43f74d 100644
--- a/arch/arm/mach-mx5/board-mx53_evk.c
+++ b/arch/arm/mach-mx5/board-mx53_evk.c
@@ -34,7 +34,7 @@
 #include <mach/imx-uart.h>
 #include <mach/iomux-mx53.h>
 
-#define SMD_FEC_PHY_RST		IMX_GPIO_NR(7, 6)
+#define MX53_EVK_FEC_PHY_RST	IMX_GPIO_NR(7, 6)
 #define EVK_ECSPI1_CS0		IMX_GPIO_NR(2, 30)
 #define EVK_ECSPI1_CS1		IMX_GPIO_NR(3, 19)
 
@@ -82,15 +82,14 @@ static inline void mx53_evk_fec_reset(void)
 	int ret;
 
 	/* reset FEC PHY */
-	ret = gpio_request(SMD_FEC_PHY_RST, "fec-phy-reset");
+	ret = gpio_request_one(MX53_EVK_FEC_PHY_RST, GPIOF_OUT_INIT_LOW,
+							"fec-phy-reset");
 	if (ret) {
 		printk(KERN_ERR"failed to get GPIO_FEC_PHY_RESET: %d\n", ret);
 		return;
 	}
-	gpio_direction_output(SMD_FEC_PHY_RST, 0);
-	gpio_set_value(SMD_FEC_PHY_RST, 0);
 	msleep(1);
-	gpio_set_value(SMD_FEC_PHY_RST, 1);
+	gpio_set_value(MX53_EVK_FEC_PHY_RST, 1);
 }
 
 static struct fec_platform_data mx53_evk_fec_pdata = {
diff --git a/arch/arm/mach-mx5/board-mx53_loco.c b/arch/arm/mach-mx5/board-mx53_loco.c
index 0a18f8d23eb..10a1bea1054 100644
--- a/arch/arm/mach-mx5/board-mx53_loco.c
+++ b/arch/arm/mach-mx5/board-mx53_loco.c
@@ -36,6 +36,9 @@
 #include "crm_regs.h"
 #include "devices-imx53.h"
 
+#define MX53_LOCO_POWER			IMX_GPIO_NR(1, 8)
+#define MX53_LOCO_UI1			IMX_GPIO_NR(2, 14)
+#define MX53_LOCO_UI2			IMX_GPIO_NR(2, 15)
 #define LOCO_FEC_PHY_RST		IMX_GPIO_NR(7, 6)
 
 static iomux_v3_cfg_t mx53_loco_pads[] = {
@@ -180,6 +183,27 @@ static iomux_v3_cfg_t mx53_loco_pads[] = {
 	MX53_PAD_GPIO_8__GPIO1_8,
 };
 
+#define GPIO_BUTTON(gpio_num, ev_code, act_low, descr, wake)	\
+{								\
+	.gpio		= gpio_num,				\
+	.type		= EV_KEY,				\
+	.code		= ev_code,				\
+	.active_low	= act_low,				\
+	.desc		= "btn " descr,				\
+	.wakeup		= wake,					\
+}
+
+static const struct gpio_keys_button loco_buttons[] __initconst = {
+	GPIO_BUTTON(MX53_LOCO_POWER, KEY_POWER, 1, "power", 0),
+	GPIO_BUTTON(MX53_LOCO_UI1, KEY_VOLUMEUP, 1, "volume-up", 0),
+	GPIO_BUTTON(MX53_LOCO_UI2, KEY_VOLUMEDOWN, 1, "volume-down", 0),
+};
+
+static const struct gpio_keys_platform_data loco_button_data __initconst = {
+	.buttons        = loco_buttons,
+	.nbuttons       = ARRAY_SIZE(loco_buttons),
+};
+
 static inline void mx53_loco_fec_reset(void)
 {
 	int ret;
@@ -215,6 +239,7 @@ static void __init mx53_loco_board_init(void)
 	imx53_add_imx_i2c(1, &mx53_loco_i2c_data);
 	imx53_add_sdhci_esdhc_imx(0, NULL);
 	imx53_add_sdhci_esdhc_imx(2, NULL);
+	imx_add_gpio_keys(&loco_button_data);
 }
 
 static void __init mx53_loco_timer_init(void)
diff --git a/arch/arm/mach-mx5/clock-mx51-mx53.c b/arch/arm/mach-mx5/clock-mx51-mx53.c
index 652ace41382..fdbc05ed551 100644
--- a/arch/arm/mach-mx5/clock-mx51-mx53.c
+++ b/arch/arm/mach-mx5/clock-mx51-mx53.c
@@ -865,6 +865,13 @@ static struct clk aips_tz2_clk = {
 	.disable = _clk_ccgr_disable_inwait,
 };
 
+static struct clk gpc_dvfs_clk = {
+	.enable_reg = MXC_CCM_CCGR5,
+	.enable_shift = MXC_CCM_CCGRx_CG12_OFFSET,
+	.enable = _clk_ccgr_enable,
+	.disable = _clk_ccgr_disable,
+};
+
 static struct clk gpt_32k_clk = {
 	.id = 0,
 	.parent = &ckil_clk,
@@ -1448,6 +1455,7 @@ static struct clk_lookup mx51_lookups[] = {
 	_REGISTER_CLOCK("imx-ipuv3", NULL, ipu_clk)
 	_REGISTER_CLOCK("imx-ipuv3", "di0", ipu_di0_clk)
 	_REGISTER_CLOCK("imx-ipuv3", "di1", ipu_di1_clk)
+	_REGISTER_CLOCK(NULL, "gpc_dvfs", gpc_dvfs_clk)
 };
 
 static struct clk_lookup mx53_lookups[] = {
@@ -1511,6 +1519,7 @@ int __init mx51_clocks_init(unsigned long ckil, unsigned long osc,
 	clk_enable(&iim_clk);
 	mx51_revision();
 	clk_disable(&iim_clk);
+	mx51_display_revision();
 
 	/* move usb_phy_clk to 24MHz */
 	clk_set_parent(&usb_phy1_clk, &osc_clk);
diff --git a/arch/arm/mach-mx5/cpu.c b/arch/arm/mach-mx5/cpu.c
index df46b5e6085..472bdfab2e5 100644
--- a/arch/arm/mach-mx5/cpu.c
+++ b/arch/arm/mach-mx5/cpu.c
@@ -21,6 +21,7 @@
 static int cpu_silicon_rev = -1;
 
 #define IIM_SREV 0x24
+#define MX50_HW_ADADIG_DIGPROG	0xB0
 
 static int get_mx51_srev(void)
 {
@@ -51,6 +52,26 @@ int mx51_revision(void)
 }
 EXPORT_SYMBOL(mx51_revision);
 
+void mx51_display_revision(void)
+{
+	int rev;
+	char *srev;
+	rev = mx51_revision();
+
+	switch (rev) {
+	case IMX_CHIP_REVISION_2_0:
+		srev = IMX_CHIP_REVISION_2_0_STRING;
+		break;
+	case IMX_CHIP_REVISION_3_0:
+		srev = IMX_CHIP_REVISION_3_0_STRING;
+		break;
+	default:
+		srev = IMX_CHIP_REVISION_UNKNOWN_STRING;
+	}
+	printk(KERN_INFO "CPU identified as i.MX51, silicon rev %s\n", srev);
+}
+EXPORT_SYMBOL(mx51_display_revision);
+
 #ifdef CONFIG_NEON
 
 /*
@@ -107,6 +128,44 @@ int mx53_revision(void)
 }
 EXPORT_SYMBOL(mx53_revision);
 
+static int get_mx50_srev(void)
+{
+	void __iomem *anatop = ioremap(MX50_ANATOP_BASE_ADDR, SZ_8K);
+	u32 rev;
+
+	if (!anatop) {
+		cpu_silicon_rev = -EINVAL;
+		return 0;
+	}
+
+	rev = readl(anatop + MX50_HW_ADADIG_DIGPROG);
+	rev &= 0xff;
+
+	iounmap(anatop);
+	if (rev == 0x0)
+		return IMX_CHIP_REVISION_1_0;
+	else if (rev == 0x1)
+		return IMX_CHIP_REVISION_1_1;
+	return 0;
+}
+
+/*
+ * Returns:
+ *	the silicon revision of the cpu
+ *	-EINVAL - not a mx50
+ */
+int mx50_revision(void)
+{
+	if (!cpu_is_mx50())
+		return -EINVAL;
+
+	if (cpu_silicon_rev == -1)
+		cpu_silicon_rev = get_mx50_srev();
+
+	return cpu_silicon_rev;
+}
+EXPORT_SYMBOL(mx50_revision);
+
 static int __init post_cpu_init(void)
 {
 	unsigned int reg;
diff --git a/arch/arm/mach-mx5/eukrea_mbimxsd-baseboard.c b/arch/arm/mach-mx5/eukrea_mbimxsd-baseboard.c
index c372a437369..e6c1119c20a 100644
--- a/arch/arm/mach-mx5/eukrea_mbimxsd-baseboard.c
+++ b/arch/arm/mach-mx5/eukrea_mbimxsd-baseboard.c
@@ -67,6 +67,10 @@ static iomux_v3_cfg_t eukrea_mbimxsd_pads[] = {
 	MX51_PAD_SD1_DATA1__SD1_DATA1,
 	MX51_PAD_SD1_DATA2__SD1_DATA2,
 	MX51_PAD_SD1_DATA3__SD1_DATA3,
+	/* SD1 CD */
+	_MX51_PAD_GPIO1_0__SD1_CD | MUX_PAD_CTRL(PAD_CTL_PUS_22K_UP |
+			PAD_CTL_PKE | PAD_CTL_SRE_FAST |
+			PAD_CTL_DSE_HIGH | PAD_CTL_PUE | PAD_CTL_HYS),
 };
 
 #define GPIO_LED1	IMX_GPIO_NR(3, 30)
diff --git a/arch/arm/mach-mx5/mx51_efika.c b/arch/arm/mach-mx5/mx51_efika.c
index 51a67fc7f0e..d0c7075937c 100644
--- a/arch/arm/mach-mx5/mx51_efika.c
+++ b/arch/arm/mach-mx5/mx51_efika.c
@@ -42,7 +42,6 @@
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
 #include <asm/mach/time.h>
-#include <asm/mach-types.h>
 
 #include "devices-imx51.h"
 #include "devices.h"
@@ -572,8 +571,10 @@ static struct mc13xxx_regulator_init_data mx51_efika_regulators[] = {
 
 static struct mc13xxx_platform_data mx51_efika_mc13892_data = {
 	.flags = MC13XXX_USE_RTC | MC13XXX_USE_REGULATOR,
-	.num_regulators = ARRAY_SIZE(mx51_efika_regulators),
-	.regulators = mx51_efika_regulators,
+	.regulators = {
+		.num_regulators = ARRAY_SIZE(mx51_efika_regulators),
+		.regulators = mx51_efika_regulators,
+	},
 };
 
 static struct spi_board_info mx51_efika_spi_board_info[] __initdata = {
diff --git a/arch/arm/mach-mx5/system.c b/arch/arm/mach-mx5/system.c
new file mode 100644
index 00000000000..76ae8dc33e0
--- /dev/null
+++ b/arch/arm/mach-mx5/system.c
@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) 2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ */
+
+/*
+ * The code contained herein is licensed under the GNU General Public
+ * License. You may obtain a copy of the GNU General Public License
+ * Version 2 or later at the following locations:
+ *
+ * http://www.opensource.org/licenses/gpl-license.html
+ * http://www.gnu.org/copyleft/gpl.html
+ */
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <mach/hardware.h>
+#include "crm_regs.h"
+
+/* set cpu low power mode before WFI instruction. This function is called
+  * mx5 because it can be used for mx50, mx51, and mx53.*/
+void mx5_cpu_lp_set(enum mxc_cpu_pwr_mode mode)
+{
+	u32 plat_lpc, arm_srpgcr, ccm_clpcr;
+	u32 empgc0, empgc1;
+	int stop_mode = 0;
+
+	/* always allow platform to issue a deep sleep mode request */
+	plat_lpc = __raw_readl(MXC_CORTEXA8_PLAT_LPC) &
+	    ~(MXC_CORTEXA8_PLAT_LPC_DSM);
+	ccm_clpcr = __raw_readl(MXC_CCM_CLPCR) & ~(MXC_CCM_CLPCR_LPM_MASK);
+	arm_srpgcr = __raw_readl(MXC_SRPG_ARM_SRPGCR) & ~(MXC_SRPGCR_PCR);
+	empgc0 = __raw_readl(MXC_SRPG_EMPGC0_SRPGCR) & ~(MXC_SRPGCR_PCR);
+	empgc1 = __raw_readl(MXC_SRPG_EMPGC1_SRPGCR) & ~(MXC_SRPGCR_PCR);
+
+	switch (mode) {
+	case WAIT_CLOCKED:
+		break;
+	case WAIT_UNCLOCKED:
+		ccm_clpcr |= 0x1 << MXC_CCM_CLPCR_LPM_OFFSET;
+		break;
+	case WAIT_UNCLOCKED_POWER_OFF:
+	case STOP_POWER_OFF:
+		plat_lpc |= MXC_CORTEXA8_PLAT_LPC_DSM
+			    | MXC_CORTEXA8_PLAT_LPC_DBG_DSM;
+		if (mode == WAIT_UNCLOCKED_POWER_OFF) {
+			ccm_clpcr |= 0x1 << MXC_CCM_CLPCR_LPM_OFFSET;
+			ccm_clpcr &= ~MXC_CCM_CLPCR_VSTBY;
+			ccm_clpcr &= ~MXC_CCM_CLPCR_SBYOS;
+			stop_mode = 0;
+		} else {
+			ccm_clpcr |= 0x2 << MXC_CCM_CLPCR_LPM_OFFSET;
+			ccm_clpcr |= 0x3 << MXC_CCM_CLPCR_STBY_COUNT_OFFSET;
+			ccm_clpcr |= MXC_CCM_CLPCR_VSTBY;
+			ccm_clpcr |= MXC_CCM_CLPCR_SBYOS;
+			stop_mode = 1;
+		}
+		arm_srpgcr |= MXC_SRPGCR_PCR;
+
+		if (tzic_enable_wake(1) != 0)
+			return;
+		break;
+	case STOP_POWER_ON:
+		ccm_clpcr |= 0x2 << MXC_CCM_CLPCR_LPM_OFFSET;
+		break;
+	default:
+		printk(KERN_WARNING "UNKNOWN cpu power mode: %d\n", mode);
+		return;
+	}
+
+	__raw_writel(plat_lpc, MXC_CORTEXA8_PLAT_LPC);
+	__raw_writel(ccm_clpcr, MXC_CCM_CLPCR);
+	__raw_writel(arm_srpgcr, MXC_SRPG_ARM_SRPGCR);
+
+	/* Enable NEON SRPG for all but MX50TO1.0. */
+	if (mx50_revision() != IMX_CHIP_REVISION_1_0)
+		__raw_writel(arm_srpgcr, MXC_SRPG_NEON_SRPGCR);
+
+	if (stop_mode) {
+		empgc0 |= MXC_SRPGCR_PCR;
+		empgc1 |= MXC_SRPGCR_PCR;
+
+		__raw_writel(empgc0, MXC_SRPG_EMPGC0_SRPGCR);
+		__raw_writel(empgc1, MXC_SRPG_EMPGC1_SRPGCR);
+	}
+}
diff --git a/arch/arm/mach-mxs/Kconfig b/arch/arm/mach-mxs/Kconfig
index 4f6f174af6c..4522fbb235d 100644
--- a/arch/arm/mach-mxs/Kconfig
+++ b/arch/arm/mach-mxs/Kconfig
@@ -22,6 +22,7 @@ config MACH_MX23EVK
 	select SOC_IMX23
 	select MXS_HAVE_AMBA_DUART
 	select MXS_HAVE_PLATFORM_AUART
+	select MXS_HAVE_PLATFORM_MXS_MMC
 	select MXS_HAVE_PLATFORM_MXSFB
 	default y
 	help
@@ -35,6 +36,7 @@ config MACH_MX28EVK
 	select MXS_HAVE_PLATFORM_AUART
 	select MXS_HAVE_PLATFORM_FEC
 	select MXS_HAVE_PLATFORM_FLEXCAN
+	select MXS_HAVE_PLATFORM_MXS_MMC
 	select MXS_HAVE_PLATFORM_MXSFB
 	select MXS_OCOTP
 	default y
diff --git a/arch/arm/mach-mxs/clock-mx23.c b/arch/arm/mach-mxs/clock-mx23.c
index d133c7f3094..c3577ea789a 100644
--- a/arch/arm/mach-mxs/clock-mx23.c
+++ b/arch/arm/mach-mxs/clock-mx23.c
@@ -521,6 +521,15 @@ static int clk_misc_init(void)
 	__raw_writel(BM_CLKCTRL_CPU_INTERRUPT_WAIT,
 			CLKCTRL_BASE_ADDR + HW_CLKCTRL_CPU_SET);
 
+	/*
+	 * 480 MHz seems too high to be ssp clock source directly,
+	 * so set frac to get a 288 MHz ref_io.
+	 */
+	reg = __raw_readl(CLKCTRL_BASE_ADDR + HW_CLKCTRL_FRAC);
+	reg &= ~BM_CLKCTRL_FRAC_IOFRAC;
+	reg |= 30 << BP_CLKCTRL_FRAC_IOFRAC;
+	__raw_writel(reg, CLKCTRL_BASE_ADDR + HW_CLKCTRL_FRAC);
+
 	return 0;
 }
 
@@ -528,6 +537,12 @@ int __init mx23_clocks_init(void)
 {
 	clk_misc_init();
 
+	/*
+	 * source ssp clock from ref_io than ref_xtal,
+	 * as ref_xtal only provides 24 MHz as maximum.
+	 */
+	clk_set_parent(&ssp_clk, &ref_io_clk);
+
 	clk_enable(&cpu_clk);
 	clk_enable(&hbus_clk);
 	clk_enable(&xbus_clk);
diff --git a/arch/arm/mach-mxs/clock-mx28.c b/arch/arm/mach-mxs/clock-mx28.c
index 5e489a2b202..1ad97fed1e9 100644
--- a/arch/arm/mach-mxs/clock-mx28.c
+++ b/arch/arm/mach-mxs/clock-mx28.c
@@ -618,6 +618,8 @@ static struct clk_lookup lookups[] = {
 	_REGISTER_CLOCK("pll2", NULL, pll2_clk)
 	_REGISTER_CLOCK("mxs-dma-apbh", NULL, hbus_clk)
 	_REGISTER_CLOCK("mxs-dma-apbx", NULL, xbus_clk)
+	_REGISTER_CLOCK("mxs-mmc.0", NULL, ssp0_clk)
+	_REGISTER_CLOCK("mxs-mmc.1", NULL, ssp1_clk)
 	_REGISTER_CLOCK("flexcan.0", NULL, can0_clk)
 	_REGISTER_CLOCK("flexcan.1", NULL, can1_clk)
 	_REGISTER_CLOCK(NULL, "usb0", usb0_clk)
@@ -737,6 +739,15 @@ static int clk_misc_init(void)
 	reg |= BM_CLKCTRL_ENET_CLK_OUT_EN;
 	__raw_writel(reg, CLKCTRL_BASE_ADDR + HW_CLKCTRL_ENET);
 
+	/*
+	 * 480 MHz seems too high to be ssp clock source directly,
+	 * so set frac0 to get a 288 MHz ref_io0.
+	 */
+	reg = __raw_readl(CLKCTRL_BASE_ADDR + HW_CLKCTRL_FRAC0);
+	reg &= ~BM_CLKCTRL_FRAC0_IO0FRAC;
+	reg |= 30 << BP_CLKCTRL_FRAC0_IO0FRAC;
+	__raw_writel(reg, CLKCTRL_BASE_ADDR + HW_CLKCTRL_FRAC0);
+
 	return 0;
 }
 
@@ -744,6 +755,13 @@ int __init mx28_clocks_init(void)
 {
 	clk_misc_init();
 
+	/*
+	 * source ssp clock from ref_io0 than ref_xtal,
+	 * as ref_xtal only provides 24 MHz as maximum.
+	 */
+	clk_set_parent(&ssp0_clk, &ref_io0_clk);
+	clk_set_parent(&ssp1_clk, &ref_io0_clk);
+
 	clk_enable(&cpu_clk);
 	clk_enable(&hbus_clk);
 	clk_enable(&xbus_clk);
diff --git a/arch/arm/mach-mxs/devices-mx23.h b/arch/arm/mach-mxs/devices-mx23.h
index c7e14f4e366..c6f345febd3 100644
--- a/arch/arm/mach-mxs/devices-mx23.h
+++ b/arch/arm/mach-mxs/devices-mx23.h
@@ -21,6 +21,10 @@ extern const struct mxs_auart_data mx23_auart_data[] __initconst;
 #define mx23_add_auart0()		mx23_add_auart(0)
 #define mx23_add_auart1()		mx23_add_auart(1)
 
+extern const struct mxs_mxs_mmc_data mx23_mxs_mmc_data[] __initconst;
+#define mx23_add_mxs_mmc(id, pdata) \
+	mxs_add_mxs_mmc(&mx23_mxs_mmc_data[id], pdata)
+
 #define mx23_add_mxs_pwm(id)		mxs_add_mxs_pwm(MX23_PWM_BASE_ADDR, id)
 
 struct platform_device *__init mx23_add_mxsfb(
diff --git a/arch/arm/mach-mxs/devices-mx28.h b/arch/arm/mach-mxs/devices-mx28.h
index 9d08555c4cf..c473eddce8c 100644
--- a/arch/arm/mach-mxs/devices-mx28.h
+++ b/arch/arm/mach-mxs/devices-mx28.h
@@ -37,6 +37,10 @@ extern const struct mxs_flexcan_data mx28_flexcan_data[] __initconst;
 extern const struct mxs_i2c_data mx28_mxs_i2c_data[] __initconst;
 #define mx28_add_mxs_i2c(id)		mxs_add_mxs_i2c(&mx28_mxs_i2c_data[id])
 
+extern const struct mxs_mxs_mmc_data mx28_mxs_mmc_data[] __initconst;
+#define mx28_add_mxs_mmc(id, pdata) \
+	mxs_add_mxs_mmc(&mx28_mxs_mmc_data[id], pdata)
+
 #define mx28_add_mxs_pwm(id)		mxs_add_mxs_pwm(MX28_PWM_BASE_ADDR, id)
 
 struct platform_device *__init mx28_add_mxsfb(
diff --git a/arch/arm/mach-mxs/devices/Kconfig b/arch/arm/mach-mxs/devices/Kconfig
index 1451ad060d8..acf9eea124c 100644
--- a/arch/arm/mach-mxs/devices/Kconfig
+++ b/arch/arm/mach-mxs/devices/Kconfig
@@ -15,6 +15,9 @@ config MXS_HAVE_PLATFORM_FLEXCAN
 config MXS_HAVE_PLATFORM_MXS_I2C
 	bool
 
+config MXS_HAVE_PLATFORM_MXS_MMC
+	bool
+
 config MXS_HAVE_PLATFORM_MXS_PWM
 	bool
 
diff --git a/arch/arm/mach-mxs/devices/Makefile b/arch/arm/mach-mxs/devices/Makefile
index 0d9bea30b0a..324f2824d38 100644
--- a/arch/arm/mach-mxs/devices/Makefile
+++ b/arch/arm/mach-mxs/devices/Makefile
@@ -4,5 +4,6 @@ obj-y += platform-dma.o
 obj-$(CONFIG_MXS_HAVE_PLATFORM_FEC) += platform-fec.o
 obj-$(CONFIG_MXS_HAVE_PLATFORM_FLEXCAN) += platform-flexcan.o
 obj-$(CONFIG_MXS_HAVE_PLATFORM_MXS_I2C) += platform-mxs-i2c.o
+obj-$(CONFIG_MXS_HAVE_PLATFORM_MXS_MMC) += platform-mxs-mmc.o
 obj-$(CONFIG_MXS_HAVE_PLATFORM_MXS_PWM) += platform-mxs-pwm.o
 obj-$(CONFIG_MXS_HAVE_PLATFORM_MXSFB) += platform-mxsfb.o
diff --git a/arch/arm/mach-mxs/devices/platform-mxs-mmc.c b/arch/arm/mach-mxs/devices/platform-mxs-mmc.c
new file mode 100644
index 00000000000..382dacbeca2
--- /dev/null
+++ b/arch/arm/mach-mxs/devices/platform-mxs-mmc.c
@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2010 Pengutronix
+ * Uwe Kleine-Koenig <u.kleine-koenig@pengutronix.de>
+ *
+ * Copyright 2011 Freescale Semiconductor, Inc. All Rights Reserved.
+ *
+ * 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.
+ */
+
+#include <linux/compiler.h>
+#include <linux/err.h>
+#include <linux/init.h>
+
+#include <mach/mx23.h>
+#include <mach/mx28.h>
+#include <mach/devices-common.h>
+
+#define mxs_mxs_mmc_data_entry_single(soc, _id, hwid)			\
+	{								\
+		.id = _id,						\
+		.iobase = soc ## _SSP ## hwid ## _BASE_ADDR,		\
+		.dma = soc ## _DMA_SSP ## hwid,				\
+		.irq_err = soc ## _INT_SSP ## hwid ## _ERROR,		\
+		.irq_dma = soc ## _INT_SSP ## hwid ## _DMA,		\
+	}
+
+#define mxs_mxs_mmc_data_entry(soc, _id, hwid)				\
+	[_id] = mxs_mxs_mmc_data_entry_single(soc, _id, hwid)
+
+
+#ifdef CONFIG_SOC_IMX23
+const struct mxs_mxs_mmc_data mx23_mxs_mmc_data[] __initconst = {
+	mxs_mxs_mmc_data_entry(MX23, 0, 1),
+	mxs_mxs_mmc_data_entry(MX23, 1, 2),
+};
+#endif
+
+#ifdef CONFIG_SOC_IMX28
+const struct mxs_mxs_mmc_data mx28_mxs_mmc_data[] __initconst = {
+	mxs_mxs_mmc_data_entry(MX28, 0, 0),
+	mxs_mxs_mmc_data_entry(MX28, 1, 1),
+};
+#endif
+
+struct platform_device *__init mxs_add_mxs_mmc(
+		const struct mxs_mxs_mmc_data *data,
+		const struct mxs_mmc_platform_data *pdata)
+{
+	struct resource res[] = {
+		{
+			.start	= data->iobase,
+			.end	= data->iobase + SZ_8K - 1,
+			.flags	= IORESOURCE_MEM,
+		}, {
+			.start	= data->dma,
+			.end	= data->dma,
+			.flags	= IORESOURCE_DMA,
+		}, {
+			.start	= data->irq_err,
+			.end	= data->irq_err,
+			.flags	= IORESOURCE_IRQ,
+		}, {
+			.start	= data->irq_dma,
+			.end	= data->irq_dma,
+			.flags	= IORESOURCE_IRQ,
+		},
+	};
+
+	return mxs_add_platform_device("mxs-mmc", data->id,
+			res, ARRAY_SIZE(res), pdata, sizeof(*pdata));
+}
diff --git a/arch/arm/mach-mxs/include/mach/devices-common.h b/arch/arm/mach-mxs/include/mach/devices-common.h
index 71f24484b04..c5137f14c36 100644
--- a/arch/arm/mach-mxs/include/mach/devices-common.h
+++ b/arch/arm/mach-mxs/include/mach/devices-common.h
@@ -73,6 +73,19 @@ struct mxs_i2c_data {
 };
 struct platform_device * __init mxs_add_mxs_i2c(const struct mxs_i2c_data *data);
 
+/* mmc */
+#include <mach/mmc.h>
+struct mxs_mxs_mmc_data {
+	int id;
+	resource_size_t iobase;
+	resource_size_t dma;
+	resource_size_t irq_err;
+	resource_size_t irq_dma;
+};
+struct platform_device *__init mxs_add_mxs_mmc(
+		const struct mxs_mxs_mmc_data *data,
+		const struct mxs_mmc_platform_data *pdata);
+
 /* pwm */
 struct platform_device *__init mxs_add_mxs_pwm(
 		resource_size_t iobase, int id);
diff --git a/arch/arm/mach-mxs/mach-mx23evk.c b/arch/arm/mach-mxs/mach-mx23evk.c
index a66994f0518..214e5b641bb 100644
--- a/arch/arm/mach-mxs/mach-mx23evk.c
+++ b/arch/arm/mach-mxs/mach-mx23evk.c
@@ -28,6 +28,8 @@
 
 #define MX23EVK_LCD_ENABLE	MXS_GPIO_NR(1, 18)
 #define MX23EVK_BL_ENABLE	MXS_GPIO_NR(1, 28)
+#define MX23EVK_MMC0_WRITE_PROTECT	MXS_GPIO_NR(1, 30)
+#define MX23EVK_MMC0_SLOT_POWER		MXS_GPIO_NR(1, 29)
 
 static const iomux_cfg_t mx23evk_pads[] __initconst = {
 	/* duart */
@@ -73,6 +75,36 @@ static const iomux_cfg_t mx23evk_pads[] __initconst = {
 	MX23_PAD_LCD_RESET__GPIO_1_18 | MXS_PAD_CTRL,
 	/* backlight control */
 	MX23_PAD_PWM2__GPIO_1_28 | MXS_PAD_CTRL,
+
+	/* mmc */
+	MX23_PAD_SSP1_DATA0__SSP1_DATA0 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_SSP1_DATA1__SSP1_DATA1 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_SSP1_DATA2__SSP1_DATA2 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_SSP1_DATA3__SSP1_DATA3 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_GPMI_D08__SSP1_DATA4 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_GPMI_D09__SSP1_DATA5 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_GPMI_D10__SSP1_DATA6 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_GPMI_D11__SSP1_DATA7 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_SSP1_CMD__SSP1_CMD |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX23_PAD_SSP1_DETECT__SSP1_DETECT |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	MX23_PAD_SSP1_SCK__SSP1_SCK |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	/* write protect */
+	MX23_PAD_PWM4__GPIO_1_30 |
+		(MXS_PAD_4MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	/* slot power enable */
+	MX23_PAD_PWM3__GPIO_1_29 |
+		(MXS_PAD_4MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
 };
 
 /* mxsfb (lcdif) */
@@ -101,6 +133,11 @@ static const struct mxsfb_platform_data mx23evk_mxsfb_pdata __initconst = {
 	.ld_intf_width	= STMLCDIF_24BIT,
 };
 
+static struct mxs_mmc_platform_data mx23evk_mmc_pdata __initdata = {
+	.wp_gpio = MX23EVK_MMC0_WRITE_PROTECT,
+	.flags = SLOTF_8_BIT_CAPABLE,
+};
+
 static void __init mx23evk_init(void)
 {
 	int ret;
@@ -110,6 +147,13 @@ static void __init mx23evk_init(void)
 	mx23_add_duart();
 	mx23_add_auart0();
 
+	/* power on mmc slot by writing 0 to the gpio */
+	ret = gpio_request_one(MX23EVK_MMC0_SLOT_POWER, GPIOF_DIR_OUT,
+			       "mmc0-slot-power");
+	if (ret)
+		pr_warn("failed to request gpio mmc0-slot-power: %d\n", ret);
+	mx23_add_mxs_mmc(0, &mx23evk_mmc_pdata);
+
 	ret = gpio_request_one(MX23EVK_LCD_ENABLE, GPIOF_DIR_OUT, "lcd-enable");
 	if (ret)
 		pr_warn("failed to request gpio lcd-enable: %d\n", ret);
diff --git a/arch/arm/mach-mxs/mach-mx28evk.c b/arch/arm/mach-mxs/mach-mx28evk.c
index 08002d02267..bb329b9a260 100644
--- a/arch/arm/mach-mxs/mach-mx28evk.c
+++ b/arch/arm/mach-mxs/mach-mx28evk.c
@@ -34,6 +34,11 @@
 #define MX28EVK_LCD_ENABLE	MXS_GPIO_NR(3, 30)
 #define MX28EVK_FEC_PHY_RESET	MXS_GPIO_NR(4, 13)
 
+#define MX28EVK_MMC0_WRITE_PROTECT	MXS_GPIO_NR(2, 12)
+#define MX28EVK_MMC1_WRITE_PROTECT	MXS_GPIO_NR(0, 28)
+#define MX28EVK_MMC0_SLOT_POWER		MXS_GPIO_NR(3, 28)
+#define MX28EVK_MMC1_SLOT_POWER		MXS_GPIO_NR(3, 29)
+
 static const iomux_cfg_t mx28evk_pads[] __initconst = {
 	/* duart */
 	MX28_PAD_PWM0__DUART_RX | MXS_PAD_CTRL,
@@ -115,6 +120,65 @@ static const iomux_cfg_t mx28evk_pads[] __initconst = {
 	MX28_PAD_LCD_RESET__GPIO_3_30 | MXS_PAD_CTRL,
 	/* backlight control */
 	MX28_PAD_PWM2__GPIO_3_18 | MXS_PAD_CTRL,
+	/* mmc0 */
+	MX28_PAD_SSP0_DATA0__SSP0_D0 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_DATA1__SSP0_D1 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_DATA2__SSP0_D2 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_DATA3__SSP0_D3 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_DATA4__SSP0_D4 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_DATA5__SSP0_D5 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_DATA6__SSP0_D6 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_DATA7__SSP0_D7 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_CMD__SSP0_CMD |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_SSP0_DETECT__SSP0_CARD_DETECT |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	MX28_PAD_SSP0_SCK__SSP0_SCK |
+		(MXS_PAD_12MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	/* write protect */
+	MX28_PAD_SSP1_SCK__GPIO_2_12 |
+		(MXS_PAD_4MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	/* slot power enable */
+	MX28_PAD_PWM3__GPIO_3_28 |
+		(MXS_PAD_4MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+
+	/* mmc1 */
+	MX28_PAD_GPMI_D00__SSP1_D0 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_D01__SSP1_D1 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_D02__SSP1_D2 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_D03__SSP1_D3 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_D04__SSP1_D4 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_D05__SSP1_D5 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_D06__SSP1_D6 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_D07__SSP1_D7 |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_RDY1__SSP1_CMD |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_PULLUP),
+	MX28_PAD_GPMI_RDY0__SSP1_CARD_DETECT |
+		(MXS_PAD_8MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	MX28_PAD_GPMI_WRN__SSP1_SCK |
+		(MXS_PAD_12MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	/* write protect */
+	MX28_PAD_GPMI_RESETN__GPIO_0_28 |
+		(MXS_PAD_4MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
+	/* slot power enable */
+	MX28_PAD_PWM4__GPIO_3_29 |
+		(MXS_PAD_4MA | MXS_PAD_3V3 | MXS_PAD_NOPULL),
 };
 
 /* fec */
@@ -258,6 +322,18 @@ static const struct mxsfb_platform_data mx28evk_mxsfb_pdata __initconst = {
 	.ld_intf_width	= STMLCDIF_24BIT,
 };
 
+static struct mxs_mmc_platform_data mx28evk_mmc_pdata[] __initdata = {
+	{
+		/* mmc0 */
+		.wp_gpio = MX28EVK_MMC0_WRITE_PROTECT,
+		.flags = SLOTF_8_BIT_CAPABLE,
+	}, {
+		/* mmc1 */
+		.wp_gpio = MX28EVK_MMC1_WRITE_PROTECT,
+		.flags = SLOTF_8_BIT_CAPABLE,
+	},
+};
+
 static void __init mx28evk_init(void)
 {
 	int ret;
@@ -297,6 +373,19 @@ static void __init mx28evk_init(void)
 		gpio_set_value(MX28EVK_BL_ENABLE, 1);
 
 	mx28_add_mxsfb(&mx28evk_mxsfb_pdata);
+
+	/* power on mmc slot by writing 0 to the gpio */
+	ret = gpio_request_one(MX28EVK_MMC0_SLOT_POWER, GPIOF_DIR_OUT,
+			       "mmc0-slot-power");
+	if (ret)
+		pr_warn("failed to request gpio mmc0-slot-power: %d\n", ret);
+	mx28_add_mxs_mmc(0, &mx28evk_mmc_pdata[0]);
+
+	ret = gpio_request_one(MX28EVK_MMC1_SLOT_POWER, GPIOF_DIR_OUT,
+			       "mmc1-slot-power");
+	if (ret)
+		pr_warn("failed to request gpio mmc1-slot-power: %d\n", ret);
+	mx28_add_mxs_mmc(1, &mx28evk_mmc_pdata[1]);
 }
 
 static void __init mx28evk_timer_init(void)
diff --git a/arch/arm/mach-mxs/module-tx28.c b/arch/arm/mach-mxs/module-tx28.c
index fa0b154da67..0fcff47009c 100644
--- a/arch/arm/mach-mxs/module-tx28.c
+++ b/arch/arm/mach-mxs/module-tx28.c
@@ -45,7 +45,7 @@ static const iomux_cfg_t tx28_fec_gpio_pads[] __initconst = {
 };
 
 #define FEC_MODE (MXS_PAD_8MA | MXS_PAD_PULLUP | MXS_PAD_3V3)
-static const iomux_cfg_t tx28_fec_pads[] __initconst = {
+static const iomux_cfg_t tx28_fec0_pads[] __initconst = {
 	MX28_PAD_ENET0_MDC__ENET0_MDC | FEC_MODE,
 	MX28_PAD_ENET0_MDIO__ENET0_MDIO | FEC_MODE,
 	MX28_PAD_ENET0_RX_EN__ENET0_RX_EN | FEC_MODE,
@@ -57,7 +57,20 @@ static const iomux_cfg_t tx28_fec_pads[] __initconst = {
 	MX28_PAD_ENET_CLK__CLKCTRL_ENET | FEC_MODE,
 };
 
-static const struct fec_platform_data tx28_fec_data __initconst = {
+static const iomux_cfg_t tx28_fec1_pads[] __initconst = {
+	MX28_PAD_ENET0_RXD2__ENET1_RXD0,
+	MX28_PAD_ENET0_RXD3__ENET1_RXD1,
+	MX28_PAD_ENET0_TXD2__ENET1_TXD0,
+	MX28_PAD_ENET0_TXD3__ENET1_TXD1,
+	MX28_PAD_ENET0_COL__ENET1_TX_EN,
+	MX28_PAD_ENET0_CRS__ENET1_RX_EN,
+};
+
+static struct fec_platform_data tx28_fec0_data = {
+	.phy = PHY_INTERFACE_MODE_RMII,
+};
+
+static struct fec_platform_data tx28_fec1_data = {
 	.phy = PHY_INTERFACE_MODE_RMII,
 };
 
@@ -108,15 +121,15 @@ int __init tx28_add_fec0(void)
 	pr_debug("%s: Deasserting FEC PHY RESET\n", __func__);
 	gpio_set_value(TX28_FEC_PHY_RESET, 1);
 
-	ret = mxs_iomux_setup_multiple_pads(tx28_fec_pads,
-			ARRAY_SIZE(tx28_fec_pads));
+	ret = mxs_iomux_setup_multiple_pads(tx28_fec0_pads,
+			ARRAY_SIZE(tx28_fec0_pads));
 	if (ret) {
 		pr_debug("%s: mxs_iomux_setup_multiple_pads() failed with rc: %d\n",
 				__func__, ret);
 		goto free_gpios;
 	}
-	pr_debug("%s: Registering FEC device\n", __func__);
-	mx28_add_fec(0, &tx28_fec_data);
+	pr_debug("%s: Registering FEC0 device\n", __func__);
+	mx28_add_fec(0, &tx28_fec0_data);
 	return 0;
 
 free_gpios:
@@ -129,3 +142,19 @@ free_gpios:
 
 	return ret;
 }
+
+int __init tx28_add_fec1(void)
+{
+	int ret;
+
+	ret = mxs_iomux_setup_multiple_pads(tx28_fec1_pads,
+			ARRAY_SIZE(tx28_fec1_pads));
+	if (ret) {
+		pr_debug("%s: mxs_iomux_setup_multiple_pads() failed with rc: %d\n",
+				__func__, ret);
+		return ret;
+	}
+	pr_debug("%s: Registering FEC1 device\n", __func__);
+	mx28_add_fec(1, &tx28_fec1_data);
+	return 0;
+}
diff --git a/arch/arm/mach-mxs/module-tx28.h b/arch/arm/mach-mxs/module-tx28.h
index df9e1b6e81b..8ed425457d3 100644
--- a/arch/arm/mach-mxs/module-tx28.h
+++ b/arch/arm/mach-mxs/module-tx28.h
@@ -7,3 +7,4 @@
  * Free Software Foundation.
  */
 int __init tx28_add_fec0(void);
+int __init tx28_add_fec1(void);
diff --git a/arch/arm/mach-pxa/am200epd.c b/arch/arm/mach-pxa/am200epd.c
index 3499fada73a..4cb069fd9af 100644
--- a/arch/arm/mach-pxa/am200epd.c
+++ b/arch/arm/mach-pxa/am200epd.c
@@ -128,8 +128,8 @@ static int am200_init_gpio_regs(struct metronomefb_par *par)
 	return 0;
 
 err_req_gpio:
-	while (i > 0)
-		gpio_free(gpios[i--]);
+	while (--i >= 0)
+		gpio_free(gpios[i]);
 
 	return err;
 }
@@ -194,7 +194,7 @@ static struct notifier_block am200_fb_notif = {
 };
 
 /* this gets called as part of our init. these steps must be done now so
- * that we can use set_pxa_fb_info */
+ * that we can use pxa_set_fb_info */
 static void __init am200_presetup_fb(void)
 {
 	int fw;
@@ -249,7 +249,7 @@ static void __init am200_presetup_fb(void)
 	/* we divide since we told the LCD controller we're 16bpp */
 	am200_fb_info.modes->xres /= 2;
 
-	set_pxa_fb_info(&am200_fb_info);
+	pxa_set_fb_info(NULL, &am200_fb_info);
 
 }
 
diff --git a/arch/arm/mach-pxa/am300epd.c b/arch/arm/mach-pxa/am300epd.c
index 993d75e6639..fa8bad235d9 100644
--- a/arch/arm/mach-pxa/am300epd.c
+++ b/arch/arm/mach-pxa/am300epd.c
@@ -125,10 +125,7 @@ static int am300_init_gpio_regs(struct broadsheetfb_par *par)
 		if (err) {
 			dev_err(&am300_device->dev, "failed requesting "
 				"gpio %d, err=%d\n", i, err);
-			while (i >= DB0_GPIO_PIN)
-				gpio_free(i--);
-			i = ARRAY_SIZE(gpios) - 1;
-			goto err_req_gpio;
+			goto err_req_gpio2;
 		}
 	}
 
@@ -159,9 +156,13 @@ static int am300_init_gpio_regs(struct broadsheetfb_par *par)
 
 	return 0;
 
+err_req_gpio2:
+	while (--i >= DB0_GPIO_PIN)
+		gpio_free(i);
+	i = ARRAY_SIZE(gpios);
 err_req_gpio:
-	while (i > 0)
-		gpio_free(gpios[i--]);
+	while (--i >= 0)
+		gpio_free(gpios[i]);
 
 	return err;
 }
diff --git a/arch/arm/mach-pxa/balloon3.c b/arch/arm/mach-pxa/balloon3.c
index d2af73321da..c6661b1d30b 100644
--- a/arch/arm/mach-pxa/balloon3.c
+++ b/arch/arm/mach-pxa/balloon3.c
@@ -263,7 +263,7 @@ static void __init balloon3_lcd_init(void)
 	}
 
 	balloon3_lcd_screen.pxafb_backlight_power = balloon3_backlight_power;
-	set_pxa_fb_info(&balloon3_lcd_screen);
+	pxa_set_fb_info(NULL, &balloon3_lcd_screen);
 	return;
 
 err2:
diff --git a/arch/arm/mach-pxa/cm-x2xx.c b/arch/arm/mach-pxa/cm-x2xx.c
index b734d846816..8225e2e58c6 100644
--- a/arch/arm/mach-pxa/cm-x2xx.c
+++ b/arch/arm/mach-pxa/cm-x2xx.c
@@ -379,7 +379,7 @@ __setup("monitor=", cmx2xx_set_display);
 
 static void __init cmx2xx_init_display(void)
 {
-	set_pxa_fb_info(cmx2xx_display);
+	pxa_set_fb_info(NULL, cmx2xx_display);
 }
 #else
 static inline void cmx2xx_init_display(void) {}
diff --git a/arch/arm/mach-pxa/cm-x300.c b/arch/arm/mach-pxa/cm-x300.c
index bfca7ed2fea..7a8edba0dcd 100644
--- a/arch/arm/mach-pxa/cm-x300.c
+++ b/arch/arm/mach-pxa/cm-x300.c
@@ -296,7 +296,7 @@ static struct pxafb_mach_info cm_x300_lcd = {
 
 static void __init cm_x300_init_lcd(void)
 {
-	set_pxa_fb_info(&cm_x300_lcd);
+	pxa_set_fb_info(NULL, &cm_x300_lcd);
 }
 #else
 static inline void cm_x300_init_lcd(void) {}
diff --git a/arch/arm/mach-pxa/colibri-pxa270-income.c b/arch/arm/mach-pxa/colibri-pxa270-income.c
index ee797397dc5..44c1b77ece6 100644
--- a/arch/arm/mach-pxa/colibri-pxa270-income.c
+++ b/arch/arm/mach-pxa/colibri-pxa270-income.c
@@ -175,7 +175,7 @@ static struct pxafb_mach_info income_lcd_screen = {
 
 static void __init income_lcd_init(void)
 {
-	set_pxa_fb_info(&income_lcd_screen);
+	pxa_set_fb_info(NULL, &income_lcd_screen);
 }
 #else
 static inline void income_lcd_init(void) {}
diff --git a/arch/arm/mach-pxa/colibri-pxa3xx.c b/arch/arm/mach-pxa/colibri-pxa3xx.c
index 96b2d9fbfef..3f9be419959 100644
--- a/arch/arm/mach-pxa/colibri-pxa3xx.c
+++ b/arch/arm/mach-pxa/colibri-pxa3xx.c
@@ -105,7 +105,7 @@ void __init colibri_pxa3xx_init_lcd(int bl_pin)
 	lcd_bl_pin = bl_pin;
 	gpio_request(bl_pin, "lcd backlight");
 	gpio_direction_output(bl_pin, 0);
-	set_pxa_fb_info(&sharp_lq43_info);
+	pxa_set_fb_info(NULL, &sharp_lq43_info);
 }
 #endif
 
diff --git a/arch/arm/mach-pxa/corgi.c b/arch/arm/mach-pxa/corgi.c
index d4e705caefe..3a5507e3191 100644
--- a/arch/arm/mach-pxa/corgi.c
+++ b/arch/arm/mach-pxa/corgi.c
@@ -462,7 +462,6 @@ static struct pxaficp_platform_data corgi_ficp_platform_data = {
  * USB Device Controller
  */
 static struct pxa2xx_udc_mach_info udc_info __initdata = {
-	.gpio_vbus		= -1,
 	/* no connect GPIO; corgi can't tell connection status */
 	.gpio_pullup		= CORGI_GPIO_USB_PULLUP,
 };
diff --git a/arch/arm/mach-pxa/devices.c b/arch/arm/mach-pxa/devices.c
index c4bf08b3eb6..2e0425404de 100644
--- a/arch/arm/mach-pxa/devices.c
+++ b/arch/arm/mach-pxa/devices.c
@@ -90,7 +90,6 @@ void __init pxa_set_mci_info(struct pxamci_platform_data *info)
 
 static struct pxa2xx_udc_mach_info pxa_udc_info = {
 	.gpio_pullup = -1,
-	.gpio_vbus   = -1,
 };
 
 void __init pxa_set_udc_info(struct pxa2xx_udc_mach_info *info)
@@ -188,16 +187,12 @@ struct platform_device pxa_device_fb = {
 	.resource	= pxafb_resources,
 };
 
-void __init set_pxa_fb_info(struct pxafb_mach_info *info)
+void __init pxa_set_fb_info(struct device *parent, struct pxafb_mach_info *info)
 {
+	pxa_device_fb.dev.parent = parent;
 	pxa_register_device(&pxa_device_fb, info);
 }
 
-void __init set_pxa_fb_parent(struct device *parent_dev)
-{
-	pxa_device_fb.dev.parent = parent_dev;
-}
-
 static struct resource pxa_resource_ffuart[] = {
 	{
 		.start	= 0x40100000,
diff --git a/arch/arm/mach-pxa/em-x270.c b/arch/arm/mach-pxa/em-x270.c
index b411d7cbf5a..f8a6e9d79a3 100644
--- a/arch/arm/mach-pxa/em-x270.c
+++ b/arch/arm/mach-pxa/em-x270.c
@@ -689,7 +689,7 @@ static struct pxafb_mach_info em_x270_lcd = {
 
 static void __init em_x270_init_lcd(void)
 {
-	set_pxa_fb_info(&em_x270_lcd);
+	pxa_set_fb_info(NULL, &em_x270_lcd);
 }
 #else
 static inline void em_x270_init_lcd(void) {}
diff --git a/arch/arm/mach-pxa/eseries.c b/arch/arm/mach-pxa/eseries.c
index edca0a04329..2e3970fdde0 100644
--- a/arch/arm/mach-pxa/eseries.c
+++ b/arch/arm/mach-pxa/eseries.c
@@ -20,6 +20,7 @@
 #include <linux/mfd/t7l66xb.h>
 #include <linux/mtd/nand.h>
 #include <linux/mtd/partitions.h>
+#include <linux/usb/gpio_vbus.h>
 
 #include <video/w100fb.h>
 
@@ -51,12 +52,20 @@ void __init eseries_fixup(struct machine_desc *desc,
 		mi->bank[0].size = (64*1024*1024);
 }
 
-struct pxa2xx_udc_mach_info e7xx_udc_mach_info = {
+struct gpio_vbus_mach_info e7xx_udc_info = {
 	.gpio_vbus   = GPIO_E7XX_USB_DISC,
 	.gpio_pullup = GPIO_E7XX_USB_PULLUP,
 	.gpio_pullup_inverted = 1
 };
 
+static struct platform_device e7xx_gpio_vbus = {
+	.name	= "gpio-vbus",
+	.id	= -1,
+	.dev	= {
+		.platform_data	= &e7xx_udc_info,
+	},
+};
+
 struct pxaficp_platform_data e7xx_ficp_platform_data = {
 	.gpio_pwdown		= GPIO_E7XX_IR_OFF,
 	.transceiver_cap	= IR_SIRMODE | IR_OFF,
@@ -165,6 +174,7 @@ static struct platform_device e330_tc6387xb_device = {
 
 static struct platform_device *e330_devices[] __initdata = {
 	&e330_tc6387xb_device,
+	&e7xx_gpio_vbus,
 };
 
 static void __init e330_init(void)
@@ -175,7 +185,6 @@ static void __init e330_init(void)
 	eseries_register_clks();
 	eseries_get_tmio_gpios();
 	platform_add_devices(ARRAY_AND_SIZE(e330_devices));
-	pxa_set_udc_info(&e7xx_udc_mach_info);
 }
 
 MACHINE_START(E330, "Toshiba e330")
@@ -214,6 +223,7 @@ static struct platform_device e350_t7l66xb_device = {
 
 static struct platform_device *e350_devices[] __initdata = {
 	&e350_t7l66xb_device,
+	&e7xx_gpio_vbus,
 };
 
 static void __init e350_init(void)
@@ -224,7 +234,6 @@ static void __init e350_init(void)
 	eseries_register_clks();
 	eseries_get_tmio_gpios();
 	platform_add_devices(ARRAY_AND_SIZE(e350_devices));
-	pxa_set_udc_info(&e7xx_udc_mach_info);
 }
 
 MACHINE_START(E350, "Toshiba e350")
@@ -333,6 +342,7 @@ static struct platform_device e400_t7l66xb_device = {
 
 static struct platform_device *e400_devices[] __initdata = {
 	&e400_t7l66xb_device,
+	&e7xx_gpio_vbus,
 };
 
 static void __init e400_init(void)
@@ -344,9 +354,8 @@ static void __init e400_init(void)
 	/* Fixme - e400 may have a switched clock */
 	eseries_register_clks();
 	eseries_get_tmio_gpios();
-	set_pxa_fb_info(&e400_pxafb_mach_info);
+	pxa_set_fb_info(NULL, &e400_pxafb_mach_info);
 	platform_add_devices(ARRAY_AND_SIZE(e400_devices));
-	pxa_set_udc_info(&e7xx_udc_mach_info);
 }
 
 MACHINE_START(E400, "Toshiba e400")
@@ -519,6 +528,7 @@ static struct platform_device e740_t7l66xb_device = {
 static struct platform_device *e740_devices[] __initdata = {
 	&e740_fb_device,
 	&e740_t7l66xb_device,
+	&e7xx_gpio_vbus,
 };
 
 static void __init e740_init(void)
@@ -532,7 +542,6 @@ static void __init e740_init(void)
 			"UDCCLK", &pxa25x_device_udc.dev),
 	eseries_get_tmio_gpios();
 	platform_add_devices(ARRAY_AND_SIZE(e740_devices));
-	pxa_set_udc_info(&e7xx_udc_mach_info);
 	pxa_set_ac97_info(NULL);
 	pxa_set_ficp_info(&e7xx_ficp_platform_data);
 }
@@ -711,6 +720,7 @@ static struct platform_device e750_tc6393xb_device = {
 static struct platform_device *e750_devices[] __initdata = {
 	&e750_fb_device,
 	&e750_tc6393xb_device,
+	&e7xx_gpio_vbus,
 };
 
 static void __init e750_init(void)
@@ -723,7 +733,6 @@ static void __init e750_init(void)
 			"GPIO11_CLK", NULL),
 	eseries_get_tmio_gpios();
 	platform_add_devices(ARRAY_AND_SIZE(e750_devices));
-	pxa_set_udc_info(&e7xx_udc_mach_info);
 	pxa_set_ac97_info(NULL);
 	pxa_set_ficp_info(&e7xx_ficp_platform_data);
 }
@@ -873,12 +882,21 @@ static struct platform_device e800_fb_device = {
 
 /* --------------------------- UDC definitions --------------------------- */
 
-static struct pxa2xx_udc_mach_info e800_udc_mach_info = {
+static struct gpio_vbus_mach_info e800_udc_info = {
 	.gpio_vbus   = GPIO_E800_USB_DISC,
 	.gpio_pullup = GPIO_E800_USB_PULLUP,
 	.gpio_pullup_inverted = 1
 };
 
+static struct platform_device e800_gpio_vbus = {
+	.name	= "gpio-vbus",
+	.id	= -1,
+	.dev	= {
+		.platform_data	= &e800_udc_info,
+	},
+};
+
+
 /* ----------------- e800 tc6393xb parameters ------------------ */
 
 static struct tc6393xb_platform_data e800_tc6393xb_info = {
@@ -907,6 +925,7 @@ static struct platform_device e800_tc6393xb_device = {
 static struct platform_device *e800_devices[] __initdata = {
 	&e800_fb_device,
 	&e800_tc6393xb_device,
+	&e800_gpio_vbus,
 };
 
 static void __init e800_init(void)
@@ -919,7 +938,6 @@ static void __init e800_init(void)
 			"GPIO11_CLK", NULL),
 	eseries_get_tmio_gpios();
 	platform_add_devices(ARRAY_AND_SIZE(e800_devices));
-	pxa_set_udc_info(&e800_udc_mach_info);
 	pxa_set_ac97_info(NULL);
 }
 
diff --git a/arch/arm/mach-pxa/ezx.c b/arch/arm/mach-pxa/ezx.c
index 93f05e02431..d88aed8fbe1 100644
--- a/arch/arm/mach-pxa/ezx.c
+++ b/arch/arm/mach-pxa/ezx.c
@@ -783,7 +783,7 @@ static void __init a780_init(void)
 
 	pxa_set_i2c_info(NULL);
 
-	set_pxa_fb_info(&ezx_fb_info_1);
+	pxa_set_fb_info(NULL, &ezx_fb_info_1);
 
 	pxa_set_keypad_info(&a780_keypad_platform_data);
 
@@ -853,7 +853,7 @@ static void __init e680_init(void)
 	pxa_set_i2c_info(NULL);
 	i2c_register_board_info(0, ARRAY_AND_SIZE(e680_i2c_board_info));
 
-	set_pxa_fb_info(&ezx_fb_info_1);
+	pxa_set_fb_info(NULL, &ezx_fb_info_1);
 
 	pxa_set_keypad_info(&e680_keypad_platform_data);
 
@@ -918,7 +918,7 @@ static void __init a1200_init(void)
 	pxa_set_i2c_info(NULL);
 	i2c_register_board_info(0, ARRAY_AND_SIZE(a1200_i2c_board_info));
 
-	set_pxa_fb_info(&ezx_fb_info_2);
+	pxa_set_fb_info(NULL, &ezx_fb_info_2);
 
 	pxa_set_keypad_info(&a1200_keypad_platform_data);
 
@@ -1103,7 +1103,7 @@ static void __init a910_init(void)
 	pxa_set_i2c_info(NULL);
 	i2c_register_board_info(0, ARRAY_AND_SIZE(a910_i2c_board_info));
 
-	set_pxa_fb_info(&ezx_fb_info_2);
+	pxa_set_fb_info(NULL, &ezx_fb_info_2);
 
 	pxa_set_keypad_info(&a910_keypad_platform_data);
 
@@ -1173,7 +1173,7 @@ static void __init e6_init(void)
 	pxa_set_i2c_info(NULL);
 	i2c_register_board_info(0, ARRAY_AND_SIZE(e6_i2c_board_info));
 
-	set_pxa_fb_info(&ezx_fb_info_2);
+	pxa_set_fb_info(NULL, &ezx_fb_info_2);
 
 	pxa_set_keypad_info(&e6_keypad_platform_data);
 
@@ -1212,7 +1212,7 @@ static void __init e2_init(void)
 	pxa_set_i2c_info(NULL);
 	i2c_register_board_info(0, ARRAY_AND_SIZE(e2_i2c_board_info));
 
-	set_pxa_fb_info(&ezx_fb_info_2);
+	pxa_set_fb_info(NULL, &ezx_fb_info_2);
 
 	pxa_set_keypad_info(&e2_keypad_platform_data);
 
diff --git a/arch/arm/mach-pxa/gumstix.c b/arch/arm/mach-pxa/gumstix.c
index 6fd319ea528..d65e4bde9b9 100644
--- a/arch/arm/mach-pxa/gumstix.c
+++ b/arch/arm/mach-pxa/gumstix.c
@@ -26,6 +26,7 @@
 #include <linux/gpio.h>
 #include <linux/err.h>
 #include <linux/clk.h>
+#include <linux/usb/gpio_vbus.h>
 
 #include <asm/setup.h>
 #include <asm/memory.h>
@@ -106,14 +107,22 @@ static void __init gumstix_mmc_init(void)
 #endif
 
 #ifdef CONFIG_USB_GADGET_PXA25X
-static struct pxa2xx_udc_mach_info gumstix_udc_info __initdata = {
+static struct gpio_vbus_mach_info gumstix_udc_info = {
 	.gpio_vbus		= GPIO_GUMSTIX_USB_GPIOn,
 	.gpio_pullup		= GPIO_GUMSTIX_USB_GPIOx,
 };
 
+static struct platform_device gumstix_gpio_vbus = {
+	.name	= "gpio-vbus",
+	.id	= -1,
+	.dev	= {
+		.platform_data	= &gumstix_udc_info,
+	},
+};
+
 static void __init gumstix_udc_init(void)
 {
-	pxa_set_udc_info(&gumstix_udc_info);
+	platform_device_register(&gumstix_gpio_vbus);
 }
 #else
 static void gumstix_udc_init(void)
diff --git a/arch/arm/mach-pxa/idp.c b/arch/arm/mach-pxa/idp.c
index dd40e4a9291..f7fb64f11a7 100644
--- a/arch/arm/mach-pxa/idp.c
+++ b/arch/arm/mach-pxa/idp.c
@@ -167,7 +167,7 @@ static void __init idp_init(void)
 
 	platform_device_register(&smc91x_device);
 	//platform_device_register(&mst_audio_device);
-	set_pxa_fb_info(&sharp_lm8v31);
+	pxa_set_fb_info(NULL, &sharp_lm8v31);
 	pxa_set_mci_info(&idp_mci_platform_data);
 }
 
diff --git a/arch/arm/mach-pxa/include/mach/palmz72.h b/arch/arm/mach-pxa/include/mach/palmz72.h
index 2bbcf70dd93..0d4700a7961 100644
--- a/arch/arm/mach-pxa/include/mach/palmz72.h
+++ b/arch/arm/mach-pxa/include/mach/palmz72.h
@@ -44,6 +44,11 @@
 #define GPIO_NR_PALMZ72_BT_POWER		17
 #define GPIO_NR_PALMZ72_BT_RESET		83
 
+/* Camera */
+#define GPIO_NR_PALMZ72_CAM_PWDN		56
+#define GPIO_NR_PALMZ72_CAM_RESET		57
+#define GPIO_NR_PALMZ72_CAM_POWER		91
+
 /** Initial values **/
 
 /* Battery */
diff --git a/arch/arm/mach-pxa/include/mach/pxafb.h b/arch/arm/mach-pxa/include/mach/pxafb.h
index 160ec83f51a..01a45ac4811 100644
--- a/arch/arm/mach-pxa/include/mach/pxafb.h
+++ b/arch/arm/mach-pxa/include/mach/pxafb.h
@@ -154,8 +154,8 @@ struct pxafb_mach_info {
 	void (*pxafb_lcd_power)(int, struct fb_var_screeninfo *);
 	void (*smart_update)(struct fb_info *);
 };
-void set_pxa_fb_info(struct pxafb_mach_info *hard_pxa_fb_info);
-void set_pxa_fb_parent(struct device *parent_dev);
+
+void pxa_set_fb_info(struct device *, struct pxafb_mach_info *);
 unsigned long pxafb_get_hsync_time(struct device *dev);
 
 extern int pxafb_smart_queue(struct fb_info *info, uint16_t *cmds, int);
diff --git a/arch/arm/mach-pxa/include/mach/z2.h b/arch/arm/mach-pxa/include/mach/z2.h
index 8835c16bc82..7b0f71ef316 100644
--- a/arch/arm/mach-pxa/include/mach/z2.h
+++ b/arch/arm/mach-pxa/include/mach/z2.h
@@ -25,8 +25,7 @@
 #define	GPIO98_ZIPITZ2_LID_BUTTON	98
 
 /* Libertas GSPI8686 WiFi */
-#define	GPIO14_ZIPITZ2_WIFI_RESET	14
-#define	GPIO15_ZIPITZ2_WIFI_POWER	15
+#define	GPIO14_ZIPITZ2_WIFI_POWER	14
 #define	GPIO24_ZIPITZ2_WIFI_CS		24
 #define	GPIO36_ZIPITZ2_WIFI_IRQ		36
 
diff --git a/arch/arm/mach-pxa/littleton.c b/arch/arm/mach-pxa/littleton.c
index 87c1ed9ccd2..e5e326d2cdc 100644
--- a/arch/arm/mach-pxa/littleton.c
+++ b/arch/arm/mach-pxa/littleton.c
@@ -185,7 +185,7 @@ static struct pxafb_mach_info littleton_lcd_info = {
 
 static void littleton_init_lcd(void)
 {
-	set_pxa_fb_info(&littleton_lcd_info);
+	pxa_set_fb_info(NULL, &littleton_lcd_info);
 }
 #else
 static inline void littleton_init_lcd(void) {};
diff --git a/arch/arm/mach-pxa/lpd270.c b/arch/arm/mach-pxa/lpd270.c
index c9a3e775c2d..8aebc58c9f1 100644
--- a/arch/arm/mach-pxa/lpd270.c
+++ b/arch/arm/mach-pxa/lpd270.c
@@ -480,7 +480,7 @@ static void __init lpd270_init(void)
 	pxa_set_ac97_info(NULL);
 
 	if (lpd270_lcd_to_use != NULL)
-		set_pxa_fb_info(lpd270_lcd_to_use);
+		pxa_set_fb_info(NULL, lpd270_lcd_to_use);
 
 	pxa_set_ohci_info(&lpd270_ohci_platform_data);
 }
diff --git a/arch/arm/mach-pxa/lubbock.c b/arch/arm/mach-pxa/lubbock.c
index dca20de306b..12a2a56b215 100644
--- a/arch/arm/mach-pxa/lubbock.c
+++ b/arch/arm/mach-pxa/lubbock.c
@@ -521,7 +521,7 @@ static void __init lubbock_init(void)
 
 	clk_add_alias("SA1111_CLK", NULL, "GPIO11_CLK", NULL);
 	pxa_set_udc_info(&udc_info);
-	set_pxa_fb_info(&sharp_lm8v31);
+	pxa_set_fb_info(NULL, &sharp_lm8v31);
 	pxa_set_mci_info(&lubbock_mci_platform_data);
 	pxa_set_ficp_info(&lubbock_ficp_platform_data);
 	pxa_set_ac97_info(NULL);
diff --git a/arch/arm/mach-pxa/magician.c b/arch/arm/mach-pxa/magician.c
index 5535991c4a3..a72993dde2b 100644
--- a/arch/arm/mach-pxa/magician.c
+++ b/arch/arm/mach-pxa/magician.c
@@ -757,7 +757,7 @@ static void __init magician_init(void)
 		gpio_direction_output(GPIO104_MAGICIAN_LCD_POWER_1, 0);
 		gpio_direction_output(GPIO105_MAGICIAN_LCD_POWER_2, 0);
 		gpio_direction_output(GPIO106_MAGICIAN_LCD_POWER_3, 0);
-		set_pxa_fb_info(lcd_select ? &samsung_info : &toppoly_info);
+		pxa_set_fb_info(NULL, lcd_select ? &samsung_info : &toppoly_info);
 	} else
 		pr_err("LCD detection: CPLD mapping failed\n");
 }
diff --git a/arch/arm/mach-pxa/mainstone.c b/arch/arm/mach-pxa/mainstone.c
index f9542220595..8306b227e61 100644
--- a/arch/arm/mach-pxa/mainstone.c
+++ b/arch/arm/mach-pxa/mainstone.c
@@ -592,7 +592,7 @@ static void __init mainstone_init(void)
 	else
 		mainstone_pxafb_info.modes = &toshiba_ltm035a776c_mode;
 
-	set_pxa_fb_info(&mainstone_pxafb_info);
+	pxa_set_fb_info(NULL, &mainstone_pxafb_info);
 	mainstone_backlight_register();
 
 	pxa_set_mci_info(&mainstone_mci_platform_data);
diff --git a/arch/arm/mach-pxa/mioa701.c b/arch/arm/mach-pxa/mioa701.c
index 78d98a8607e..dd13bb63259 100644
--- a/arch/arm/mach-pxa/mioa701.c
+++ b/arch/arm/mach-pxa/mioa701.c
@@ -795,7 +795,7 @@ static void __init mioa701_machine_init(void)
 	pxa_set_stuart_info(NULL);
 	mio_gpio_request(ARRAY_AND_SIZE(global_gpios));
 	bootstrap_init();
-	set_pxa_fb_info(&mioa701_pxafb_info);
+	pxa_set_fb_info(NULL, &mioa701_pxafb_info);
 	pxa_set_mci_info(&mioa701_mci_info);
 	pxa_set_keypad_info(&mioa701_keypad_info);
 	pxa_set_udc_info(&mioa701_udc_info);
diff --git a/arch/arm/mach-pxa/palm27x.c b/arch/arm/mach-pxa/palm27x.c
index 72adb3ae2b4..325c245c0a0 100644
--- a/arch/arm/mach-pxa/palm27x.c
+++ b/arch/arm/mach-pxa/palm27x.c
@@ -1,8 +1,7 @@
 /*
  * Common code for Palm LD, T5, TX, Z72
  *
- * Copyright (C) 2010
- * Marek Vasut <marek.vasut@gmail.com>
+ * Copyright (C) 2010-2011 Marek Vasut <marek.vasut@gmail.com>
  *
  * 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
@@ -158,7 +157,7 @@ void __init palm27x_lcd_init(int power, struct pxafb_mode_info *mode)
 		palm27x_lcd_screen.pxafb_lcd_power = palm27x_lcd_ctl;
 	}
 
-	set_pxa_fb_info(&palm27x_lcd_screen);
+	pxa_set_fb_info(NULL, &palm27x_lcd_screen);
 }
 #endif
 
diff --git a/arch/arm/mach-pxa/palmtc.c b/arch/arm/mach-pxa/palmtc.c
index a09a2374697..fb06bd04727 100644
--- a/arch/arm/mach-pxa/palmtc.c
+++ b/arch/arm/mach-pxa/palmtc.c
@@ -507,7 +507,7 @@ static struct pxafb_mach_info palmtc_lcd_screen = {
 
 static void __init palmtc_lcd_init(void)
 {
-	set_pxa_fb_info(&palmtc_lcd_screen);
+	pxa_set_fb_info(NULL, &palmtc_lcd_screen);
 }
 #else
 static inline void palmtc_lcd_init(void) {}
diff --git a/arch/arm/mach-pxa/palmte2.c b/arch/arm/mach-pxa/palmte2.c
index 3f25014a136..726f5b98dcd 100644
--- a/arch/arm/mach-pxa/palmte2.c
+++ b/arch/arm/mach-pxa/palmte2.c
@@ -136,30 +136,14 @@ static struct platform_device palmte2_pxa_keys = {
 /******************************************************************************
  * Backlight
  ******************************************************************************/
+static struct gpio palmte_bl_gpios[] = {
+	{ GPIO_NR_PALMTE2_BL_POWER, GPIOF_INIT_LOW, "Backlight power" },
+	{ GPIO_NR_PALMTE2_LCD_POWER, GPIOF_INIT_LOW, "LCD power" },
+};
+
 static int palmte2_backlight_init(struct device *dev)
 {
-	int ret;
-
-	ret = gpio_request(GPIO_NR_PALMTE2_BL_POWER, "BL POWER");
-	if (ret)
-		goto err;
-	ret = gpio_direction_output(GPIO_NR_PALMTE2_BL_POWER, 0);
-	if (ret)
-		goto err2;
-	ret = gpio_request(GPIO_NR_PALMTE2_LCD_POWER, "LCD POWER");
-	if (ret)
-		goto err2;
-	ret = gpio_direction_output(GPIO_NR_PALMTE2_LCD_POWER, 0);
-	if (ret)
-		goto err3;
-
-	return 0;
-err3:
-	gpio_free(GPIO_NR_PALMTE2_LCD_POWER);
-err2:
-	gpio_free(GPIO_NR_PALMTE2_BL_POWER);
-err:
-	return ret;
+	return gpio_request_array(ARRAY_AND_SIZE(palmte_bl_gpios));
 }
 
 static int palmte2_backlight_notify(struct device *dev, int brightness)
@@ -171,8 +155,7 @@ static int palmte2_backlight_notify(struct device *dev, int brightness)
 
 static void palmte2_backlight_exit(struct device *dev)
 {
-	gpio_free(GPIO_NR_PALMTE2_BL_POWER);
-	gpio_free(GPIO_NR_PALMTE2_LCD_POWER);
+	gpio_free_array(ARRAY_AND_SIZE(palmte_bl_gpios));
 }
 
 static struct platform_pwm_backlight_data palmte2_backlight_data = {
@@ -363,7 +346,7 @@ static void __init palmte2_init(void)
 	pxa_set_btuart_info(NULL);
 	pxa_set_stuart_info(NULL);
 
-	set_pxa_fb_info(&palmte2_lcd_screen);
+	pxa_set_fb_info(NULL, &palmte2_lcd_screen);
 	pxa_set_mci_info(&palmte2_mci_platform_data);
 	palmte2_udc_init();
 	pxa_set_ac97_info(&palmte2_ac97_pdata);
diff --git a/arch/arm/mach-pxa/palmz72.c b/arch/arm/mach-pxa/palmz72.c
index 3010193b081..3b8a4f37dbb 100644
--- a/arch/arm/mach-pxa/palmz72.c
+++ b/arch/arm/mach-pxa/palmz72.c
@@ -30,6 +30,7 @@
 #include <linux/wm97xx.h>
 #include <linux/power_supply.h>
 #include <linux/usb/gpio_vbus.h>
+#include <linux/i2c-gpio.h>
 
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
@@ -47,6 +48,9 @@
 #include <mach/palm27x.h>
 
 #include <mach/pm.h>
+#include <mach/camera.h>
+
+#include <media/soc_camera.h>
 
 #include "generic.h"
 #include "devices.h"
@@ -103,6 +107,28 @@ static unsigned long palmz72_pin_config[] __initdata = {
 	GPIO22_GPIO,	/* LCD border color */
 	GPIO96_GPIO,	/* lcd power */
 
+	/* PXA Camera */
+	GPIO81_CIF_DD_0,
+	GPIO48_CIF_DD_5,
+	GPIO50_CIF_DD_3,
+	GPIO51_CIF_DD_2,
+	GPIO52_CIF_DD_4,
+	GPIO53_CIF_MCLK,
+	GPIO54_CIF_PCLK,
+	GPIO55_CIF_DD_1,
+	GPIO84_CIF_FV,
+	GPIO85_CIF_LV,
+	GPIO93_CIF_DD_6,
+	GPIO108_CIF_DD_7,
+
+	GPIO56_GPIO,	/* OV9640 Powerdown */
+	GPIO57_GPIO,	/* OV9640 Reset */
+	GPIO91_GPIO,	/* OV9640 Power */
+
+	/* I2C */
+	GPIO117_GPIO,	/* I2C_SCL */
+	GPIO118_GPIO,	/* I2C_SDA */
+
 	/* Misc. */
 	GPIO0_GPIO	| WAKEUP_ON_LEVEL_HIGH,	/* power detect */
 	GPIO88_GPIO,				/* green led */
@@ -254,6 +280,106 @@ device_initcall(palmz72_pm_init);
 #endif
 
 /******************************************************************************
+ * SoC Camera
+ ******************************************************************************/
+#if defined(CONFIG_SOC_CAMERA_OV9640) || \
+	defined(CONFIG_SOC_CAMERA_OV9640_MODULE)
+static struct pxacamera_platform_data palmz72_pxacamera_platform_data = {
+	.flags		= PXA_CAMERA_MASTER | PXA_CAMERA_DATAWIDTH_8 |
+			PXA_CAMERA_PCLK_EN | PXA_CAMERA_MCLK_EN,
+	.mclk_10khz	= 2600,
+};
+
+/* Board I2C devices. */
+static struct i2c_board_info palmz72_i2c_device[] = {
+	{
+		I2C_BOARD_INFO("ov9640", 0x30),
+	}
+};
+
+static int palmz72_camera_power(struct device *dev, int power)
+{
+	gpio_set_value(GPIO_NR_PALMZ72_CAM_PWDN, !power);
+	mdelay(50);
+	return 0;
+}
+
+static int palmz72_camera_reset(struct device *dev)
+{
+	gpio_set_value(GPIO_NR_PALMZ72_CAM_RESET, 1);
+	mdelay(50);
+	gpio_set_value(GPIO_NR_PALMZ72_CAM_RESET, 0);
+	mdelay(50);
+	return 0;
+}
+
+static struct soc_camera_link palmz72_iclink = {
+	.bus_id		= 0, /* Match id in pxa27x_device_camera in device.c */
+	.board_info	= &palmz72_i2c_device[0],
+	.i2c_adapter_id	= 0,
+	.module_name	= "ov96xx",
+	.power		= &palmz72_camera_power,
+	.reset		= &palmz72_camera_reset,
+	.flags		= SOCAM_DATAWIDTH_8,
+};
+
+static struct i2c_gpio_platform_data palmz72_i2c_bus_data = {
+	.sda_pin	= 118,
+	.scl_pin	= 117,
+	.udelay		= 10,
+	.timeout	= 100,
+};
+
+static struct platform_device palmz72_i2c_bus_device = {
+	.name		= "i2c-gpio",
+	.id		= 0, /* we use this as a replacement for i2c-pxa */
+	.dev		= {
+		.platform_data	= &palmz72_i2c_bus_data,
+	}
+};
+
+static struct platform_device palmz72_camera = {
+	.name	= "soc-camera-pdrv",
+	.id	= -1,
+	.dev	= {
+		.platform_data	= &palmz72_iclink,
+	},
+};
+
+/* Here we request the camera GPIOs and configure them. We power up the camera
+ * module, deassert the reset pin, but put it into powerdown (low to no power
+ * consumption) mode. This allows us to later bring the module up fast. */
+static struct gpio palmz72_camera_gpios[] = {
+	{ GPIO_NR_PALMZ72_CAM_POWER,	GPIOF_INIT_HIGH,"Camera DVDD" },
+	{ GPIO_NR_PALMZ72_CAM_RESET,	GPIOF_INIT_LOW,	"Camera RESET" },
+	{ GPIO_NR_PALMZ72_CAM_PWDN,	GPIOF_INIT_LOW,	"Camera PWDN" },
+};
+
+static inline void __init palmz72_cam_gpio_init(void)
+{
+	int ret;
+
+	ret = gpio_request_array(ARRAY_AND_SIZE(palmz72_camera_gpios));
+	if (!ret)
+		gpio_free_array(ARRAY_AND_SIZE(palmz72_camera_gpios));
+	else
+		printk(KERN_ERR "Camera GPIO init failed!\n");
+
+	return;
+}
+
+static void __init palmz72_camera_init(void)
+{
+	palmz72_cam_gpio_init();
+	pxa_set_camera_info(&palmz72_pxacamera_platform_data);
+	platform_device_register(&palmz72_i2c_bus_device);
+	platform_device_register(&palmz72_camera);
+}
+#else
+static inline void palmz72_camera_init(void) {}
+#endif
+
+/******************************************************************************
  * Machine init
  ******************************************************************************/
 static void __init palmz72_init(void)
@@ -276,6 +402,7 @@ static void __init palmz72_init(void)
 	palm27x_pmic_init();
 	palmz72_kpc_init();
 	palmz72_leds_init();
+	palmz72_camera_init();
 }
 
 MACHINE_START(PALMZ72, "Palm Zire72")
diff --git a/arch/arm/mach-pxa/pcm990-baseboard.c b/arch/arm/mach-pxa/pcm990-baseboard.c
index 9dbf3ccd415..a70fb848dcd 100644
--- a/arch/arm/mach-pxa/pcm990-baseboard.c
+++ b/arch/arm/mach-pxa/pcm990-baseboard.c
@@ -515,7 +515,7 @@ void __init pcm990_baseboard_init(void)
 	pcm990_init_irq();
 
 #ifndef CONFIG_PCM990_DISPLAY_NONE
-	set_pxa_fb_info(&pcm990_fbinfo);
+	pxa_set_fb_info(NULL, &pcm990_fbinfo);
 #endif
 	platform_device_register(&pcm990_backlight_device);
 
diff --git a/arch/arm/mach-pxa/poodle.c b/arch/arm/mach-pxa/poodle.c
index 35353af345d..16d14fd79b4 100644
--- a/arch/arm/mach-pxa/poodle.c
+++ b/arch/arm/mach-pxa/poodle.c
@@ -445,8 +445,7 @@ static void __init poodle_init(void)
 	if (ret)
 		pr_warning("poodle: Unable to register LoCoMo device\n");
 
-	set_pxa_fb_parent(&poodle_locomo_device.dev);
-	set_pxa_fb_info(&poodle_fb_info);
+	pxa_set_fb_info(&poodle_locomo_device.dev, &poodle_fb_info);
 	pxa_set_udc_info(&udc_info);
 	pxa_set_mci_info(&poodle_mci_platform_data);
 	pxa_set_ficp_info(&poodle_ficp_platform_data);
diff --git a/arch/arm/mach-pxa/raumfeld.c b/arch/arm/mach-pxa/raumfeld.c
index 47094188e02..cd1861351f7 100644
--- a/arch/arm/mach-pxa/raumfeld.c
+++ b/arch/arm/mach-pxa/raumfeld.c
@@ -597,7 +597,7 @@ static void __init raumfeld_lcd_init(void)
 {
 	int ret;
 
-	set_pxa_fb_info(&raumfeld_sharp_lcd_info);
+	pxa_set_fb_info(NULL, &raumfeld_sharp_lcd_info);
 
 	/* Earlier devices had the backlight regulator controlled
 	 * via PWM, later versions use another controller for that */
diff --git a/arch/arm/mach-pxa/saar.c b/arch/arm/mach-pxa/saar.c
index eb83c89428e..fee97a93512 100644
--- a/arch/arm/mach-pxa/saar.c
+++ b/arch/arm/mach-pxa/saar.c
@@ -473,7 +473,7 @@ static struct pxafb_mach_info saar_lcd_info = {
 
 static void __init saar_init_lcd(void)
 {
-	set_pxa_fb_info(&saar_lcd_info);
+	pxa_set_fb_info(NULL, &saar_lcd_info);
 }
 #else
 static inline void saar_init_lcd(void) {}
diff --git a/arch/arm/mach-pxa/spitz.c b/arch/arm/mach-pxa/spitz.c
index 38e2c0912b9..01c576963e9 100644
--- a/arch/arm/mach-pxa/spitz.c
+++ b/arch/arm/mach-pxa/spitz.c
@@ -724,7 +724,7 @@ static struct pxafb_mach_info spitz_pxafb_info = {
 
 static void __init spitz_lcd_init(void)
 {
-	set_pxa_fb_info(&spitz_pxafb_info);
+	pxa_set_fb_info(NULL, &spitz_pxafb_info);
 }
 #else
 static inline void spitz_lcd_init(void) {}
diff --git a/arch/arm/mach-pxa/tavorevb.c b/arch/arm/mach-pxa/tavorevb.c
index 9cecf8366db..53d4a472b69 100644
--- a/arch/arm/mach-pxa/tavorevb.c
+++ b/arch/arm/mach-pxa/tavorevb.c
@@ -466,7 +466,7 @@ static void __init tavorevb_init_lcd(void)
 {
 	platform_device_register(&tavorevb_backlight_devices[0]);
 	platform_device_register(&tavorevb_backlight_devices[1]);
-	set_pxa_fb_info(&tavorevb_lcd_info);
+	pxa_set_fb_info(NULL, &tavorevb_lcd_info);
 }
 #else
 static inline void tavorevb_init_lcd(void) {}
diff --git a/arch/arm/mach-pxa/time.c b/arch/arm/mach-pxa/time.c
index e7f64d9b4f2..428da3ff33a 100644
--- a/arch/arm/mach-pxa/time.c
+++ b/arch/arm/mach-pxa/time.c
@@ -100,7 +100,6 @@ pxa_osmr0_set_mode(enum clock_event_mode mode, struct clock_event_device *dev)
 static struct clock_event_device ckevt_pxa_osmr0 = {
 	.name		= "osmr0",
 	.features	= CLOCK_EVT_FEAT_ONESHOT,
-	.shift		= 32,
 	.rating		= 200,
 	.set_next_event	= pxa_osmr0_set_next_event,
 	.set_mode	= pxa_osmr0_set_mode,
@@ -135,8 +134,8 @@ static void __init pxa_timer_init(void)
 
 	init_sched_clock(&cd, pxa_update_sched_clock, 32, clock_tick_rate);
 
-	ckevt_pxa_osmr0.mult =
-		div_sc(clock_tick_rate, NSEC_PER_SEC, ckevt_pxa_osmr0.shift);
+	clocksource_calc_mult_shift(&cksrc_pxa_oscr0, clock_tick_rate, 4);
+	clockevents_calc_mult_shift(&ckevt_pxa_osmr0, clock_tick_rate, 4);
 	ckevt_pxa_osmr0.max_delta_ns =
 		clockevent_delta2ns(0x7fffffff, &ckevt_pxa_osmr0);
 	ckevt_pxa_osmr0.min_delta_ns =
diff --git a/arch/arm/mach-pxa/tosa.c b/arch/arm/mach-pxa/tosa.c
index 5ad3807af33..5fa145778e7 100644
--- a/arch/arm/mach-pxa/tosa.c
+++ b/arch/arm/mach-pxa/tosa.c
@@ -35,6 +35,7 @@
 #include <linux/spi/pxa2xx_spi.h>
 #include <linux/input/matrix_keypad.h>
 #include <linux/i2c/pxa-i2c.h>
+#include <linux/usb/gpio_vbus.h>
 
 #include <asm/setup.h>
 #include <asm/mach-types.h>
@@ -240,12 +241,20 @@ static struct scoop_pcmcia_config tosa_pcmcia_config = {
 /*
  * USB Device Controller
  */
-static struct pxa2xx_udc_mach_info udc_info __initdata = {
+static struct gpio_vbus_mach_info tosa_udc_info = {
 	.gpio_pullup		= TOSA_GPIO_USB_PULLUP,
 	.gpio_vbus		= TOSA_GPIO_USB_IN,
 	.gpio_vbus_inverted	= 1,
 };
 
+static struct platform_device tosa_gpio_vbus = {
+	.name	= "gpio-vbus",
+	.id	= -1,
+	.dev	= {
+		.platform_data	= &tosa_udc_info,
+	},
+};
+
 /*
  * MMC/SD Device
  */
@@ -891,6 +900,7 @@ static struct platform_device *devices[] __initdata = {
 	&tosa_bt_device,
 	&sharpsl_rom_device,
 	&wm9712_device,
+	&tosa_gpio_vbus,
 };
 
 static void tosa_poweroff(void)
@@ -937,7 +947,6 @@ static void __init tosa_init(void)
 	dummy = gpiochip_reserve(TOSA_TC6393XB_GPIO_BASE, 16);
 
 	pxa_set_mci_info(&tosa_mci_platform_data);
-	pxa_set_udc_info(&udc_info);
 	pxa_set_ficp_info(&tosa_ficp_platform_data);
 	pxa_set_i2c_info(NULL);
 	pxa_set_ac97_info(NULL);
diff --git a/arch/arm/mach-pxa/trizeps4.c b/arch/arm/mach-pxa/trizeps4.c
index 857bb2e6348..b9cfbebdfe9 100644
--- a/arch/arm/mach-pxa/trizeps4.c
+++ b/arch/arm/mach-pxa/trizeps4.c
@@ -516,9 +516,9 @@ static void __init trizeps4_init(void)
 	pxa_set_stuart_info(NULL);
 
 	if (0)	/* dont know how to determine LCD */
-		set_pxa_fb_info(&sharp_lcd);
+		pxa_set_fb_info(NULL, &sharp_lcd);
 	else
-		set_pxa_fb_info(&toshiba_lcd);
+		pxa_set_fb_info(NULL, &toshiba_lcd);
 
 	pxa_set_mci_info(&trizeps4_mci_platform_data);
 #ifndef STATUS_LEDS_ON_STUART_PINS
diff --git a/arch/arm/mach-pxa/viper.c b/arch/arm/mach-pxa/viper.c
index 12279214c87..0fbe78df846 100644
--- a/arch/arm/mach-pxa/viper.c
+++ b/arch/arm/mach-pxa/viper.c
@@ -932,7 +932,7 @@ static void __init viper_init(void)
 	/* Wake-up serial console */
 	viper_init_serial_gpio();
 
-	set_pxa_fb_info(&fb_info);
+	pxa_set_fb_info(NULL, &fb_info);
 
 	/* v1 hardware cannot use the datacs line */
 	version = viper_hw_version();
diff --git a/arch/arm/mach-pxa/vpac270.c b/arch/arm/mach-pxa/vpac270.c
index e709fd45926..f71d377c864 100644
--- a/arch/arm/mach-pxa/vpac270.c
+++ b/arch/arm/mach-pxa/vpac270.c
@@ -572,7 +572,7 @@ static void __init vpac270_lcd_init(void)
 	}
 
 	vpac270_lcd_screen.pxafb_lcd_power = vpac270_lcd_power;
-	set_pxa_fb_info(&vpac270_lcd_screen);
+	pxa_set_fb_info(NULL, &vpac270_lcd_screen);
 	return;
 
 err2:
diff --git a/arch/arm/mach-pxa/z2.c b/arch/arm/mach-pxa/z2.c
index aaf883754ef..fbe9e02e2f9 100644
--- a/arch/arm/mach-pxa/z2.c
+++ b/arch/arm/mach-pxa/z2.c
@@ -91,13 +91,13 @@ static unsigned long z2_pin_config[] = {
 	GPIO47_STUART_TXD,
 
 	/* Keypad */
-	GPIO100_KP_MKIN_0	| WAKEUP_ON_LEVEL_HIGH,
-	GPIO101_KP_MKIN_1	| WAKEUP_ON_LEVEL_HIGH,
-	GPIO102_KP_MKIN_2	| WAKEUP_ON_LEVEL_HIGH,
-	GPIO34_KP_MKIN_3	| WAKEUP_ON_LEVEL_HIGH,
-	GPIO38_KP_MKIN_4	| WAKEUP_ON_LEVEL_HIGH,
-	GPIO16_KP_MKIN_5	| WAKEUP_ON_LEVEL_HIGH,
-	GPIO17_KP_MKIN_6	| WAKEUP_ON_LEVEL_HIGH,
+	GPIO100_KP_MKIN_0,
+	GPIO101_KP_MKIN_1,
+	GPIO102_KP_MKIN_2,
+	GPIO34_KP_MKIN_3,
+	GPIO38_KP_MKIN_4,
+	GPIO16_KP_MKIN_5,
+	GPIO17_KP_MKIN_6,
 	GPIO103_KP_MKOUT_0,
 	GPIO104_KP_MKOUT_1,
 	GPIO105_KP_MKOUT_2,
@@ -138,8 +138,7 @@ static unsigned long z2_pin_config[] = {
 	GPIO1_GPIO,		/* Power button */
 	GPIO37_GPIO,		/* Headphone detect */
 	GPIO98_GPIO,		/* Lid switch */
-	GPIO14_GPIO,		/* WiFi Reset */
-	GPIO15_GPIO,		/* WiFi Power */
+	GPIO14_GPIO,		/* WiFi Power */
 	GPIO24_GPIO,		/* WiFi CS */
 	GPIO36_GPIO,		/* WiFi IRQ */
 	GPIO88_GPIO,		/* LCD CS */
@@ -204,7 +203,7 @@ static struct platform_pwm_backlight_data z2_backlight_data[] = {
 		/* Keypad Backlight */
 		.pwm_id		= 1,
 		.max_brightness	= 1023,
-		.dft_brightness	= 512,
+		.dft_brightness	= 0,
 		.pwm_period_ns	= 1260320,
 	},
 	[1] = {
@@ -271,7 +270,7 @@ static struct pxafb_mach_info z2_lcd_screen = {
 
 static void __init z2_lcd_init(void)
 {
-	set_pxa_fb_info(&z2_lcd_screen);
+	pxa_set_fb_info(NULL, &z2_lcd_screen);
 }
 #else
 static inline void z2_lcd_init(void) {}
@@ -309,12 +308,12 @@ struct gpio_led z2_gpio_leds[] = {
 	.active_low		= 1,
 }, {
 	.name			= "z2:green:charged",
-	.default_trigger	= "none",
+	.default_trigger	= "mmc0",
 	.gpio			= GPIO85_ZIPITZ2_LED_CHARGED,
 	.active_low		= 1,
 }, {
 	.name			= "z2:amber:charging",
-	.default_trigger	= "none",
+	.default_trigger	= "Z2-charging-or-full",
 	.gpio			= GPIO83_ZIPITZ2_LED_CHARGING,
 	.active_low		= 1,
 },
@@ -427,8 +426,22 @@ static inline void z2_mkp_init(void) {}
  ******************************************************************************/
 #if defined(CONFIG_KEYBOARD_GPIO) || defined(CONFIG_KEYBOARD_GPIO_MODULE)
 static struct gpio_keys_button z2_pxa_buttons[] = {
-	{KEY_POWER, GPIO1_ZIPITZ2_POWER_BUTTON, 0, "Power Button" },
-	{KEY_CLOSE, GPIO98_ZIPITZ2_LID_BUTTON, 0, "Lid Button" },
+	{
+		.code		= KEY_POWER,
+		.gpio		= GPIO1_ZIPITZ2_POWER_BUTTON,
+		.active_low	= 0,
+		.desc		= "Power Button",
+		.wakeup		= 1,
+		.type		= EV_KEY,
+	},
+	{
+		.code		= SW_LID,
+		.gpio		= GPIO98_ZIPITZ2_LID_BUTTON,
+		.active_low	= 1,
+		.desc		= "Lid Switch",
+		.wakeup		= 0,
+		.type		= EV_SW,
+	},
 };
 
 static struct gpio_keys_platform_data z2_pxa_keys_data = {
@@ -461,9 +474,9 @@ static struct z2_battery_info batt_chip_info = {
 	.batt_I2C_addr	= 0x55,
 	.batt_I2C_reg	= 2,
 	.charge_gpio	= GPIO0_ZIPITZ2_AC_DETECT,
-	.min_voltage	= 2400000,
-	.max_voltage	= 3700000,
-	.batt_div	= 69,
+	.min_voltage	= 3475000,
+	.max_voltage	= 4190000,
+	.batt_div	= 59,
 	.batt_mult	= 1000000,
 	.batt_tech	= POWER_SUPPLY_TECHNOLOGY_LION,
 	.batt_name	= "Z2",
@@ -497,26 +510,16 @@ static int z2_lbs_spi_setup(struct spi_device *spi)
 {
 	int ret = 0;
 
-	ret = gpio_request(GPIO15_ZIPITZ2_WIFI_POWER, "WiFi Power");
+	ret = gpio_request(GPIO14_ZIPITZ2_WIFI_POWER, "WiFi Power");
 	if (ret)
 		goto err;
 
-	ret = gpio_direction_output(GPIO15_ZIPITZ2_WIFI_POWER, 1);
+	ret = gpio_direction_output(GPIO14_ZIPITZ2_WIFI_POWER, 1);
 	if (ret)
 		goto err2;
 
-	ret = gpio_request(GPIO14_ZIPITZ2_WIFI_RESET, "WiFi Reset");
-	if (ret)
-		goto err2;
-
-	ret = gpio_direction_output(GPIO14_ZIPITZ2_WIFI_RESET, 0);
-	if (ret)
-		goto err3;
-
-	/* Reset the card */
+	/* Wait until card is powered on */
 	mdelay(180);
-	gpio_set_value(GPIO14_ZIPITZ2_WIFI_RESET, 1);
-	mdelay(20);
 
 	spi->bits_per_word = 16;
 	spi->mode = SPI_MODE_2,
@@ -525,22 +528,18 @@ static int z2_lbs_spi_setup(struct spi_device *spi)
 
 	return 0;
 
-err3:
-	gpio_free(GPIO14_ZIPITZ2_WIFI_RESET);
 err2:
-	gpio_free(GPIO15_ZIPITZ2_WIFI_POWER);
+	gpio_free(GPIO14_ZIPITZ2_WIFI_POWER);
 err:
 	return ret;
 };
 
 static int z2_lbs_spi_teardown(struct spi_device *spi)
 {
-	gpio_set_value(GPIO14_ZIPITZ2_WIFI_RESET, 0);
-	gpio_set_value(GPIO15_ZIPITZ2_WIFI_POWER, 0);
-	gpio_free(GPIO14_ZIPITZ2_WIFI_RESET);
-	gpio_free(GPIO15_ZIPITZ2_WIFI_POWER);
-	return 0;
+	gpio_set_value(GPIO14_ZIPITZ2_WIFI_POWER, 0);
+	gpio_free(GPIO14_ZIPITZ2_WIFI_POWER);
 
+	return 0;
 };
 
 static struct pxa2xx_spi_chip z2_lbs_chip_info = {
diff --git a/arch/arm/mach-pxa/zeus.c b/arch/arm/mach-pxa/zeus.c
index 730f51e57c1..5891590126f 100644
--- a/arch/arm/mach-pxa/zeus.c
+++ b/arch/arm/mach-pxa/zeus.c
@@ -846,7 +846,7 @@ static void __init zeus_init(void)
 	if (zeus_setup_fb_gpios())
 		pr_err("Failed to setup fb gpios\n");
 	else
-		set_pxa_fb_info(&zeus_fb_info);
+		pxa_set_fb_info(NULL, &zeus_fb_info);
 
 	pxa_set_mci_info(&zeus_mci_platform_data);
 	pxa_set_udc_info(&zeus_udc_info);
diff --git a/arch/arm/mach-pxa/zylonite.c b/arch/arm/mach-pxa/zylonite.c
index a4c784aab76..5821185f77a 100644
--- a/arch/arm/mach-pxa/zylonite.c
+++ b/arch/arm/mach-pxa/zylonite.c
@@ -208,7 +208,7 @@ static void __init zylonite_init_lcd(void)
 	platform_device_register(&zylonite_backlight_device);
 
 	if (lcd_id & 0x20) {
-		set_pxa_fb_info(&zylonite_sharp_lcd_info);
+		pxa_set_fb_info(NULL, &zylonite_sharp_lcd_info);
 		return;
 	}
 
@@ -220,7 +220,7 @@ static void __init zylonite_init_lcd(void)
 	else
 		zylonite_toshiba_lcd_info.modes = &toshiba_ltm04c380k_mode;
 
-	set_pxa_fb_info(&zylonite_toshiba_lcd_info);
+	pxa_set_fb_info(NULL, &zylonite_toshiba_lcd_info);
 }
 #else
 static inline void zylonite_init_lcd(void) {}
diff --git a/arch/arm/plat-mxc/devices/platform-fec.c b/arch/arm/plat-mxc/devices/platform-fec.c
index 6561c9df5f0..ccc789e21da 100644
--- a/arch/arm/plat-mxc/devices/platform-fec.c
+++ b/arch/arm/plat-mxc/devices/platform-fec.c
@@ -53,7 +53,7 @@ struct platform_device *__init imx_add_fec(
 	struct resource res[] = {
 		{
 			.start = data->iobase,
-			.end = data->iobase + SZ_4K,
+			.end = data->iobase + SZ_4K - 1,
 			.flags = IORESOURCE_MEM,
 		}, {
 			.start = data->irq,
diff --git a/arch/arm/plat-mxc/devices/platform-imxdi_rtc.c b/arch/arm/plat-mxc/devices/platform-imxdi_rtc.c
index 10653cc8d1f..805336fdc25 100644
--- a/arch/arm/plat-mxc/devices/platform-imxdi_rtc.c
+++ b/arch/arm/plat-mxc/devices/platform-imxdi_rtc.c
@@ -27,7 +27,7 @@ struct platform_device *__init imx_add_imxdi_rtc(
 	struct resource res[] = {
 		{
 			.start = data->iobase,
-			.end = data->iobase + SZ_16K,
+			.end = data->iobase + SZ_16K - 1,
 			.flags = IORESOURCE_MEM,
 		}, {
 			.start = data->irq,
diff --git a/arch/arm/plat-mxc/include/mach/audmux.h b/arch/arm/plat-mxc/include/mach/audmux.h
index 5cd6466964a..6fda788ed0e 100644
--- a/arch/arm/plat-mxc/include/mach/audmux.h
+++ b/arch/arm/plat-mxc/include/mach/audmux.h
@@ -15,6 +15,14 @@
 #define MX31_AUDMUX_PORT5_SSI_PINS_5	4
 #define MX31_AUDMUX_PORT6_SSI_PINS_6	5
 
+#define MX51_AUDMUX_PORT1_SSI0		0
+#define MX51_AUDMUX_PORT2_SSI1		1
+#define MX51_AUDMUX_PORT3		2
+#define MX51_AUDMUX_PORT4		3
+#define MX51_AUDMUX_PORT5		4
+#define MX51_AUDMUX_PORT6		5
+#define MX51_AUDMUX_PORT7		6
+
 /* Register definitions for the i.MX21/27 Digital Audio Multiplexer */
 #define MXC_AUDMUX_V1_PCR_INMMASK(x)	((x) & 0xff)
 #define MXC_AUDMUX_V1_PCR_INMEN		(1 << 8)
@@ -28,7 +36,7 @@
 #define MXC_AUDMUX_V1_PCR_TCLKDIR	(1 << 30)
 #define MXC_AUDMUX_V1_PCR_TFSDIR	(1 << 31)
 
-/* Register definitions for the i.MX25/31/35 Digital Audio Multiplexer */
+/* Register definitions for the i.MX25/31/35/51 Digital Audio Multiplexer */
 #define MXC_AUDMUX_V2_PTCR_TFSDIR	(1 << 31)
 #define MXC_AUDMUX_V2_PTCR_TFSEL(x)	(((x) & 0xf) << 27)
 #define MXC_AUDMUX_V2_PTCR_TCLKDIR	(1 << 26)
diff --git a/arch/arm/plat-mxc/include/mach/iomux-mx2x.h b/arch/arm/plat-mxc/include/mach/iomux-mx2x.h
index c4f116d214f..7a9b20abda0 100644
--- a/arch/arm/plat-mxc/include/mach/iomux-mx2x.h
+++ b/arch/arm/plat-mxc/include/mach/iomux-mx2x.h
@@ -90,12 +90,12 @@
 #define PC31_PF_SSI3_CLK	(GPIO_PORTC | GPIO_PF | GPIO_IN | 31)
 #define PD17_PF_I2C_DATA	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 17)
 #define PD18_PF_I2C_CLK		(GPIO_PORTD | GPIO_PF | GPIO_OUT | 18)
-#define PD19_PF_CSPI2_SS2	(GPIO_PORTD | GPIO_PF | 19)
-#define PD20_PF_CSPI2_SS1	(GPIO_PORTD | GPIO_PF | 20)
-#define PD21_PF_CSPI2_SS0	(GPIO_PORTD | GPIO_PF | 21)
-#define PD22_PF_CSPI2_SCLK	(GPIO_PORTD | GPIO_PF | 22)
-#define PD23_PF_CSPI2_MISO	(GPIO_PORTD | GPIO_PF | 23)
-#define PD24_PF_CSPI2_MOSI	(GPIO_PORTD | GPIO_PF | 24)
+#define PD19_PF_CSPI2_SS2	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 19)
+#define PD20_PF_CSPI2_SS1	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 20)
+#define PD21_PF_CSPI2_SS0	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 21)
+#define PD22_PF_CSPI2_SCLK	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 22)
+#define PD23_PF_CSPI2_MISO	(GPIO_PORTD | GPIO_PF | GPIO_IN | 23)
+#define PD24_PF_CSPI2_MOSI	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 24)
 #define PD25_PF_CSPI1_RDY	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 25)
 #define PD26_PF_CSPI1_SS2	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 26)
 #define PD27_PF_CSPI1_SS1	(GPIO_PORTD | GPIO_PF | GPIO_OUT | 27)
diff --git a/arch/arm/plat-mxc/include/mach/mx50.h b/arch/arm/plat-mxc/include/mach/mx50.h
index aaec2a6e7b3..5f2da75a47f 100644
--- a/arch/arm/plat-mxc/include/mach/mx50.h
+++ b/arch/arm/plat-mxc/include/mach/mx50.h
@@ -282,4 +282,8 @@
 #define MX50_INT_APBHDMA_CHAN6	116
 #define MX50_INT_APBHDMA_CHAN7	117
 
+#if !defined(__ASSEMBLY__) && !defined(__MXC_BOOT_UNCOMPRESS)
+extern int mx50_revision(void);
+#endif
+
 #endif /* ifndef __MACH_MX50_H__ */
diff --git a/arch/arm/plat-mxc/include/mach/mx51.h b/arch/arm/plat-mxc/include/mach/mx51.h
index 1eb339e6c85..dede19a766f 100644
--- a/arch/arm/plat-mxc/include/mach/mx51.h
+++ b/arch/arm/plat-mxc/include/mach/mx51.h
@@ -347,6 +347,7 @@
 
 #if !defined(__ASSEMBLY__) && !defined(__MXC_BOOT_UNCOMPRESS)
 extern int mx51_revision(void);
+extern void mx51_display_revision(void);
 #endif
 
 /* tape-out 1 defines */
diff --git a/arch/arm/plat-mxc/include/mach/mxc.h b/arch/arm/plat-mxc/include/mach/mxc.h
index 7e072637eef..1aea818d9d3 100644
--- a/arch/arm/plat-mxc/include/mach/mxc.h
+++ b/arch/arm/plat-mxc/include/mach/mxc.h
@@ -51,6 +51,20 @@
 #define IMX_CHIP_REVISION_3_3		0x33
 #define IMX_CHIP_REVISION_UNKNOWN	0xff
 
+#define IMX_CHIP_REVISION_1_0_STRING		"1.0"
+#define IMX_CHIP_REVISION_1_1_STRING		"1.1"
+#define IMX_CHIP_REVISION_1_2_STRING		"1.2"
+#define IMX_CHIP_REVISION_1_3_STRING		"1.3"
+#define IMX_CHIP_REVISION_2_0_STRING		"2.0"
+#define IMX_CHIP_REVISION_2_1_STRING		"2.1"
+#define IMX_CHIP_REVISION_2_2_STRING		"2.2"
+#define IMX_CHIP_REVISION_2_3_STRING		"2.3"
+#define IMX_CHIP_REVISION_3_0_STRING		"3.0"
+#define IMX_CHIP_REVISION_3_1_STRING		"3.1"
+#define IMX_CHIP_REVISION_3_2_STRING		"3.2"
+#define IMX_CHIP_REVISION_3_3_STRING		"3.3"
+#define IMX_CHIP_REVISION_UNKNOWN_STRING	"unknown"
+
 #ifndef __ASSEMBLY__
 extern unsigned int __mxc_cpu_type;
 #endif
@@ -181,6 +195,15 @@ struct cpu_op {
 	u32 cpu_rate;
 };
 
+int tzic_enable_wake(int is_idle);
+enum mxc_cpu_pwr_mode {
+	WAIT_CLOCKED,		/* wfi only */
+	WAIT_UNCLOCKED,		/* WAIT */
+	WAIT_UNCLOCKED_POWER_OFF,	/* WAIT + SRPG */
+	STOP_POWER_ON,		/* just STOP */
+	STOP_POWER_OFF,		/* STOP + SRPG */
+};
+
 extern struct cpu_op *(*get_cpu_op)(int *op);
 #endif
 
diff --git a/arch/arm/plat-mxc/include/mach/system.h b/arch/arm/plat-mxc/include/mach/system.h
index 95be51bfe9a..0417da9f710 100644
--- a/arch/arm/plat-mxc/include/mach/system.h
+++ b/arch/arm/plat-mxc/include/mach/system.h
@@ -20,6 +20,8 @@
 #include <mach/hardware.h>
 #include <mach/common.h>
 
+extern void mx5_cpu_lp_set(enum mxc_cpu_pwr_mode mode);
+
 static inline void arch_idle(void)
 {
 #ifdef CONFIG_ARCH_MXC91231
@@ -54,7 +56,9 @@ static inline void arch_idle(void)
 			"orr %0, %0, #0x00000004\n"
 			"mcr p15, 0, %0, c1, c0, 0\n"
 			: "=r" (reg));
-	} else
+	} else if (cpu_is_mx51())
+		mx5_cpu_lp_set(WAIT_UNCLOCKED_POWER_OFF);
+	else
 		cpu_do_idle();
 }
 
diff --git a/arch/arm/plat-mxc/time.c b/arch/arm/plat-mxc/time.c
index 9f0c2610595..2237ff8b434 100644
--- a/arch/arm/plat-mxc/time.c
+++ b/arch/arm/plat-mxc/time.c
@@ -27,6 +27,7 @@
 #include <linux/clk.h>
 
 #include <mach/hardware.h>
+#include <asm/sched_clock.h>
 #include <asm/mach/time.h>
 #include <mach/common.h>
 
@@ -105,6 +106,11 @@ static void gpt_irq_acknowledge(void)
 		__raw_writel(V2_TSTAT_OF1, timer_base + V2_TSTAT);
 }
 
+static cycle_t dummy_get_cycles(struct clocksource *cs)
+{
+	return 0;
+}
+
 static cycle_t mx1_2_get_cycles(struct clocksource *cs)
 {
 	return __raw_readl(timer_base + MX1_2_TCN);
@@ -118,18 +124,35 @@ static cycle_t v2_get_cycles(struct clocksource *cs)
 static struct clocksource clocksource_mxc = {
 	.name 		= "mxc_timer1",
 	.rating		= 200,
-	.read		= mx1_2_get_cycles,
+	.read		= dummy_get_cycles,
 	.mask		= CLOCKSOURCE_MASK(32),
 	.flags		= CLOCK_SOURCE_IS_CONTINUOUS,
 };
 
+static DEFINE_CLOCK_DATA(cd);
+unsigned long long notrace sched_clock(void)
+{
+	cycle_t cyc = clocksource_mxc.read(&clocksource_mxc);
+
+	return cyc_to_sched_clock(&cd, cyc, (u32)~0);
+}
+
+static void notrace mxc_update_sched_clock(void)
+{
+	cycle_t cyc = clocksource_mxc.read(&clocksource_mxc);
+	update_sched_clock(&cd, cyc, (u32)~0);
+}
+
 static int __init mxc_clocksource_init(struct clk *timer_clk)
 {
 	unsigned int c = clk_get_rate(timer_clk);
 
 	if (timer_is_v2())
 		clocksource_mxc.read = v2_get_cycles;
+	else
+		clocksource_mxc.read = mx1_2_get_cycles;
 
+	init_sched_clock(&cd, mxc_update_sched_clock, 32, c);
 	clocksource_register_hz(&clocksource_mxc, c);
 
 	return 0;
diff --git a/drivers/ata/pata_palmld.c b/drivers/ata/pata_palmld.c
index a2a73d95384..b86d7e22595 100644
--- a/drivers/ata/pata_palmld.c
+++ b/drivers/ata/pata_palmld.c
@@ -33,6 +33,11 @@
 
 #define DRV_NAME "pata_palmld"
 
+static struct gpio palmld_hdd_gpios[] = {
+	{ GPIO_NR_PALMLD_IDE_PWEN,	GPIOF_INIT_HIGH,	"HDD Power" },
+	{ GPIO_NR_PALMLD_IDE_RESET,	GPIOF_INIT_LOW,		"HDD Reset" },
+};
+
 static struct scsi_host_template palmld_sht = {
 	ATA_PIO_SHT(DRV_NAME),
 };
@@ -52,28 +57,23 @@ static __devinit int palmld_pata_probe(struct platform_device *pdev)
 
 	/* allocate host */
 	host = ata_host_alloc(&pdev->dev, 1);
-	if (!host)
-		return -ENOMEM;
+	if (!host) {
+		ret = -ENOMEM;
+		goto err1;
+	}
 
 	/* remap drive's physical memory address */
 	mem = devm_ioremap(&pdev->dev, PALMLD_IDE_PHYS, 0x1000);
-	if (!mem)
-		return -ENOMEM;
+	if (!mem) {
+		ret = -ENOMEM;
+		goto err1;
+	}
 
 	/* request and activate power GPIO, IRQ GPIO */
-	ret = gpio_request(GPIO_NR_PALMLD_IDE_PWEN, "HDD PWR");
+	ret = gpio_request_array(palmld_hdd_gpios,
+				ARRAY_SIZE(palmld_hdd_gpios));
 	if (ret)
 		goto err1;
-	ret = gpio_direction_output(GPIO_NR_PALMLD_IDE_PWEN, 1);
-	if (ret)
-		goto err2;
-
-	ret = gpio_request(GPIO_NR_PALMLD_IDE_RESET, "HDD RST");
-	if (ret)
-		goto err2;
-	ret = gpio_direction_output(GPIO_NR_PALMLD_IDE_RESET, 0);
-	if (ret)
-		goto err3;
 
 	/* reset the drive */
 	gpio_set_value(GPIO_NR_PALMLD_IDE_RESET, 0);
@@ -96,13 +96,15 @@ static __devinit int palmld_pata_probe(struct platform_device *pdev)
 	ata_sff_std_ports(&ap->ioaddr);
 
 	/* activate host */
-	return ata_host_activate(host, 0, NULL, IRQF_TRIGGER_RISING,
+	ret = ata_host_activate(host, 0, NULL, IRQF_TRIGGER_RISING,
 					&palmld_sht);
+	if (ret)
+		goto err2;
+
+	return ret;
 
-err3:
-	gpio_free(GPIO_NR_PALMLD_IDE_RESET);
 err2:
-	gpio_free(GPIO_NR_PALMLD_IDE_PWEN);
+	gpio_free_array(palmld_hdd_gpios, ARRAY_SIZE(palmld_hdd_gpios));
 err1:
 	return ret;
 }
@@ -116,8 +118,7 @@ static __devexit int palmld_pata_remove(struct platform_device *dev)
 	/* power down the HDD */
 	gpio_set_value(GPIO_NR_PALMLD_IDE_PWEN, 0);
 
-	gpio_free(GPIO_NR_PALMLD_IDE_RESET);
-	gpio_free(GPIO_NR_PALMLD_IDE_PWEN);
+	gpio_free_array(palmld_hdd_gpios, ARRAY_SIZE(palmld_hdd_gpios));
 
 	return 0;
 }
diff --git a/drivers/pcmcia/pxa2xx_colibri.c b/drivers/pcmcia/pxa2xx_colibri.c
index a52039564e7..443cb7fc872 100644
--- a/drivers/pcmcia/pxa2xx_colibri.c
+++ b/drivers/pcmcia/pxa2xx_colibri.c
@@ -34,14 +34,24 @@
 #define	COLIBRI320_DETECT_GPIO	81
 #define	COLIBRI320_READY_GPIO	29
 
-static struct {
-	int	reset_gpio;
-	int	ppen_gpio;
-	int	bvd1_gpio;
-	int	bvd2_gpio;
-	int	detect_gpio;
-	int	ready_gpio;
-} colibri_pcmcia_gpio;
+enum {
+	DETECT = 0,
+	READY = 1,
+	BVD1 = 2,
+	BVD2 = 3,
+	PPEN = 4,
+	RESET = 5,
+};
+
+/* Contents of this array are configured on-the-fly in init function */
+static struct gpio colibri_pcmcia_gpios[] = {
+	{ 0,	GPIOF_IN,	"PCMCIA Detect" },
+	{ 0,	GPIOF_IN,	"PCMCIA Ready" },
+	{ 0,	GPIOF_IN,	"PCMCIA BVD1" },
+	{ 0,	GPIOF_IN,	"PCMCIA BVD2" },
+	{ 0,	GPIOF_INIT_LOW,	"PCMCIA PPEN" },
+	{ 0,	GPIOF_INIT_HIGH,"PCMCIA Reset" },
+};
 
 static struct pcmcia_irqs colibri_irqs[] = {
 	{
@@ -54,88 +64,42 @@ static int colibri_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 {
 	int ret;
 
-	ret = gpio_request(colibri_pcmcia_gpio.detect_gpio, "DETECT");
+	ret = gpio_request_array(colibri_pcmcia_gpios,
+				ARRAY_SIZE(colibri_pcmcia_gpios));
 	if (ret)
 		goto err1;
-	ret = gpio_direction_input(colibri_pcmcia_gpio.detect_gpio);
-	if (ret)
-		goto err2;
-
-	ret = gpio_request(colibri_pcmcia_gpio.ready_gpio, "READY");
-	if (ret)
-		goto err2;
-	ret = gpio_direction_input(colibri_pcmcia_gpio.ready_gpio);
-	if (ret)
-		goto err3;
 
-	ret = gpio_request(colibri_pcmcia_gpio.bvd1_gpio, "BVD1");
-	if (ret)
-		goto err3;
-	ret = gpio_direction_input(colibri_pcmcia_gpio.bvd1_gpio);
-	if (ret)
-		goto err4;
+	colibri_irqs[0].irq = gpio_to_irq(colibri_pcmcia_gpios[DETECT].gpio);
+	skt->socket.pci_irq = gpio_to_irq(colibri_pcmcia_gpios[READY].gpio);
 
-	ret = gpio_request(colibri_pcmcia_gpio.bvd2_gpio, "BVD2");
-	if (ret)
-		goto err4;
-	ret = gpio_direction_input(colibri_pcmcia_gpio.bvd2_gpio);
-	if (ret)
-		goto err5;
-
-	ret = gpio_request(colibri_pcmcia_gpio.ppen_gpio, "PPEN");
-	if (ret)
-		goto err5;
-	ret = gpio_direction_output(colibri_pcmcia_gpio.ppen_gpio, 0);
-	if (ret)
-		goto err6;
-
-	ret = gpio_request(colibri_pcmcia_gpio.reset_gpio, "RESET");
-	if (ret)
-		goto err6;
-	ret = gpio_direction_output(colibri_pcmcia_gpio.reset_gpio, 1);
+	ret = soc_pcmcia_request_irqs(skt, colibri_irqs,
+					ARRAY_SIZE(colibri_irqs));
 	if (ret)
-		goto err7;
-
-	colibri_irqs[0].irq = gpio_to_irq(colibri_pcmcia_gpio.detect_gpio);
-	skt->socket.pci_irq = gpio_to_irq(colibri_pcmcia_gpio.ready_gpio);
+		goto err2;
 
-	return soc_pcmcia_request_irqs(skt, colibri_irqs,
-					ARRAY_SIZE(colibri_irqs));
+	return ret;
 
-err7:
-	gpio_free(colibri_pcmcia_gpio.detect_gpio);
-err6:
-	gpio_free(colibri_pcmcia_gpio.ready_gpio);
-err5:
-	gpio_free(colibri_pcmcia_gpio.bvd1_gpio);
-err4:
-	gpio_free(colibri_pcmcia_gpio.bvd2_gpio);
-err3:
-	gpio_free(colibri_pcmcia_gpio.reset_gpio);
 err2:
-	gpio_free(colibri_pcmcia_gpio.ppen_gpio);
+	gpio_free_array(colibri_pcmcia_gpios,
+			ARRAY_SIZE(colibri_pcmcia_gpios));
 err1:
 	return ret;
 }
 
 static void colibri_pcmcia_hw_shutdown(struct soc_pcmcia_socket *skt)
 {
-	gpio_free(colibri_pcmcia_gpio.detect_gpio);
-	gpio_free(colibri_pcmcia_gpio.ready_gpio);
-	gpio_free(colibri_pcmcia_gpio.bvd1_gpio);
-	gpio_free(colibri_pcmcia_gpio.bvd2_gpio);
-	gpio_free(colibri_pcmcia_gpio.reset_gpio);
-	gpio_free(colibri_pcmcia_gpio.ppen_gpio);
+	gpio_free_array(colibri_pcmcia_gpios,
+			ARRAY_SIZE(colibri_pcmcia_gpios));
 }
 
 static void colibri_pcmcia_socket_state(struct soc_pcmcia_socket *skt,
 					struct pcmcia_state *state)
 {
 
-	state->detect = !!gpio_get_value(colibri_pcmcia_gpio.detect_gpio);
-	state->ready  = !!gpio_get_value(colibri_pcmcia_gpio.ready_gpio);
-	state->bvd1   = !!gpio_get_value(colibri_pcmcia_gpio.bvd1_gpio);
-	state->bvd2   = !!gpio_get_value(colibri_pcmcia_gpio.bvd2_gpio);
+	state->detect = !!gpio_get_value(colibri_pcmcia_gpios[DETECT].gpio);
+	state->ready  = !!gpio_get_value(colibri_pcmcia_gpios[READY].gpio);
+	state->bvd1   = !!gpio_get_value(colibri_pcmcia_gpios[BVD1].gpio);
+	state->bvd2   = !!gpio_get_value(colibri_pcmcia_gpios[BVD2].gpio);
 	state->wrprot = 0;
 	state->vs_3v  = 1;
 	state->vs_Xv  = 0;
@@ -145,9 +109,10 @@ static int
 colibri_pcmcia_configure_socket(struct soc_pcmcia_socket *skt,
 				const socket_state_t *state)
 {
-	gpio_set_value(colibri_pcmcia_gpio.ppen_gpio,
+	gpio_set_value(colibri_pcmcia_gpios[PPEN].gpio,
 			!(state->Vcc == 33 && state->Vpp < 50));
-	gpio_set_value(colibri_pcmcia_gpio.reset_gpio, state->flags & SS_RESET);
+	gpio_set_value(colibri_pcmcia_gpios[RESET].gpio,
+			state->flags & SS_RESET);
 	return 0;
 }
 
@@ -190,20 +155,20 @@ static int __init colibri_pcmcia_init(void)
 
 	/* Colibri PXA270 */
 	if (machine_is_colibri()) {
-		colibri_pcmcia_gpio.reset_gpio	= COLIBRI270_RESET_GPIO;
-		colibri_pcmcia_gpio.ppen_gpio	= COLIBRI270_PPEN_GPIO;
-		colibri_pcmcia_gpio.bvd1_gpio	= COLIBRI270_BVD1_GPIO;
-		colibri_pcmcia_gpio.bvd2_gpio	= COLIBRI270_BVD2_GPIO;
-		colibri_pcmcia_gpio.detect_gpio	= COLIBRI270_DETECT_GPIO;
-		colibri_pcmcia_gpio.ready_gpio	= COLIBRI270_READY_GPIO;
+		colibri_pcmcia_gpios[RESET].gpio	= COLIBRI270_RESET_GPIO;
+		colibri_pcmcia_gpios[PPEN].gpio		= COLIBRI270_PPEN_GPIO;
+		colibri_pcmcia_gpios[BVD1].gpio		= COLIBRI270_BVD1_GPIO;
+		colibri_pcmcia_gpios[BVD2].gpio		= COLIBRI270_BVD2_GPIO;
+		colibri_pcmcia_gpios[DETECT].gpio	= COLIBRI270_DETECT_GPIO;
+		colibri_pcmcia_gpios[READY].gpio	= COLIBRI270_READY_GPIO;
 	/* Colibri PXA320 */
 	} else if (machine_is_colibri320()) {
-		colibri_pcmcia_gpio.reset_gpio	= COLIBRI320_RESET_GPIO;
-		colibri_pcmcia_gpio.ppen_gpio	= COLIBRI320_PPEN_GPIO;
-		colibri_pcmcia_gpio.bvd1_gpio	= COLIBRI320_BVD1_GPIO;
-		colibri_pcmcia_gpio.bvd2_gpio	= COLIBRI320_BVD2_GPIO;
-		colibri_pcmcia_gpio.detect_gpio	= COLIBRI320_DETECT_GPIO;
-		colibri_pcmcia_gpio.ready_gpio	= COLIBRI320_READY_GPIO;
+		colibri_pcmcia_gpios[RESET].gpio	= COLIBRI320_RESET_GPIO;
+		colibri_pcmcia_gpios[PPEN].gpio		= COLIBRI320_PPEN_GPIO;
+		colibri_pcmcia_gpios[BVD1].gpio		= COLIBRI320_BVD1_GPIO;
+		colibri_pcmcia_gpios[BVD2].gpio		= COLIBRI320_BVD2_GPIO;
+		colibri_pcmcia_gpios[DETECT].gpio	= COLIBRI320_DETECT_GPIO;
+		colibri_pcmcia_gpios[READY].gpio	= COLIBRI320_READY_GPIO;
 	}
 
 	ret = platform_device_add_data(colibri_pcmcia_device,
diff --git a/drivers/pcmcia/pxa2xx_palmld.c b/drivers/pcmcia/pxa2xx_palmld.c
index 6fb6f7f0672..69f73670949 100644
--- a/drivers/pcmcia/pxa2xx_palmld.c
+++ b/drivers/pcmcia/pxa2xx_palmld.c
@@ -4,7 +4,7 @@
  * Driver for Palm LifeDrive PCMCIA
  *
  * Copyright (C) 2006 Alex Osborne <ato@meshy.org>
- * Copyright (C) 2007-2008 Marek Vasut <marek.vasut@gmail.com>
+ * Copyright (C) 2007-2011 Marek Vasut <marek.vasut@gmail.com>
  *
  * 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
@@ -20,49 +20,27 @@
 #include <mach/palmld.h>
 #include "soc_common.h"
 
+static struct gpio palmld_pcmcia_gpios[] = {
+	{ GPIO_NR_PALMLD_PCMCIA_POWER,	GPIOF_INIT_LOW,	"PCMCIA Power" },
+	{ GPIO_NR_PALMLD_PCMCIA_RESET,	GPIOF_INIT_HIGH,"PCMCIA Reset" },
+	{ GPIO_NR_PALMLD_PCMCIA_READY,	GPIOF_IN,	"PCMCIA Ready" },
+};
+
 static int palmld_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 {
 	int ret;
 
-	ret = gpio_request(GPIO_NR_PALMLD_PCMCIA_POWER, "PCMCIA PWR");
-	if (ret)
-		goto err1;
-	ret = gpio_direction_output(GPIO_NR_PALMLD_PCMCIA_POWER, 0);
-	if (ret)
-		goto err2;
-
-	ret = gpio_request(GPIO_NR_PALMLD_PCMCIA_RESET, "PCMCIA RST");
-	if (ret)
-		goto err2;
-	ret = gpio_direction_output(GPIO_NR_PALMLD_PCMCIA_RESET, 1);
-	if (ret)
-		goto err3;
-
-	ret = gpio_request(GPIO_NR_PALMLD_PCMCIA_READY, "PCMCIA RDY");
-	if (ret)
-		goto err3;
-	ret = gpio_direction_input(GPIO_NR_PALMLD_PCMCIA_READY);
-	if (ret)
-		goto err4;
+	ret = gpio_request_array(palmld_pcmcia_gpios,
+				ARRAY_SIZE(palmld_pcmcia_gpios));
 
 	skt->socket.pci_irq = IRQ_GPIO(GPIO_NR_PALMLD_PCMCIA_READY);
-	return 0;
 
-err4:
-	gpio_free(GPIO_NR_PALMLD_PCMCIA_READY);
-err3:
-	gpio_free(GPIO_NR_PALMLD_PCMCIA_RESET);
-err2:
-	gpio_free(GPIO_NR_PALMLD_PCMCIA_POWER);
-err1:
 	return ret;
 }
 
 static void palmld_pcmcia_hw_shutdown(struct soc_pcmcia_socket *skt)
 {
-	gpio_free(GPIO_NR_PALMLD_PCMCIA_READY);
-	gpio_free(GPIO_NR_PALMLD_PCMCIA_RESET);
-	gpio_free(GPIO_NR_PALMLD_PCMCIA_POWER);
+	gpio_free_array(palmld_pcmcia_gpios, ARRAY_SIZE(palmld_pcmcia_gpios));
 }
 
 static void palmld_pcmcia_socket_state(struct soc_pcmcia_socket *skt,
diff --git a/drivers/pcmcia/pxa2xx_palmtc.c b/drivers/pcmcia/pxa2xx_palmtc.c
index 459a232d66b..d0ad6a76bbd 100644
--- a/drivers/pcmcia/pxa2xx_palmtc.c
+++ b/drivers/pcmcia/pxa2xx_palmtc.c
@@ -4,7 +4,7 @@
  * Driver for Palm Tungsten|C PCMCIA
  *
  * Copyright (C) 2008 Alex Osborne <ato@meshy.org>
- * Copyright (C) 2009 Marek Vasut <marek.vasut@gmail.com>
+ * Copyright (C) 2009-2011 Marek Vasut <marek.vasut@gmail.com>
  *
  * 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
@@ -21,79 +21,30 @@
 #include <mach/palmtc.h>
 #include "soc_common.h"
 
+static struct gpio palmtc_pcmcia_gpios[] = {
+	{ GPIO_NR_PALMTC_PCMCIA_POWER1,	GPIOF_INIT_LOW,	"PCMCIA Power 1" },
+	{ GPIO_NR_PALMTC_PCMCIA_POWER2,	GPIOF_INIT_LOW,	"PCMCIA Power 2" },
+	{ GPIO_NR_PALMTC_PCMCIA_POWER3,	GPIOF_INIT_LOW,	"PCMCIA Power 3" },
+	{ GPIO_NR_PALMTC_PCMCIA_RESET,	GPIOF_INIT_HIGH,"PCMCIA Reset" },
+	{ GPIO_NR_PALMTC_PCMCIA_READY,	GPIOF_IN,	"PCMCIA Ready" },
+	{ GPIO_NR_PALMTC_PCMCIA_PWRREADY, GPIOF_IN,	"PCMCIA Power Ready" },
+};
+
 static int palmtc_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 {
 	int ret;
 
-	ret = gpio_request(GPIO_NR_PALMTC_PCMCIA_POWER1, "PCMCIA PWR1");
-	if (ret)
-		goto err1;
-	ret = gpio_direction_output(GPIO_NR_PALMTC_PCMCIA_POWER1, 0);
-	if (ret)
-		goto err2;
-
-	ret = gpio_request(GPIO_NR_PALMTC_PCMCIA_POWER2, "PCMCIA PWR2");
-	if (ret)
-		goto err2;
-	ret = gpio_direction_output(GPIO_NR_PALMTC_PCMCIA_POWER2, 0);
-	if (ret)
-		goto err3;
-
-	ret = gpio_request(GPIO_NR_PALMTC_PCMCIA_POWER3, "PCMCIA PWR3");
-	if (ret)
-		goto err3;
-	ret = gpio_direction_output(GPIO_NR_PALMTC_PCMCIA_POWER3, 0);
-	if (ret)
-		goto err4;
-
-	ret = gpio_request(GPIO_NR_PALMTC_PCMCIA_RESET, "PCMCIA RST");
-	if (ret)
-		goto err4;
-	ret = gpio_direction_output(GPIO_NR_PALMTC_PCMCIA_RESET, 1);
-	if (ret)
-		goto err5;
-
-	ret = gpio_request(GPIO_NR_PALMTC_PCMCIA_READY, "PCMCIA RDY");
-	if (ret)
-		goto err5;
-	ret = gpio_direction_input(GPIO_NR_PALMTC_PCMCIA_READY);
-	if (ret)
-		goto err6;
-
-	ret = gpio_request(GPIO_NR_PALMTC_PCMCIA_PWRREADY, "PCMCIA PWRRDY");
-	if (ret)
-		goto err6;
-	ret = gpio_direction_input(GPIO_NR_PALMTC_PCMCIA_PWRREADY);
-	if (ret)
-		goto err7;
+	ret = gpio_request_array(palmtc_pcmcia_gpios,
+				ARRAY_SIZE(palmtc_pcmcia_gpios));
 
 	skt->socket.pci_irq = IRQ_GPIO(GPIO_NR_PALMTC_PCMCIA_READY);
-	return 0;
 
-err7:
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_PWRREADY);
-err6:
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_READY);
-err5:
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_RESET);
-err4:
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_POWER3);
-err3:
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_POWER2);
-err2:
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_POWER1);
-err1:
 	return ret;
 }
 
 static void palmtc_pcmcia_hw_shutdown(struct soc_pcmcia_socket *skt)
 {
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_PWRREADY);
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_READY);
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_RESET);
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_POWER3);
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_POWER2);
-	gpio_free(GPIO_NR_PALMTC_PCMCIA_POWER1);
+	gpio_free_array(palmtc_pcmcia_gpios, ARRAY_SIZE(palmtc_pcmcia_gpios));
 }
 
 static void palmtc_pcmcia_socket_state(struct soc_pcmcia_socket *skt,
diff --git a/drivers/pcmcia/pxa2xx_palmtx.c b/drivers/pcmcia/pxa2xx_palmtx.c
index b07b247a399..1a258045040 100644
--- a/drivers/pcmcia/pxa2xx_palmtx.c
+++ b/drivers/pcmcia/pxa2xx_palmtx.c
@@ -3,7 +3,7 @@
  *
  * Driver for Palm T|X PCMCIA
  *
- * Copyright (C) 2007-2008 Marek Vasut <marek.vasut@gmail.com>
+ * Copyright (C) 2007-2011 Marek Vasut <marek.vasut@gmail.com>
  *
  * 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
@@ -13,67 +13,34 @@
 
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/gpio.h>
 
 #include <asm/mach-types.h>
-
-#include <mach/gpio.h>
 #include <mach/palmtx.h>
-
 #include "soc_common.h"
 
+static struct gpio palmtx_pcmcia_gpios[] = {
+	{ GPIO_NR_PALMTX_PCMCIA_POWER1,	GPIOF_INIT_LOW,	"PCMCIA Power 1" },
+	{ GPIO_NR_PALMTX_PCMCIA_POWER2,	GPIOF_INIT_LOW,	"PCMCIA Power 2" },
+	{ GPIO_NR_PALMTX_PCMCIA_RESET,	GPIOF_INIT_HIGH,"PCMCIA Reset" },
+	{ GPIO_NR_PALMTX_PCMCIA_READY,	GPIOF_IN,	"PCMCIA Ready" },
+};
+
 static int palmtx_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 {
 	int ret;
 
-	ret = gpio_request(GPIO_NR_PALMTX_PCMCIA_POWER1, "PCMCIA PWR1");
-	if (ret)
-		goto err1;
-	ret = gpio_direction_output(GPIO_NR_PALMTX_PCMCIA_POWER1, 0);
-	if (ret)
-		goto err2;
-
-	ret = gpio_request(GPIO_NR_PALMTX_PCMCIA_POWER2, "PCMCIA PWR2");
-	if (ret)
-		goto err2;
-	ret = gpio_direction_output(GPIO_NR_PALMTX_PCMCIA_POWER2, 0);
-	if (ret)
-		goto err3;
-
-	ret = gpio_request(GPIO_NR_PALMTX_PCMCIA_RESET, "PCMCIA RST");
-	if (ret)
-		goto err3;
-	ret = gpio_direction_output(GPIO_NR_PALMTX_PCMCIA_RESET, 1);
-	if (ret)
-		goto err4;
-
-	ret = gpio_request(GPIO_NR_PALMTX_PCMCIA_READY, "PCMCIA RDY");
-	if (ret)
-		goto err4;
-	ret = gpio_direction_input(GPIO_NR_PALMTX_PCMCIA_READY);
-	if (ret)
-		goto err5;
+	ret = gpio_request_array(palmtx_pcmcia_gpios,
+				ARRAY_SIZE(palmtx_pcmcia_gpios));
 
 	skt->socket.pci_irq = gpio_to_irq(GPIO_NR_PALMTX_PCMCIA_READY);
-	return 0;
 
-err5:
-	gpio_free(GPIO_NR_PALMTX_PCMCIA_READY);
-err4:
-	gpio_free(GPIO_NR_PALMTX_PCMCIA_RESET);
-err3:
-	gpio_free(GPIO_NR_PALMTX_PCMCIA_POWER2);
-err2:
-	gpio_free(GPIO_NR_PALMTX_PCMCIA_POWER1);
-err1:
 	return ret;
 }
 
 static void palmtx_pcmcia_hw_shutdown(struct soc_pcmcia_socket *skt)
 {
-	gpio_free(GPIO_NR_PALMTX_PCMCIA_READY);
-	gpio_free(GPIO_NR_PALMTX_PCMCIA_RESET);
-	gpio_free(GPIO_NR_PALMTX_PCMCIA_POWER2);
-	gpio_free(GPIO_NR_PALMTX_PCMCIA_POWER1);
+	gpio_free_array(palmtx_pcmcia_gpios, ARRAY_SIZE(palmtx_pcmcia_gpios));
 }
 
 static void palmtx_pcmcia_socket_state(struct soc_pcmcia_socket *skt,
diff --git a/drivers/pcmcia/pxa2xx_vpac270.c b/drivers/pcmcia/pxa2xx_vpac270.c
index 55627eccee8..435002dfc3c 100644
--- a/drivers/pcmcia/pxa2xx_vpac270.c
+++ b/drivers/pcmcia/pxa2xx_vpac270.c
@@ -3,8 +3,7 @@
  *
  * Driver for Voipac PXA270 PCMCIA and CF sockets
  *
- * Copyright (C) 2010
- * Marek Vasut <marek.vasut@gmail.com>
+ * Copyright (C) 2010-2011 Marek Vasut <marek.vasut@gmail.com>
  *
  * 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
@@ -22,6 +21,19 @@
 
 #include "soc_common.h"
 
+static struct gpio vpac270_pcmcia_gpios[] = {
+	{ GPIO84_VPAC270_PCMCIA_CD,	GPIOF_IN,	"PCMCIA Card Detect" },
+	{ GPIO35_VPAC270_PCMCIA_RDY,	GPIOF_IN,	"PCMCIA Ready" },
+	{ GPIO107_VPAC270_PCMCIA_PPEN,	GPIOF_INIT_LOW,	"PCMCIA PPEN" },
+	{ GPIO11_VPAC270_PCMCIA_RESET,	GPIOF_INIT_LOW,	"PCMCIA Reset" },
+};
+
+static struct gpio vpac270_cf_gpios[] = {
+	{ GPIO17_VPAC270_CF_CD,		GPIOF_IN,	"CF Card Detect" },
+	{ GPIO12_VPAC270_CF_RDY,	GPIOF_IN,	"CF Ready" },
+	{ GPIO16_VPAC270_CF_RESET,	GPIOF_INIT_LOW,	"CF Reset" },
+};
+
 static struct pcmcia_irqs cd_irqs[] = {
 	{
 		.sock = 0,
@@ -40,96 +52,34 @@ static int vpac270_pcmcia_hw_init(struct soc_pcmcia_socket *skt)
 	int ret;
 
 	if (skt->nr == 0) {
-		ret = gpio_request(GPIO84_VPAC270_PCMCIA_CD, "PCMCIA CD");
-		if (ret)
-			goto err1;
-		ret = gpio_direction_input(GPIO84_VPAC270_PCMCIA_CD);
-		if (ret)
-			goto err2;
-
-		ret = gpio_request(GPIO35_VPAC270_PCMCIA_RDY, "PCMCIA RDY");
-		if (ret)
-			goto err2;
-		ret = gpio_direction_input(GPIO35_VPAC270_PCMCIA_RDY);
-		if (ret)
-			goto err3;
-
-		ret = gpio_request(GPIO107_VPAC270_PCMCIA_PPEN, "PCMCIA PPEN");
-		if (ret)
-			goto err3;
-		ret = gpio_direction_output(GPIO107_VPAC270_PCMCIA_PPEN, 0);
-		if (ret)
-			goto err4;
-
-		ret = gpio_request(GPIO11_VPAC270_PCMCIA_RESET, "PCMCIA RESET");
-		if (ret)
-			goto err4;
-		ret = gpio_direction_output(GPIO11_VPAC270_PCMCIA_RESET, 0);
-		if (ret)
-			goto err5;
+		ret = gpio_request_array(vpac270_pcmcia_gpios,
+				ARRAY_SIZE(vpac270_pcmcia_gpios));
 
 		skt->socket.pci_irq = gpio_to_irq(GPIO35_VPAC270_PCMCIA_RDY);
 
-		return soc_pcmcia_request_irqs(skt, &cd_irqs[0], 1);
-
-err5:
-		gpio_free(GPIO11_VPAC270_PCMCIA_RESET);
-err4:
-		gpio_free(GPIO107_VPAC270_PCMCIA_PPEN);
-err3:
-		gpio_free(GPIO35_VPAC270_PCMCIA_RDY);
-err2:
-		gpio_free(GPIO84_VPAC270_PCMCIA_CD);
-err1:
-		return ret;
-
+		if (!ret)
+			ret = soc_pcmcia_request_irqs(skt, &cd_irqs[0], 1);
 	} else {
-		ret = gpio_request(GPIO17_VPAC270_CF_CD, "CF CD");
-		if (ret)
-			goto err6;
-		ret = gpio_direction_input(GPIO17_VPAC270_CF_CD);
-		if (ret)
-			goto err7;
-
-		ret = gpio_request(GPIO12_VPAC270_CF_RDY, "CF RDY");
-		if (ret)
-			goto err7;
-		ret = gpio_direction_input(GPIO12_VPAC270_CF_RDY);
-		if (ret)
-			goto err8;
-
-		ret = gpio_request(GPIO16_VPAC270_CF_RESET, "CF RESET");
-		if (ret)
-			goto err8;
-		ret = gpio_direction_output(GPIO16_VPAC270_CF_RESET, 0);
-		if (ret)
-			goto err9;
+		ret = gpio_request_array(vpac270_cf_gpios,
+				ARRAY_SIZE(vpac270_cf_gpios));
 
 		skt->socket.pci_irq = gpio_to_irq(GPIO12_VPAC270_CF_RDY);
 
-		return soc_pcmcia_request_irqs(skt, &cd_irqs[1], 1);
-
-err9:
-		gpio_free(GPIO16_VPAC270_CF_RESET);
-err8:
-		gpio_free(GPIO12_VPAC270_CF_RDY);
-err7:
-		gpio_free(GPIO17_VPAC270_CF_CD);
-err6:
-		return ret;
-
+		if (!ret)
+			ret = soc_pcmcia_request_irqs(skt, &cd_irqs[1], 1);
 	}
+
+	return ret;
 }
 
 static void vpac270_pcmcia_hw_shutdown(struct soc_pcmcia_socket *skt)
 {
-	gpio_free(GPIO11_VPAC270_PCMCIA_RESET);
-	gpio_free(GPIO107_VPAC270_PCMCIA_PPEN);
-	gpio_free(GPIO35_VPAC270_PCMCIA_RDY);
-	gpio_free(GPIO84_VPAC270_PCMCIA_CD);
-	gpio_free(GPIO16_VPAC270_CF_RESET);
-	gpio_free(GPIO12_VPAC270_CF_RDY);
-	gpio_free(GPIO17_VPAC270_CF_CD);
+	if (skt->nr == 0)
+		gpio_request_array(vpac270_pcmcia_gpios,
+					ARRAY_SIZE(vpac270_pcmcia_gpios));
+	else
+		gpio_request_array(vpac270_cf_gpios,
+					ARRAY_SIZE(vpac270_cf_gpios));
 }
 
 static void vpac270_pcmcia_socket_state(struct soc_pcmcia_socket *skt,
diff --git a/drivers/usb/gadget/pxa25x_udc.c b/drivers/usb/gadget/pxa25x_udc.c
index b37f92cb71b..444b60aa15e 100644
--- a/drivers/usb/gadget/pxa25x_udc.c
+++ b/drivers/usb/gadget/pxa25x_udc.c
@@ -139,24 +139,6 @@ static const char ep0name [] = "ep0";
 static void pxa25x_ep_fifo_flush (struct usb_ep *ep);
 static void nuke (struct pxa25x_ep *, int status);
 
-/* one GPIO should be used to detect VBUS from the host */
-static int is_vbus_present(void)
-{
-	struct pxa2xx_udc_mach_info		*mach = the_controller->mach;
-
-	if (gpio_is_valid(mach->gpio_vbus)) {
-		int value = gpio_get_value(mach->gpio_vbus);
-
-		if (mach->gpio_vbus_inverted)
-			return !value;
-		else
-			return !!value;
-	}
-	if (mach->udc_is_connected)
-		return mach->udc_is_connected();
-	return 1;
-}
-
 /* one GPIO should control a D+ pullup, so host sees this device (or not) */
 static void pullup_off(void)
 {
@@ -1055,7 +1037,7 @@ udc_seq_show(struct seq_file *m, void *_d)
 		"%s version: %s\nGadget driver: %s\nHost %s\n\n",
 		driver_name, DRIVER_VERSION SIZE_STR "(pio)",
 		dev->driver ? dev->driver->driver.name : "(none)",
-		is_vbus_present() ? "full speed" : "disconnected");
+		dev->gadget.speed == USB_SPEED_FULL ? "full speed" : "disconnected");
 
 	/* registers for device and ep0 */
 	seq_printf(m,
@@ -1094,7 +1076,7 @@ udc_seq_show(struct seq_file *m, void *_d)
 			(tmp & UDCCFR_ACM) ? " acm" : "");
 	}
 
-	if (!is_vbus_present() || !dev->driver)
+	if (dev->gadget.speed != USB_SPEED_FULL || !dev->driver)
 		goto done;
 
 	seq_printf(m, "ep0 IN %lu/%lu, OUT %lu/%lu\nirqs %lu\n\n",
@@ -1435,14 +1417,6 @@ lubbock_vbus_irq(int irq, void *_dev)
 
 #endif
 
-static irqreturn_t udc_vbus_irq(int irq, void *_dev)
-{
-	struct pxa25x_udc	*dev = _dev;
-
-	pxa25x_udc_vbus_session(&dev->gadget, is_vbus_present());
-	return IRQ_HANDLED;
-}
-
 
 /*-------------------------------------------------------------------------*/
 
@@ -1766,12 +1740,9 @@ pxa25x_udc_irq(int irq, void *_dev)
 		if (unlikely(udccr & UDCCR_SUSIR)) {
 			udc_ack_int_UDCCR(UDCCR_SUSIR);
 			handled = 1;
-			DBG(DBG_VERBOSE, "USB suspend%s\n", is_vbus_present()
-				? "" : "+disconnect");
+			DBG(DBG_VERBOSE, "USB suspend\n");
 
-			if (!is_vbus_present())
-				stop_activity(dev, dev->driver);
-			else if (dev->gadget.speed != USB_SPEED_UNKNOWN
+			if (dev->gadget.speed != USB_SPEED_UNKNOWN
 					&& dev->driver
 					&& dev->driver->suspend)
 				dev->driver->suspend(&dev->gadget);
@@ -1786,8 +1757,7 @@ pxa25x_udc_irq(int irq, void *_dev)
 
 			if (dev->gadget.speed != USB_SPEED_UNKNOWN
 					&& dev->driver
-					&& dev->driver->resume
-					&& is_vbus_present())
+					&& dev->driver->resume)
 				dev->driver->resume(&dev->gadget);
 		}
 
@@ -2137,7 +2107,7 @@ static struct pxa25x_udc memory = {
 static int __init pxa25x_udc_probe(struct platform_device *pdev)
 {
 	struct pxa25x_udc *dev = &memory;
-	int retval, vbus_irq, irq;
+	int retval, irq;
 	u32 chiprev;
 
 	/* insist on Intel/ARM/XScale */
@@ -2199,19 +2169,6 @@ static int __init pxa25x_udc_probe(struct platform_device *pdev)
 
 	dev->transceiver = otg_get_transceiver();
 
-	if (gpio_is_valid(dev->mach->gpio_vbus)) {
-		if ((retval = gpio_request(dev->mach->gpio_vbus,
-				"pxa25x_udc GPIO VBUS"))) {
-			dev_dbg(&pdev->dev,
-				"can't get vbus gpio %d, err: %d\n",
-				dev->mach->gpio_vbus, retval);
-			goto err_gpio_vbus;
-		}
-		gpio_direction_input(dev->mach->gpio_vbus);
-		vbus_irq = gpio_to_irq(dev->mach->gpio_vbus);
-	} else
-		vbus_irq = 0;
-
 	if (gpio_is_valid(dev->mach->gpio_pullup)) {
 		if ((retval = gpio_request(dev->mach->gpio_pullup,
 				"pca25x_udc GPIO PULLUP"))) {
@@ -2237,7 +2194,7 @@ static int __init pxa25x_udc_probe(struct platform_device *pdev)
 	udc_disable(dev);
 	udc_reinit(dev);
 
-	dev->vbus = !!is_vbus_present();
+	dev->vbus = 0;
 
 	/* irq setup after old hardware state is cleaned up */
 	retval = request_irq(irq, pxa25x_udc_irq,
@@ -2273,22 +2230,10 @@ lubbock_fail0:
 		}
 	} else
 #endif
-	if (vbus_irq) {
-		retval = request_irq(vbus_irq, udc_vbus_irq,
-				IRQF_DISABLED | IRQF_SAMPLE_RANDOM |
-				IRQF_TRIGGER_RISING | IRQF_TRIGGER_FALLING,
-				driver_name, dev);
-		if (retval != 0) {
-			pr_err("%s: can't get irq %i, err %d\n",
-				driver_name, vbus_irq, retval);
-			goto err_vbus_irq;
-		}
-	}
 	create_debug_files(dev);
 
 	return 0;
 
- err_vbus_irq:
 #ifdef	CONFIG_ARCH_LUBBOCK
 	free_irq(LUBBOCK_USB_DISC_IRQ, dev);
  err_irq_lub:
@@ -2298,9 +2243,6 @@ lubbock_fail0:
 	if (gpio_is_valid(dev->mach->gpio_pullup))
 		gpio_free(dev->mach->gpio_pullup);
  err_gpio_pullup:
-	if (gpio_is_valid(dev->mach->gpio_vbus))
-		gpio_free(dev->mach->gpio_vbus);
- err_gpio_vbus:
 	if (dev->transceiver) {
 		otg_put_transceiver(dev->transceiver);
 		dev->transceiver = NULL;
@@ -2337,10 +2279,6 @@ static int __exit pxa25x_udc_remove(struct platform_device *pdev)
 		free_irq(LUBBOCK_USB_IRQ, dev);
 	}
 #endif
-	if (gpio_is_valid(dev->mach->gpio_vbus)) {
-		free_irq(gpio_to_irq(dev->mach->gpio_vbus), dev);
-		gpio_free(dev->mach->gpio_vbus);
-	}
 	if (gpio_is_valid(dev->mach->gpio_pullup))
 		gpio_free(dev->mach->gpio_pullup);
 
diff --git a/drivers/video/pxafb.c b/drivers/video/pxafb.c
index 825b665245b..a2e5b5100ab 100644
--- a/drivers/video/pxafb.c
+++ b/drivers/video/pxafb.c
@@ -627,7 +627,12 @@ static void overlay1fb_enable(struct pxafb_layer *ofb)
 
 static void overlay1fb_disable(struct pxafb_layer *ofb)
 {
-	uint32_t lccr5 = lcd_readl(ofb->fbi, LCCR5);
+	uint32_t lccr5;
+
+	if (!(lcd_readl(ofb->fbi, OVL1C1) & OVLxC1_OEN))
+		return;
+
+	lccr5 = lcd_readl(ofb->fbi, LCCR5);
 
 	lcd_writel(ofb->fbi, OVL1C1, ofb->control[0] & ~OVLxC1_OEN);
 
@@ -685,7 +690,12 @@ static void overlay2fb_enable(struct pxafb_layer *ofb)
 
 static void overlay2fb_disable(struct pxafb_layer *ofb)
 {
-	uint32_t lccr5 = lcd_readl(ofb->fbi, LCCR5);
+	uint32_t lccr5;
+
+	if (!(lcd_readl(ofb->fbi, OVL2C1) & OVLxC1_OEN))
+		return;
+
+	lccr5 = lcd_readl(ofb->fbi, LCCR5);
 
 	lcd_writel(ofb->fbi, OVL2C1, ofb->control[0] & ~OVLxC1_OEN);
 
@@ -720,12 +730,10 @@ static int overlayfb_open(struct fb_info *info, int user)
 	if (user == 0)
 		return -ENODEV;
 
-	/* allow only one user at a time */
-	if (atomic_inc_and_test(&ofb->usage))
-		return -EBUSY;
+	if (ofb->usage++ == 0)
+		/* unblank the base framebuffer */
+		fb_blank(&ofb->fbi->fb, FB_BLANK_UNBLANK);
 
-	/* unblank the base framebuffer */
-	fb_blank(&ofb->fbi->fb, FB_BLANK_UNBLANK);
 	return 0;
 }
 
@@ -733,12 +741,15 @@ static int overlayfb_release(struct fb_info *info, int user)
 {
 	struct pxafb_layer *ofb = (struct pxafb_layer*) info;
 
-	atomic_dec(&ofb->usage);
-	ofb->ops->disable(ofb);
+	if (ofb->usage == 1) {
+		ofb->ops->disable(ofb);
+		ofb->fb.var.height	= -1;
+		ofb->fb.var.width	= -1;
+		ofb->fb.var.xres = ofb->fb.var.xres_virtual = 0;
+		ofb->fb.var.yres = ofb->fb.var.yres_virtual = 0;
 
-	free_pages_exact(ofb->video_mem, ofb->video_mem_size);
-	ofb->video_mem = NULL;
-	ofb->video_mem_size = 0;
+		ofb->usage--;
+	}
 	return 0;
 }
 
@@ -750,7 +761,7 @@ static int overlayfb_check_var(struct fb_var_screeninfo *var,
 	int xpos, ypos, pfor, bpp;
 
 	xpos = NONSTD_TO_XPOS(var->nonstd);
-	ypos = NONSTD_TO_XPOS(var->nonstd);
+	ypos = NONSTD_TO_YPOS(var->nonstd);
 	pfor = NONSTD_TO_PFOR(var->nonstd);
 
 	bpp = pxafb_var_to_bpp(var);
@@ -794,7 +805,7 @@ static int overlayfb_check_var(struct fb_var_screeninfo *var,
 	return 0;
 }
 
-static int overlayfb_map_video_memory(struct pxafb_layer *ofb)
+static int overlayfb_check_video_memory(struct pxafb_layer *ofb)
 {
 	struct fb_var_screeninfo *var = &ofb->fb.var;
 	int pfor = NONSTD_TO_PFOR(var->nonstd);
@@ -812,27 +823,11 @@ static int overlayfb_map_video_memory(struct pxafb_layer *ofb)
 
 	size = PAGE_ALIGN(ofb->fb.fix.line_length * var->yres_virtual);
 
-	/* don't re-allocate if the original video memory is enough */
 	if (ofb->video_mem) {
 		if (ofb->video_mem_size >= size)
 			return 0;
-
-		free_pages_exact(ofb->video_mem, ofb->video_mem_size);
 	}
-
-	ofb->video_mem = alloc_pages_exact(size, GFP_KERNEL | __GFP_ZERO);
-	if (ofb->video_mem == NULL)
-		return -ENOMEM;
-
-	ofb->video_mem_phys = virt_to_phys(ofb->video_mem);
-	ofb->video_mem_size = size;
-
-	mutex_lock(&ofb->fb.mm_lock);
-	ofb->fb.fix.smem_start	= ofb->video_mem_phys;
-	ofb->fb.fix.smem_len	= ofb->fb.fix.line_length * var->yres_virtual;
-	mutex_unlock(&ofb->fb.mm_lock);
-	ofb->fb.screen_base	= ofb->video_mem;
-	return 0;
+	return -EINVAL;
 }
 
 static int overlayfb_set_par(struct fb_info *info)
@@ -841,13 +836,13 @@ static int overlayfb_set_par(struct fb_info *info)
 	struct fb_var_screeninfo *var = &info->var;
 	int xpos, ypos, pfor, bpp, ret;
 
-	ret = overlayfb_map_video_memory(ofb);
+	ret = overlayfb_check_video_memory(ofb);
 	if (ret)
 		return ret;
 
 	bpp  = pxafb_var_to_bpp(var);
 	xpos = NONSTD_TO_XPOS(var->nonstd);
-	ypos = NONSTD_TO_XPOS(var->nonstd);
+	ypos = NONSTD_TO_YPOS(var->nonstd);
 	pfor = NONSTD_TO_PFOR(var->nonstd);
 
 	ofb->control[0] = OVLxC1_PPL(var->xres) | OVLxC1_LPO(var->yres) |
@@ -891,7 +886,7 @@ static void __devinit init_pxafb_overlay(struct pxafb_info *fbi,
 
 	ofb->id = id;
 	ofb->ops = &ofb_ops[id];
-	atomic_set(&ofb->usage, 0);
+	ofb->usage = 0;
 	ofb->fbi = fbi;
 	init_completion(&ofb->branch_done);
 }
@@ -904,29 +899,60 @@ static inline int pxafb_overlay_supported(void)
 	return 0;
 }
 
-static int __devinit pxafb_overlay_init(struct pxafb_info *fbi)
+static int __devinit pxafb_overlay_map_video_memory(struct pxafb_info *pxafb,
+	struct pxafb_layer *ofb)
+{
+	/* We assume that user will use at most video_mem_size for overlay fb,
+	 * anyway, it's useless to use 16bpp main plane and 24bpp overlay
+	 */
+	ofb->video_mem = alloc_pages_exact(PAGE_ALIGN(pxafb->video_mem_size),
+		GFP_KERNEL | __GFP_ZERO);
+	if (ofb->video_mem == NULL)
+		return -ENOMEM;
+
+	ofb->video_mem_phys = virt_to_phys(ofb->video_mem);
+	ofb->video_mem_size = PAGE_ALIGN(pxafb->video_mem_size);
+
+	mutex_lock(&ofb->fb.mm_lock);
+	ofb->fb.fix.smem_start	= ofb->video_mem_phys;
+	ofb->fb.fix.smem_len	= pxafb->video_mem_size;
+	mutex_unlock(&ofb->fb.mm_lock);
+
+	ofb->fb.screen_base	= ofb->video_mem;
+
+	return 0;
+}
+
+static void __devinit pxafb_overlay_init(struct pxafb_info *fbi)
 {
 	int i, ret;
 
 	if (!pxafb_overlay_supported())
-		return 0;
+		return;
 
 	for (i = 0; i < 2; i++) {
-		init_pxafb_overlay(fbi, &fbi->overlay[i], i);
-		ret = register_framebuffer(&fbi->overlay[i].fb);
+		struct pxafb_layer *ofb = &fbi->overlay[i];
+		init_pxafb_overlay(fbi, ofb, i);
+		ret = register_framebuffer(&ofb->fb);
 		if (ret) {
 			dev_err(fbi->dev, "failed to register overlay %d\n", i);
-			return ret;
+			continue;
 		}
+		ret = pxafb_overlay_map_video_memory(fbi, ofb);
+		if (ret) {
+			dev_err(fbi->dev,
+				"failed to map video memory for overlay %d\n",
+				i);
+			unregister_framebuffer(&ofb->fb);
+			continue;
+		}
+		ofb->registered = 1;
 	}
 
 	/* mask all IU/BS/EOF/SOF interrupts */
 	lcd_writel(fbi, LCCR5, ~0);
 
-	/* place overlay(s) on top of base */
-	fbi->lccr0 |= LCCR0_OUC;
 	pr_info("PXA Overlay driver loaded successfully!\n");
-	return 0;
 }
 
 static void __devexit pxafb_overlay_exit(struct pxafb_info *fbi)
@@ -936,8 +962,15 @@ static void __devexit pxafb_overlay_exit(struct pxafb_info *fbi)
 	if (!pxafb_overlay_supported())
 		return;
 
-	for (i = 0; i < 2; i++)
-		unregister_framebuffer(&fbi->overlay[i].fb);
+	for (i = 0; i < 2; i++) {
+		struct pxafb_layer *ofb = &fbi->overlay[i];
+		if (ofb->registered) {
+			if (ofb->video_mem)
+				free_pages_exact(ofb->video_mem,
+					ofb->video_mem_size);
+			unregister_framebuffer(&ofb->fb);
+		}
+	}
 }
 #else
 static inline void pxafb_overlay_init(struct pxafb_info *fbi) {}
@@ -1368,7 +1401,8 @@ static int pxafb_activate_var(struct fb_var_screeninfo *var,
 	    (lcd_readl(fbi, LCCR3) != fbi->reg_lccr3) ||
 	    (lcd_readl(fbi, LCCR4) != fbi->reg_lccr4) ||
 	    (lcd_readl(fbi, FDADR0) != fbi->fdadr[0]) ||
-	    (lcd_readl(fbi, FDADR1) != fbi->fdadr[1]))
+	    ((fbi->lccr0 & LCCR0_SDS) &&
+	    (lcd_readl(fbi, FDADR1) != fbi->fdadr[1])))
 		pxafb_schedule_work(fbi, C_REENABLE);
 
 	return 0;
@@ -1420,7 +1454,8 @@ static void pxafb_enable_controller(struct pxafb_info *fbi)
 	lcd_writel(fbi, LCCR0, fbi->reg_lccr0 & ~LCCR0_ENB);
 
 	lcd_writel(fbi, FDADR0, fbi->fdadr[0]);
-	lcd_writel(fbi, FDADR1, fbi->fdadr[1]);
+	if (fbi->lccr0 & LCCR0_SDS)
+		lcd_writel(fbi, FDADR1, fbi->fdadr[1]);
 	lcd_writel(fbi, LCCR0, fbi->reg_lccr0 | LCCR0_ENB);
 }
 
@@ -1613,7 +1648,8 @@ pxafb_freq_transition(struct notifier_block *nb, unsigned long val, void *data)
 
 	switch (val) {
 	case CPUFREQ_PRECHANGE:
-		set_ctrlr_state(fbi, C_DISABLE_CLKCHANGE);
+		if (!fbi->overlay[0].usage && !fbi->overlay[1].usage)
+			set_ctrlr_state(fbi, C_DISABLE_CLKCHANGE);
 		break;
 
 	case CPUFREQ_POSTCHANGE:
@@ -1806,6 +1842,12 @@ static struct pxafb_info * __devinit pxafb_init_fbinfo(struct device *dev)
 
 	pxafb_decode_mach_info(fbi, inf);
 
+#ifdef CONFIG_FB_PXA_OVERLAY
+	/* place overlay(s) on top of base */
+	if (pxafb_overlay_supported())
+		fbi->lccr0 |= LCCR0_OUC;
+#endif
+
 	init_waitqueue_head(&fbi->ctrlr_wait);
 	INIT_WORK(&fbi->task, pxafb_task);
 	mutex_init(&fbi->ctrlr_lock);
diff --git a/drivers/video/pxafb.h b/drivers/video/pxafb.h
index 2353521c5c8..26ba9fa3f73 100644
--- a/drivers/video/pxafb.h
+++ b/drivers/video/pxafb.h
@@ -92,7 +92,8 @@ struct pxafb_layer_ops {
 struct pxafb_layer {
 	struct fb_info		fb;
 	int			id;
-	atomic_t		usage;
+	int			registered;
+	uint32_t		usage;
 	uint32_t		control[2];
 
 	struct pxafb_layer_ops	*ops;