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

79 files changed, 880 insertions, 175 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index 93d595a7477a..4f8f3a4e4e0d 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 833306ee9e7f..ea297ac70bc6 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 5eec099e0c72..56b930a13443 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 cb705c28de02..6269053505f7 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 80761474c0f8..2e288b38b4ad 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 b3ecfb22d241..036ba1a4704b 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 83ee08847d4d..159340da9191 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 4f63048be3ca..0b9338cec516 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 b2ecd194e76d..bea4e4135f9d 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 7b5735c5ea59..2af3f43f74db 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 0a18f8d23eb0..10a1bea10548 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 652ace413825..fdbc05ed5513 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 df46b5e60857..472bdfab2e55 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 c372a4373691..e6c1119c20ae 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 51a67fc7f0ef..d0c7075937cf 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 000000000000..76ae8dc33e00
--- /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 4f6f174af6c8..4522fbb235d5 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 d133c7f30940..c3577ea789ac 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 5e489a2b2023..1ad97fed1e94 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 c7e14f4e3669..c6f345febd39 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 9d08555c4cf0..c473eddce8cf 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 1451ad060d82..acf9eea124c0 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 0d9bea30b0a2..324f2824d38d 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 000000000000..382dacbeca21
--- /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 71f24484b044..c5137f14c364 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 a66994f0518f..214e5b641bbc 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 08002d02267a..bb329b9a2608 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 fa0b154da67b..0fcff47009cf 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 df9e1b6e81bf..8ed425457d30 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 3499fada73ae..4cb069fd9af2 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 993d75e66390..fa8bad235d9f 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 d2af73321dae..c6661b1d30b2 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 b734d8468168..8225e2e58c6e 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 bfca7ed2fea3..7a8edba0dcdf 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 ee797397dc5b..44c1b77ece67 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 96b2d9fbfef0..3f9be419959d 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 d4e705caefea..3a5507e31919 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 c4bf08b3eb61..2e0425404de5 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 b411d7cbf5a1..f8a6e9d79a3a 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 edca0a043293..2e3970fdde0b 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 93f05e024313..d88aed8fbe15 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 6fd319ea5284..d65e4bde9b91 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 dd40e4a9291c..f7fb64f11a7d 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 2bbcf70dd935..0d4700a79612 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 160ec83f51a6..01a45ac48114 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 8835c16bc82f..7b0f71ef3167 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 87c1ed9ccd2f..e5e326d2cdc9 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 c9a3e775c2de..8aebc58c9f1d 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 dca20de306bb..12a2a56b2157 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 5535991c4a3c..a72993dde2b3 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 f9542220595a..8306b227e61c 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 78d98a8607ec..dd13bb63259b 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 72adb3ae2b43..325c245c0a0d 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 a09a2374697b..fb06bd047272 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 3f25014a136c..726f5b98dcd3 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 3010193b081e..3b8a4f37dbbe 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 9dbf3ccd4150..a70fb848dcde 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 35353af345d5..16d14fd79b4b 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 47094188e029..cd1861351f75 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 eb83c89428ef..fee97a935122 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 38e2c0912b9a..01c576963e94 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 9cecf8366db8..53d4a472b699 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 e7f64d9b4f2d..428da3ff33a5 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 5ad3807af334..5fa145778e7d 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 857bb2e63486..b9cfbebdfe9c 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 12279214c875..0fbe78df8461 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 e709fd459268..f71d377c8640 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 aaf883754ef4..fbe9e02e2f9f 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 730f51e57c17..5891590126f7 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 a4c784aab764..5821185f77ab 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 6561c9df5f0d..ccc789e21daa 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 10653cc8d1fa..805336fdc252 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 5cd6466964af..6fda788ed0e9 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 c4f116d214f2..7a9b20abda09 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 aaec2a6e7b3a..5f2da75a47f4 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 1eb339e6c857..dede19a766ff 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 7e072637eefa..1aea818d9d31 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 95be51bfe9a9..0417da9f710d 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 9f0c2610595e..2237ff8b434f 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;