234 files changed, 8861 insertions, 2080 deletions

diff --git a/arch/arm/Kconfig b/arch/arm/Kconfig
index b527bf5701c9..a19a5266d5fc 100644
--- a/arch/arm/Kconfig
+++ b/arch/arm/Kconfig
@@ -720,9 +720,11 @@ config ARCH_S5PC100
 config ARCH_S5PV210
         bool "Samsung S5PV210/S5PC110"
         select CPU_V7
+        select ARCH_SPARSEMEM_ENABLE
         select GENERIC_GPIO
         select HAVE_CLK
         select ARM_L1_CACHE_SHIFT_6
+        select ARCH_HAS_CPUFREQ
         select ARCH_USES_GETTIMEOFFSET
         select HAVE_S3C2410_I2C
         select HAVE_S3C_RTC
@@ -733,9 +735,13 @@ config ARCH_S5PV210
 config ARCH_S5PV310
         bool "Samsung S5PV310/S5PC210"
         select CPU_V7
+        select ARCH_SPARSEMEM_ENABLE
         select GENERIC_GPIO
         select HAVE_CLK
         select GENERIC_CLOCKEVENTS
+        select HAVE_S3C_RTC
+        select HAVE_S3C2410_I2C
+        select HAVE_S3C2410_WATCHDOG
         help
           Samsung S5PV310 series based systems
 
@@ -1662,6 +1668,12 @@ if ARCH_HAS_CPUFREQ
 
 source "drivers/cpufreq/Kconfig"
 
+config CPU_FREQ_IMX
+        tristate "CPUfreq driver for i.MX CPUs"
+        depends on ARCH_MXC && CPU_FREQ
+        help
+          This enables the CPUfreq driver for i.MX CPUs.
+
 config CPU_FREQ_SA1100
         bool
 
diff --git a/arch/arm/configs/mx51_defconfig b/arch/arm/configs/mx51_defconfig
index 163cfee7644c..5c7a87260fab 100644
--- a/arch/arm/configs/mx51_defconfig
+++ b/arch/arm/configs/mx51_defconfig
@@ -82,6 +82,7 @@ CONFIG_FEC=y
 CONFIG_INPUT_FF_MEMLESS=m
 # CONFIG_INPUT_MOUSEDEV_PSAUX is not set
 CONFIG_INPUT_EVDEV=y
+CONFIG_KEYBOARD_GPIO=y
 CONFIG_INPUT_EVBUG=m
 CONFIG_MOUSE_PS2=m
 CONFIG_MOUSE_PS2_ELANTECH=y
diff --git a/arch/arm/include/asm/hardware/cache-l2x0.h b/arch/arm/include/asm/hardware/cache-l2x0.h
index 6bcba48800fe..cc42d5fdee17 100644
--- a/arch/arm/include/asm/hardware/cache-l2x0.h
+++ b/arch/arm/include/asm/hardware/cache-l2x0.h
@@ -21,9 +21,6 @@
 #define __ASM_ARM_HARDWARE_L2X0_H
 
 #define L2X0_CACHE_ID                   0x000
-#define   L2X0_CACHE_ID_PART_MASK       (0xf << 6)
-#define   L2X0_CACHE_ID_PART_L210       (1 << 6)
-#define   L2X0_CACHE_ID_PART_L310       (3 << 6)
 #define L2X0_CACHE_TYPE                 0x004
 #define L2X0_CTRL                       0x100
 #define L2X0_AUX_CTRL                   0x104
@@ -53,6 +50,16 @@
 #define L2X0_LINE_DATA                  0xF10
 #define L2X0_LINE_TAG                   0xF30
 #define L2X0_DEBUG_CTRL                 0xF40
+#define L2X0_PREFETCH_CTRL              0xF60
+#define L2X0_POWER_CTRL                 0xF80
+#define   L2X0_DYNAMIC_CLK_GATING_EN    (1 << 1)
+#define   L2X0_STNDBY_MODE_EN           (1 << 0)
+
+/* Registers shifts and masks */
+#define L2X0_CACHE_ID_PART_MASK         (0xf << 6)
+#define L2X0_CACHE_ID_PART_L210         (1 << 6)
+#define L2X0_CACHE_ID_PART_L310         (3 << 6)
+#define L2X0_AUX_CTRL_WAY_SIZE_MASK     (0x3 << 17)
 
 #ifndef __ASSEMBLY__
 extern void __init l2x0_init(void __iomem *base, __u32 aux_val, __u32 aux_mask);
diff --git a/arch/arm/include/asm/memblock.h b/arch/arm/include/asm/memblock.h
index fdbc43b2e6c0..b8da2e415e4e 100644
--- a/arch/arm/include/asm/memblock.h
+++ b/arch/arm/include/asm/memblock.h
@@ -1,13 +1,6 @@
 #ifndef _ASM_ARM_MEMBLOCK_H
 #define _ASM_ARM_MEMBLOCK_H
 
-#ifdef CONFIG_MMU
-extern phys_addr_t lowmem_end_addr;
-#define MEMBLOCK_REAL_LIMIT     lowmem_end_addr
-#else
-#define MEMBLOCK_REAL_LIMIT     0
-#endif
-
 struct meminfo;
 struct machine_desc;
 
diff --git a/arch/arm/include/asm/outercache.h b/arch/arm/include/asm/outercache.h
index 25f76bae57ab..fc1900925275 100644
--- a/arch/arm/include/asm/outercache.h
+++ b/arch/arm/include/asm/outercache.h
@@ -25,6 +25,9 @@ struct outer_cache_fns {
         void (*inv_range)(unsigned long, unsigned long);
         void (*clean_range)(unsigned long, unsigned long);
         void (*flush_range)(unsigned long, unsigned long);
+        void (*flush_all)(void);
+        void (*inv_all)(void);
+        void (*disable)(void);
 #ifdef CONFIG_OUTER_CACHE_SYNC
         void (*sync)(void);
 #endif
@@ -50,6 +53,24 @@ static inline void outer_flush_range(unsigned long start, unsigned long end)
                 outer_cache.flush_range(start, end);
 }
 
+static inline void outer_flush_all(void)
+{
+        if (outer_cache.flush_all)
+                outer_cache.flush_all();
+}
+
+static inline void outer_inv_all(void)
+{
+        if (outer_cache.inv_all)
+                outer_cache.inv_all();
+}
+
+static inline void outer_disable(void)
+{
+        if (outer_cache.disable)
+                outer_cache.disable();
+}
+
 #else
 
 static inline void outer_inv_range(unsigned long start, unsigned long end)
@@ -58,6 +79,9 @@ static inline void outer_clean_range(unsigned long start, unsigned long end)
 { }
 static inline void outer_flush_range(unsigned long start, unsigned long end)
 { }
+static inline void outer_flush_all(void) { }
+static inline void outer_inv_all(void) { }
+static inline void outer_disable(void) { }
 
 #endif
 
diff --git a/arch/arm/kernel/machine_kexec.c b/arch/arm/kernel/machine_kexec.c
index 1fc74cbd1a19..3a8fd5140d7a 100644
--- a/arch/arm/kernel/machine_kexec.c
+++ b/arch/arm/kernel/machine_kexec.c
@@ -78,7 +78,10 @@ void machine_kexec(struct kimage *image)
         local_fiq_disable();
         setup_mm_for_reboot(0); /* mode is not used, so just pass 0*/
         flush_cache_all();
+        outer_flush_all();
+        outer_disable();
         cpu_proc_fin();
+        outer_inv_all();
         flush_cache_all();
         cpu_reset(reboot_code_buffer_phys);
 }
diff --git a/arch/arm/kernel/vmlinux.lds.S b/arch/arm/kernel/vmlinux.lds.S
index 1953e3d21abf..cead8893b46b 100644
--- a/arch/arm/kernel/vmlinux.lds.S
+++ b/arch/arm/kernel/vmlinux.lds.S
@@ -113,6 +113,7 @@ SECTIONS
                         *(.rodata.*)
                         *(.glue_7)
                         *(.glue_7t)
+                . = ALIGN(4);
                 *(.got)                 /* Global offset table          */
                         ARM_CPU_KEEP(PROC_INFO)
         }
diff --git a/arch/arm/mach-imx/mach-mx27_3ds.c b/arch/arm/mach-imx/mach-mx27_3ds.c
index b8bbd31aa850..84a5ba03f1ba 100644
--- a/arch/arm/mach-imx/mach-mx27_3ds.c
+++ b/arch/arm/mach-imx/mach-mx27_3ds.c
@@ -23,16 +23,20 @@
 #include <linux/platform_device.h>
 #include <linux/gpio.h>
 #include <linux/input/matrix_keypad.h>
+#include <linux/irq.h>
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
 #include <asm/mach/time.h>
 #include <mach/hardware.h>
 #include <mach/common.h>
 #include <mach/iomux-mx27.h>
+#include <mach/mmc.h>
 
 #include "devices-imx27.h"
 #include "devices.h"
 
+#define SD1_EN_GPIO (GPIO_PORTB + 25)
+
 static const int mx27pdk_pins[] __initconst = {
         /* UART1 */
         PE12_PF_UART1_TXD,
@@ -58,6 +62,14 @@ static const int mx27pdk_pins[] __initconst = {
         PD15_AOUT_FEC_COL,
         PD16_AIN_FEC_TX_ER,
         PF23_AIN_FEC_TX_EN,
+        /* SDHC1 */
+        PE18_PF_SD1_D0,
+        PE19_PF_SD1_D1,
+        PE20_PF_SD1_D2,
+        PE21_PF_SD1_D3,
+        PE22_PF_SD1_CMD,
+        PE23_PF_SD1_CLK,
+        SD1_EN_GPIO | GPIO_GPIO | GPIO_OUT,
 };
 
 static const struct imxuart_platform_data uart_pdata __initconst = {
@@ -85,13 +97,39 @@ static struct matrix_keymap_data mx27_3ds_keymap_data = {
         .keymap_size    = ARRAY_SIZE(mx27_3ds_keymap),
 };
 
+static int mx27_3ds_sdhc1_init(struct device *dev, irq_handler_t detect_irq,
+                                void *data)
+{
+        return request_irq(IRQ_GPIOB(26), detect_irq, IRQF_TRIGGER_FALLING |
+                        IRQF_TRIGGER_RISING, "sdhc1-card-detect", data);
+}
+
+static void mx27_3ds_sdhc1_exit(struct device *dev, void *data)
+{
+        free_irq(IRQ_GPIOB(26), data);
+}
+
+static struct imxmmc_platform_data sdhc1_pdata = {
+        .init = mx27_3ds_sdhc1_init,
+        .exit = mx27_3ds_sdhc1_exit,
+};
+
+static void mx27_3ds_sdhc1_enable_level_translator(void)
+{
+        /* Turn on TXB0108 OE pin */
+        gpio_request(SD1_EN_GPIO, "sd1_enable");
+        gpio_direction_output(SD1_EN_GPIO, 1);
+}
+
 static void __init mx27pdk_init(void)
 {
         mxc_gpio_setup_multiple_pins(mx27pdk_pins, ARRAY_SIZE(mx27pdk_pins),
                 "mx27pdk");
+        mx27_3ds_sdhc1_enable_level_translator();
         imx27_add_imx_uart0(&uart_pdata);
         imx27_add_fec(NULL);
         mxc_register_device(&imx_kpp_device, &mx27_3ds_keymap_data);
+        mxc_register_device(&mxc_sdhc_device0, &sdhc1_pdata);
 }
 
 static void __init mx27pdk_timer_init(void)
diff --git a/arch/arm/mach-ixp2000/core.c b/arch/arm/mach-ixp2000/core.c
index babb22597163..e24e3d05397f 100644
--- a/arch/arm/mach-ixp2000/core.c
+++ b/arch/arm/mach-ixp2000/core.c
@@ -197,7 +197,7 @@ unsigned long ixp2000_gettimeoffset (void)
         return offset / ticks_per_usec;
 }
 
-static int ixp2000_timer_interrupt(int irq, void *dev_id)
+static irqreturn_t ixp2000_timer_interrupt(int irq, void *dev_id)
 {
         /* clear timer 1 */
         ixp2000_reg_wrb(IXP2000_T1_CLR, 1);
diff --git a/arch/arm/mach-mx25/Kconfig b/arch/arm/mach-mx25/Kconfig
index aa57e35ce3cd..38ca09a5df9d 100644
--- a/arch/arm/mach-mx25/Kconfig
+++ b/arch/arm/mach-mx25/Kconfig
@@ -6,6 +6,7 @@ config MACH_MX25_3DS
         bool "Support MX25PDK (3DS) Platform"
         select IMX_HAVE_PLATFORM_IMX_UART
         select IMX_HAVE_PLATFORM_MXC_NAND
+        select IMX_HAVE_PLATFORM_ESDHC
 
 config MACH_EUKREA_CPUIMX25
         bool "Support Eukrea CPUIMX25 Platform"
diff --git a/arch/arm/mach-mx25/mach-mx25_3ds.c b/arch/arm/mach-mx25/mach-mx25_3ds.c
index 80805107a73e..f8be1eb0c062 100644
--- a/arch/arm/mach-mx25/mach-mx25_3ds.c
+++ b/arch/arm/mach-mx25/mach-mx25_3ds.c
@@ -96,6 +96,14 @@ static struct pad_desc mx25pdk_pads[] = {
         MX25_PAD_KPP_COL1__KPP_COL1,
         MX25_PAD_KPP_COL2__KPP_COL2,
         MX25_PAD_KPP_COL3__KPP_COL3,
+
+        /* SD1 */
+        MX25_PAD_SD1_CMD__SD1_CMD,
+        MX25_PAD_SD1_CLK__SD1_CLK,
+        MX25_PAD_SD1_DATA0__SD1_DATA0,
+        MX25_PAD_SD1_DATA1__SD1_DATA1,
+        MX25_PAD_SD1_DATA2__SD1_DATA2,
+        MX25_PAD_SD1_DATA3__SD1_DATA3,
 };
 
 static const struct fec_platform_data mx25_fec_pdata __initconst = {
@@ -193,6 +201,8 @@ static void __init mx25pdk_init(void)
         mx25pdk_fec_reset();
         imx25_add_fec(&mx25_fec_pdata);
         mxc_register_device(&mx25_kpp_device, &mx25pdk_keymap_data);
+
+        imx25_add_esdhc(0, NULL);
 }
 
 static void __init mx25pdk_timer_init(void)
diff --git a/arch/arm/mach-mx3/Kconfig b/arch/arm/mach-mx3/Kconfig
index 096fd33f8ab9..5000ac1f93e3 100644
--- a/arch/arm/mach-mx3/Kconfig
+++ b/arch/arm/mach-mx3/Kconfig
@@ -143,8 +143,10 @@ config MACH_ARMADILLO5X0
 config MACH_MX35_3DS
         bool "Support MX35PDK platform"
         select ARCH_MX35
+        select MXC_DEBUG_BOARD
         select IMX_HAVE_PLATFORM_IMX_UART
         select IMX_HAVE_PLATFORM_MXC_NAND
+        select IMX_HAVE_PLATFORM_ESDHC
         default n
         help
           Include support for MX35PDK platform. This includes specific
diff --git a/arch/arm/mach-mx3/devices.c b/arch/arm/mach-mx3/devices.c
index f4dff11aaee7..d4da9496089a 100644
--- a/arch/arm/mach-mx3/devices.c
+++ b/arch/arm/mach-mx3/devices.c
@@ -72,24 +72,24 @@ struct platform_device mxc_w1_master_device = {
 #ifdef CONFIG_ARCH_MX31
 static struct resource mxcsdhc0_resources[] = {
         {
-                .start = MMC_SDHC1_BASE_ADDR,
-                .end = MMC_SDHC1_BASE_ADDR + SZ_16K - 1,
+                .start = MX31_MMC_SDHC1_BASE_ADDR,
+                .end = MX31_MMC_SDHC1_BASE_ADDR + SZ_16K - 1,
                 .flags = IORESOURCE_MEM,
         }, {
-                .start = MXC_INT_MMC_SDHC1,
-                .end = MXC_INT_MMC_SDHC1,
+                .start = MX31_INT_MMC_SDHC1,
+                .end = MX31_INT_MMC_SDHC1,
                 .flags = IORESOURCE_IRQ,
         },
 };
 
 static struct resource mxcsdhc1_resources[] = {
         {
-                .start = MMC_SDHC2_BASE_ADDR,
-                .end = MMC_SDHC2_BASE_ADDR + SZ_16K - 1,
+                .start = MX31_MMC_SDHC2_BASE_ADDR,
+                .end = MX31_MMC_SDHC2_BASE_ADDR + SZ_16K - 1,
                 .flags = IORESOURCE_MEM,
         }, {
-                .start = MXC_INT_MMC_SDHC2,
-                .end = MXC_INT_MMC_SDHC2,
+                .start = MX31_INT_MMC_SDHC2,
+                .end = MX31_INT_MMC_SDHC2,
                 .flags = IORESOURCE_IRQ,
         },
 };
diff --git a/arch/arm/mach-mx3/mach-mx31_3ds.c b/arch/arm/mach-mx3/mach-mx31_3ds.c
index 5c1d0e86c91e..0ad9e7821082 100644
--- a/arch/arm/mach-mx3/mach-mx31_3ds.c
+++ b/arch/arm/mach-mx3/mach-mx31_3ds.c
@@ -38,39 +38,9 @@
 #include "devices-imx31.h"
 #include "devices.h"
 
-/* Definitions for components on the Debug board */
-
-/* Base address of CPLD controller on the Debug board */
-#define DEBUG_BASE_ADDRESS              CS5_IO_ADDRESS(MX3x_CS5_BASE_ADDR)
-
-/* LAN9217 ethernet base address */
-#define LAN9217_BASE_ADDR               MX3x_CS5_BASE_ADDR
-
-/* CPLD config and interrupt base address */
-#define CPLD_ADDR                       (DEBUG_BASE_ADDRESS + 0x20000)
-
-/* status, interrupt */
-#define CPLD_INT_STATUS_REG             (CPLD_ADDR + 0x10)
-#define CPLD_INT_MASK_REG               (CPLD_ADDR + 0x38)
-#define CPLD_INT_RESET_REG              (CPLD_ADDR + 0x20)
-/* magic word for debug CPLD */
-#define CPLD_MAGIC_NUMBER1_REG          (CPLD_ADDR + 0x40)
-#define CPLD_MAGIC_NUMBER2_REG          (CPLD_ADDR + 0x48)
-/* CPLD code version */
-#define CPLD_CODE_VER_REG               (CPLD_ADDR + 0x50)
-/* magic word for debug CPLD */
-#define CPLD_MAGIC_NUMBER3_REG          (CPLD_ADDR + 0x58)
-
 /* CPLD IRQ line for external uart, external ethernet etc */
 #define EXPIO_PARENT_INT        IOMUX_TO_IRQ(MX31_PIN_GPIO1_1)
 
-#define MXC_EXP_IO_BASE         (MXC_BOARD_IRQ_START)
-#define MXC_IRQ_TO_EXPIO(irq)   ((irq) - MXC_EXP_IO_BASE)
-
-#define EXPIO_INT_ENET          (MXC_EXP_IO_BASE + 0)
-
-#define MXC_MAX_EXP_IO_LINES    16
-
 /*
  * This file contains the board-specific initialization routines.
  */
@@ -272,7 +242,7 @@ static void __init mxc_board_init(void)
         imx31_add_imx_uart0(&uart_pdata);
         imx31_add_mxc_nand(&mx31_3ds_nand_board_info);
 
-        imx31_add_spi_imx0(&spi1_pdata);
+        imx31_add_spi_imx1(&spi1_pdata);
         spi_register_board_info(mx31_3ds_spi_devs,
                                                 ARRAY_SIZE(mx31_3ds_spi_devs));
 
@@ -281,9 +251,9 @@ static void __init mxc_board_init(void)
         mx31_3ds_usbotg_init();
         mxc_register_device(&mxc_otg_udc_device, &usbotg_pdata);
 
-        if (!mxc_expio_init(CS5_BASE_ADDR, EXPIO_PARENT_INT))
-                printk(KERN_WARNING "Init of the debugboard failed, all "
-                                    "devices on the board are unusable.\n");
+        if (mxc_expio_init(MX31_CS5_BASE_ADDR, EXPIO_PARENT_INT))
+                printk(KERN_WARNING "Init of the debug board failed, all "
+                                    "devices on the debug board are unusable.\n");
 }
 
 static void __init mx31_3ds_timer_init(void)
diff --git a/arch/arm/mach-mx3/mach-mx35_3ds.c b/arch/arm/mach-mx3/mach-mx35_3ds.c
index 05f628d90725..b66a75aa2e88 100644
--- a/arch/arm/mach-mx3/mach-mx35_3ds.c
+++ b/arch/arm/mach-mx3/mach-mx35_3ds.c
@@ -38,11 +38,15 @@
 #include <mach/hardware.h>
 #include <mach/common.h>
 #include <mach/iomux-mx35.h>
+#include <mach/irqs.h>
+#include <mach/3ds_debugboard.h>
 #include <mach/mxc_ehci.h>
 
 #include "devices-imx35.h"
 #include "devices.h"
 
+#define EXPIO_PARENT_INT        (MXC_INTERNAL_IRQS + GPIO_PORTA + 1)
+
 static const struct imxuart_platform_data uart_pdata __initconst = {
         .flags = IMXUART_HAVE_RTSCTS,
 };
@@ -108,6 +112,13 @@ static struct pad_desc mx35pdk_pads[] = {
         /* USBH1 */
         MX35_PAD_I2C2_CLK__USB_TOP_USBH2_PWR,
         MX35_PAD_I2C2_DAT__USB_TOP_USBH2_OC,
+        /* SDCARD */
+        MX35_PAD_SD1_CMD__ESDHC1_CMD,
+        MX35_PAD_SD1_CLK__ESDHC1_CLK,
+        MX35_PAD_SD1_DATA0__ESDHC1_DAT0,
+        MX35_PAD_SD1_DATA1__ESDHC1_DAT1,
+        MX35_PAD_SD1_DATA2__ESDHC1_DAT2,
+        MX35_PAD_SD1_DATA3__ESDHC1_DAT3,
 };
 
 /* OTG config */
@@ -140,6 +151,11 @@ static void __init mxc_board_init(void)
         mxc_register_device(&mxc_usbh1, &usb_host_pdata);
 
         imx35_add_mxc_nand(&mx35pdk_nand_board_info);
+        imx35_add_esdhc(0, NULL);
+
+        if (mxc_expio_init(MX35_CS5_BASE_ADDR, EXPIO_PARENT_INT))
+                pr_warn("Init of the debugboard failed, all "
+                                "devices on the debugboard are unusable.\n");
 }
 
 static void __init mx35pdk_timer_init(void)
diff --git a/arch/arm/mach-mx5/Kconfig b/arch/arm/mach-mx5/Kconfig
index a2df9ac37996..3ec910a7a182 100644
--- a/arch/arm/mach-mx5/Kconfig
+++ b/arch/arm/mach-mx5/Kconfig
@@ -6,6 +6,7 @@ config ARCH_MX51
         select MXC_TZIC
         select ARCH_MXC_IOMUX_V3
         select ARCH_MXC_AUDMUX_V2
+        select ARCH_HAS_CPUFREQ
 
 comment "MX5 platforms:"
 
@@ -13,6 +14,7 @@ config MACH_MX51_BABBAGE
         bool "Support MX51 BABBAGE platforms"
         select IMX_HAVE_PLATFORM_IMX_I2C
         select IMX_HAVE_PLATFORM_IMX_UART
+        select IMX_HAVE_PLATFORM_ESDHC
         help
           Include support for MX51 Babbage platform, also known as MX51EVK in
           u-boot. This includes specific configurations for the board and its
diff --git a/arch/arm/mach-mx5/Makefile b/arch/arm/mach-mx5/Makefile
index 1769c161a60d..462f177eddfe 100644
--- a/arch/arm/mach-mx5/Makefile
+++ b/arch/arm/mach-mx5/Makefile
@@ -5,6 +5,7 @@
 # Object file lists.
 obj-y   := cpu.o mm.o clock-mx51.o devices.o
 
+obj-$(CONFIG_CPU_FREQ_IMX)    += cpu_op-mx51.o
 obj-$(CONFIG_MACH_MX51_BABBAGE) += board-mx51_babbage.o
 obj-$(CONFIG_MACH_MX51_3DS) += board-mx51_3ds.o
 obj-$(CONFIG_MACH_EUKREA_CPUIMX51) += board-cpuimx51.o
diff --git a/arch/arm/mach-mx5/board-mx51_babbage.c b/arch/arm/mach-mx5/board-mx51_babbage.c
index 0821fe9b3b27..acbe30df2e69 100644
--- a/arch/arm/mach-mx5/board-mx51_babbage.c
+++ b/arch/arm/mach-mx5/board-mx51_babbage.c
@@ -1,5 +1,5 @@
 /*
- * Copyright 2009 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2009-2010 Freescale Semiconductor, Inc. All Rights Reserved.
  * Copyright (C) 2009-2010 Amit Kucheria <amit.kucheria@canonical.com>
  *
  * The code contained herein is licensed under the GNU General Public
@@ -18,6 +18,8 @@
 #include <linux/io.h>
 #include <linux/fsl_devices.h>
 #include <linux/fec.h>
+#include <linux/gpio_keys.h>
+#include <linux/input.h>
 
 #include <mach/common.h>
 #include <mach/hardware.h>
@@ -32,11 +34,13 @@
 
 #include "devices-imx51.h"
 #include "devices.h"
+#include "cpu_op-mx51.h"
 
 #define BABBAGE_USB_HUB_RESET   (0*32 + 7)      /* GPIO_1_7 */
 #define BABBAGE_USBH1_STP       (0*32 + 27)     /* GPIO_1_27 */
 #define BABBAGE_PHY_RESET       (1*32 + 5)      /* GPIO_2_5 */
 #define BABBAGE_FEC_PHY_RESET   (1*32 + 14)     /* GPIO_2_14 */
+#define BABBAGE_POWER_KEY       (1*32 + 21)     /* GPIO_2_21 */
 
 /* USB_CTRL_1 */
 #define MX51_USB_CTRL_1_OFFSET                  0x10
@@ -46,6 +50,21 @@
 #define MX51_USB_PLL_DIV_19_2_MHZ       0x01
 #define MX51_USB_PLL_DIV_24_MHZ 0x02
 
+static struct gpio_keys_button babbage_buttons[] = {
+        {
+                .gpio           = BABBAGE_POWER_KEY,
+                .code           = BTN_0,
+                .desc           = "PWR",
+                .active_low     = 1,
+                .wakeup         = 1,
+        },
+};
+
+static const struct gpio_keys_platform_data imx_button_data __initconst = {
+        .buttons        = babbage_buttons,
+        .nbuttons       = ARRAY_SIZE(babbage_buttons),
+};
+
 static struct pad_desc mx51babbage_pads[] = {
         /* UART1 */
         MX51_PAD_UART1_RXD__UART1_RXD,
@@ -112,6 +131,22 @@ static struct pad_desc mx51babbage_pads[] = {
 
         /* FEC PHY reset line */
         MX51_PAD_EIM_A20__GPIO_2_14,
+
+        /* SD 1 */
+        MX51_PAD_SD1_CMD__SD1_CMD,
+        MX51_PAD_SD1_CLK__SD1_CLK,
+        MX51_PAD_SD1_DATA0__SD1_DATA0,
+        MX51_PAD_SD1_DATA1__SD1_DATA1,
+        MX51_PAD_SD1_DATA2__SD1_DATA2,
+        MX51_PAD_SD1_DATA3__SD1_DATA3,
+
+        /* SD 2 */
+        MX51_PAD_SD2_CMD__SD2_CMD,
+        MX51_PAD_SD2_CLK__SD2_CLK,
+        MX51_PAD_SD2_DATA0__SD2_DATA0,
+        MX51_PAD_SD2_DATA1__SD2_DATA1,
+        MX51_PAD_SD2_DATA2__SD2_DATA2,
+        MX51_PAD_SD2_DATA3__SD2_DATA3,
 };
 
 /* Serial ports */
@@ -281,13 +316,22 @@ __setup("otg_mode=", babbage_otg_mode);
 static void __init mxc_board_init(void)
 {
         struct pad_desc usbh1stp = MX51_PAD_USBH1_STP__USBH1_STP;
+        struct pad_desc power_key = MX51_PAD_EIM_A27__GPIO_2_21;
 
+#if defined(CONFIG_CPU_FREQ_IMX)
+        get_cpu_op = mx51_get_cpu_op;
+#endif
         mxc_iomux_v3_setup_multiple_pads(mx51babbage_pads,
                                         ARRAY_SIZE(mx51babbage_pads));
         mxc_init_imx_uart();
         babbage_fec_reset();
         imx51_add_fec(NULL);
 
+        /* Set the PAD settings for the pwr key. */
+        power_key.pad_ctrl = MX51_GPIO_PAD_CTRL_2;
+        mxc_iomux_v3_setup_pad(&power_key);
+        imx51_add_gpio_keys(&imx_button_data);
+
         imx51_add_imx_i2c(0, &babbage_i2c_data);
         imx51_add_imx_i2c(1, &babbage_i2c_data);
         mxc_register_device(&mxc_hsi2c_device, &babbage_hsi2c_data);
@@ -304,6 +348,9 @@ static void __init mxc_board_init(void)
         /* setback USBH1_STP to be function */
         mxc_iomux_v3_setup_pad(&usbh1stp);
         babbage_usbhub_reset();
+
+        imx51_add_esdhc(0, NULL);
+        imx51_add_esdhc(1, NULL);
 }
 
 static void __init mx51_babbage_timer_init(void)
diff --git a/arch/arm/mach-mx5/clock-mx51.c b/arch/arm/mach-mx5/clock-mx51.c
index f2aae92cf0e2..8ac36d882927 100644
--- a/arch/arm/mach-mx5/clock-mx51.c
+++ b/arch/arm/mach-mx5/clock-mx51.c
@@ -362,7 +362,7 @@ static int _clk_lp_apm_set_parent(struct clk *clk, struct clk *parent)
         return 0;
 }
 
-static unsigned long clk_arm_get_rate(struct clk *clk)
+static unsigned long clk_cpu_get_rate(struct clk *clk)
 {
         u32 cacrr, div;
         unsigned long parent_rate;
@@ -374,6 +374,22 @@ static unsigned long clk_arm_get_rate(struct clk *clk)
         return parent_rate / div;
 }
 
+static int clk_cpu_set_rate(struct clk *clk, unsigned long rate)
+{
+        u32 reg, cpu_podf;
+        unsigned long parent_rate;
+
+        parent_rate = clk_get_rate(clk->parent);
+        cpu_podf = parent_rate / rate - 1;
+        /* use post divider to change freq */
+        reg = __raw_readl(MXC_CCM_CACRR);
+        reg &= ~MXC_CCM_CACRR_ARM_PODF_MASK;
+        reg |= cpu_podf << MXC_CCM_CACRR_ARM_PODF_OFFSET;
+        __raw_writel(reg, MXC_CCM_CACRR);
+
+        return 0;
+}
+
 static int _clk_periph_apm_set_parent(struct clk *clk, struct clk *parent)
 {
         u32 reg, mux;
@@ -736,7 +752,8 @@ static struct clk periph_apm_clk = {
 
 static struct clk cpu_clk = {
         .parent = &pll1_sw_clk,
-        .get_rate = clk_arm_get_rate,
+        .get_rate = clk_cpu_get_rate,
+        .set_rate = clk_cpu_set_rate,
 };
 
 static struct clk ahb_clk = {
@@ -1064,6 +1081,7 @@ static struct clk_lookup lookups[] = {
         _REGISTER_CLOCK("imx51-cspi.0", NULL, cspi_clk)
         _REGISTER_CLOCK("sdhci-esdhc-imx.0", NULL, esdhc1_clk)
         _REGISTER_CLOCK("sdhci-esdhc-imx.1", NULL, esdhc2_clk)
+        _REGISTER_CLOCK(NULL, "cpu_clk", cpu_clk)
 };
 
 static void clk_tree_init(void)
diff --git a/arch/arm/mach-mx5/cpu_op-mx51.c b/arch/arm/mach-mx5/cpu_op-mx51.c
new file mode 100644
index 000000000000..9d34c3d4c024
--- /dev/null
+++ b/arch/arm/mach-mx5/cpu_op-mx51.c
@@ -0,0 +1,29 @@
+/*
+ * Copyright (C) 2010 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/types.h>
+#include <mach/hardware.h>
+#include <linux/kernel.h>
+
+static struct cpu_op mx51_cpu_op[] = {
+        {
+        .cpu_rate = 160000000,},
+        {
+        .cpu_rate = 800000000,},
+};
+
+struct cpu_op *mx51_get_cpu_op(int *op)
+{
+        *op = ARRAY_SIZE(mx51_cpu_op);
+        return mx51_cpu_op;
+}
diff --git a/arch/arm/mach-mx5/cpu_op-mx51.h b/arch/arm/mach-mx5/cpu_op-mx51.h
new file mode 100644
index 000000000000..97477fecb469
--- /dev/null
+++ b/arch/arm/mach-mx5/cpu_op-mx51.h
@@ -0,0 +1,14 @@
+/*
+ * Copyright (C) 2010 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
+ */
+
+extern struct cpu_op *mx51_get_cpu_op(int *op);
diff --git a/arch/arm/mach-mx5/devices-imx51.h b/arch/arm/mach-mx5/devices-imx51.h
index 5cc910e60538..8c50cb5d05f5 100644
--- a/arch/arm/mach-mx5/devices-imx51.h
+++ b/arch/arm/mach-mx5/devices-imx51.h
@@ -13,6 +13,8 @@ extern const struct imx_fec_data imx51_fec_data __initconst;
 #define imx51_add_fec(pdata)    \
         imx_add_fec(&imx51_fec_data, pdata)
 
+#define imx51_add_gpio_keys(pdata) imx_add_gpio_keys(pdata)
+
 extern const struct imx_imx_i2c_data imx51_imx_i2c_data[] __initconst;
 #define imx51_add_imx_i2c(id, pdata)    \
         imx_add_imx_i2c(&imx51_imx_i2c_data[id], pdata)
diff --git a/arch/arm/mach-omap2/omap4-common.c b/arch/arm/mach-omap2/omap4-common.c
index 923f9f5f91ce..2f895553e6a8 100644
--- a/arch/arm/mach-omap2/omap4-common.c
+++ b/arch/arm/mach-omap2/omap4-common.c
@@ -44,6 +44,13 @@ void __init gic_init_irq(void)
 }
 
 #ifdef CONFIG_CACHE_L2X0
+
+static void omap4_l2x0_disable(void)
+{
+        /* Disable PL310 L2 Cache controller */
+        omap_smc1(0x102, 0x0);
+}
+
 static int __init omap_l2_cache_init(void)
 {
         /*
@@ -70,6 +77,12 @@ static int __init omap_l2_cache_init(void)
         else
                 l2x0_init(l2cache_base, 0x0e070000, 0xc0000fff);
 
+        /*
+         * Override default outer_cache.disable with a OMAP4
+         * specific one
+        */
+        outer_cache.disable = omap4_l2x0_disable;
+
         return 0;
 }
 early_initcall(omap_l2_cache_init);
diff --git a/arch/arm/mach-s3c2410/include/mach/gpio.h b/arch/arm/mach-s3c2410/include/mach/gpio.h
index b649bf2ccd5c..f7f6b07df30e 100644
--- a/arch/arm/mach-s3c2410/include/mach/gpio.h
+++ b/arch/arm/mach-s3c2410/include/mach/gpio.h
@@ -22,6 +22,8 @@
 
 #ifdef CONFIG_CPU_S3C244X
 #define ARCH_NR_GPIOS   (32 * 9 + CONFIG_S3C24XX_GPIO_EXTRA)
+#elif defined(CONFIG_CPU_S3C2443) || defined(CONFIG_CPU_S3C2416)
+#define ARCH_NR_GPIOS   (32 * 12 + CONFIG_S3C24XX_GPIO_EXTRA)
 #else
 #define ARCH_NR_GPIOS   (256 + CONFIG_S3C24XX_GPIO_EXTRA)
 #endif
@@ -30,8 +32,10 @@
 #include <mach/gpio-nrs.h>
 #include <mach/gpio-fns.h>
 
-#ifdef CONFIG_CPU_S3C24XX
-#define S3C_GPIO_END    (S3C2410_GPIO_BANKJ + 32)
+#ifdef CONFIG_CPU_S3C244X
+#define S3C_GPIO_END    (S3C2410_GPJ(0) + 32)
+#elif defined(CONFIG_CPU_S3C2443) || defined(CONFIG_CPU_S3C2416)
+#define S3C_GPIO_END    (S3C2410_GPM(0) + 32)
 #else
-#define S3C_GPIO_END    (S3C2410_GPIO_BANKH + 32)
+#define S3C_GPIO_END    (S3C2410_GPH(0) + 32)
 #endif
diff --git a/arch/arm/mach-s3c2410/include/mach/regs-s3c2443-clock.h b/arch/arm/mach-s3c2410/include/mach/regs-s3c2443-clock.h
index 08ab9dfb6ae6..101aeea22310 100644
--- a/arch/arm/mach-s3c2410/include/mach/regs-s3c2443-clock.h
+++ b/arch/arm/mach-s3c2410/include/mach/regs-s3c2443-clock.h
@@ -118,6 +118,8 @@
 #define S3C2443_SCLKCON_UARTCLK         (1<<8)
 #define S3C2443_SCLKCON_USBHOST         (1<<1)
 
+#define S3C2443_PWRCFG_SLEEP            (1<<15)
+
 #include <asm/div64.h>
 
 static inline unsigned int
diff --git a/arch/arm/mach-s3c2410/include/mach/vmalloc.h b/arch/arm/mach-s3c2410/include/mach/vmalloc.h
index 54297eb0bf5e..7a311e8dddba 100644
--- a/arch/arm/mach-s3c2410/include/mach/vmalloc.h
+++ b/arch/arm/mach-s3c2410/include/mach/vmalloc.h
@@ -15,6 +15,6 @@
 #ifndef __ASM_ARCH_VMALLOC_H
 #define __ASM_ARCH_VMALLOC_H
 
-#define VMALLOC_END     0xE0000000UL
+#define VMALLOC_END     0xF6000000UL
 
 #endif /* __ASM_ARCH_VMALLOC_H */
diff --git a/arch/arm/mach-s3c2412/s3c2412.c b/arch/arm/mach-s3c2412/s3c2412.c
index bef39f77729d..4c6df51ddf33 100644
--- a/arch/arm/mach-s3c2412/s3c2412.c
+++ b/arch/arm/mach-s3c2412/s3c2412.c
@@ -51,6 +51,7 @@
 #include <plat/clock.h>
 #include <plat/pm.h>
 #include <plat/pll.h>
+#include <plat/nand-core.h>
 
 #ifndef CONFIG_CPU_S3C2412_ONLY
 void __iomem *s3c24xx_va_gpio2 = S3C24XX_VA_GPIO;
@@ -92,7 +93,7 @@ void __init s3c2412_init_uarts(struct s3c2410_uartcfg *cfg, int no)
         /* rename devices that are s3c2412/s3c2413 specific */
         s3c_device_sdi.name  = "s3c2412-sdi";
         s3c_device_lcd.name  = "s3c2412-lcd";
-        s3c_device_nand.name = "s3c2412-nand";
+        s3c_nand_setname("s3c2412-nand");
 
         /* alter IRQ of SDI controller */
 
diff --git a/arch/arm/mach-s3c2416/Kconfig b/arch/arm/mach-s3c2416/Kconfig
index 657e4fe17f39..87b9c9f003bd 100644
--- a/arch/arm/mach-s3c2416/Kconfig
+++ b/arch/arm/mach-s3c2416/Kconfig
@@ -25,6 +25,11 @@ config S3C2416_DMA
         help
           Internal config node for S3C2416 DMA support
 
+config S3C2416_PM
+        bool
+        help
+          Internal config node to apply S3C2416 power management
+
 menu "S3C2416 Machines"
 
 config MACH_SMDK2416
@@ -33,6 +38,7 @@ config MACH_SMDK2416
         select S3C_DEV_FB
         select S3C_DEV_HSMMC
         select S3C_DEV_HSMMC1
+        select S3C2416_PM if PM
         help
           Say Y here if you are using an SMDK2416
 
diff --git a/arch/arm/mach-s3c2416/Makefile b/arch/arm/mach-s3c2416/Makefile
index 6c12c7bf40ad..ef038d62ffdb 100644
--- a/arch/arm/mach-s3c2416/Makefile
+++ b/arch/arm/mach-s3c2416/Makefile
@@ -11,7 +11,7 @@ obj-				:=
 
 obj-$(CONFIG_CPU_S3C2416)       += s3c2416.o clock.o
 obj-$(CONFIG_CPU_S3C2416)       += irq.o
-
+obj-$(CONFIG_S3C2416_PM)        += pm.o
 #obj-$(CONFIG_S3C2416_DMA)      += dma.o
 
 # Machine support
diff --git a/arch/arm/mach-s3c2416/irq.c b/arch/arm/mach-s3c2416/irq.c
index 89f521d59d06..084d121f368c 100644
--- a/arch/arm/mach-s3c2416/irq.c
+++ b/arch/arm/mach-s3c2416/irq.c
@@ -243,6 +243,8 @@ static int __init s3c2416_irq_add(struct sys_device *sysdev)
 
 static struct sysdev_driver s3c2416_irq_driver = {
         .add            = s3c2416_irq_add,
+        .suspend        = s3c24xx_irq_suspend,
+        .resume         = s3c24xx_irq_resume,
 };
 
 static int __init s3c2416_irq_init(void)
diff --git a/arch/arm/mach-s3c2416/pm.c b/arch/arm/mach-s3c2416/pm.c
new file mode 100644
index 000000000000..4a04205b04d5
--- /dev/null
+++ b/arch/arm/mach-s3c2416/pm.c
@@ -0,0 +1,84 @@
+/* linux/arch/arm/mach-s3c2416/pm.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * S3C2416 - PM support (Based on Ben Dooks' S3C2412 PM support)
+ *
+ * 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/sysdev.h>
+#include <linux/io.h>
+
+#include <asm/cacheflush.h>
+
+#include <mach/regs-power.h>
+#include <mach/regs-s3c2443-clock.h>
+
+#include <plat/cpu.h>
+#include <plat/pm.h>
+
+extern void s3c2412_sleep_enter(void);
+
+static void s3c2416_cpu_suspend(void)
+{
+        flush_cache_all();
+
+        /* enable wakeup sources regardless of battery state */
+        __raw_writel(S3C2443_PWRCFG_SLEEP, S3C2443_PWRCFG);
+
+        /* set the mode as sleep, 2BED represents "Go to BED" */
+        __raw_writel(0x2BED, S3C2443_PWRMODE);
+
+        s3c2412_sleep_enter();
+}
+
+static void s3c2416_pm_prepare(void)
+{
+        /*
+         * write the magic value u-boot uses to check for resume into
+         * the INFORM0 register, and ensure INFORM1 is set to the
+         * correct address to resume from.
+         */
+        __raw_writel(0x2BED, S3C2412_INFORM0);
+        __raw_writel(virt_to_phys(s3c_cpu_resume), S3C2412_INFORM1);
+}
+
+static int s3c2416_pm_add(struct sys_device *sysdev)
+{
+        pm_cpu_prep = s3c2416_pm_prepare;
+        pm_cpu_sleep = s3c2416_cpu_suspend;
+
+        return 0;
+}
+
+static int s3c2416_pm_suspend(struct sys_device *dev, pm_message_t state)
+{
+        return 0;
+}
+
+static int s3c2416_pm_resume(struct sys_device *dev)
+{
+        /* unset the return-from-sleep amd inform flags */
+        __raw_writel(0x0, S3C2443_PWRMODE);
+        __raw_writel(0x0, S3C2412_INFORM0);
+        __raw_writel(0x0, S3C2412_INFORM1);
+
+        return 0;
+}
+
+static struct sysdev_driver s3c2416_pm_driver = {
+        .add            = s3c2416_pm_add,
+        .suspend        = s3c2416_pm_suspend,
+        .resume         = s3c2416_pm_resume,
+};
+
+static __init int s3c2416_pm_init(void)
+{
+        return sysdev_driver_register(&s3c2416_sysclass, &s3c2416_pm_driver);
+}
+
+arch_initcall(s3c2416_pm_init);
diff --git a/arch/arm/mach-s3c2416/s3c2416.c b/arch/arm/mach-s3c2416/s3c2416.c
index bc30245e133b..63f39cdc0972 100644
--- a/arch/arm/mach-s3c2416/s3c2416.c
+++ b/arch/arm/mach-s3c2416/s3c2416.c
@@ -56,6 +56,7 @@
 
 #include <plat/iic-core.h>
 #include <plat/fb-core.h>
+#include <plat/nand-core.h>
 
 static struct map_desc s3c2416_iodesc[] __initdata = {
         IODESC_ENT(WATCHDOG),
@@ -100,7 +101,7 @@ void __init s3c2416_init_uarts(struct s3c2410_uartcfg *cfg, int no)
 {
         s3c24xx_init_uartdevs("s3c2440-uart", s3c2410_uart_resources, cfg, no);
 
-        s3c_device_nand.name = "s3c2416-nand";
+        s3c_nand_setname("s3c2412-nand");
 }
 
 /* s3c2416_map_io
diff --git a/arch/arm/mach-s3c2440/s3c244x.c b/arch/arm/mach-s3c2440/s3c244x.c
index 5e4a97e76533..90c1707b9c95 100644
--- a/arch/arm/mach-s3c2440/s3c244x.c
+++ b/arch/arm/mach-s3c2440/s3c244x.c
@@ -44,6 +44,7 @@
 #include <plat/cpu.h>
 #include <plat/pm.h>
 #include <plat/pll.h>
+#include <plat/nand-core.h>
 
 static struct map_desc s3c244x_iodesc[] __initdata = {
         IODESC_ENT(CLKPWR),
@@ -68,7 +69,7 @@ void __init s3c244x_map_io(void)
 
         s3c_device_sdi.name  = "s3c2440-sdi";
         s3c_device_i2c0.name  = "s3c2440-i2c";
-        s3c_device_nand.name = "s3c2440-nand";
+        s3c_nand_setname("s3c2440-nand");
         s3c_device_ts.name = "s3c2440-ts";
         s3c_device_usbgadget.name = "s3c2440-usbgadget";
 }
diff --git a/arch/arm/mach-s3c2443/s3c2443.c b/arch/arm/mach-s3c2443/s3c2443.c
index 839b6b2ced74..33d18dd1ebd5 100644
--- a/arch/arm/mach-s3c2443/s3c2443.c
+++ b/arch/arm/mach-s3c2443/s3c2443.c
@@ -36,6 +36,7 @@
 #include <plat/devs.h>
 #include <plat/cpu.h>
 #include <plat/fb-core.h>
+#include <plat/nand-core.h>
 
 static struct map_desc s3c2443_iodesc[] __initdata = {
         IODESC_ENT(WATCHDOG),
@@ -62,7 +63,7 @@ int __init s3c2443_init(void)
 
         s3c24xx_reset_hook = s3c2443_hard_reset;
 
-        s3c_device_nand.name = "s3c2412-nand";
+        s3c_nand_setname("s3c2412-nand");
         s3c_fb_setname("s3c2443-fb");
 
         /* change WDT IRQ number */
diff --git a/arch/arm/mach-s3c24a0/include/mach/vmalloc.h b/arch/arm/mach-s3c24a0/include/mach/vmalloc.h
index 914656820794..6480b15277f3 100644
--- a/arch/arm/mach-s3c24a0/include/mach/vmalloc.h
+++ b/arch/arm/mach-s3c24a0/include/mach/vmalloc.h
@@ -12,6 +12,6 @@
 #ifndef __ASM_ARCH_VMALLOC_H
 #define __ASM_ARCH_VMALLOC_H
 
-#define VMALLOC_END       (0xe0000000UL)
+#define VMALLOC_END     0xF6000000UL
 
 #endif /* __ASM_ARCH_VMALLOC_H */
diff --git a/arch/arm/mach-s3c64xx/Kconfig b/arch/arm/mach-s3c64xx/Kconfig
index 546db5cb8929..1ca7bdc6485c 100644
--- a/arch/arm/mach-s3c64xx/Kconfig
+++ b/arch/arm/mach-s3c64xx/Kconfig
@@ -98,12 +98,33 @@ config MACH_ANW6410
         help
           Machine support for the A&W6410
 
+config MACH_MINI6410
+        bool "MINI6410"
+        select CPU_S3C6410
+        select S3C_DEV_HSMMC
+        select S3C_DEV_HSMMC1
+        select S3C64XX_SETUP_SDHCI
+        select S3C_DEV_USB_HOST
+        select S3C_DEV_NAND
+        select S3C_DEV_FB
+        select S3C64XX_SETUP_FB_24BPP
+        select SAMSUNG_DEV_ADC
+        select SAMSUNG_DEV_TS
+        help
+          Machine support for the FriendlyARM MINI6410
+
 config MACH_REAL6410
         bool "REAL6410"
         select CPU_S3C6410
         select S3C_DEV_HSMMC
         select S3C_DEV_HSMMC1
         select S3C64XX_SETUP_SDHCI
+        select S3C_DEV_FB
+        select S3C64XX_SETUP_FB_24BPP
+        select S3C_DEV_NAND
+        select SAMSUNG_DEV_ADC
+        select SAMSUNG_DEV_TS
+        select S3C_DEV_USB_HOST
         help
           Machine support for the CoreWind REAL6410
 
diff --git a/arch/arm/mach-s3c64xx/Makefile b/arch/arm/mach-s3c64xx/Makefile
index 90221a2e0c55..4657363f0674 100644
--- a/arch/arm/mach-s3c64xx/Makefile
+++ b/arch/arm/mach-s3c64xx/Makefile
@@ -53,6 +53,7 @@ obj-$(CONFIG_MACH_ANW6410)	+= mach-anw6410.o
 obj-$(CONFIG_MACH_SMDK6400)     += mach-smdk6400.o
 obj-$(CONFIG_MACH_SMDK6410)     += mach-smdk6410.o
 obj-$(CONFIG_MACH_REAL6410)     += mach-real6410.o
+obj-$(CONFIG_MACH_MINI6410)     += mach-mini6410.o
 obj-$(CONFIG_MACH_NCP)          += mach-ncp.o
 obj-$(CONFIG_MACH_HMT)          += mach-hmt.o
 obj-$(CONFIG_MACH_SMARTQ)       += mach-smartq.o
diff --git a/arch/arm/mach-s3c64xx/dev-audio.c b/arch/arm/mach-s3c64xx/dev-audio.c
index 3838335f125b..76426a32c013 100644
--- a/arch/arm/mach-s3c64xx/dev-audio.c
+++ b/arch/arm/mach-s3c64xx/dev-audio.c
@@ -22,27 +22,16 @@
 #include <plat/audio.h>
 #include <plat/gpio-cfg.h>
 
-#include <mach/gpio-bank-c.h>
-#include <mach/gpio-bank-d.h>
-#include <mach/gpio-bank-e.h>
-#include <mach/gpio-bank-h.h>
-
 static int s3c64xx_i2sv3_cfg_gpio(struct platform_device *pdev)
 {
+        unsigned int base;
+
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S3C64XX_GPD(0), S3C64XX_GPD0_I2S0_CLK);
-                s3c_gpio_cfgpin(S3C64XX_GPD(1), S3C64XX_GPD1_I2S0_CDCLK);
-                s3c_gpio_cfgpin(S3C64XX_GPD(2), S3C64XX_GPD2_I2S0_LRCLK);
-                s3c_gpio_cfgpin(S3C64XX_GPD(3), S3C64XX_GPD3_I2S0_DI);
-                s3c_gpio_cfgpin(S3C64XX_GPD(4), S3C64XX_GPD4_I2S0_D0);
+                base = S3C64XX_GPD(0);
                 break;
         case 1:
-                s3c_gpio_cfgpin(S3C64XX_GPE(0), S3C64XX_GPE0_I2S1_CLK);
-                s3c_gpio_cfgpin(S3C64XX_GPE(1), S3C64XX_GPE1_I2S1_CDCLK);
-                s3c_gpio_cfgpin(S3C64XX_GPE(2), S3C64XX_GPE2_I2S1_LRCLK);
-                s3c_gpio_cfgpin(S3C64XX_GPE(3), S3C64XX_GPE3_I2S1_DI);
-                s3c_gpio_cfgpin(S3C64XX_GPE(4), S3C64XX_GPE4_I2S1_D0);
+                base = S3C64XX_GPE(0);
                 break;
         default:
                 printk(KERN_DEBUG "Invalid I2S Controller number: %d\n",
@@ -50,18 +39,17 @@ static int s3c64xx_i2sv3_cfg_gpio(struct platform_device *pdev)
                 return -EINVAL;
         }
 
+        s3c_gpio_cfgpin_range(base, 5, S3C_GPIO_SFN(3));
+
         return 0;
 }
 
 static int s3c64xx_i2sv4_cfg_gpio(struct platform_device *pdev)
 {
-        s3c_gpio_cfgpin(S3C64XX_GPC(4), S3C64XX_GPC4_I2S_V40_DO0);
-        s3c_gpio_cfgpin(S3C64XX_GPC(5), S3C64XX_GPC5_I2S_V40_DO1);
-        s3c_gpio_cfgpin(S3C64XX_GPC(7), S3C64XX_GPC7_I2S_V40_DO2);
-        s3c_gpio_cfgpin(S3C64XX_GPH(6), S3C64XX_GPH6_I2S_V40_BCLK);
-        s3c_gpio_cfgpin(S3C64XX_GPH(7), S3C64XX_GPH7_I2S_V40_CDCLK);
-        s3c_gpio_cfgpin(S3C64XX_GPH(8), S3C64XX_GPH8_I2S_V40_LRCLK);
-        s3c_gpio_cfgpin(S3C64XX_GPH(9), S3C64XX_GPH9_I2S_V40_DI);
+        s3c_gpio_cfgpin(S3C64XX_GPC(4), S3C_GPIO_SFN(5));
+        s3c_gpio_cfgpin(S3C64XX_GPC(5), S3C_GPIO_SFN(5));
+        s3c_gpio_cfgpin(S3C64XX_GPC(7), S3C_GPIO_SFN(5));
+        s3c_gpio_cfgpin_range(S3C64XX_GPH(6), 4, S3C_GPIO_SFN(5));
 
         return 0;
 }
@@ -170,20 +158,14 @@ EXPORT_SYMBOL(s3c64xx_device_iisv4);
 
 static int s3c64xx_pcm_cfg_gpio(struct platform_device *pdev)
 {
+        unsigned int base;
+
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S3C64XX_GPD(0), S3C64XX_GPD0_PCM0_SCLK);
-                s3c_gpio_cfgpin(S3C64XX_GPD(1), S3C64XX_GPD1_PCM0_EXTCLK);
-                s3c_gpio_cfgpin(S3C64XX_GPD(2), S3C64XX_GPD2_PCM0_FSYNC);
-                s3c_gpio_cfgpin(S3C64XX_GPD(3), S3C64XX_GPD3_PCM0_SIN);
-                s3c_gpio_cfgpin(S3C64XX_GPD(4), S3C64XX_GPD4_PCM0_SOUT);
+                base = S3C64XX_GPD(0);
                 break;
         case 1:
-                s3c_gpio_cfgpin(S3C64XX_GPE(0), S3C64XX_GPE0_PCM1_SCLK);
-                s3c_gpio_cfgpin(S3C64XX_GPE(1), S3C64XX_GPE1_PCM1_EXTCLK);
-                s3c_gpio_cfgpin(S3C64XX_GPE(2), S3C64XX_GPE2_PCM1_FSYNC);
-                s3c_gpio_cfgpin(S3C64XX_GPE(3), S3C64XX_GPE3_PCM1_SIN);
-                s3c_gpio_cfgpin(S3C64XX_GPE(4), S3C64XX_GPE4_PCM1_SOUT);
+                base = S3C64XX_GPE(0);
                 break;
         default:
                 printk(KERN_DEBUG "Invalid PCM Controller number: %d\n",
@@ -191,6 +173,7 @@ static int s3c64xx_pcm_cfg_gpio(struct platform_device *pdev)
                 return -EINVAL;
         }
 
+        s3c_gpio_cfgpin_range(base, 5, S3C_GPIO_SFN(2));
         return 0;
 }
 
@@ -264,24 +247,12 @@ EXPORT_SYMBOL(s3c64xx_device_pcm1);
 
 static int s3c64xx_ac97_cfg_gpd(struct platform_device *pdev)
 {
-        s3c_gpio_cfgpin(S3C64XX_GPD(0), S3C64XX_GPD0_AC97_BITCLK);
-        s3c_gpio_cfgpin(S3C64XX_GPD(1), S3C64XX_GPD1_AC97_nRESET);
-        s3c_gpio_cfgpin(S3C64XX_GPD(2), S3C64XX_GPD2_AC97_SYNC);
-        s3c_gpio_cfgpin(S3C64XX_GPD(3), S3C64XX_GPD3_AC97_SDI);
-        s3c_gpio_cfgpin(S3C64XX_GPD(4), S3C64XX_GPD4_AC97_SDO);
-
-        return 0;
+        return s3c_gpio_cfgpin_range(S3C64XX_GPD(0), 5, S3C_GPIO_SFN(4));
 }
 
 static int s3c64xx_ac97_cfg_gpe(struct platform_device *pdev)
 {
-        s3c_gpio_cfgpin(S3C64XX_GPE(0), S3C64XX_GPE0_AC97_BITCLK);
-        s3c_gpio_cfgpin(S3C64XX_GPE(1), S3C64XX_GPE1_AC97_nRESET);
-        s3c_gpio_cfgpin(S3C64XX_GPE(2), S3C64XX_GPE2_AC97_SYNC);
-        s3c_gpio_cfgpin(S3C64XX_GPE(3), S3C64XX_GPE3_AC97_SDI);
-        s3c_gpio_cfgpin(S3C64XX_GPE(4), S3C64XX_GPE4_AC97_SDO);
-
-        return 0;
+        return s3c_gpio_cfgpin_range(S3C64XX_GPE(0), 5, S3C_GPIO_SFN(4));
 }
 
 static struct resource s3c64xx_ac97_resource[] = {
diff --git a/arch/arm/mach-s3c64xx/gpiolib.c b/arch/arm/mach-s3c64xx/gpiolib.c
index 300dee4a667b..fd99a82e82c4 100644
--- a/arch/arm/mach-s3c64xx/gpiolib.c
+++ b/arch/arm/mach-s3c64xx/gpiolib.c
@@ -195,11 +195,6 @@ static struct s3c_gpio_cfg gpio_2bit_cfg_eint11 = {
         .get_pull       = s3c_gpio_getpull_updown,
 };
 
-int s3c64xx_gpio2int_gpn(struct gpio_chip *chip, unsigned pin)
-{
-        return IRQ_EINT(0) + pin;
-}
-
 static struct s3c_gpio_chip gpio_2bit[] = {
         {
                 .base   = S3C64XX_GPF_BASE,
@@ -227,12 +222,13 @@ static struct s3c_gpio_chip gpio_2bit[] = {
                 },
         }, {
                 .base   = S3C64XX_GPN_BASE,
+                .irq_base = IRQ_EINT(0),
                 .config = &gpio_2bit_cfg_eint10,
                 .chip   = {
                         .base   = S3C64XX_GPN(0),
                         .ngpio  = S3C64XX_GPIO_N_NR,
                         .label  = "GPN",
-                        .to_irq = s3c64xx_gpio2int_gpn,
+                        .to_irq = samsung_gpiolib_to_irq,
                 },
         }, {
                 .base   = S3C64XX_GPO_BASE,
diff --git a/arch/arm/mach-s3c64xx/include/mach/vmalloc.h b/arch/arm/mach-s3c64xx/include/mach/vmalloc.h
index bc0e91389864..23f75e556a30 100644
--- a/arch/arm/mach-s3c64xx/include/mach/vmalloc.h
+++ b/arch/arm/mach-s3c64xx/include/mach/vmalloc.h
@@ -15,6 +15,6 @@
 #ifndef __ASM_ARCH_VMALLOC_H
 #define __ASM_ARCH_VMALLOC_H
 
-#define VMALLOC_END     0xE0000000UL
+#define VMALLOC_END     0xF6000000UL
 
 #endif /* __ASM_ARCH_VMALLOC_H */
diff --git a/arch/arm/mach-s3c64xx/mach-mini6410.c b/arch/arm/mach-s3c64xx/mach-mini6410.c
new file mode 100644
index 000000000000..249c62956471
--- /dev/null
+++ b/arch/arm/mach-s3c64xx/mach-mini6410.c
@@ -0,0 +1,357 @@
+/* linux/arch/arm/mach-s3c64xx/mach-mini6410.c
+ *
+ * Copyright 2010 Darius Augulis <augulis.darius@gmail.com>
+ * Copyright 2008 Openmoko, Inc.
+ * Copyright 2008 Simtec Electronics
+ *      Ben Dooks <ben@simtec.co.uk>
+ *      http://armlinux.simtec.co.uk/
+ *
+ * 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/init.h>
+#include <linux/interrupt.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/kernel.h>
+#include <linux/list.h>
+#include <linux/dm9000.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
+#include <linux/serial_core.h>
+#include <linux/types.h>
+
+#include <asm/mach-types.h>
+#include <asm/mach/arch.h>
+#include <asm/mach/map.h>
+
+#include <mach/map.h>
+#include <mach/regs-fb.h>
+#include <mach/regs-gpio.h>
+#include <mach/regs-modem.h>
+#include <mach/regs-srom.h>
+#include <mach/s3c6410.h>
+
+#include <plat/adc.h>
+#include <plat/cpu.h>
+#include <plat/devs.h>
+#include <plat/fb.h>
+#include <plat/nand.h>
+#include <plat/regs-serial.h>
+#include <plat/ts.h>
+
+#include <video/platform_lcd.h>
+
+#define UCON (S3C2410_UCON_DEFAULT | S3C2410_UCON_UCLK)
+#define ULCON (S3C2410_LCON_CS8 | S3C2410_LCON_PNONE | S3C2410_LCON_STOPB)
+#define UFCON (S3C2410_UFCON_RXTRIG8 | S3C2410_UFCON_FIFOMODE)
+
+static struct s3c2410_uartcfg mini6410_uartcfgs[] __initdata = {
+        [0] = {
+                .hwport = 0,
+                .flags  = 0,
+                .ucon   = UCON,
+                .ulcon  = ULCON,
+                .ufcon  = UFCON,
+        },
+        [1] = {
+                .hwport = 1,
+                .flags  = 0,
+                .ucon   = UCON,
+                .ulcon  = ULCON,
+                .ufcon  = UFCON,
+        },
+        [2] = {
+                .hwport = 2,
+                .flags  = 0,
+                .ucon   = UCON,
+                .ulcon  = ULCON,
+                .ufcon  = UFCON,
+        },
+        [3] = {
+                .hwport = 3,
+                .flags  = 0,
+                .ucon   = UCON,
+                .ulcon  = ULCON,
+                .ufcon  = UFCON,
+        },
+};
+
+/* DM9000AEP 10/100 ethernet controller */
+
+static struct resource mini6410_dm9k_resource[] = {
+        [0] = {
+                .start  = S3C64XX_PA_XM0CSN1,
+                .end    = S3C64XX_PA_XM0CSN1 + 1,
+                .flags  = IORESOURCE_MEM
+        },
+        [1] = {
+                .start  = S3C64XX_PA_XM0CSN1 + 4,
+                .end    = S3C64XX_PA_XM0CSN1 + 5,
+                .flags  = IORESOURCE_MEM
+        },
+        [2] = {
+                .start  = S3C_EINT(7),
+                .end    = S3C_EINT(7),
+                .flags  = IORESOURCE_IRQ | IORESOURCE_IRQ_HIGHLEVEL
+        }
+};
+
+static struct dm9000_plat_data mini6410_dm9k_pdata = {
+        .flags          = (DM9000_PLATF_16BITONLY | DM9000_PLATF_NO_EEPROM),
+};
+
+static struct platform_device mini6410_device_eth = {
+        .name           = "dm9000",
+        .id             = -1,
+        .num_resources  = ARRAY_SIZE(mini6410_dm9k_resource),
+        .resource       = mini6410_dm9k_resource,
+        .dev            = {
+                .platform_data  = &mini6410_dm9k_pdata,
+        },
+};
+
+static struct mtd_partition mini6410_nand_part[] = {
+        [0] = {
+                .name   = "uboot",
+                .size   = SZ_1M,
+                .offset = 0,
+        },
+        [1] = {
+                .name   = "kernel",
+                .size   = SZ_2M,
+                .offset = SZ_1M,
+        },
+        [2] = {
+                .name   = "rootfs",
+                .size   = MTDPART_SIZ_FULL,
+                .offset = SZ_1M + SZ_2M,
+        },
+};
+
+static struct s3c2410_nand_set mini6410_nand_sets[] = {
+        [0] = {
+                .name           = "nand",
+                .nr_chips       = 1,
+                .nr_partitions  = ARRAY_SIZE(mini6410_nand_part),
+                .partitions     = mini6410_nand_part,
+        },
+};
+
+static struct s3c2410_platform_nand mini6410_nand_info = {
+        .tacls          = 25,
+        .twrph0         = 55,
+        .twrph1         = 40,
+        .nr_sets        = ARRAY_SIZE(mini6410_nand_sets),
+        .sets           = mini6410_nand_sets,
+};
+
+static struct s3c_fb_pd_win mini6410_fb_win[] = {
+        {
+                .win_mode       = {     /* 4.3" 480x272 */
+                        .left_margin    = 3,
+                        .right_margin   = 2,
+                        .upper_margin   = 1,
+                        .lower_margin   = 1,
+                        .hsync_len      = 40,
+                        .vsync_len      = 1,
+                        .xres           = 480,
+                        .yres           = 272,
+                },
+                .max_bpp        = 32,
+                .default_bpp    = 16,
+        }, {
+                .win_mode       = {     /* 7.0" 800x480 */
+                        .left_margin    = 8,
+                        .right_margin   = 13,
+                        .upper_margin   = 7,
+                        .lower_margin   = 5,
+                        .hsync_len      = 3,
+                        .vsync_len      = 1,
+                        .xres           = 800,
+                        .yres           = 480,
+                },
+                .max_bpp        = 32,
+                .default_bpp    = 16,
+        },
+};
+
+static struct s3c_fb_platdata mini6410_lcd_pdata __initdata = {
+        .setup_gpio     = s3c64xx_fb_gpio_setup_24bpp,
+        .win[0]         = &mini6410_fb_win[0],
+        .vidcon0        = VIDCON0_VIDOUT_RGB | VIDCON0_PNRMODE_RGB,
+        .vidcon1        = VIDCON1_INV_HSYNC | VIDCON1_INV_VSYNC,
+};
+
+static void mini6410_lcd_power_set(struct plat_lcd_data *pd,
+                                   unsigned int power)
+{
+        if (power)
+                gpio_direction_output(S3C64XX_GPE(0), 1);
+        else
+                gpio_direction_output(S3C64XX_GPE(0), 0);
+}
+
+static struct plat_lcd_data mini6410_lcd_power_data = {
+        .set_power      = mini6410_lcd_power_set,
+};
+
+static struct platform_device mini6410_lcd_powerdev = {
+        .name                   = "platform-lcd",
+        .dev.parent             = &s3c_device_fb.dev,
+        .dev.platform_data      = &mini6410_lcd_power_data,
+};
+
+static struct s3c2410_ts_mach_info s3c_ts_platform __initdata = {
+        .delay                  = 10000,
+        .presc                  = 49,
+        .oversampling_shift     = 2,
+};
+
+static struct platform_device *mini6410_devices[] __initdata = {
+        &mini6410_device_eth,
+        &s3c_device_hsmmc0,
+        &s3c_device_hsmmc1,
+        &s3c_device_ohci,
+        &s3c_device_nand,
+        &s3c_device_fb,
+        &mini6410_lcd_powerdev,
+        &s3c_device_adc,
+        &s3c_device_ts,
+};
+
+static void __init mini6410_map_io(void)
+{
+        u32 tmp;
+
+        s3c64xx_init_io(NULL, 0);
+        s3c24xx_init_clocks(12000000);
+        s3c24xx_init_uarts(mini6410_uartcfgs, ARRAY_SIZE(mini6410_uartcfgs));
+
+        /* set the LCD type */
+        tmp = __raw_readl(S3C64XX_SPCON);
+        tmp &= ~S3C64XX_SPCON_LCD_SEL_MASK;
+        tmp |= S3C64XX_SPCON_LCD_SEL_RGB;
+        __raw_writel(tmp, S3C64XX_SPCON);
+
+        /* remove the LCD bypass */
+        tmp = __raw_readl(S3C64XX_MODEM_MIFPCON);
+        tmp &= ~MIFPCON_LCD_BYPASS;
+        __raw_writel(tmp, S3C64XX_MODEM_MIFPCON);
+}
+
+/*
+ * mini6410_features string
+ *
+ * 0-9 LCD configuration
+ *
+ */
+static char mini6410_features_str[12] __initdata = "0";
+
+static int __init mini6410_features_setup(char *str)
+{
+        if (str)
+                strlcpy(mini6410_features_str, str,
+                        sizeof(mini6410_features_str));
+        return 1;
+}
+
+__setup("mini6410=", mini6410_features_setup);
+
+#define FEATURE_SCREEN (1 << 0)
+
+struct mini6410_features_t {
+        int done;
+        int lcd_index;
+};
+
+static void mini6410_parse_features(
+                struct mini6410_features_t *features,
+                const char *features_str)
+{
+        const char *fp = features_str;
+
+        features->done = 0;
+        features->lcd_index = 0;
+
+        while (*fp) {
+                char f = *fp++;
+
+                switch (f) {
+                case '0'...'9': /* tft screen */
+                        if (features->done & FEATURE_SCREEN) {
+                                printk(KERN_INFO "MINI6410: '%c' ignored, "
+                                        "screen type already set\n", f);
+                        } else {
+                                int li = f - '0';
+                                if (li >= ARRAY_SIZE(mini6410_fb_win))
+                                        printk(KERN_INFO "MINI6410: '%c' out "
+                                                "of range LCD mode\n", f);
+                                else {
+                                        features->lcd_index = li;
+                                }
+                        }
+                        features->done |= FEATURE_SCREEN;
+                        break;
+                }
+        }
+}
+
+static void __init mini6410_machine_init(void)
+{
+        u32 cs1;
+        struct mini6410_features_t features = { 0 };
+
+        printk(KERN_INFO "MINI6410: Option string mini6410=%s\n",
+                        mini6410_features_str);
+
+        /* Parse the feature string */
+        mini6410_parse_features(&features, mini6410_features_str);
+
+        mini6410_lcd_pdata.win[0] = &mini6410_fb_win[features.lcd_index];
+
+        printk(KERN_INFO "MINI6410: selected LCD display is %dx%d\n",
+                mini6410_lcd_pdata.win[0]->win_mode.xres,
+                mini6410_lcd_pdata.win[0]->win_mode.yres);
+
+        s3c_nand_set_platdata(&mini6410_nand_info);
+        s3c_fb_set_platdata(&mini6410_lcd_pdata);
+        s3c24xx_ts_set_platdata(&s3c_ts_platform);
+
+        /* configure nCS1 width to 16 bits */
+
+        cs1 = __raw_readl(S3C64XX_SROM_BW) &
+                ~(S3C64XX_SROM_BW__CS_MASK << S3C64XX_SROM_BW__NCS1__SHIFT);
+        cs1 |= ((1 << S3C64XX_SROM_BW__DATAWIDTH__SHIFT) |
+                (1 << S3C64XX_SROM_BW__WAITENABLE__SHIFT) |
+                (1 << S3C64XX_SROM_BW__BYTEENABLE__SHIFT)) <<
+                        S3C64XX_SROM_BW__NCS1__SHIFT;
+        __raw_writel(cs1, S3C64XX_SROM_BW);
+
+        /* set timing for nCS1 suitable for ethernet chip */
+
+        __raw_writel((0 << S3C64XX_SROM_BCX__PMC__SHIFT) |
+                (6 << S3C64XX_SROM_BCX__TACP__SHIFT) |
+                (4 << S3C64XX_SROM_BCX__TCAH__SHIFT) |
+                (1 << S3C64XX_SROM_BCX__TCOH__SHIFT) |
+                (13 << S3C64XX_SROM_BCX__TACC__SHIFT) |
+                (4 << S3C64XX_SROM_BCX__TCOS__SHIFT) |
+                (0 << S3C64XX_SROM_BCX__TACS__SHIFT), S3C64XX_SROM_BC1);
+
+        gpio_request(S3C64XX_GPF(15), "LCD power");
+        gpio_request(S3C64XX_GPE(0), "LCD power");
+
+        platform_add_devices(mini6410_devices, ARRAY_SIZE(mini6410_devices));
+}
+
+MACHINE_START(MINI6410, "MINI6410")
+        /* Maintainer: Darius Augulis <augulis.darius@gmail.com> */
+        .boot_params    = S3C64XX_PA_SDRAM + 0x100,
+        .init_irq       = s3c6410_init_irq,
+        .map_io         = mini6410_map_io,
+        .init_machine   = mini6410_machine_init,
+        .timer          = &s3c24xx_timer,
+MACHINE_END
diff --git a/arch/arm/mach-s3c64xx/mach-real6410.c b/arch/arm/mach-s3c64xx/mach-real6410.c
index 4b4475da8ec6..f9ef9b5c5f5a 100644
--- a/arch/arm/mach-s3c64xx/mach-real6410.c
+++ b/arch/arm/mach-s3c64xx/mach-real6410.c
@@ -12,23 +12,39 @@
  *
 */
 
-#include <linux/kernel.h>
-#include <linux/types.h>
+#include <linux/init.h>
 #include <linux/interrupt.h>
+#include <linux/fb.h>
+#include <linux/gpio.h>
+#include <linux/kernel.h>
 #include <linux/list.h>
-#include <linux/init.h>
 #include <linux/dm9000.h>
-#include <linux/serial_core.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/partitions.h>
 #include <linux/platform_device.h>
+#include <linux/serial_core.h>
+#include <linux/types.h>
+
 #include <asm/mach-types.h>
 #include <asm/mach/arch.h>
 #include <asm/mach/map.h>
+
 #include <mach/map.h>
-#include <mach/s3c6410.h>
+#include <mach/regs-fb.h>
+#include <mach/regs-gpio.h>
+#include <mach/regs-modem.h>
 #include <mach/regs-srom.h>
+#include <mach/s3c6410.h>
+
+#include <plat/adc.h>
 #include <plat/cpu.h>
 #include <plat/devs.h>
+#include <plat/fb.h>
+#include <plat/nand.h>
 #include <plat/regs-serial.h>
+#include <plat/ts.h>
+
+#include <video/platform_lcd.h>
 
 #define UCON (S3C2410_UCON_DEFAULT | S3C2410_UCON_UCLK)
 #define ULCON (S3C2410_LCON_CS8 | S3C2410_LCON_PNONE | S3C2410_LCON_STOPB)
@@ -99,22 +115,192 @@ static struct platform_device real6410_device_eth = {
         },
 };
 
+static struct s3c_fb_pd_win real6410_fb_win[] = {
+        {
+                .win_mode       = {     /* 4.3" 480x272 */
+                        .left_margin    = 3,
+                        .right_margin   = 2,
+                        .upper_margin   = 1,
+                        .lower_margin   = 1,
+                        .hsync_len      = 40,
+                        .vsync_len      = 1,
+                        .xres           = 480,
+                        .yres           = 272,
+                },
+                .max_bpp        = 32,
+                .default_bpp    = 16,
+        }, {
+                .win_mode       = {     /* 7.0" 800x480 */
+                        .left_margin    = 8,
+                        .right_margin   = 13,
+                        .upper_margin   = 7,
+                        .lower_margin   = 5,
+                        .hsync_len      = 3,
+                        .vsync_len      = 1,
+                        .xres           = 800,
+                        .yres           = 480,
+                },
+                .max_bpp        = 32,
+                .default_bpp    = 16,
+        },
+};
+
+static struct s3c_fb_platdata real6410_lcd_pdata __initdata = {
+        .setup_gpio     = s3c64xx_fb_gpio_setup_24bpp,
+        .win[0]         = &real6410_fb_win[0],
+        .vidcon0        = VIDCON0_VIDOUT_RGB | VIDCON0_PNRMODE_RGB,
+        .vidcon1        = VIDCON1_INV_HSYNC | VIDCON1_INV_VSYNC,
+};
+
+static struct mtd_partition real6410_nand_part[] = {
+        [0] = {
+                .name   = "uboot",
+                .size   = SZ_1M,
+                .offset = 0,
+        },
+        [1] = {
+                .name   = "kernel",
+                .size   = SZ_2M,
+                .offset = SZ_1M,
+        },
+        [2] = {
+                .name   = "rootfs",
+                .size   = MTDPART_SIZ_FULL,
+                .offset = SZ_1M + SZ_2M,
+        },
+};
+
+static struct s3c2410_nand_set real6410_nand_sets[] = {
+        [0] = {
+                .name           = "nand",
+                .nr_chips       = 1,
+                .nr_partitions  = ARRAY_SIZE(real6410_nand_part),
+                .partitions     = real6410_nand_part,
+        },
+};
+
+static struct s3c2410_platform_nand real6410_nand_info = {
+        .tacls          = 25,
+        .twrph0         = 55,
+        .twrph1         = 40,
+        .nr_sets        = ARRAY_SIZE(real6410_nand_sets),
+        .sets           = real6410_nand_sets,
+};
+
 static struct platform_device *real6410_devices[] __initdata = {
         &real6410_device_eth,
         &s3c_device_hsmmc0,
         &s3c_device_hsmmc1,
+        &s3c_device_fb,
+        &s3c_device_nand,
+        &s3c_device_adc,
+        &s3c_device_ts,
+        &s3c_device_ohci,
+};
+
+static struct s3c2410_ts_mach_info s3c_ts_platform __initdata = {
+        .delay                  = 10000,
+        .presc                  = 49,
+        .oversampling_shift     = 2,
 };
 
 static void __init real6410_map_io(void)
 {
+        u32 tmp;
+
         s3c64xx_init_io(NULL, 0);
         s3c24xx_init_clocks(12000000);
         s3c24xx_init_uarts(real6410_uartcfgs, ARRAY_SIZE(real6410_uartcfgs));
+
+        /* set the LCD type */
+        tmp = __raw_readl(S3C64XX_SPCON);
+        tmp &= ~S3C64XX_SPCON_LCD_SEL_MASK;
+        tmp |= S3C64XX_SPCON_LCD_SEL_RGB;
+        __raw_writel(tmp, S3C64XX_SPCON);
+
+        /* remove the LCD bypass */
+        tmp = __raw_readl(S3C64XX_MODEM_MIFPCON);
+        tmp &= ~MIFPCON_LCD_BYPASS;
+        __raw_writel(tmp, S3C64XX_MODEM_MIFPCON);
+}
+
+/*
+ * real6410_features string
+ *
+ * 0-9 LCD configuration
+ *
+ */
+static char real6410_features_str[12] __initdata = "0";
+
+static int __init real6410_features_setup(char *str)
+{
+        if (str)
+                strlcpy(real6410_features_str, str,
+                        sizeof(real6410_features_str));
+        return 1;
+}
+
+__setup("real6410=", real6410_features_setup);
+
+#define FEATURE_SCREEN (1 << 0)
+
+struct real6410_features_t {
+        int done;
+        int lcd_index;
+};
+
+static void real6410_parse_features(
+                struct real6410_features_t *features,
+                const char *features_str)
+{
+        const char *fp = features_str;
+
+        features->done = 0;
+        features->lcd_index = 0;
+
+        while (*fp) {
+                char f = *fp++;
+
+                switch (f) {
+                case '0'...'9': /* tft screen */
+                        if (features->done & FEATURE_SCREEN) {
+                                printk(KERN_INFO "REAL6410: '%c' ignored, "
+                                        "screen type already set\n", f);
+                        } else {
+                                int li = f - '0';
+                                if (li >= ARRAY_SIZE(real6410_fb_win))
+                                        printk(KERN_INFO "REAL6410: '%c' out "
+                                                "of range LCD mode\n", f);
+                                else {
+                                        features->lcd_index = li;
+                                }
+                        }
+                        features->done |= FEATURE_SCREEN;
+                        break;
+                }
+        }
 }
 
 static void __init real6410_machine_init(void)
 {
         u32 cs1;
+        struct real6410_features_t features = { 0 };
+
+        printk(KERN_INFO "REAL6410: Option string real6410=%s\n",
+                        real6410_features_str);
+
+        /* Parse the feature string */
+        real6410_parse_features(&features, real6410_features_str);
+
+        real6410_lcd_pdata.win[0] = &real6410_fb_win[features.lcd_index];
+
+        printk(KERN_INFO "REAL6410: selected LCD display is %dx%d\n",
+                real6410_lcd_pdata.win[0]->win_mode.xres,
+                real6410_lcd_pdata.win[0]->win_mode.yres);
+
+        s3c_fb_set_platdata(&real6410_lcd_pdata);
+        s3c_nand_set_platdata(&real6410_nand_info);
+        s3c24xx_ts_set_platdata(&s3c_ts_platform);
 
         /* configure nCS1 width to 16 bits */
 
@@ -136,6 +322,8 @@ static void __init real6410_machine_init(void)
                 (4 << S3C64XX_SROM_BCX__TCOS__SHIFT) |
                 (0 << S3C64XX_SROM_BCX__TACS__SHIFT), S3C64XX_SROM_BC1);
 
+        gpio_request(S3C64XX_GPF(15), "LCD power");
+
         platform_add_devices(real6410_devices, ARRAY_SIZE(real6410_devices));
 }
 
diff --git a/arch/arm/mach-s3c64xx/setup-fb-24bpp.c b/arch/arm/mach-s3c64xx/setup-fb-24bpp.c
index 000736877df2..8f3091182f9c 100644
--- a/arch/arm/mach-s3c64xx/setup-fb-24bpp.c
+++ b/arch/arm/mach-s3c64xx/setup-fb-24bpp.c
@@ -23,15 +23,6 @@
 
 extern void s3c64xx_fb_gpio_setup_24bpp(void)
 {
-        unsigned int gpio;
-
-        for (gpio = S3C64XX_GPI(0); gpio <= S3C64XX_GPI(15); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
-
-        for (gpio = S3C64XX_GPJ(0); gpio <= S3C64XX_GPJ(11); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S3C64XX_GPI(0), 16, S3C_GPIO_SFN(2));
+        s3c_gpio_cfgrange_nopull(S3C64XX_GPJ(0), 12, S3C_GPIO_SFN(2));
 }
diff --git a/arch/arm/mach-s3c64xx/setup-ide.c b/arch/arm/mach-s3c64xx/setup-ide.c
index c12c315f33bc..41b425602d88 100644
--- a/arch/arm/mach-s3c64xx/setup-ide.c
+++ b/arch/arm/mach-s3c64xx/setup-ide.c
@@ -17,11 +17,11 @@
 #include <mach/map.h>
 #include <mach/regs-clock.h>
 #include <plat/gpio-cfg.h>
+#include <plat/ata.h>
 
 void s3c64xx_ide_setup_gpio(void)
 {
         u32 reg;
-        u32 gpio = 0;
 
         reg = readl(S3C_MEM_SYS_CFG) & (~0x3f);
 
@@ -32,15 +32,12 @@ void s3c64xx_ide_setup_gpio(void)
         s3c_gpio_cfgpin(S3C64XX_GPB(4), S3C_GPIO_SFN(4));
 
         /* Set XhiDATA[15:0] pins as CF Data[15:0] */
-        for (gpio = S3C64XX_GPK(0); gpio <= S3C64XX_GPK(15); gpio++)
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(5));
+        s3c_gpio_cfgpin_range(S3C64XX_GPK(0), 16, S3C_GPIO_SFN(5));
 
         /* Set XhiADDR[2:0] pins as CF ADDR[2:0] */
-        for (gpio = S3C64XX_GPL(0); gpio <= S3C64XX_GPL(2); gpio++)
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(6));
+        s3c_gpio_cfgpin_range(S3C64XX_GPL(0), 3, S3C_GPIO_SFN(6));
 
         /* Set Xhi ctrl pins as CF ctrl pins(IORDY, IOWR, IORD, CE[0:1]) */
         s3c_gpio_cfgpin(S3C64XX_GPM(5), S3C_GPIO_SFN(1));
-        for (gpio = S3C64XX_GPM(0); gpio <= S3C64XX_GPM(4); gpio++)
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(6));
+        s3c_gpio_cfgpin_range(S3C64XX_GPM(0), 5, S3C_GPIO_SFN(6));
 }
diff --git a/arch/arm/mach-s3c64xx/setup-keypad.c b/arch/arm/mach-s3c64xx/setup-keypad.c
index abc34e4e1a93..f8ed0d22db70 100644
--- a/arch/arm/mach-s3c64xx/setup-keypad.c
+++ b/arch/arm/mach-s3c64xx/setup-keypad.c
@@ -12,23 +12,13 @@
 
 #include <linux/gpio.h>
 #include <plat/gpio-cfg.h>
+#include <plat/keypad.h>
 
 void samsung_keypad_cfg_gpio(unsigned int rows, unsigned int cols)
 {
-        unsigned int gpio;
-        unsigned int end;
-
         /* Set all the necessary GPK pins to special-function 3: KP_ROW[x] */
-        end = S3C64XX_GPK(8 + rows);
-        for (gpio = S3C64XX_GPK(8); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S3C64XX_GPK(8), 8 + rows, S3C_GPIO_SFN(3));
 
         /* Set all the necessary GPL pins to special-function 3: KP_COL[x] */
-        end = S3C64XX_GPL(0 + cols);
-        for (gpio = S3C64XX_GPL(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S3C64XX_GPL(0), cols, S3C_GPIO_SFN(3));
 }
diff --git a/arch/arm/mach-s3c64xx/setup-sdhci-gpio.c b/arch/arm/mach-s3c64xx/setup-sdhci-gpio.c
index 322359591374..6eac071afae2 100644
--- a/arch/arm/mach-s3c64xx/setup-sdhci-gpio.c
+++ b/arch/arm/mach-s3c64xx/setup-sdhci-gpio.c
@@ -24,16 +24,9 @@
 void s3c64xx_setup_sdhci0_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
-        unsigned int end;
 
-        end = S3C64XX_GPG(2 + width);
-
-        /* Set all the necessary GPG pins to special-function 0 */
-        for (gpio = S3C64XX_GPG(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        /* Set all the necessary GPG pins to special-function 2 */
+        s3c_gpio_cfgrange_nopull(S3C64XX_GPG(0), 2 + width, S3C_GPIO_SFN(2));
 
         if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
                 s3c_gpio_setpull(S3C64XX_GPG(6), S3C_GPIO_PULL_UP);
@@ -44,16 +37,9 @@ void s3c64xx_setup_sdhci0_cfg_gpio(struct platform_device *dev, int width)
 void s3c64xx_setup_sdhci1_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
-        unsigned int end;
 
-        end = S3C64XX_GPH(2 + width);
-
-        /* Set all the necessary GPG pins to special-function 0 */
-        for (gpio = S3C64XX_GPH(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        /* Set all the necessary GPH pins to special-function 2 */
+        s3c_gpio_cfgrange_nopull(S3C64XX_GPH(0), 2 + width, S3C_GPIO_SFN(2));
 
         if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
                 s3c_gpio_setpull(S3C64XX_GPG(6), S3C_GPIO_PULL_UP);
@@ -63,20 +49,9 @@ void s3c64xx_setup_sdhci1_cfg_gpio(struct platform_device *dev, int width)
 
 void s3c64xx_setup_sdhci2_cfg_gpio(struct platform_device *dev, int width)
 {
-        unsigned int gpio;
-        unsigned int end;
+        /* Set all the necessary GPH pins to special-function 3 */
+        s3c_gpio_cfgrange_nopull(S3C64XX_GPH(6), width, S3C_GPIO_SFN(3));
 
-        end = S3C64XX_GPH(6 + width);
-
-        /* Set all the necessary GPH pins to special-function 1 */
-        for (gpio = S3C64XX_GPH(6); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
-
-        /* Set all the necessary GPC pins to special-function 1 */
-        for (gpio = S3C64XX_GPC(4); gpio < S3C64XX_GPC(6); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        /* Set all the necessary GPC pins to special-function 3 */
+        s3c_gpio_cfgrange_nopull(S3C64XX_GPC(4), 2, S3C_GPIO_SFN(3));
 }
diff --git a/arch/arm/mach-s5p6442/Kconfig b/arch/arm/mach-s5p6442/Kconfig
index 0fda0a5df968..33569e4007c4 100644
--- a/arch/arm/mach-s5p6442/Kconfig
+++ b/arch/arm/mach-s5p6442/Kconfig
@@ -11,7 +11,6 @@ if ARCH_S5P6442
 
 config CPU_S5P6442
         bool
-        select PLAT_S5P
         select S3C_PL330_DMA
         help
           Enable S5P6442 CPU support
diff --git a/arch/arm/mach-s5p6442/clock.c b/arch/arm/mach-s5p6442/clock.c
index dcd20f17212a..16d6e7e61b50 100644
--- a/arch/arm/mach-s5p6442/clock.c
+++ b/arch/arm/mach-s5p6442/clock.c
@@ -192,6 +192,11 @@ static struct clk clk_pclkd1 = {
         .parent         = &clk_hclkd1,
 };
 
+int s5p6442_clk_ip0_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKGATE_IP0, clk, enable);
+}
+
 int s5p6442_clk_ip3_ctrl(struct clk *clk, int enable)
 {
         return s5p_gatectrl(S5P_CLKGATE_IP3, clk, enable);
@@ -335,6 +340,16 @@ void __init_or_cpufreq s5p6442_setup_clocks(void)
         clk_pclkd1.rate = pclkd1;
 }
 
+static struct clk init_clocks_disable[] = {
+        {
+                .name           = "pdma",
+                .id             = -1,
+                .parent         = &clk_pclkd1,
+                .enable         = s5p6442_clk_ip0_ctrl,
+                .ctrlbit        = (1 << 3),
+        },
+};
+
 static struct clk init_clocks[] = {
         {
                 .name           = "systimer",
@@ -393,10 +408,23 @@ static struct clk *clks[] __initdata = {
 
 void __init s5p6442_register_clocks(void)
 {
+        struct clk *clkptr;
+        int i, ret;
+
         s3c24xx_register_clocks(clks, ARRAY_SIZE(clks));
 
         s3c_register_clksrc(clksrcs, ARRAY_SIZE(clksrcs));
         s3c_register_clocks(init_clocks, ARRAY_SIZE(init_clocks));
 
+        clkptr = init_clocks_disable;
+        for (i = 0; i < ARRAY_SIZE(init_clocks_disable); i++, clkptr++) {
+                ret = s3c24xx_register_clock(clkptr);
+                if (ret < 0) {
+                        printk(KERN_ERR "Fail to register clock %s (%d)\n",
+                                        clkptr->name, ret);
+                } else
+                        (clkptr->enable)(clkptr, 0);
+        }
+
         s3c_pwmclk_init();
 }
diff --git a/arch/arm/mach-s5p6442/dev-audio.c b/arch/arm/mach-s5p6442/dev-audio.c
index 7a4e34720b7b..3462197ff352 100644
--- a/arch/arm/mach-s5p6442/dev-audio.c
+++ b/arch/arm/mach-s5p6442/dev-audio.c
@@ -21,22 +21,16 @@
 
 static int s5p6442_cfg_i2s(struct platform_device *pdev)
 {
+        unsigned int base;
+
         /* configure GPIO for i2s port */
         switch (pdev->id) {
         case 1:
-                s3c_gpio_cfgpin(S5P6442_GPC1(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPC1(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPC1(2), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPC1(3), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPC1(4), S3C_GPIO_SFN(2));
+                base = S5P6442_GPC1(0);
                 break;
 
         case -1:
-                s3c_gpio_cfgpin(S5P6442_GPC0(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPC0(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPC0(2), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPC0(3), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPC0(4), S3C_GPIO_SFN(2));
+                base = S5P6442_GPC0(0);
                 break;
 
         default:
@@ -44,6 +38,7 @@ static int s5p6442_cfg_i2s(struct platform_device *pdev)
                 return -EINVAL;
         }
 
+        s3c_gpio_cfgpin_range(base, 5, S3C_GPIO_SFN(2));
         return 0;
 }
 
@@ -111,21 +106,15 @@ struct platform_device s5p6442_device_iis1 = {
 
 static int s5p6442_pcm_cfg_gpio(struct platform_device *pdev)
 {
+        unsigned int base;
+
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S5P6442_GPC0(0), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5P6442_GPC0(1), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5P6442_GPC0(2), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5P6442_GPC0(3), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5P6442_GPC0(4), S3C_GPIO_SFN(3));
+                base = S5P6442_GPC0(0);
                 break;
 
         case 1:
-                s3c_gpio_cfgpin(S5P6442_GPC1(0), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5P6442_GPC1(1), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5P6442_GPC1(2), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5P6442_GPC1(3), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5P6442_GPC1(4), S3C_GPIO_SFN(3));
+                base = S5P6442_GPC1(0);
                 break;
 
         default:
@@ -133,6 +122,7 @@ static int s5p6442_pcm_cfg_gpio(struct platform_device *pdev)
                 return -EINVAL;
         }
 
+        s3c_gpio_cfgpin_range(base, 5, S3C_GPIO_SFN(3));
         return 0;
 }
 
diff --git a/arch/arm/mach-s5p6442/dev-spi.c b/arch/arm/mach-s5p6442/dev-spi.c
index e894651a88bd..cce8c2470709 100644
--- a/arch/arm/mach-s5p6442/dev-spi.c
+++ b/arch/arm/mach-s5p6442/dev-spi.c
@@ -38,11 +38,9 @@ static int s5p6442_spi_cfg_gpio(struct platform_device *pdev)
         switch (pdev->id) {
         case 0:
                 s3c_gpio_cfgpin(S5P6442_GPB(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPB(2), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6442_GPB(3), S3C_GPIO_SFN(2));
                 s3c_gpio_setpull(S5P6442_GPB(0), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6442_GPB(2), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6442_GPB(3), S3C_GPIO_PULL_UP);
+                s3c_gpio_cfgall_range(S5P6442_GPB(2), 2,
+                                      S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
                 break;
 
         default:
diff --git a/arch/arm/mach-s5p6442/dma.c b/arch/arm/mach-s5p6442/dma.c
index ad4f8704b93d..7dfb13654f8a 100644
--- a/arch/arm/mach-s5p6442/dma.c
+++ b/arch/arm/mach-s5p6442/dma.c
@@ -82,7 +82,7 @@ static struct s3c_pl330_platdata s5p6442_pdma_pdata = {
 
 static struct platform_device s5p6442_device_pdma = {
         .name           = "s3c-pl330",
-        .id             = 1,
+        .id             = -1,
         .num_resources  = ARRAY_SIZE(s5p6442_pdma_resource),
         .resource       = s5p6442_pdma_resource,
         .dev            = {
diff --git a/arch/arm/mach-s5p6442/include/mach/regs-clock.h b/arch/arm/mach-s5p6442/include/mach/regs-clock.h
index d8360b5d4ece..00828a336991 100644
--- a/arch/arm/mach-s5p6442/include/mach/regs-clock.h
+++ b/arch/arm/mach-s5p6442/include/mach/regs-clock.h
@@ -46,6 +46,7 @@
 #define S5P_CLK_DIV5            S5P_CLKREG(0x314)
 #define S5P_CLK_DIV6            S5P_CLKREG(0x318)
 
+#define S5P_CLKGATE_IP0         S5P_CLKREG(0x460)
 #define S5P_CLKGATE_IP3         S5P_CLKREG(0x46C)
 
 /* CLK_OUT */
diff --git a/arch/arm/mach-s5p6442/include/mach/vmalloc.h b/arch/arm/mach-s5p6442/include/mach/vmalloc.h
index f5c83f02c18e..4aa55e55ac47 100644
--- a/arch/arm/mach-s5p6442/include/mach/vmalloc.h
+++ b/arch/arm/mach-s5p6442/include/mach/vmalloc.h
@@ -12,6 +12,6 @@
 #ifndef __ASM_ARCH_VMALLOC_H
 #define __ASM_ARCH_VMALLOC_H
 
-#define VMALLOC_END     0xE0000000UL
+#define VMALLOC_END     0xF6000000UL
 
 #endif /* __ASM_ARCH_VMALLOC_H */
diff --git a/arch/arm/mach-s5p64x0/Kconfig b/arch/arm/mach-s5p64x0/Kconfig
index fbcae9352022..164d2783d381 100644
--- a/arch/arm/mach-s5p64x0/Kconfig
+++ b/arch/arm/mach-s5p64x0/Kconfig
@@ -9,14 +9,12 @@ if ARCH_S5P64X0
 
 config CPU_S5P6440
         bool
-        select PLAT_S5P
         select S3C_PL330_DMA
         help
           Enable S5P6440 CPU support
 
 config CPU_S5P6450
         bool
-        select PLAT_S5P
         select S3C_PL330_DMA
         help
           Enable S5P6450 CPU support
diff --git a/arch/arm/mach-s5p64x0/clock-s5p6440.c b/arch/arm/mach-s5p64x0/clock-s5p6440.c
index f93dcd8b4d6a..e4883dc1c8d7 100644
--- a/arch/arm/mach-s5p64x0/clock-s5p6440.c
+++ b/arch/arm/mach-s5p64x0/clock-s5p6440.c
@@ -79,13 +79,16 @@ static int s5p6440_epll_set_rate(struct clk *clk, unsigned long rate)
         __raw_writel(epll_con, S5P64X0_EPLL_CON);
         __raw_writel(epll_con_k, S5P64X0_EPLL_CON_K);
 
+        printk(KERN_WARNING "EPLL Rate changes from %lu to %lu\n",
+                        clk->rate, rate);
+
         clk->rate = rate;
 
         return 0;
 }
 
 static struct clk_ops s5p6440_epll_ops = {
-        .get_rate = s5p64x0_epll_get_rate,
+        .get_rate = s5p_epll_get_rate,
         .set_rate = s5p6440_epll_set_rate,
 };
 
@@ -150,6 +153,12 @@ static struct clk init_clocks_disable[] = {
                 .enable         = s5p64x0_hclk0_ctrl,
                 .ctrlbit        = (1 << 8),
         }, {
+                .name           = "pdma",
+                .id             = -1,
+                .parent         = &clk_hclk_low.clk,
+                .enable         = s5p64x0_hclk0_ctrl,
+                .ctrlbit        = (1 << 12),
+        }, {
                 .name           = "hsmmc",
                 .id             = 0,
                 .parent         = &clk_hclk_low.clk,
@@ -331,12 +340,6 @@ static struct clk init_clocks[] = {
                 .enable         = s5p64x0_hclk0_ctrl,
                 .ctrlbit        = (1 << 21),
         }, {
-                .name           = "dma",
-                .id             = -1,
-                .parent         = &clk_hclk_low.clk,
-                .enable         = s5p64x0_hclk0_ctrl,
-                .ctrlbit        = (1 << 12),
-        }, {
                 .name           = "uart",
                 .id             = 0,
                 .parent         = &clk_pclk_low.clk,
@@ -548,7 +551,7 @@ void __init_or_cpufreq s5p6440_setup_clocks(void)
 
         /* Set S5P6440 functions for clk_fout_epll */
 
-        clk_fout_epll.enable = s5p64x0_epll_enable;
+        clk_fout_epll.enable = s5p_epll_enable;
         clk_fout_epll.ops = &s5p6440_epll_ops;
 
         clk_48m.enable = s5p64x0_clk48m_ctrl;
diff --git a/arch/arm/mach-s5p64x0/clock-s5p6450.c b/arch/arm/mach-s5p64x0/clock-s5p6450.c
index f9afb05b217c..7dbf3c968f53 100644
--- a/arch/arm/mach-s5p64x0/clock-s5p6450.c
+++ b/arch/arm/mach-s5p64x0/clock-s5p6450.c
@@ -80,13 +80,16 @@ static int s5p6450_epll_set_rate(struct clk *clk, unsigned long rate)
         __raw_writel(epll_con, S5P64X0_EPLL_CON);
         __raw_writel(epll_con_k, S5P64X0_EPLL_CON_K);
 
+        printk(KERN_WARNING "EPLL Rate changes from %lu to %lu\n",
+                        clk->rate, rate);
+
         clk->rate = rate;
 
         return 0;
 }
 
 static struct clk_ops s5p6450_epll_ops = {
-        .get_rate = s5p64x0_epll_get_rate,
+        .get_rate = s5p_epll_get_rate,
         .set_rate = s5p6450_epll_set_rate,
 };
 
@@ -186,6 +189,12 @@ static struct clk init_clocks_disable[] = {
                 .enable         = s5p64x0_hclk0_ctrl,
                 .ctrlbit        = (1 << 3),
         }, {
+                .name           = "pdma",
+                .id             = -1,
+                .parent         = &clk_hclk_low.clk,
+                .enable         = s5p64x0_hclk0_ctrl,
+                .ctrlbit        = (1 << 12),
+        }, {
                 .name           = "hsmmc",
                 .id             = 0,
                 .parent         = &clk_hclk_low.clk,
@@ -283,12 +292,6 @@ static struct clk init_clocks[] = {
                 .enable         = s5p64x0_hclk0_ctrl,
                 .ctrlbit        = (1 << 21),
         }, {
-                .name           = "dma",
-                .id             = -1,
-                .parent         = &clk_hclk_low.clk,
-                .enable         = s5p64x0_hclk0_ctrl,
-                .ctrlbit        = (1 << 12),
-        }, {
                 .name           = "uart",
                 .id             = 0,
                 .parent         = &clk_pclk_low.clk,
@@ -581,7 +584,7 @@ void __init_or_cpufreq s5p6450_setup_clocks(void)
 
         /* Set S5P6450 functions for clk_fout_epll */
 
-        clk_fout_epll.enable = s5p64x0_epll_enable;
+        clk_fout_epll.enable = s5p_epll_enable;
         clk_fout_epll.ops = &s5p6450_epll_ops;
 
         clk_48m.enable = s5p64x0_clk48m_ctrl;
diff --git a/arch/arm/mach-s5p64x0/clock.c b/arch/arm/mach-s5p64x0/clock.c
index 523ba8039ac2..b52c6e2f37a6 100644
--- a/arch/arm/mach-s5p64x0/clock.c
+++ b/arch/arm/mach-s5p64x0/clock.c
@@ -73,24 +73,6 @@ static const u32 clock_table[][3] = {
         {L2 * 1000, (3 << ARM_DIV_RATIO_SHIFT), (0 << S5P64X0_CLKDIV0_HCLK_SHIFT)},
 };
 
-int s5p64x0_epll_enable(struct clk *clk, int enable)
-{
-        unsigned int ctrlbit = clk->ctrlbit;
-        unsigned int epll_con = __raw_readl(S5P64X0_EPLL_CON) & ~ctrlbit;
-
-        if (enable)
-                __raw_writel(epll_con | ctrlbit, S5P64X0_EPLL_CON);
-        else
-                __raw_writel(epll_con, S5P64X0_EPLL_CON);
-
-        return 0;
-}
-
-unsigned long s5p64x0_epll_get_rate(struct clk *clk)
-{
-        return clk->rate;
-}
-
 unsigned long s5p64x0_armclk_get_rate(struct clk *clk)
 {
         unsigned long rate = clk_get_rate(clk->parent);
diff --git a/arch/arm/mach-s5p64x0/dev-audio.c b/arch/arm/mach-s5p64x0/dev-audio.c
index fa097bd68ca4..396bacc0a39a 100644
--- a/arch/arm/mach-s5p64x0/dev-audio.c
+++ b/arch/arm/mach-s5p64x0/dev-audio.c
@@ -24,13 +24,8 @@ static int s5p6440_cfg_i2s(struct platform_device *pdev)
         /* configure GPIO for i2s port */
         switch (pdev->id) {
         case -1:
-                s3c_gpio_cfgpin(S5P6440_GPR(4), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6440_GPR(5), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6440_GPR(6), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6440_GPR(7), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6440_GPR(8), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6440_GPR(13), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6440_GPR(14), S3C_GPIO_SFN(5));
+                s3c_gpio_cfgpin_range(S5P6440_GPR(4), 5, S3C_GPIO_SFN(5));
+                s3c_gpio_cfgpin_range(S5P6440_GPR(13), 2, S3C_GPIO_SFN(5));
                 break;
 
         default:
@@ -47,13 +42,9 @@ static int s5p6450_cfg_i2s(struct platform_device *pdev)
         switch (pdev->id) {
         case -1:
                 s3c_gpio_cfgpin(S5P6450_GPB(4), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6450_GPR(4), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6450_GPR(5), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6450_GPR(6), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6450_GPR(7), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6450_GPR(8), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6450_GPR(13), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5P6450_GPR(14), S3C_GPIO_SFN(5));
+                s3c_gpio_cfgpin_range(S5P6450_GPR(4), 5, S3C_GPIO_SFN(5));
+                s3c_gpio_cfgpin_range(S5P6450_GPR(13), 2, S3C_GPIO_SFN(5));
+
                 break;
 
         default:
@@ -116,11 +107,8 @@ static int s5p6440_pcm_cfg_gpio(struct platform_device *pdev)
 {
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S5P6440_GPR(7), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6440_GPR(13), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6440_GPR(14), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6440_GPR(8), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6440_GPR(6), S3C_GPIO_SFN(2));
+                s3c_gpio_cfgpin_range(S5P6440_GPR(6), 3, S3C_GPIO_SFN(2));
+                s3c_gpio_cfgpin_range(S5P6440_GPR(13), 2, S3C_GPIO_SFN(2));
                 break;
 
         default:
diff --git a/arch/arm/mach-s5p64x0/dev-spi.c b/arch/arm/mach-s5p64x0/dev-spi.c
index 5b69ec4c8af3..e78ee18c76e3 100644
--- a/arch/arm/mach-s5p64x0/dev-spi.c
+++ b/arch/arm/mach-s5p64x0/dev-spi.c
@@ -39,23 +39,15 @@ static char *s5p64x0_spi_src_clks[] = {
  */
 static int s5p6440_spi_cfg_gpio(struct platform_device *pdev)
 {
+        unsigned int base;
+
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S5P6440_GPC(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6440_GPC(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6440_GPC(2), S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(S5P6440_GPC(0), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6440_GPC(1), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6440_GPC(2), S3C_GPIO_PULL_UP);
+                base = S5P6440_GPC(0);
                 break;
 
         case 1:
-                s3c_gpio_cfgpin(S5P6440_GPC(4), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6440_GPC(5), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6440_GPC(6), S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(S5P6440_GPC(4), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6440_GPC(5), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6440_GPC(6), S3C_GPIO_PULL_UP);
+                base = S5P6440_GPC(4);
                 break;
 
         default:
@@ -63,28 +55,23 @@ static int s5p6440_spi_cfg_gpio(struct platform_device *pdev)
                 return -EINVAL;
         }
 
+        s3c_gpio_cfgall_range(base, 3,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
+
         return 0;
 }
 
 static int s5p6450_spi_cfg_gpio(struct platform_device *pdev)
 {
+        unsigned int base;
+
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S5P6450_GPC(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6450_GPC(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6450_GPC(2), S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(S5P6450_GPC(0), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6450_GPC(1), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6450_GPC(2), S3C_GPIO_PULL_UP);
+                base = S5P6450_GPC(0);
                 break;
 
         case 1:
-                s3c_gpio_cfgpin(S5P6450_GPC(4), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6450_GPC(5), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5P6450_GPC(6), S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(S5P6450_GPC(4), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6450_GPC(5), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5P6450_GPC(6), S3C_GPIO_PULL_UP);
+                base = S5P6450_GPC(4);
                 break;
 
         default:
@@ -92,6 +79,9 @@ static int s5p6450_spi_cfg_gpio(struct platform_device *pdev)
                 return -EINVAL;
         }
 
+        s3c_gpio_cfgall_range(base, 3,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
+
         return 0;
 }
 
diff --git a/arch/arm/mach-s5p64x0/dma.c b/arch/arm/mach-s5p64x0/dma.c
index 29a8c2410049..d7ad944b3475 100644
--- a/arch/arm/mach-s5p64x0/dma.c
+++ b/arch/arm/mach-s5p64x0/dma.c
@@ -122,7 +122,7 @@ static struct s3c_pl330_platdata s5p6450_pdma_pdata = {
 
 static struct platform_device s5p64x0_device_pdma = {
         .name           = "s3c-pl330",
-        .id             = 0,
+        .id             = -1,
         .num_resources  = ARRAY_SIZE(s5p64x0_pdma_resource),
         .resource       = s5p64x0_pdma_resource,
         .dev            = {
diff --git a/arch/arm/mach-s5p64x0/include/mach/regs-clock.h b/arch/arm/mach-s5p64x0/include/mach/regs-clock.h
index 58e1bc813804..a133f22fa155 100644
--- a/arch/arm/mach-s5p64x0/include/mach/regs-clock.h
+++ b/arch/arm/mach-s5p64x0/include/mach/regs-clock.h
@@ -60,4 +60,6 @@
 #define ARM_DIV_RATIO_SHIFT             0
 #define ARM_DIV_MASK                    (0xF << ARM_DIV_RATIO_SHIFT)
 
+#define S5P_EPLL_CON                    S5P64X0_EPLL_CON
+
 #endif /* __ASM_ARCH_REGS_CLOCK_H */
diff --git a/arch/arm/mach-s5p64x0/include/mach/vmalloc.h b/arch/arm/mach-s5p64x0/include/mach/vmalloc.h
index 97a9df38f1cf..38dcc71a03cc 100644
--- a/arch/arm/mach-s5p64x0/include/mach/vmalloc.h
+++ b/arch/arm/mach-s5p64x0/include/mach/vmalloc.h
@@ -15,6 +15,6 @@
 #ifndef __ASM_ARCH_VMALLOC_H
 #define __ASM_ARCH_VMALLOC_H
 
-#define VMALLOC_END     0xE0000000UL
+#define VMALLOC_END     0xF6000000UL
 
 #endif /* __ASM_ARCH_VMALLOC_H */
diff --git a/arch/arm/mach-s5p64x0/setup-i2c0.c b/arch/arm/mach-s5p64x0/setup-i2c0.c
index dc4cc65a5019..46b463917c54 100644
--- a/arch/arm/mach-s5p64x0/setup-i2c0.c
+++ b/arch/arm/mach-s5p64x0/setup-i2c0.c
@@ -25,18 +25,14 @@ struct platform_device; /* don't need the contents */
 
 void s5p6440_i2c0_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5P6440_GPB(5), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5P6440_GPB(5), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5P6440_GPB(6), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5P6440_GPB(6), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5P6440_GPB(5), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
 
 void s5p6450_i2c0_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5P6450_GPB(5), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5P6450_GPB(5), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5P6450_GPB(6), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5P6450_GPB(6), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5P6450_GPB(5), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
 
 void s3c_i2c0_cfg_gpio(struct platform_device *dev) { }
diff --git a/arch/arm/mach-s5p64x0/setup-i2c1.c b/arch/arm/mach-s5p64x0/setup-i2c1.c
index 2edd7912f8e4..6ad3b986021c 100644
--- a/arch/arm/mach-s5p64x0/setup-i2c1.c
+++ b/arch/arm/mach-s5p64x0/setup-i2c1.c
@@ -25,18 +25,14 @@ struct platform_device; /* don't need the contents */
 
 void s5p6440_i2c1_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5P6440_GPR(9), S3C_GPIO_SFN(6));
-        s3c_gpio_setpull(S5P6440_GPR(9), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5P6440_GPR(10), S3C_GPIO_SFN(6));
-        s3c_gpio_setpull(S5P6440_GPR(10), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5P6440_GPR(9), 2,
+                              S3C_GPIO_SFN(6), S3C_GPIO_PULL_UP);
 }
 
 void s5p6450_i2c1_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5P6450_GPR(9), S3C_GPIO_SFN(6));
-        s3c_gpio_setpull(S5P6450_GPR(9), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5P6450_GPR(10), S3C_GPIO_SFN(6));
-        s3c_gpio_setpull(S5P6450_GPR(10), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5P6450_GPR(9), 2,
+                              S3C_GPIO_SFN(6), S3C_GPIO_PULL_UP);
 }
 
 void s3c_i2c1_cfg_gpio(struct platform_device *dev) { }
diff --git a/arch/arm/mach-s5pc100/Kconfig b/arch/arm/mach-s5pc100/Kconfig
index 77ae4bfb74ba..b8fbf2fcba6f 100644
--- a/arch/arm/mach-s5pc100/Kconfig
+++ b/arch/arm/mach-s5pc100/Kconfig
@@ -9,7 +9,6 @@ if ARCH_S5PC100
 
 config CPU_S5PC100
         bool
-        select PLAT_S5P
         select S5P_EXT_INT
         select S3C_PL330_DMA
         help
diff --git a/arch/arm/mach-s5pc100/Makefile b/arch/arm/mach-s5pc100/Makefile
index a021ed1fb4b6..eecab57d2e5d 100644
--- a/arch/arm/mach-s5pc100/Makefile
+++ b/arch/arm/mach-s5pc100/Makefile
@@ -11,7 +11,7 @@ obj-				:=
 
 # Core support for S5PC100 system
 
-obj-$(CONFIG_CPU_S5PC100)       += cpu.o init.o clock.o gpiolib.o irq-gpio.o
+obj-$(CONFIG_CPU_S5PC100)       += cpu.o init.o clock.o gpiolib.o
 obj-$(CONFIG_CPU_S5PC100)       += setup-i2c0.o
 obj-$(CONFIG_CPU_S5PC100)       += dma.o
 
diff --git a/arch/arm/mach-s5pc100/clock.c b/arch/arm/mach-s5pc100/clock.c
index 084abd13b0a5..2d4a761a5163 100644
--- a/arch/arm/mach-s5pc100/clock.c
+++ b/arch/arm/mach-s5pc100/clock.c
@@ -273,24 +273,6 @@ static struct clksrc_clk clk_div_hdmi = {
         .reg_div = { .reg = S5P_CLK_DIV3, .shift = 28, .size = 4 },
 };
 
-static int s5pc100_epll_enable(struct clk *clk, int enable)
-{
-        unsigned int ctrlbit = clk->ctrlbit;
-        unsigned int epll_con = __raw_readl(S5P_EPLL_CON) & ~ctrlbit;
-
-        if (enable)
-                __raw_writel(epll_con | ctrlbit, S5P_EPLL_CON);
-        else
-                __raw_writel(epll_con, S5P_EPLL_CON);
-
-        return 0;
-}
-
-static unsigned long s5pc100_epll_get_rate(struct clk *clk)
-{
-        return clk->rate;
-}
-
 static u32 epll_div[][4] = {
         { 32750000,     131, 3, 4 },
         { 32768000,     131, 3, 4 },
@@ -341,13 +323,16 @@ static int s5pc100_epll_set_rate(struct clk *clk, unsigned long rate)
 
         __raw_writel(epll_con, S5P_EPLL_CON);
 
+        printk(KERN_WARNING "EPLL Rate changes from %lu to %lu\n",
+                        clk->rate, rate);
+
         clk->rate = rate;
 
         return 0;
 }
 
 static struct clk_ops s5pc100_epll_ops = {
-        .get_rate = s5pc100_epll_get_rate,
+        .get_rate = s5p_epll_get_rate,
         .set_rate = s5pc100_epll_set_rate,
 };
 
@@ -691,55 +676,55 @@ static struct clk init_clocks_disable[] = {
         }, {
                 .name           = "iis",
                 .id             = 0,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 0),
         }, {
                 .name           = "iis",
                 .id             = 1,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 1),
         }, {
                 .name           = "iis",
                 .id             = 2,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 2),
         }, {
                 .name           = "ac97",
                 .id             = -1,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 3),
         }, {
                 .name           = "pcm",
                 .id             = 0,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 4),
         }, {
                 .name           = "pcm",
                 .id             = 1,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 5),
         }, {
                 .name           = "spdif",
                 .id             = -1,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 6),
         }, {
                 .name           = "adc",
                 .id             = -1,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 7),
         }, {
                 .name           = "keypad",
                 .id             = -1,
-                .parent         = &clk_div_d1_bus.clk,
+                .parent         = &clk_div_pclkd1.clk,
                 .enable         = s5pc100_d1_5_ctrl,
                 .ctrlbit        = (1 << 8),
         }, {
@@ -848,6 +833,18 @@ struct clksrc_sources clk_src_group3 = {
         .nr_sources     = ARRAY_SIZE(clk_src_group3_list),
 };
 
+static struct clksrc_clk clk_sclk_audio0 = {
+        .clk    = {
+                .name           = "sclk_audio",
+                .id             = 0,
+                .ctrlbit        = (1 << 8),
+                .enable         = s5pc100_sclk1_ctrl,
+        },
+        .sources = &clk_src_group3,
+        .reg_src = { .reg = S5P_CLK_SRC3, .shift = 12, .size = 3 },
+        .reg_div = { .reg = S5P_CLK_DIV4, .shift = 12, .size = 4 },
+};
+
 static struct clk *clk_src_group4_list[] = {
         [0] = &clk_mout_epll.clk,
         [1] = &clk_div_mpll.clk,
@@ -862,6 +859,18 @@ struct clksrc_sources clk_src_group4 = {
         .nr_sources     = ARRAY_SIZE(clk_src_group4_list),
 };
 
+static struct clksrc_clk clk_sclk_audio1 = {
+        .clk    = {
+                .name           = "sclk_audio",
+                .id             = 1,
+                .ctrlbit        = (1 << 9),
+                .enable         = s5pc100_sclk1_ctrl,
+        },
+        .sources = &clk_src_group4,
+        .reg_src = { .reg = S5P_CLK_SRC3, .shift = 16, .size = 3 },
+        .reg_div = { .reg = S5P_CLK_DIV4, .shift = 16, .size = 4 },
+};
+
 static struct clk *clk_src_group5_list[] = {
         [0] = &clk_mout_epll.clk,
         [1] = &clk_div_mpll.clk,
@@ -875,6 +884,18 @@ struct clksrc_sources clk_src_group5 = {
         .nr_sources     = ARRAY_SIZE(clk_src_group5_list),
 };
 
+static struct clksrc_clk clk_sclk_audio2 = {
+        .clk    = {
+                .name           = "sclk_audio",
+                .id             = 2,
+                .ctrlbit        = (1 << 10),
+                .enable         = s5pc100_sclk1_ctrl,
+        },
+        .sources = &clk_src_group5,
+        .reg_src = { .reg = S5P_CLK_SRC3, .shift = 20, .size = 3 },
+        .reg_div = { .reg = S5P_CLK_DIV4, .shift = 20, .size = 4 },
+};
+
 static struct clk *clk_src_group6_list[] = {
         [0] = &s5p_clk_27m,
         [1] = &clk_vclk54m,
@@ -944,6 +965,64 @@ struct clksrc_sources clk_src_pwi = {
         .nr_sources     = ARRAY_SIZE(clk_src_pwi_list),
 };
 
+static struct clk *clk_sclk_spdif_list[] = {
+        [0] = &clk_sclk_audio0.clk,
+        [1] = &clk_sclk_audio1.clk,
+        [2] = &clk_sclk_audio2.clk,
+};
+
+struct clksrc_sources clk_src_sclk_spdif = {
+        .sources        = clk_sclk_spdif_list,
+        .nr_sources     = ARRAY_SIZE(clk_sclk_spdif_list),
+};
+
+static int s5pc100_spdif_set_rate(struct clk *clk, unsigned long rate)
+{
+        struct clk *pclk;
+        int ret;
+
+        pclk = clk_get_parent(clk);
+        if (IS_ERR(pclk))
+                return -EINVAL;
+
+        ret = pclk->ops->set_rate(pclk, rate);
+        clk_put(pclk);
+
+        return ret;
+}
+
+static unsigned long s5pc100_spdif_get_rate(struct clk *clk)
+{
+        struct clk *pclk;
+        int rate;
+
+        pclk = clk_get_parent(clk);
+        if (IS_ERR(pclk))
+                return -EINVAL;
+
+        rate = pclk->ops->get_rate(clk);
+        clk_put(pclk);
+
+        return rate;
+}
+
+static struct clk_ops s5pc100_sclk_spdif_ops = {
+        .set_rate       = s5pc100_spdif_set_rate,
+        .get_rate       = s5pc100_spdif_get_rate,
+};
+
+static struct clksrc_clk clk_sclk_spdif = {
+        .clk    = {
+                .name           = "sclk_spdif",
+                .id             = -1,
+                .ctrlbit        = (1 << 11),
+                .enable         = s5pc100_sclk1_ctrl,
+                .ops            = &s5pc100_sclk_spdif_ops,
+        },
+        .sources = &clk_src_sclk_spdif,
+        .reg_src = { .reg = S5P_CLK_SRC3, .shift = 24, .size = 2 },
+};
+
 static struct clksrc_clk clksrcs[] = {
         {
                 .clk    = {
@@ -1001,39 +1080,6 @@ static struct clksrc_clk clksrcs[] = {
                 .reg_src = { .reg = S5P_CLK_SRC2, .shift = 28, .size = 2 },
         }, {
                 .clk    = {
-                        .name           = "sclk_audio",
-                        .id             = 0,
-                        .ctrlbit        = (1 << 8),
-                        .enable         = s5pc100_sclk1_ctrl,
-
-                },
-                .sources = &clk_src_group3,
-                .reg_src = { .reg = S5P_CLK_SRC3, .shift = 12, .size = 3 },
-                .reg_div = { .reg = S5P_CLK_DIV4, .shift = 12, .size = 4 },
-        }, {
-                .clk    = {
-                        .name           = "sclk_audio",
-                        .id             = 1,
-                        .ctrlbit        = (1 << 9),
-                        .enable         = s5pc100_sclk1_ctrl,
-
-                },
-                .sources = &clk_src_group4,
-                .reg_src = { .reg = S5P_CLK_SRC3, .shift = 16, .size = 3 },
-                .reg_div = { .reg = S5P_CLK_DIV4, .shift = 16, .size = 4 },
-        }, {
-                .clk    = {
-                        .name           = "sclk_audio",
-                        .id             = 2,
-                        .ctrlbit        = (1 << 10),
-                        .enable         = s5pc100_sclk1_ctrl,
-
-                },
-                .sources = &clk_src_group5,
-                .reg_src = { .reg = S5P_CLK_SRC3, .shift = 20, .size = 3 },
-                .reg_div = { .reg = S5P_CLK_DIV4, .shift = 20, .size = 4 },
-        }, {
-                .clk    = {
                         .name           = "sclk_lcd",
                         .id             = -1,
                         .ctrlbit        = (1 << 0),
@@ -1179,6 +1225,10 @@ static struct clksrc_clk *sysclks[] = {
         &clk_div_pclkd1,
         &clk_div_cam,
         &clk_div_hdmi,
+        &clk_sclk_audio0,
+        &clk_sclk_audio1,
+        &clk_sclk_audio2,
+        &clk_sclk_spdif,
 };
 
 void __init_or_cpufreq s5pc100_setup_clocks(void)
@@ -1196,7 +1246,7 @@ void __init_or_cpufreq s5pc100_setup_clocks(void)
         unsigned int ptr;
 
         /* Set S5PC100 functions for clk_fout_epll */
-        clk_fout_epll.enable = s5pc100_epll_enable;
+        clk_fout_epll.enable = s5p_epll_enable;
         clk_fout_epll.ops = &s5pc100_epll_ops;
 
         printk(KERN_DEBUG "%s: registering clocks\n", __func__);
diff --git a/arch/arm/mach-s5pc100/dev-audio.c b/arch/arm/mach-s5pc100/dev-audio.c
index a699ed6acc23..564e195ec493 100644
--- a/arch/arm/mach-s5pc100/dev-audio.c
+++ b/arch/arm/mach-s5pc100/dev-audio.c
@@ -24,19 +24,11 @@ static int s5pc100_cfg_i2s(struct platform_device *pdev)
         /* configure GPIO for i2s port */
         switch (pdev->id) {
         case 1:
-                s3c_gpio_cfgpin(S5PC100_GPC(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PC100_GPC(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PC100_GPC(2), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PC100_GPC(3), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PC100_GPC(4), S3C_GPIO_SFN(2));
+                s3c_gpio_cfgpin_range(S5PC100_GPC(0), 5, S3C_GPIO_SFN(2));
                 break;
 
         case 2:
-                s3c_gpio_cfgpin(S5PC100_GPG3(0), S3C_GPIO_SFN(4));
-                s3c_gpio_cfgpin(S5PC100_GPG3(1), S3C_GPIO_SFN(4));
-                s3c_gpio_cfgpin(S5PC100_GPG3(2), S3C_GPIO_SFN(4));
-                s3c_gpio_cfgpin(S5PC100_GPG3(3), S3C_GPIO_SFN(4));
-                s3c_gpio_cfgpin(S5PC100_GPG3(4), S3C_GPIO_SFN(4));
+                s3c_gpio_cfgpin_range(S5PC100_GPG3(0), 5, S3C_GPIO_SFN(4));
                 break;
 
         case -1: /* Dedicated pins */
@@ -144,19 +136,11 @@ static int s5pc100_pcm_cfg_gpio(struct platform_device *pdev)
 {
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S5PC100_GPG3(0), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5PC100_GPG3(1), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5PC100_GPG3(2), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5PC100_GPG3(3), S3C_GPIO_SFN(5));
-                s3c_gpio_cfgpin(S5PC100_GPG3(4), S3C_GPIO_SFN(5));
+                s3c_gpio_cfgpin_range(S5PC100_GPG3(0), 5, S3C_GPIO_SFN(5));
                 break;
 
         case 1:
-                s3c_gpio_cfgpin(S5PC100_GPC(0), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PC100_GPC(1), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PC100_GPC(2), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PC100_GPC(3), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PC100_GPC(4), S3C_GPIO_SFN(3));
+                s3c_gpio_cfgpin_range(S5PC100_GPC(0), 5, S3C_GPIO_SFN(3));
                 break;
 
         default:
@@ -231,13 +215,7 @@ struct platform_device s5pc100_device_pcm1 = {
 
 static int s5pc100_ac97_cfg_gpio(struct platform_device *pdev)
 {
-        s3c_gpio_cfgpin(S5PC100_GPC(0), S3C_GPIO_SFN(4));
-        s3c_gpio_cfgpin(S5PC100_GPC(1), S3C_GPIO_SFN(4));
-        s3c_gpio_cfgpin(S5PC100_GPC(2), S3C_GPIO_SFN(4));
-        s3c_gpio_cfgpin(S5PC100_GPC(3), S3C_GPIO_SFN(4));
-        s3c_gpio_cfgpin(S5PC100_GPC(4), S3C_GPIO_SFN(4));
-
-        return 0;
+        return s3c_gpio_cfgpin_range(S5PC100_GPC(0), 5, S3C_GPIO_SFN(4));
 }
 
 static struct resource s5pc100_ac97_resource[] = {
@@ -285,3 +263,57 @@ struct platform_device s5pc100_device_ac97 = {
                 .coherent_dma_mask = DMA_BIT_MASK(32),
         },
 };
+
+/* S/PDIF Controller platform_device */
+static int s5pc100_spdif_cfg_gpd(struct platform_device *pdev)
+{
+        s3c_gpio_cfgpin_range(S5PC100_GPD(5), 2, S3C_GPIO_SFN(3));
+
+        return 0;
+}
+
+static int s5pc100_spdif_cfg_gpg3(struct platform_device *pdev)
+{
+        s3c_gpio_cfgpin_range(S5PC100_GPG3(5), 2, S3C_GPIO_SFN(3));
+
+        return 0;
+}
+
+static struct resource s5pc100_spdif_resource[] = {
+        [0] = {
+                .start  = S5PC100_PA_SPDIF,
+                .end    = S5PC100_PA_SPDIF + 0x100 - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = DMACH_SPDIF,
+                .end    = DMACH_SPDIF,
+                .flags  = IORESOURCE_DMA,
+        },
+};
+
+static struct s3c_audio_pdata s5p_spdif_pdata = {
+        .cfg_gpio = s5pc100_spdif_cfg_gpd,
+};
+
+static u64 s5pc100_spdif_dmamask = DMA_BIT_MASK(32);
+
+struct platform_device s5pc100_device_spdif = {
+        .name           = "samsung-spdif",
+        .id             = -1,
+        .num_resources  = ARRAY_SIZE(s5pc100_spdif_resource),
+        .resource       = s5pc100_spdif_resource,
+        .dev = {
+                .platform_data = &s5p_spdif_pdata,
+                .dma_mask = &s5pc100_spdif_dmamask,
+                .coherent_dma_mask = DMA_BIT_MASK(32),
+        },
+};
+
+void __init s5pc100_spdif_setup_gpio(int gpio)
+{
+        if (gpio == S5PC100_SPDIF_GPD)
+                s5p_spdif_pdata.cfg_gpio = s5pc100_spdif_cfg_gpd;
+        else
+                s5p_spdif_pdata.cfg_gpio = s5pc100_spdif_cfg_gpg3;
+}
diff --git a/arch/arm/mach-s5pc100/dev-spi.c b/arch/arm/mach-s5pc100/dev-spi.c
index a0ef7c302c16..57b19794d9bb 100644
--- a/arch/arm/mach-s5pc100/dev-spi.c
+++ b/arch/arm/mach-s5pc100/dev-spi.c
@@ -38,30 +38,20 @@ static int s5pc100_spi_cfg_gpio(struct platform_device *pdev)
 {
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S5PC100_GPB(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PC100_GPB(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PC100_GPB(2), S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(S5PC100_GPB(0), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PC100_GPB(1), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PC100_GPB(2), S3C_GPIO_PULL_UP);
+                s3c_gpio_cfgall_range(S5PC100_GPB(0), 3,
+                                      S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
                 break;
 
         case 1:
-                s3c_gpio_cfgpin(S5PC100_GPB(4), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PC100_GPB(5), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PC100_GPB(6), S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(S5PC100_GPB(4), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PC100_GPB(5), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PC100_GPB(6), S3C_GPIO_PULL_UP);
+                s3c_gpio_cfgall_range(S5PC100_GPB(4), 3,
+                                      S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
                 break;
 
         case 2:
                 s3c_gpio_cfgpin(S5PC100_GPG3(0), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PC100_GPG3(2), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PC100_GPG3(3), S3C_GPIO_SFN(3));
                 s3c_gpio_setpull(S5PC100_GPG3(0), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PC100_GPG3(2), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PC100_GPG3(3), S3C_GPIO_PULL_UP);
+                s3c_gpio_cfgall_range(S5PC100_GPB(2), 2,
+                                      S3C_GPIO_SFN(3), S3C_GPIO_PULL_UP);
                 break;
 
         default:
diff --git a/arch/arm/mach-s5pc100/dma.c b/arch/arm/mach-s5pc100/dma.c
index 0f5517571e2c..bf4cd0fb97c6 100644
--- a/arch/arm/mach-s5pc100/dma.c
+++ b/arch/arm/mach-s5pc100/dma.c
@@ -81,7 +81,7 @@ static struct s3c_pl330_platdata s5pc100_pdma0_pdata = {
 
 static struct platform_device s5pc100_device_pdma0 = {
         .name           = "s3c-pl330",
-        .id             = 1,
+        .id             = 0,
         .num_resources  = ARRAY_SIZE(s5pc100_pdma0_resource),
         .resource       = s5pc100_pdma0_resource,
         .dev            = {
@@ -143,7 +143,7 @@ static struct s3c_pl330_platdata s5pc100_pdma1_pdata = {
 
 static struct platform_device s5pc100_device_pdma1 = {
         .name           = "s3c-pl330",
-        .id             = 2,
+        .id             = 1,
         .num_resources  = ARRAY_SIZE(s5pc100_pdma1_resource),
         .resource       = s5pc100_pdma1_resource,
         .dev            = {
diff --git a/arch/arm/mach-s5pc100/gpiolib.c b/arch/arm/mach-s5pc100/gpiolib.c
index 0fab7f2cd8bf..20856eb7dd51 100644
--- a/arch/arm/mach-s5pc100/gpiolib.c
+++ b/arch/arm/mach-s5pc100/gpiolib.c
@@ -1,5 +1,7 @@
-/*
- * arch/arm/plat-s5pc100/gpiolib.c
+/* linux/arch/arm/mach-s5pc100/gpiolib.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
  *
  *  Copyright 2009 Samsung Electronics Co
  *  Kyungmin Park <kyungmin.park@samsung.com>
@@ -61,30 +63,6 @@
  * L3   8       4Bit    None
  */
 
-static int s5pc100_gpiolib_to_irq(struct gpio_chip *chip, unsigned int offset)
-{
-        return S3C_IRQ_GPIO(chip->base + offset);
-}
-
-static int s5pc100_gpiolib_to_eint(struct gpio_chip *chip, unsigned int offset)
-{
-        int base;
-
-        base = chip->base - S5PC100_GPH0(0);
-        if (base == 0)
-                return IRQ_EINT(offset);
-        base = chip->base - S5PC100_GPH1(0);
-        if (base == 0)
-                return IRQ_EINT(8 + offset);
-        base = chip->base - S5PC100_GPH2(0);
-        if (base == 0)
-                return IRQ_EINT(16 + offset);
-        base = chip->base - S5PC100_GPH3(0);
-        if (base == 0)
-                return IRQ_EINT(24 + offset);
-        return -EINVAL;
-}
-
 static struct s3c_gpio_cfg gpio_cfg = {
         .set_config     = s3c_gpio_setcfg_s3c64xx_4bit,
         .set_pull       = s3c_gpio_setpull_updown,
@@ -104,209 +82,150 @@ static struct s3c_gpio_cfg gpio_cfg_noint = {
         .get_pull       = s3c_gpio_getpull_updown,
 };
 
+/*
+ * GPIO bank's base address given the index of the bank in the
+ * list of all gpio banks.
+ */
+#define S5PC100_BANK_BASE(bank_nr)      (S5P_VA_GPIO + ((bank_nr) * 0x20))
+
+/*
+ * Following are the gpio banks in S5PC100.
+ *
+ * The 'config' member when left to NULL, is initialized to the default
+ * structure gpio_cfg in the init function below.
+ *
+ * The 'base' member is also initialized in the init function below.
+ * Note: The initialization of 'base' member of s3c_gpio_chip structure
+ * uses the above macro and depends on the banks being listed in order here.
+ */
 static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
         {
-                .base   = S5PC100_GPA0_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPA0(0),
                         .ngpio  = S5PC100_GPIO_A0_NR,
                         .label  = "GPA0",
                 },
         }, {
-                .base   = S5PC100_GPA1_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPA1(0),
                         .ngpio  = S5PC100_GPIO_A1_NR,
                         .label  = "GPA1",
                 },
         }, {
-                .base   = S5PC100_GPB_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPB(0),
                         .ngpio  = S5PC100_GPIO_B_NR,
                         .label  = "GPB",
                 },
         }, {
-                .base   = S5PC100_GPC_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPC(0),
                         .ngpio  = S5PC100_GPIO_C_NR,
                         .label  = "GPC",
                 },
         }, {
-                .base   = S5PC100_GPD_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPD(0),
                         .ngpio  = S5PC100_GPIO_D_NR,
                         .label  = "GPD",
                 },
         }, {
-                .base   = S5PC100_GPE0_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPE0(0),
                         .ngpio  = S5PC100_GPIO_E0_NR,
                         .label  = "GPE0",
                 },
         }, {
-                .base   = S5PC100_GPE1_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPE1(0),
                         .ngpio  = S5PC100_GPIO_E1_NR,
                         .label  = "GPE1",
                 },
         }, {
-                .base   = S5PC100_GPF0_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPF0(0),
                         .ngpio  = S5PC100_GPIO_F0_NR,
                         .label  = "GPF0",
                 },
         }, {
-                .base   = S5PC100_GPF1_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPF1(0),
                         .ngpio  = S5PC100_GPIO_F1_NR,
                         .label  = "GPF1",
                 },
         }, {
-                .base   = S5PC100_GPF2_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPF2(0),
                         .ngpio  = S5PC100_GPIO_F2_NR,
                         .label  = "GPF2",
                 },
         }, {
-                .base   = S5PC100_GPF3_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPF3(0),
                         .ngpio  = S5PC100_GPIO_F3_NR,
                         .label  = "GPF3",
                 },
         }, {
-                .base   = S5PC100_GPG0_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPG0(0),
                         .ngpio  = S5PC100_GPIO_G0_NR,
                         .label  = "GPG0",
                 },
         }, {
-                .base   = S5PC100_GPG1_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPG1(0),
                         .ngpio  = S5PC100_GPIO_G1_NR,
                         .label  = "GPG1",
                 },
         }, {
-                .base   = S5PC100_GPG2_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPG2(0),
                         .ngpio  = S5PC100_GPIO_G2_NR,
                         .label  = "GPG2",
                 },
         }, {
-                .base   = S5PC100_GPG3_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPG3(0),
                         .ngpio  = S5PC100_GPIO_G3_NR,
                         .label  = "GPG3",
                 },
         }, {
-                .base   = S5PC100_GPH0_BASE,
-                .config = &gpio_cfg_eint,
-                .chip   = {
-                        .base   = S5PC100_GPH0(0),
-                        .ngpio  = S5PC100_GPIO_H0_NR,
-                        .label  = "GPH0",
-                },
-        }, {
-                .base   = S5PC100_GPH1_BASE,
-                .config = &gpio_cfg_eint,
-                .chip   = {
-                        .base   = S5PC100_GPH1(0),
-                        .ngpio  = S5PC100_GPIO_H1_NR,
-                        .label  = "GPH1",
-                },
-        }, {
-                .base   = S5PC100_GPH2_BASE,
-                .config = &gpio_cfg_eint,
-                .chip   = {
-                        .base   = S5PC100_GPH2(0),
-                        .ngpio  = S5PC100_GPIO_H2_NR,
-                        .label  = "GPH2",
-                },
-        }, {
-                .base   = S5PC100_GPH3_BASE,
-                .config = &gpio_cfg_eint,
-                .chip   = {
-                        .base   = S5PC100_GPH3(0),
-                        .ngpio  = S5PC100_GPIO_H3_NR,
-                        .label  = "GPH3",
-                },
-        }, {
-                .base   = S5PC100_GPI_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPI(0),
                         .ngpio  = S5PC100_GPIO_I_NR,
                         .label  = "GPI",
                 },
         }, {
-                .base   = S5PC100_GPJ0_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPJ0(0),
                         .ngpio  = S5PC100_GPIO_J0_NR,
                         .label  = "GPJ0",
                 },
         }, {
-                .base   = S5PC100_GPJ1_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPJ1(0),
                         .ngpio  = S5PC100_GPIO_J1_NR,
                         .label  = "GPJ1",
                 },
         }, {
-                .base   = S5PC100_GPJ2_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPJ2(0),
                         .ngpio  = S5PC100_GPIO_J2_NR,
                         .label  = "GPJ2",
                 },
         }, {
-                .base   = S5PC100_GPJ3_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPJ3(0),
                         .ngpio  = S5PC100_GPIO_J3_NR,
                         .label  = "GPJ3",
                 },
         }, {
-                .base   = S5PC100_GPJ4_BASE,
-                .config = &gpio_cfg,
                 .chip   = {
                         .base   = S5PC100_GPJ4(0),
                         .ngpio  = S5PC100_GPIO_J4_NR,
                         .label  = "GPJ4",
                 },
         }, {
-                .base   = S5PC100_GPK0_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPK0(0),
@@ -314,7 +233,6 @@ static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
                         .label  = "GPK0",
                 },
         }, {
-                .base   = S5PC100_GPK1_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPK1(0),
@@ -322,7 +240,6 @@ static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
                         .label  = "GPK1",
                 },
         }, {
-                .base   = S5PC100_GPK2_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPK2(0),
@@ -330,7 +247,6 @@ static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
                         .label  = "GPK2",
                 },
         }, {
-                .base   = S5PC100_GPK3_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPK3(0),
@@ -338,7 +254,6 @@ static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
                         .label  = "GPK3",
                 },
         }, {
-                .base   = S5PC100_GPL0_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPL0(0),
@@ -346,7 +261,6 @@ static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
                         .label  = "GPL0",
                 },
         }, {
-                .base   = S5PC100_GPL1_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPL1(0),
@@ -354,7 +268,6 @@ static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
                         .label  = "GPL1",
                 },
         }, {
-                .base   = S5PC100_GPL2_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPL2(0),
@@ -362,7 +275,6 @@ static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
                         .label  = "GPL2",
                 },
         }, {
-                .base   = S5PC100_GPL3_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPL3(0),
@@ -370,56 +282,72 @@ static struct s3c_gpio_chip s5pc100_gpio_chips[] = {
                         .label  = "GPL3",
                 },
         }, {
-                .base   = S5PC100_GPL4_BASE,
                 .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PC100_GPL4(0),
                         .ngpio  = S5PC100_GPIO_L4_NR,
                         .label  = "GPL4",
                 },
+        }, {
+                .base   = (S5P_VA_GPIO + 0xC00),
+                .config = &gpio_cfg_eint,
+                .irq_base = IRQ_EINT(0),
+                .chip   = {
+                        .base   = S5PC100_GPH0(0),
+                        .ngpio  = S5PC100_GPIO_H0_NR,
+                        .label  = "GPH0",
+                        .to_irq = samsung_gpiolib_to_irq,
+                },
+        }, {
+                .base   = (S5P_VA_GPIO + 0xC20),
+                .config = &gpio_cfg_eint,
+                .irq_base = IRQ_EINT(8),
+                .chip   = {
+                        .base   = S5PC100_GPH1(0),
+                        .ngpio  = S5PC100_GPIO_H1_NR,
+                        .label  = "GPH1",
+                        .to_irq = samsung_gpiolib_to_irq,
+                },
+        }, {
+                .base   = (S5P_VA_GPIO + 0xC40),
+                .config = &gpio_cfg_eint,
+                .irq_base = IRQ_EINT(16),
+                .chip   = {
+                        .base   = S5PC100_GPH2(0),
+                        .ngpio  = S5PC100_GPIO_H2_NR,
+                        .label  = "GPH2",
+                        .to_irq = samsung_gpiolib_to_irq,
+                },
+        }, {
+                .base   = (S5P_VA_GPIO + 0xC60),
+                .config = &gpio_cfg_eint,
+                .irq_base = IRQ_EINT(24),
+                .chip   = {
+                        .base   = S5PC100_GPH3(0),
+                        .ngpio  = S5PC100_GPIO_H3_NR,
+                        .label  = "GPH3",
+                        .to_irq = samsung_gpiolib_to_irq,
+                },
         },
 };
 
-/* FIXME move from irq-gpio.c */
-extern struct irq_chip s5pc100_gpioint;
-extern void s5pc100_irq_gpioint_handler(unsigned int irq, struct irq_desc *desc);
-
-static __init void s5pc100_gpiolib_link(struct s3c_gpio_chip *chip)
+static __init int s5pc100_gpiolib_init(void)
 {
-        /* Interrupt */
-        if (chip->config == &gpio_cfg) {
-                int i, irq;
-
-                chip->chip.to_irq = s5pc100_gpiolib_to_irq;
+        struct s3c_gpio_chip *chip = s5pc100_gpio_chips;
+        int nr_chips = ARRAY_SIZE(s5pc100_gpio_chips);
+        int gpioint_group = 0;
+        int i;
 
-                for (i = 0;  i < chip->chip.ngpio; i++) {
-                        irq = S3C_IRQ_GPIO_BASE + chip->chip.base + i;
-                        set_irq_chip(irq, &s5pc100_gpioint);
-                        set_irq_data(irq, &chip->chip);
-                        set_irq_handler(irq, handle_level_irq);
-                        set_irq_flags(irq, IRQF_VALID);
+        for (i = 0; i < nr_chips; i++, chip++) {
+                if (chip->config == NULL) {
+                        chip->config = &gpio_cfg;
+                        chip->group = gpioint_group++;
                 }
-        } else if (chip->config == &gpio_cfg_eint) {
-                chip->chip.to_irq = s5pc100_gpiolib_to_eint;
+                if (chip->base == NULL)
+                        chip->base = S5PC100_BANK_BASE(i);
         }
-}
-
-static __init int s5pc100_gpiolib_init(void)
-{
-        struct s3c_gpio_chip *chip;
-        int nr_chips;
-
-        chip = s5pc100_gpio_chips;
-        nr_chips = ARRAY_SIZE(s5pc100_gpio_chips);
-
-        for (; nr_chips > 0; nr_chips--, chip++)
-                s5pc100_gpiolib_link(chip);
-
-        samsung_gpiolib_add_4bit_chips(s5pc100_gpio_chips,
-                                       ARRAY_SIZE(s5pc100_gpio_chips));
 
-        /* Interrupt */
-        set_irq_chained_handler(IRQ_GPIOINT, s5pc100_irq_gpioint_handler);
+        samsung_gpiolib_add_4bit_chips(s5pc100_gpio_chips, nr_chips);
 
         return 0;
 }
diff --git a/arch/arm/mach-s5pc100/include/mach/gpio.h b/arch/arm/mach-s5pc100/include/mach/gpio.h
index 71ae1f52df1d..29a8a12d9b4f 100644
--- a/arch/arm/mach-s5pc100/include/mach/gpio.h
+++ b/arch/arm/mach-s5pc100/include/mach/gpio.h
@@ -146,13 +146,6 @@ enum s5p_gpio_number {
 /* define the number of gpios we need to the one after the MP04() range */
 #define ARCH_NR_GPIOS           (S5PC100_GPIO_END + 1)
 
-#define EINT_MODE               S3C_GPIO_SFN(0x2)
-
-#define EINT_GPIO_0(x)          S5PC100_GPH0(x)
-#define EINT_GPIO_1(x)          S5PC100_GPH1(x)
-#define EINT_GPIO_2(x)          S5PC100_GPH2(x)
-#define EINT_GPIO_3(x)          S5PC100_GPH3(x)
-
 #include <asm-generic/gpio.h>
 
 #endif /* __ASM_ARCH_GPIO_H */
diff --git a/arch/arm/mach-s5pc100/include/mach/irqs.h b/arch/arm/mach-s5pc100/include/mach/irqs.h
index 06513e647242..d2eb4757381f 100644
--- a/arch/arm/mach-s5pc100/include/mach/irqs.h
+++ b/arch/arm/mach-s5pc100/include/mach/irqs.h
@@ -48,8 +48,8 @@
 #define IRQ_SPI1                S5P_IRQ_VIC1(16)
 #define IRQ_SPI2                S5P_IRQ_VIC1(17)
 #define IRQ_IRDA                S5P_IRQ_VIC1(18)
-#define IRQ_CAN0                S5P_IRQ_VIC1(19)
-#define IRQ_CAN1                S5P_IRQ_VIC1(20)
+#define IRQ_IIC2                S5P_IRQ_VIC1(19)
+#define IRQ_IIC3                S5P_IRQ_VIC1(20)
 #define IRQ_HSIRX               S5P_IRQ_VIC1(21)
 #define IRQ_HSITX               S5P_IRQ_VIC1(22)
 #define IRQ_UHOST               S5P_IRQ_VIC1(23)
@@ -100,11 +100,12 @@
 #define S5P_EINT_BASE1          (S5P_IRQ_VIC0(0))
 #define S5P_EINT_BASE2          (IRQ_VIC_END + 1)
 
-#define S3C_IRQ_GPIO_BASE       (IRQ_EINT(31) + 1)
-#define S3C_IRQ_GPIO(x)         (S3C_IRQ_GPIO_BASE + (x))
+/* GPIO interrupt */
+#define S5P_GPIOINT_BASE        (IRQ_EINT(31) + 1)
+#define S5P_GPIOINT_GROUP_MAXNR 21
 
-/* Until MP04 Groups -> 40 (exactly 39) Groups * 8 ~= 320 GPIOs */
-#define NR_IRQS         (S3C_IRQ_GPIO(320) + 1)
+/* Set the default NR_IRQS */
+#define NR_IRQS                 (IRQ_EINT(31) + S5P_GPIOINT_COUNT + 1)
 
 /* Compatibility */
 #define IRQ_LCD_FIFO            IRQ_LCD0
diff --git a/arch/arm/mach-s5pc100/include/mach/map.h b/arch/arm/mach-s5pc100/include/mach/map.h
index 8751ef4a6804..32e9cab5c864 100644
--- a/arch/arm/mach-s5pc100/include/mach/map.h
+++ b/arch/arm/mach-s5pc100/include/mach/map.h
@@ -110,6 +110,8 @@
 #define S5PC100_PA_PCM0         0xF2400000
 #define S5PC100_PA_PCM1         0xF2500000
 
+#define S5PC100_PA_SPDIF        0xF2600000
+
 #define S5PC100_PA_TSADC        (0xF3000000)
 
 /* KEYPAD */
diff --git a/arch/arm/mach-s5pc100/include/mach/regs-gpio.h b/arch/arm/mach-s5pc100/include/mach/regs-gpio.h
index dd6295e1251d..0bf73209ec7b 100644
--- a/arch/arm/mach-s5pc100/include/mach/regs-gpio.h
+++ b/arch/arm/mach-s5pc100/include/mach/regs-gpio.h
@@ -11,43 +11,6 @@
 
 #include <mach/map.h>
 
-/* S5PC100 */
-#define S5PC100_GPIO_BASE       S5P_VA_GPIO
-#define S5PC100_GPA0_BASE       (S5PC100_GPIO_BASE + 0x0000)
-#define S5PC100_GPA1_BASE       (S5PC100_GPIO_BASE + 0x0020)
-#define S5PC100_GPB_BASE        (S5PC100_GPIO_BASE + 0x0040)
-#define S5PC100_GPC_BASE        (S5PC100_GPIO_BASE + 0x0060)
-#define S5PC100_GPD_BASE        (S5PC100_GPIO_BASE + 0x0080)
-#define S5PC100_GPE0_BASE       (S5PC100_GPIO_BASE + 0x00A0)
-#define S5PC100_GPE1_BASE       (S5PC100_GPIO_BASE + 0x00C0)
-#define S5PC100_GPF0_BASE       (S5PC100_GPIO_BASE + 0x00E0)
-#define S5PC100_GPF1_BASE       (S5PC100_GPIO_BASE + 0x0100)
-#define S5PC100_GPF2_BASE       (S5PC100_GPIO_BASE + 0x0120)
-#define S5PC100_GPF3_BASE       (S5PC100_GPIO_BASE + 0x0140)
-#define S5PC100_GPG0_BASE       (S5PC100_GPIO_BASE + 0x0160)
-#define S5PC100_GPG1_BASE       (S5PC100_GPIO_BASE + 0x0180)
-#define S5PC100_GPG2_BASE       (S5PC100_GPIO_BASE + 0x01A0)
-#define S5PC100_GPG3_BASE       (S5PC100_GPIO_BASE + 0x01C0)
-#define S5PC100_GPH0_BASE       (S5PC100_GPIO_BASE + 0x0C00)
-#define S5PC100_GPH1_BASE       (S5PC100_GPIO_BASE + 0x0C20)
-#define S5PC100_GPH2_BASE       (S5PC100_GPIO_BASE + 0x0C40)
-#define S5PC100_GPH3_BASE       (S5PC100_GPIO_BASE + 0x0C60)
-#define S5PC100_GPI_BASE        (S5PC100_GPIO_BASE + 0x01E0)
-#define S5PC100_GPJ0_BASE       (S5PC100_GPIO_BASE + 0x0200)
-#define S5PC100_GPJ1_BASE       (S5PC100_GPIO_BASE + 0x0220)
-#define S5PC100_GPJ2_BASE       (S5PC100_GPIO_BASE + 0x0240)
-#define S5PC100_GPJ3_BASE       (S5PC100_GPIO_BASE + 0x0260)
-#define S5PC100_GPJ4_BASE       (S5PC100_GPIO_BASE + 0x0280)
-#define S5PC100_GPK0_BASE       (S5PC100_GPIO_BASE + 0x02A0)
-#define S5PC100_GPK1_BASE       (S5PC100_GPIO_BASE + 0x02C0)
-#define S5PC100_GPK2_BASE       (S5PC100_GPIO_BASE + 0x02E0)
-#define S5PC100_GPK3_BASE       (S5PC100_GPIO_BASE + 0x0300)
-#define S5PC100_GPL0_BASE       (S5PC100_GPIO_BASE + 0x0320)
-#define S5PC100_GPL1_BASE       (S5PC100_GPIO_BASE + 0x0340)
-#define S5PC100_GPL2_BASE       (S5PC100_GPIO_BASE + 0x0360)
-#define S5PC100_GPL3_BASE       (S5PC100_GPIO_BASE + 0x0380)
-#define S5PC100_GPL4_BASE       (S5PC100_GPIO_BASE + 0x03A0)
-
 #define S5PC100EINT30CON                (S5P_VA_GPIO + 0xE00)
 #define S5P_EINT_CON(x)                 (S5PC100EINT30CON + ((x) * 0x4))
 
@@ -64,12 +27,12 @@
 
 #define eint_irq_to_bit(irq)            (1 << (EINT_OFFSET(irq) & 0x7))
 
-/* values for S5P_EXTINT0 */
-#define S5P_EXTINT_LOWLEV               (0x00)
-#define S5P_EXTINT_HILEV                (0x01)
-#define S5P_EXTINT_FALLEDGE             (0x02)
-#define S5P_EXTINT_RISEEDGE             (0x03)
-#define S5P_EXTINT_BOTHEDGE             (0x04)
+#define EINT_MODE               S3C_GPIO_SFN(0x2)
+
+#define EINT_GPIO_0(x)          S5PC100_GPH0(x)
+#define EINT_GPIO_1(x)          S5PC100_GPH1(x)
+#define EINT_GPIO_2(x)          S5PC100_GPH2(x)
+#define EINT_GPIO_3(x)          S5PC100_GPH3(x)
 
 #endif /* __ASM_MACH_S5PC100_REGS_GPIO_H */
 
diff --git a/arch/arm/mach-s5pc100/include/mach/vmalloc.h b/arch/arm/mach-s5pc100/include/mach/vmalloc.h
index be9df79903ed..44c8e5726d9d 100644
--- a/arch/arm/mach-s5pc100/include/mach/vmalloc.h
+++ b/arch/arm/mach-s5pc100/include/mach/vmalloc.h
@@ -12,6 +12,6 @@
 #ifndef __ASM_ARCH_VMALLOC_H
 #define __ASM_ARCH_VMALLOC_H
 
-#define VMALLOC_END       (0xe0000000UL)
+#define VMALLOC_END     0xF6000000UL
 
 #endif /* __ASM_ARCH_VMALLOC_H */
diff --git a/arch/arm/mach-s5pc100/irq-gpio.c b/arch/arm/mach-s5pc100/irq-gpio.c
deleted file mode 100644
index 2bf86c18bc73..000000000000
--- a/arch/arm/mach-s5pc100/irq-gpio.c
+++ /dev/null
@@ -1,266 +0,0 @@
-/*
- * arch/arm/mach-s5pc100/irq-gpio.c
- *
- * Copyright (C) 2009 Samsung Electronics
- *
- * S5PC100 - Interrupt handling for IRQ_GPIO${group}(x)
- *
- * 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/kernel.h>
-#include <linux/interrupt.h>
-#include <linux/irq.h>
-#include <linux/io.h>
-#include <linux/gpio.h>
-
-#include <mach/map.h>
-#include <plat/gpio-cfg.h>
-
-#define S5P_GPIOREG(x)          (S5P_VA_GPIO + (x))
-
-#define CON_OFFSET                      0x700
-#define MASK_OFFSET                     0x900
-#define PEND_OFFSET                     0xA00
-#define CON_OFFSET_2                    0xE00
-#define MASK_OFFSET_2                   0xF00
-#define PEND_OFFSET_2                   0xF40
-
-#define GPIOINT_LEVEL_LOW               0x0
-#define GPIOINT_LEVEL_HIGH              0x1
-#define GPIOINT_EDGE_FALLING            0x2
-#define GPIOINT_EDGE_RISING             0x3
-#define GPIOINT_EDGE_BOTH               0x4
-
-static int group_to_con_offset(int group)
-{
-        return group << 2;
-}
-
-static int group_to_mask_offset(int group)
-{
-        return group << 2;
-}
-
-static int group_to_pend_offset(int group)
-{
-        return group << 2;
-}
-
-static int s5pc100_get_start(unsigned int group)
-{
-        switch (group) {
-        case 0: return S5PC100_GPIO_A0_START;
-        case 1: return S5PC100_GPIO_A1_START;
-        case 2: return S5PC100_GPIO_B_START;
-        case 3: return S5PC100_GPIO_C_START;
-        case 4: return S5PC100_GPIO_D_START;
-        case 5: return S5PC100_GPIO_E0_START;
-        case 6: return S5PC100_GPIO_E1_START;
-        case 7: return S5PC100_GPIO_F0_START;
-        case 8: return S5PC100_GPIO_F1_START;
-        case 9: return S5PC100_GPIO_F2_START;
-        case 10: return S5PC100_GPIO_F3_START;
-        case 11: return S5PC100_GPIO_G0_START;
-        case 12: return S5PC100_GPIO_G1_START;
-        case 13: return S5PC100_GPIO_G2_START;
-        case 14: return S5PC100_GPIO_G3_START;
-        case 15: return S5PC100_GPIO_I_START;
-        case 16: return S5PC100_GPIO_J0_START;
-        case 17: return S5PC100_GPIO_J1_START;
-        case 18: return S5PC100_GPIO_J2_START;
-        case 19: return S5PC100_GPIO_J3_START;
-        case 20: return S5PC100_GPIO_J4_START;
-        default:
-                BUG();
-        }
-
-        return -EINVAL;
-}
-
-static int s5pc100_get_group(unsigned int irq)
-{
-        irq -= S3C_IRQ_GPIO(0);
-
-        switch (irq) {
-        case S5PC100_GPIO_A0_START ... S5PC100_GPIO_A1_START - 1:
-                return 0;
-        case S5PC100_GPIO_A1_START ... S5PC100_GPIO_B_START - 1:
-                return 1;
-        case S5PC100_GPIO_B_START ... S5PC100_GPIO_C_START - 1:
-                return 2;
-        case S5PC100_GPIO_C_START ... S5PC100_GPIO_D_START - 1:
-                return 3;
-        case S5PC100_GPIO_D_START ... S5PC100_GPIO_E0_START - 1:
-                return 4;
-        case S5PC100_GPIO_E0_START ... S5PC100_GPIO_E1_START - 1:
-                return 5;
-        case S5PC100_GPIO_E1_START ... S5PC100_GPIO_F0_START - 1:
-                return 6;
-        case S5PC100_GPIO_F0_START ... S5PC100_GPIO_F1_START - 1:
-                return 7;
-        case S5PC100_GPIO_F1_START ... S5PC100_GPIO_F2_START - 1:
-                return 8;
-        case S5PC100_GPIO_F2_START ... S5PC100_GPIO_F3_START - 1:
-                return 9;
-        case S5PC100_GPIO_F3_START ... S5PC100_GPIO_G0_START - 1:
-                return 10;
-        case S5PC100_GPIO_G0_START ... S5PC100_GPIO_G1_START - 1:
-                return 11;
-        case S5PC100_GPIO_G1_START ... S5PC100_GPIO_G2_START - 1:
-                return 12;
-        case S5PC100_GPIO_G2_START ... S5PC100_GPIO_G3_START - 1:
-                return 13;
-        case S5PC100_GPIO_G3_START ... S5PC100_GPIO_H0_START - 1:
-                return 14;
-        case S5PC100_GPIO_I_START ... S5PC100_GPIO_J0_START - 1:
-                return 15;
-        case S5PC100_GPIO_J0_START ... S5PC100_GPIO_J1_START - 1:
-                return 16;
-        case S5PC100_GPIO_J1_START ... S5PC100_GPIO_J2_START - 1:
-                return 17;
-        case S5PC100_GPIO_J2_START ... S5PC100_GPIO_J3_START - 1:
-                return 18;
-        case S5PC100_GPIO_J3_START ... S5PC100_GPIO_J4_START - 1:
-                return 19;
-        case S5PC100_GPIO_J4_START ... S5PC100_GPIO_K0_START - 1:
-                return 20;
-        default:
-                BUG();
-        }
-
-        return -EINVAL;
-}
-
-static int s5pc100_get_offset(unsigned int irq)
-{
-        struct gpio_chip *chip = get_irq_data(irq);
-        return irq - S3C_IRQ_GPIO(chip->base);
-}
-
-static void s5pc100_gpioint_ack(unsigned int irq)
-{
-        int group, offset, pend_offset;
-        unsigned int value;
-
-        group = s5pc100_get_group(irq);
-        offset = s5pc100_get_offset(irq);
-        pend_offset = group_to_pend_offset(group);
-
-        value = __raw_readl(S5P_GPIOREG(PEND_OFFSET) + pend_offset);
-        value |= 1 << offset;
-        __raw_writel(value, S5P_GPIOREG(PEND_OFFSET) + pend_offset);
-}
-
-static void s5pc100_gpioint_mask(unsigned int irq)
-{
-        int group, offset, mask_offset;
-        unsigned int value;
-
-        group = s5pc100_get_group(irq);
-        offset = s5pc100_get_offset(irq);
-        mask_offset = group_to_mask_offset(group);
-
-        value = __raw_readl(S5P_GPIOREG(MASK_OFFSET) + mask_offset);
-        value |= 1 << offset;
-        __raw_writel(value, S5P_GPIOREG(MASK_OFFSET) + mask_offset);
-}
-
-static void s5pc100_gpioint_unmask(unsigned int irq)
-{
-        int group, offset, mask_offset;
-        unsigned int value;
-
-        group = s5pc100_get_group(irq);
-        offset = s5pc100_get_offset(irq);
-        mask_offset = group_to_mask_offset(group);
-
-        value = __raw_readl(S5P_GPIOREG(MASK_OFFSET) + mask_offset);
-        value &= ~(1 << offset);
-        __raw_writel(value, S5P_GPIOREG(MASK_OFFSET) + mask_offset);
-}
-
-static void s5pc100_gpioint_mask_ack(unsigned int irq)
-{
-        s5pc100_gpioint_mask(irq);
-        s5pc100_gpioint_ack(irq);
-}
-
-static int s5pc100_gpioint_set_type(unsigned int irq, unsigned int type)
-{
-        int group, offset, con_offset;
-        unsigned int value;
-
-        group = s5pc100_get_group(irq);
-        offset = s5pc100_get_offset(irq);
-        con_offset = group_to_con_offset(group);
-
-        switch (type) {
-        case IRQ_TYPE_NONE:
-                printk(KERN_WARNING "No irq type\n");
-                return -EINVAL;
-        case IRQ_TYPE_EDGE_RISING:
-                type = GPIOINT_EDGE_RISING;
-                break;
-        case IRQ_TYPE_EDGE_FALLING:
-                type = GPIOINT_EDGE_FALLING;
-                break;
-        case IRQ_TYPE_EDGE_BOTH:
-                type = GPIOINT_EDGE_BOTH;
-                break;
-        case IRQ_TYPE_LEVEL_HIGH:
-                type = GPIOINT_LEVEL_HIGH;
-                break;
-        case IRQ_TYPE_LEVEL_LOW:
-                type = GPIOINT_LEVEL_LOW;
-                break;
-        default:
-                BUG();
-        }
-
-
-        value = __raw_readl(S5P_GPIOREG(CON_OFFSET) + con_offset);
-        value &= ~(0xf << (offset * 0x4));
-        value |= (type << (offset * 0x4));
-        __raw_writel(value, S5P_GPIOREG(CON_OFFSET) + con_offset);
-
-        return 0;
-}
-
-struct irq_chip s5pc100_gpioint = {
-        .name           = "GPIO",
-        .ack            = s5pc100_gpioint_ack,
-        .mask           = s5pc100_gpioint_mask,
-        .mask_ack       = s5pc100_gpioint_mask_ack,
-        .unmask         = s5pc100_gpioint_unmask,
-        .set_type       = s5pc100_gpioint_set_type,
-};
-
-void s5pc100_irq_gpioint_handler(unsigned int irq, struct irq_desc *desc)
-{
-        int group, offset, pend_offset, mask_offset;
-        int real_irq, group_end;
-        unsigned int pend, mask;
-
-        group_end = 21;
-
-        for (group = 0; group < group_end; group++) {
-                pend_offset = group_to_pend_offset(group);
-                pend = __raw_readl(S5P_GPIOREG(PEND_OFFSET) + pend_offset);
-                if (!pend)
-                        continue;
-
-                mask_offset = group_to_mask_offset(group);
-                mask = __raw_readl(S5P_GPIOREG(MASK_OFFSET) + mask_offset);
-                pend &= ~mask;
-
-                for (offset = 0; offset < 8; offset++) {
-                        if (pend & (1 << offset)) {
-                                real_irq = s5pc100_get_start(group) + offset;
-                                generic_handle_irq(S3C_IRQ_GPIO(real_irq));
-                        }
-                }
-        }
-}
diff --git a/arch/arm/mach-s5pc100/mach-smdkc100.c b/arch/arm/mach-s5pc100/mach-smdkc100.c
index 880fb075092c..18b405d514d6 100644
--- a/arch/arm/mach-s5pc100/mach-smdkc100.c
+++ b/arch/arm/mach-s5pc100/mach-smdkc100.c
@@ -47,6 +47,7 @@
 #include <plat/adc.h>
 #include <plat/keypad.h>
 #include <plat/ts.h>
+#include <plat/audio.h>
 
 /* Following are default values for UCON, ULCON and UFCON UART registers */
 #define SMDKC100_UCON_DEFAULT   (S3C2410_UCON_TXILEVEL |        \
@@ -196,6 +197,7 @@ static struct platform_device *smdkc100_devices[] __initdata = {
         &s5p_device_fimc0,
         &s5p_device_fimc1,
         &s5p_device_fimc2,
+        &s5pc100_device_spdif,
 };
 
 static struct s3c2410_ts_mach_info s3c_ts_platform __initdata = {
@@ -226,6 +228,8 @@ static void __init smdkc100_machine_init(void)
 
         samsung_keypad_set_platdata(&smdkc100_keypad_data);
 
+        s5pc100_spdif_setup_gpio(S5PC100_SPDIF_GPD);
+
         /* LCD init */
         gpio_request(S5PC100_GPD(0), "GPD");
         gpio_request(S5PC100_GPH0(6), "GPH0");
diff --git a/arch/arm/mach-s5pc100/setup-fb-24bpp.c b/arch/arm/mach-s5pc100/setup-fb-24bpp.c
index 6eba6cb8e2f4..d31c0f3fe222 100644
--- a/arch/arm/mach-s5pc100/setup-fb-24bpp.c
+++ b/arch/arm/mach-s5pc100/setup-fb-24bpp.c
@@ -22,27 +22,15 @@
 
 #define DISR_OFFSET     0x7008
 
-void s5pc100_fb_gpio_setup_24bpp(void)
+static void s5pc100_fb_setgpios(unsigned int base, unsigned int nr)
 {
-        unsigned int gpio = 0;
-
-        for (gpio = S5PC100_GPF0(0); gpio <= S5PC100_GPF0(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
-
-        for (gpio = S5PC100_GPF1(0); gpio <= S5PC100_GPF1(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
-
-        for (gpio = S5PC100_GPF2(0); gpio <= S5PC100_GPF2(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(base, nr, S3C_GPIO_SFN(2));
+}
 
-        for (gpio = S5PC100_GPF3(0); gpio <= S5PC100_GPF3(3); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+void s5pc100_fb_gpio_setup_24bpp(void)
+{
+        s5pc100_fb_setgpios(S5PC100_GPF0(0), 8);
+        s5pc100_fb_setgpios(S5PC100_GPF1(0), 8);
+        s5pc100_fb_setgpios(S5PC100_GPF2(0), 8);
+        s5pc100_fb_setgpios(S5PC100_GPF3(0), 4);
 }
diff --git a/arch/arm/mach-s5pc100/setup-i2c0.c b/arch/arm/mach-s5pc100/setup-i2c0.c
index dd3174e6ecc5..eaef7a3bda49 100644
--- a/arch/arm/mach-s5pc100/setup-i2c0.c
+++ b/arch/arm/mach-s5pc100/setup-i2c0.c
@@ -23,8 +23,6 @@ struct platform_device; /* don't need the contents */
 
 void s3c_i2c0_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5PC100_GPD(3), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PC100_GPD(3), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5PC100_GPD(4), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PC100_GPD(4), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5PC100_GPD(3), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
diff --git a/arch/arm/mach-s5pc100/setup-i2c1.c b/arch/arm/mach-s5pc100/setup-i2c1.c
index d1fec26b69ee..aaff74a90dee 100644
--- a/arch/arm/mach-s5pc100/setup-i2c1.c
+++ b/arch/arm/mach-s5pc100/setup-i2c1.c
@@ -23,8 +23,6 @@ struct platform_device; /* don't need the contents */
 
 void s3c_i2c1_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5PC100_GPD(5), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PC100_GPD(5), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5PC100_GPD(6), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PC100_GPD(6), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5PC100_GPD(5), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
diff --git a/arch/arm/mach-s5pc100/setup-ide.c b/arch/arm/mach-s5pc100/setup-ide.c
index 83575671fb59..223aae044466 100644
--- a/arch/arm/mach-s5pc100/setup-ide.c
+++ b/arch/arm/mach-s5pc100/setup-ide.c
@@ -17,52 +17,39 @@
 #include <mach/regs-clock.h>
 #include <plat/gpio-cfg.h>
 
+static void s5pc100_ide_cfg_gpios(unsigned int base, unsigned int nr)
+{
+        s3c_gpio_cfgrange_nopull(base, nr, S3C_GPIO_SFN(4));
+
+        for (; nr > 0; nr--, base++)
+                s5p_gpio_set_drvstr(base, S5P_GPIO_DRVSTR_LV4);
+}
+
 void s5pc100_ide_setup_gpio(void)
 {
         u32 reg;
-        u32 gpio = 0;
 
         /* Independent CF interface, CF chip select configuration */
         reg = readl(S5PC100_MEM_SYS_CFG) & (~0x3f);
         writel(reg | MEM_SYS_CFG_EBI_FIX_PRI_CFCON, S5PC100_MEM_SYS_CFG);
 
         /* CF_Add[0 - 2], CF_IORDY, CF_INTRQ, CF_DMARQ, CF_DMARST, CF_DMACK */
-        for (gpio = S5PC100_GPJ0(0); gpio <= S5PC100_GPJ0(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(4));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
+        s5pc100_ide_cfg_gpios(S5PC100_GPJ0(0), 8);
 
         /*CF_Data[0 - 7] */
-        for (gpio = S5PC100_GPJ2(0); gpio <= S5PC100_GPJ2(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(4));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
+        s5pc100_ide_cfg_gpios(S5PC100_GPJ2(0), 8);
 
         /* CF_Data[8 - 15] */
-        for (gpio = S5PC100_GPJ3(0); gpio <= S5PC100_GPJ3(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(4));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
+        s5pc100_ide_cfg_gpios(S5PC100_GPJ3(0), 8);
 
         /* CF_CS0, CF_CS1, CF_IORD, CF_IOWR */
-        for (gpio = S5PC100_GPJ4(0); gpio <= S5PC100_GPJ4(3); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(4));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
+        s5pc100_ide_cfg_gpios(S5PC100_GPJ4(0), 4);
 
         /* EBI_OE, EBI_WE */
-        for (gpio = S5PC100_GPK0(6); gpio <= S5PC100_GPK0(7); gpio++)
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(0));
+        s3c_gpio_cfgpin_range(S5PC100_GPK0(6), 2, S3C_GPIO_SFN(0));
 
         /* CF_OE, CF_WE */
-        for (gpio = S5PC100_GPK1(6); gpio <= S5PC100_GPK1(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PC100_GPK1(6), 8, S3C_GPIO_SFN(2));
 
         /* CF_CD */
         s3c_gpio_cfgpin(S5PC100_GPK3(5), S3C_GPIO_SFN(2));
diff --git a/arch/arm/mach-s5pc100/setup-keypad.c b/arch/arm/mach-s5pc100/setup-keypad.c
index d0837a72a58e..ada377f0c206 100644
--- a/arch/arm/mach-s5pc100/setup-keypad.c
+++ b/arch/arm/mach-s5pc100/setup-keypad.c
@@ -15,20 +15,9 @@
 
 void samsung_keypad_cfg_gpio(unsigned int rows, unsigned int cols)
 {
-        unsigned int gpio;
-        unsigned int end;
-
         /* Set all the necessary GPH3 pins to special-function 3: KP_ROW[x] */
-        end = S5PC100_GPH3(rows);
-        for (gpio = S5PC100_GPH3(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PC100_GPH3(0), rows, S3C_GPIO_SFN(3));
 
         /* Set all the necessary GPH2 pins to special-function 3: KP_COL[x] */
-        end = S5PC100_GPH2(cols);
-        for (gpio = S5PC100_GPH2(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PC100_GPH2(0), cols, S3C_GPIO_SFN(3));
 }
diff --git a/arch/arm/mach-s5pc100/setup-sdhci-gpio.c b/arch/arm/mach-s5pc100/setup-sdhci-gpio.c
index dc7208c639ea..03c02d04c68c 100644
--- a/arch/arm/mach-s5pc100/setup-sdhci-gpio.c
+++ b/arch/arm/mach-s5pc100/setup-sdhci-gpio.c
@@ -25,8 +25,6 @@
 void s5pc100_setup_sdhci0_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
-        unsigned int end;
         unsigned int num;
 
         num = width;
@@ -34,20 +32,11 @@ void s5pc100_setup_sdhci0_cfg_gpio(struct platform_device *dev, int width)
         if (width == 8)
                 num = width - 2;
 
-        end = S5PC100_GPG0(2 + num);
-
         /* Set all the necessary GPG0/GPG1 pins to special-function 0 */
-        for (gpio = S5PC100_GPG0(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PC100_GPG0(0), 2 + num, S3C_GPIO_SFN(2));
 
-        if (width == 8) {
-                for (gpio = S5PC100_GPG1(0); gpio <= S5PC100_GPG1(1); gpio++) {
-                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                }
-        }
+        if (width == 8)
+                s3c_gpio_cfgrange_nopull(S5PC100_GPG1(0), 2, S3C_GPIO_SFN(2));
 
         if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
                 s3c_gpio_setpull(S5PC100_GPG1(2), S3C_GPIO_PULL_UP);
@@ -58,16 +47,9 @@ void s5pc100_setup_sdhci0_cfg_gpio(struct platform_device *dev, int width)
 void s5pc100_setup_sdhci1_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
-        unsigned int end;
-
-        end = S5PC100_GPG2(2 + width);
 
         /* Set all the necessary GPG2 pins to special-function 2 */
-        for (gpio = S5PC100_GPG2(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PC100_GPG2(0), 2 + width, S3C_GPIO_SFN(2));
 
         if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
                 s3c_gpio_setpull(S5PC100_GPG2(6), S3C_GPIO_PULL_UP);
@@ -78,16 +60,9 @@ void s5pc100_setup_sdhci1_cfg_gpio(struct platform_device *dev, int width)
 void s5pc100_setup_sdhci2_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
-        unsigned int end;
-
-        end = S5PC100_GPG3(2 + width);
 
         /* Set all the necessary GPG3 pins to special-function 2 */
-        for (gpio = S5PC100_GPG3(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PC100_GPG3(0), 2 + width, S3C_GPIO_SFN(2));
 
         if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
                 s3c_gpio_setpull(S5PC100_GPG3(6), S3C_GPIO_PULL_UP);
diff --git a/arch/arm/mach-s5pv210/Kconfig b/arch/arm/mach-s5pv210/Kconfig
index 5315fec3db86..862f239a0fdb 100644
--- a/arch/arm/mach-s5pv210/Kconfig
+++ b/arch/arm/mach-s5pv210/Kconfig
@@ -11,9 +11,9 @@ if ARCH_S5PV210
 
 config CPU_S5PV210
         bool
-        select PLAT_S5P
         select S3C_PL330_DMA
         select S5P_EXT_INT
+        select S5PV210_PM if PM
         help
           Enable S5PV210 CPU support
 
@@ -58,7 +58,6 @@ menu "S5PC110 Machines"
 config MACH_AQUILA
         bool "Aquila"
         select CPU_S5PV210
-        select ARCH_SPARSEMEM_ENABLE
         select S3C_DEV_FB
         select S5P_DEV_FIMC0
         select S5P_DEV_FIMC1
@@ -75,7 +74,7 @@ config MACH_AQUILA
 config MACH_GONI
         bool "GONI"
         select CPU_S5PV210
-        select ARCH_SPARSEMEM_ENABLE
+        select S5P_GPIO_INT
         select S3C_DEV_FB
         select S5P_DEV_FIMC0
         select S5P_DEV_FIMC1
@@ -83,8 +82,15 @@ config MACH_GONI
         select S3C_DEV_HSMMC
         select S3C_DEV_HSMMC1
         select S3C_DEV_HSMMC2
+        select S3C_DEV_I2C1
+        select S3C_DEV_I2C2
+        select S3C_DEV_USB_HSOTG
         select S5P_DEV_ONENAND
+        select SAMSUNG_DEV_KEYPAD
         select S5PV210_SETUP_FB_24BPP
+        select S5PV210_SETUP_I2C1
+        select S5PV210_SETUP_I2C2
+        select S5PV210_SETUP_KEYPAD
         select S5PV210_SETUP_SDHCI
         help
           Machine support for Samsung GONI board
@@ -93,7 +99,6 @@ config MACH_GONI
 config MACH_SMDKC110
         bool "SMDKC110"
         select CPU_S5PV210
-        select ARCH_SPARSEMEM_ENABLE
         select S3C_DEV_I2C1
         select S3C_DEV_I2C2
         select S3C_DEV_RTC
@@ -113,7 +118,6 @@ menu "S5PV210 Machines"
 config MACH_SMDKV210
         bool "SMDKV210"
         select CPU_S5PV210
-        select ARCH_SPARSEMEM_ENABLE
         select S3C_DEV_HSMMC
         select S3C_DEV_HSMMC1
         select S3C_DEV_HSMMC2
@@ -134,6 +138,29 @@ config MACH_SMDKV210
         help
           Machine support for Samsung SMDKV210
 
+config MACH_TORBRECK
+        bool "Torbreck"
+        select CPU_S5PV210
+        select ARCH_SPARSEMEM_ENABLE
+        select S3C_DEV_HSMMC
+        select S3C_DEV_HSMMC1
+        select S3C_DEV_HSMMC2
+        select S3C_DEV_HSMMC3
+        select S3C_DEV_I2C1
+        select S3C_DEV_I2C2
+        select S3C_DEV_RTC
+        select S3C_DEV_WDT
+        select S5PV210_SETUP_I2C1
+        select S5PV210_SETUP_I2C2
+        select S5PV210_SETUP_SDHCI
+        help
+          Machine support for aESOP Torbreck
+
 endmenu
 
+config S5PV210_PM
+        bool
+        help
+          Power Management code common to S5PV210
+
 endif
diff --git a/arch/arm/mach-s5pv210/Makefile b/arch/arm/mach-s5pv210/Makefile
index 704548912408..ff1a0db57a2f 100644
--- a/arch/arm/mach-s5pv210/Makefile
+++ b/arch/arm/mach-s5pv210/Makefile
@@ -14,6 +14,8 @@ obj-				:=
 
 obj-$(CONFIG_CPU_S5PV210)       += cpu.o init.o clock.o dma.o gpiolib.o
 obj-$(CONFIG_CPU_S5PV210)       += setup-i2c0.o
+obj-$(CONFIG_S5PV210_PM)        += pm.o sleep.o
+obj-$(CONFIG_CPU_FREQ)          += cpufreq.o
 
 # machine support
 
@@ -21,6 +23,7 @@ obj-$(CONFIG_MACH_AQUILA)	+= mach-aquila.o
 obj-$(CONFIG_MACH_SMDKV210)     += mach-smdkv210.o
 obj-$(CONFIG_MACH_SMDKC110)     += mach-smdkc110.o
 obj-$(CONFIG_MACH_GONI)         += mach-goni.o
+obj-$(CONFIG_MACH_TORBRECK)     += mach-torbreck.o
 
 # device support
 
diff --git a/arch/arm/mach-s5pv210/clock.c b/arch/arm/mach-s5pv210/clock.c
index d562670e1b0b..019c3a69b0e4 100644
--- a/arch/arm/mach-s5pv210/clock.c
+++ b/arch/arm/mach-s5pv210/clock.c
@@ -31,6 +31,8 @@
 #include <plat/clock-clksrc.h>
 #include <plat/s5pv210.h>
 
+static unsigned long xtal;
+
 static struct clksrc_clk clk_mout_apll = {
         .clk    = {
                 .name           = "mout_apll",
@@ -259,6 +261,36 @@ static struct clksrc_clk clk_sclk_vpll = {
         .reg_src        = { .reg = S5P_CLK_SRC0, .shift = 12, .size = 1 },
 };
 
+static struct clk *clkset_moutdmc0src_list[] = {
+        [0] = &clk_sclk_a2m.clk,
+        [1] = &clk_mout_mpll.clk,
+        [2] = NULL,
+        [3] = NULL,
+};
+
+static struct clksrc_sources clkset_moutdmc0src = {
+        .sources        = clkset_moutdmc0src_list,
+        .nr_sources     = ARRAY_SIZE(clkset_moutdmc0src_list),
+};
+
+static struct clksrc_clk clk_mout_dmc0 = {
+        .clk    = {
+                .name           = "mout_dmc0",
+                .id             = -1,
+        },
+        .sources        = &clkset_moutdmc0src,
+        .reg_src        = { .reg = S5P_CLK_SRC6, .shift = 24, .size = 2 },
+};
+
+static struct clksrc_clk clk_sclk_dmc0 = {
+        .clk    = {
+                .name           = "sclk_dmc0",
+                .id             = -1,
+                .parent         = &clk_mout_dmc0.clk,
+        },
+        .reg_div        = { .reg = S5P_CLK_DIV6, .shift = 28, .size = 4 },
+};
+
 static unsigned long s5pv210_clk_imem_get_rate(struct clk *clk)
 {
         return clk_get_rate(clk->parent) / 2;
@@ -268,8 +300,29 @@ static struct clk_ops clk_hclk_imem_ops = {
         .get_rate       = s5pv210_clk_imem_get_rate,
 };
 
+static unsigned long s5pv210_clk_fout_apll_get_rate(struct clk *clk)
+{
+        return s5p_get_pll45xx(xtal, __raw_readl(S5P_APLL_CON), pll_4508);
+}
+
+static struct clk_ops clk_fout_apll_ops = {
+        .get_rate       = s5pv210_clk_fout_apll_get_rate,
+};
+
 static struct clk init_clocks_disable[] = {
         {
+                .name           = "pdma",
+                .id             = 0,
+                .parent         = &clk_hclk_psys.clk,
+                .enable         = s5pv210_clk_ip0_ctrl,
+                .ctrlbit        = (1 << 3),
+        }, {
+                .name           = "pdma",
+                .id             = 1,
+                .parent         = &clk_hclk_psys.clk,
+                .enable         = s5pv210_clk_ip0_ctrl,
+                .ctrlbit        = (1 << 4),
+        }, {
                 .name           = "rot",
                 .id             = -1,
                 .parent         = &clk_hclk_dsys.clk,
@@ -431,6 +484,12 @@ static struct clk init_clocks_disable[] = {
                 .parent         = &clk_p,
                 .enable         = s5pv210_clk_ip3_ctrl,
                 .ctrlbit        = (1 << 6),
+        }, {
+                .name           = "spdif",
+                .id             = -1,
+                .parent         = &clk_p,
+                .enable         = s5pv210_clk_ip3_ctrl,
+                .ctrlbit        = (1 << 0),
         },
 };
 
@@ -660,6 +719,53 @@ static struct clksrc_sources clkset_sclk_spdif = {
         .nr_sources     = ARRAY_SIZE(clkset_sclk_spdif_list),
 };
 
+static int s5pv210_spdif_set_rate(struct clk *clk, unsigned long rate)
+{
+        struct clk *pclk;
+        int ret;
+
+        pclk = clk_get_parent(clk);
+        if (IS_ERR(pclk))
+                return -EINVAL;
+
+        ret = pclk->ops->set_rate(pclk, rate);
+        clk_put(pclk);
+
+        return ret;
+}
+
+static unsigned long s5pv210_spdif_get_rate(struct clk *clk)
+{
+        struct clk *pclk;
+        int rate;
+
+        pclk = clk_get_parent(clk);
+        if (IS_ERR(pclk))
+                return -EINVAL;
+
+        rate = pclk->ops->get_rate(clk);
+        clk_put(pclk);
+
+        return rate;
+}
+
+static struct clk_ops s5pv210_sclk_spdif_ops = {
+        .set_rate       = s5pv210_spdif_set_rate,
+        .get_rate       = s5pv210_spdif_get_rate,
+};
+
+static struct clksrc_clk clk_sclk_spdif = {
+        .clk            = {
+                .name           = "sclk_spdif",
+                .id             = -1,
+                .enable         = s5pv210_clk_mask0_ctrl,
+                .ctrlbit        = (1 << 27),
+                .ops            = &s5pv210_sclk_spdif_ops,
+        },
+        .sources = &clkset_sclk_spdif,
+        .reg_src = { .reg = S5P_CLK_SRC6, .shift = 12, .size = 2 },
+};
+
 static struct clk *clkset_group2_list[] = {
         [0] = &clk_ext_xtal_mux,
         [1] = &clk_xusbxti,
@@ -744,15 +850,6 @@ static struct clksrc_clk clksrcs[] = {
                 .sources = &clkset_sclk_mixer,
                 .reg_src = { .reg = S5P_CLK_SRC1, .shift = 4, .size = 1 },
         }, {
-                .clk            = {
-                        .name           = "sclk_spdif",
-                        .id             = -1,
-                        .enable         = s5pv210_clk_mask0_ctrl,
-                        .ctrlbit        = (1 << 27),
-                },
-                .sources = &clkset_sclk_spdif,
-                .reg_src = { .reg = S5P_CLK_SRC6, .shift = 12, .size = 2 },
-        }, {
                 .clk    = {
                         .name           = "sclk_fimc",
                         .id             = 0,
@@ -953,12 +1050,93 @@ static struct clksrc_clk *sysclks[] = {
         &clk_sclk_dac,
         &clk_sclk_pixel,
         &clk_sclk_hdmi,
+        &clk_mout_dmc0,
+        &clk_sclk_dmc0,
+        &clk_sclk_audio0,
+        &clk_sclk_audio1,
+        &clk_sclk_audio2,
+        &clk_sclk_spdif,
+};
+
+static u32 epll_div[][6] = {
+        {  48000000, 0, 48, 3, 3, 0 },
+        {  96000000, 0, 48, 3, 2, 0 },
+        { 144000000, 1, 72, 3, 2, 0 },
+        { 192000000, 0, 48, 3, 1, 0 },
+        { 288000000, 1, 72, 3, 1, 0 },
+        {  32750000, 1, 65, 3, 4, 35127 },
+        {  32768000, 1, 65, 3, 4, 35127 },
+        {  45158400, 0, 45, 3, 3, 10355 },
+        {  45000000, 0, 45, 3, 3, 10355 },
+        {  45158000, 0, 45, 3, 3, 10355 },
+        {  49125000, 0, 49, 3, 3, 9961 },
+        {  49152000, 0, 49, 3, 3, 9961 },
+        {  67737600, 1, 67, 3, 3, 48366 },
+        {  67738000, 1, 67, 3, 3, 48366 },
+        {  73800000, 1, 73, 3, 3, 47710 },
+        {  73728000, 1, 73, 3, 3, 47710 },
+        {  36000000, 1, 32, 3, 4, 0 },
+        {  60000000, 1, 60, 3, 3, 0 },
+        {  72000000, 1, 72, 3, 3, 0 },
+        {  80000000, 1, 80, 3, 3, 0 },
+        {  84000000, 0, 42, 3, 2, 0 },
+        {  50000000, 0, 50, 3, 3, 0 },
+};
+
+static int s5pv210_epll_set_rate(struct clk *clk, unsigned long rate)
+{
+        unsigned int epll_con, epll_con_k;
+        unsigned int i;
+
+        /* Return if nothing changed */
+        if (clk->rate == rate)
+                return 0;
+
+        epll_con = __raw_readl(S5P_EPLL_CON);
+        epll_con_k = __raw_readl(S5P_EPLL_CON1);
+
+        epll_con_k &= ~PLL46XX_KDIV_MASK;
+        epll_con &= ~(1 << 27 |
+                        PLL46XX_MDIV_MASK << PLL46XX_MDIV_SHIFT |
+                        PLL46XX_PDIV_MASK << PLL46XX_PDIV_SHIFT |
+                        PLL46XX_SDIV_MASK << PLL46XX_SDIV_SHIFT);
+
+        for (i = 0; i < ARRAY_SIZE(epll_div); i++) {
+                if (epll_div[i][0] == rate) {
+                        epll_con_k |= epll_div[i][5] << 0;
+                        epll_con |= (epll_div[i][1] << 27 |
+                                        epll_div[i][2] << PLL46XX_MDIV_SHIFT |
+                                        epll_div[i][3] << PLL46XX_PDIV_SHIFT |
+                                        epll_div[i][4] << PLL46XX_SDIV_SHIFT);
+                        break;
+                }
+        }
+
+        if (i == ARRAY_SIZE(epll_div)) {
+                printk(KERN_ERR "%s: Invalid Clock EPLL Frequency\n",
+                                __func__);
+                return -EINVAL;
+        }
+
+        __raw_writel(epll_con, S5P_EPLL_CON);
+        __raw_writel(epll_con_k, S5P_EPLL_CON1);
+
+        printk(KERN_WARNING "EPLL Rate changes from %lu to %lu\n",
+                        clk->rate, rate);
+
+        clk->rate = rate;
+
+        return 0;
+}
+
+static struct clk_ops s5pv210_epll_ops = {
+        .set_rate = s5pv210_epll_set_rate,
+        .get_rate = s5p_epll_get_rate,
 };
 
 void __init_or_cpufreq s5pv210_setup_clocks(void)
 {
         struct clk *xtal_clk;
-        unsigned long xtal;
         unsigned long vpllsrc;
         unsigned long armclk;
         unsigned long hclk_msys;
@@ -974,6 +1152,10 @@ void __init_or_cpufreq s5pv210_setup_clocks(void)
         unsigned int ptr;
         u32 clkdiv0, clkdiv1;
 
+        /* Set functions for clk_fout_epll */
+        clk_fout_epll.enable = s5p_epll_enable;
+        clk_fout_epll.ops = &s5pv210_epll_ops;
+
         printk(KERN_DEBUG "%s: registering clocks\n", __func__);
 
         clkdiv0 = __raw_readl(S5P_CLK_DIV0);
@@ -992,11 +1174,12 @@ void __init_or_cpufreq s5pv210_setup_clocks(void)
 
         apll = s5p_get_pll45xx(xtal, __raw_readl(S5P_APLL_CON), pll_4508);
         mpll = s5p_get_pll45xx(xtal, __raw_readl(S5P_MPLL_CON), pll_4502);
-        epll = s5p_get_pll45xx(xtal, __raw_readl(S5P_EPLL_CON), pll_4500);
+        epll = s5p_get_pll46xx(xtal, __raw_readl(S5P_EPLL_CON),
+                                __raw_readl(S5P_EPLL_CON1), pll_4600);
         vpllsrc = clk_get_rate(&clk_vpllsrc.clk);
         vpll = s5p_get_pll45xx(vpllsrc, __raw_readl(S5P_VPLL_CON), pll_4502);
 
-        clk_fout_apll.rate = apll;
+        clk_fout_apll.ops = &clk_fout_apll_ops;
         clk_fout_mpll.rate = mpll;
         clk_fout_epll.rate = epll;
         clk_fout_vpll.rate = vpll;
diff --git a/arch/arm/mach-s5pv210/cpu.c b/arch/arm/mach-s5pv210/cpu.c
index 2f16bfc0a116..8eb480e201b0 100644
--- a/arch/arm/mach-s5pv210/cpu.c
+++ b/arch/arm/mach-s5pv210/cpu.c
@@ -85,6 +85,21 @@ static struct map_desc s5pv210_iodesc[] __initdata = {
                 .pfn            = __phys_to_pfn(S5PV210_PA_SROMC),
                 .length         = SZ_4K,
                 .type           = MT_DEVICE,
+        }, {
+                .virtual        = (unsigned long)S5P_VA_DMC0,
+                .pfn            = __phys_to_pfn(S5PV210_PA_DMC0),
+                .length         = SZ_4K,
+                .type           = MT_DEVICE,
+        }, {
+                .virtual        = (unsigned long)S5P_VA_DMC1,
+                .pfn            = __phys_to_pfn(S5PV210_PA_DMC1),
+                .length         = SZ_4K,
+                .type           = MT_DEVICE,
+        }, {
+                .virtual        = (unsigned long)S3C_VA_USB_HSPHY,
+                .pfn            =__phys_to_pfn(S5PV210_PA_HSPHY),
+                .length         = SZ_4K,
+                .type           = MT_DEVICE,
         }
 };
 
diff --git a/arch/arm/mach-s5pv210/cpufreq.c b/arch/arm/mach-s5pv210/cpufreq.c
new file mode 100644
index 000000000000..a6f22920a2c2
--- /dev/null
+++ b/arch/arm/mach-s5pv210/cpufreq.c
@@ -0,0 +1,484 @@
+/* linux/arch/arm/mach-s5pv210/cpufreq.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * CPU frequency scaling for S5PC110/S5PV210
+ *
+ * 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/types.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/clk.h>
+#include <linux/io.h>
+#include <linux/cpufreq.h>
+
+#include <mach/map.h>
+#include <mach/regs-clock.h>
+
+static struct clk *cpu_clk;
+static struct clk *dmc0_clk;
+static struct clk *dmc1_clk;
+static struct cpufreq_freqs freqs;
+
+/* APLL M,P,S values for 1G/800Mhz */
+#define APLL_VAL_1000   ((1 << 31) | (125 << 16) | (3 << 8) | 1)
+#define APLL_VAL_800    ((1 << 31) | (100 << 16) | (3 << 8) | 1)
+
+/*
+ * DRAM configurations to calculate refresh counter for changing
+ * frequency of memory.
+ */
+struct dram_conf {
+        unsigned long freq;     /* HZ */
+        unsigned long refresh;  /* DRAM refresh counter * 1000 */
+};
+
+/* DRAM configuration (DMC0 and DMC1) */
+static struct dram_conf s5pv210_dram_conf[2];
+
+enum perf_level {
+        L0, L1, L2, L3, L4,
+};
+
+enum s5pv210_mem_type {
+        LPDDR   = 0x1,
+        LPDDR2  = 0x2,
+        DDR2    = 0x4,
+};
+
+enum s5pv210_dmc_port {
+        DMC0 = 0,
+        DMC1,
+};
+
+static struct cpufreq_frequency_table s5pv210_freq_table[] = {
+        {L0, 1000*1000},
+        {L1, 800*1000},
+        {L2, 400*1000},
+        {L3, 200*1000},
+        {L4, 100*1000},
+        {0, CPUFREQ_TABLE_END},
+};
+
+static u32 clkdiv_val[5][11] = {
+        /*
+         * Clock divider value for following
+         * { APLL, A2M, HCLK_MSYS, PCLK_MSYS,
+         *   HCLK_DSYS, PCLK_DSYS, HCLK_PSYS, PCLK_PSYS,
+         *   ONEDRAM, MFC, G3D }
+         */
+
+        /* L0 : [1000/200/100][166/83][133/66][200/200] */
+        {0, 4, 4, 1, 3, 1, 4, 1, 3, 0, 0},
+
+        /* L1 : [800/200/100][166/83][133/66][200/200] */
+        {0, 3, 3, 1, 3, 1, 4, 1, 3, 0, 0},
+
+        /* L2 : [400/200/100][166/83][133/66][200/200] */
+        {1, 3, 1, 1, 3, 1, 4, 1, 3, 0, 0},
+
+        /* L3 : [200/200/100][166/83][133/66][200/200] */
+        {3, 3, 1, 1, 3, 1, 4, 1, 3, 0, 0},
+
+        /* L4 : [100/100/100][83/83][66/66][100/100] */
+        {7, 7, 0, 0, 7, 0, 9, 0, 7, 0, 0},
+};
+
+/*
+ * This function set DRAM refresh counter
+ * accoriding to operating frequency of DRAM
+ * ch: DMC port number 0 or 1
+ * freq: Operating frequency of DRAM(KHz)
+ */
+static void s5pv210_set_refresh(enum s5pv210_dmc_port ch, unsigned long freq)
+{
+        unsigned long tmp, tmp1;
+        void __iomem *reg = NULL;
+
+        if (ch == DMC0)
+                reg = (S5P_VA_DMC0 + 0x30);
+        else if (ch == DMC1)
+                reg = (S5P_VA_DMC1 + 0x30);
+        else
+                printk(KERN_ERR "Cannot find DMC port\n");
+
+        /* Find current DRAM frequency */
+        tmp = s5pv210_dram_conf[ch].freq;
+
+        do_div(tmp, freq);
+
+        tmp1 = s5pv210_dram_conf[ch].refresh;
+
+        do_div(tmp1, tmp);
+
+        __raw_writel(tmp1, reg);
+}
+
+int s5pv210_verify_speed(struct cpufreq_policy *policy)
+{
+        if (policy->cpu)
+                return -EINVAL;
+
+        return cpufreq_frequency_table_verify(policy, s5pv210_freq_table);
+}
+
+unsigned int s5pv210_getspeed(unsigned int cpu)
+{
+        if (cpu)
+                return 0;
+
+        return clk_get_rate(cpu_clk) / 1000;
+}
+
+static int s5pv210_target(struct cpufreq_policy *policy,
+                          unsigned int target_freq,
+                          unsigned int relation)
+{
+        unsigned long reg;
+        unsigned int index, priv_index;
+        unsigned int pll_changing = 0;
+        unsigned int bus_speed_changing = 0;
+
+        freqs.old = s5pv210_getspeed(0);
+
+        if (cpufreq_frequency_table_target(policy, s5pv210_freq_table,
+                                           target_freq, relation, &index))
+                return -EINVAL;
+
+        freqs.new = s5pv210_freq_table[index].frequency;
+        freqs.cpu = 0;
+
+        if (freqs.new == freqs.old)
+                return 0;
+
+        /* Finding current running level index */
+        if (cpufreq_frequency_table_target(policy, s5pv210_freq_table,
+                                           freqs.old, relation, &priv_index))
+                return -EINVAL;
+
+        cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+
+        if (freqs.new > freqs.old) {
+                /* Voltage up: will be implemented */
+        }
+
+        /* Check if there need to change PLL */
+        if ((index == L0) || (priv_index == L0))
+                pll_changing = 1;
+
+        /* Check if there need to change System bus clock */
+        if ((index == L4) || (priv_index == L4))
+                bus_speed_changing = 1;
+
+        if (bus_speed_changing) {
+                /*
+                 * Reconfigure DRAM refresh counter value for minimum
+                 * temporary clock while changing divider.
+                 * expected clock is 83Mhz : 7.8usec/(1/83Mhz) = 0x287
+                 */
+                if (pll_changing)
+                        s5pv210_set_refresh(DMC1, 83000);
+                else
+                        s5pv210_set_refresh(DMC1, 100000);
+
+                s5pv210_set_refresh(DMC0, 83000);
+        }
+
+        /*
+         * APLL should be changed in this level
+         * APLL -> MPLL(for stable transition) -> APLL
+         * Some clock source's clock API are not prepared.
+         * Do not use clock API in below code.
+         */
+        if (pll_changing) {
+                /*
+                 * 1. Temporary Change divider for MFC and G3D
+                 * SCLKA2M(200/1=200)->(200/4=50)Mhz
+                 */
+                reg = __raw_readl(S5P_CLK_DIV2);
+                reg &= ~(S5P_CLKDIV2_G3D_MASK | S5P_CLKDIV2_MFC_MASK);
+                reg |= (3 << S5P_CLKDIV2_G3D_SHIFT) |
+                        (3 << S5P_CLKDIV2_MFC_SHIFT);
+                __raw_writel(reg, S5P_CLK_DIV2);
+
+                /* For MFC, G3D dividing */
+                do {
+                        reg = __raw_readl(S5P_CLKDIV_STAT0);
+                } while (reg & ((1 << 16) | (1 << 17)));
+
+                /*
+                 * 2. Change SCLKA2M(200Mhz)to SCLKMPLL in MFC_MUX, G3D MUX
+                 * (200/4=50)->(667/4=166)Mhz
+                 */
+                reg = __raw_readl(S5P_CLK_SRC2);
+                reg &= ~(S5P_CLKSRC2_G3D_MASK | S5P_CLKSRC2_MFC_MASK);
+                reg |= (1 << S5P_CLKSRC2_G3D_SHIFT) |
+                        (1 << S5P_CLKSRC2_MFC_SHIFT);
+                __raw_writel(reg, S5P_CLK_SRC2);
+
+                do {
+                        reg = __raw_readl(S5P_CLKMUX_STAT1);
+                } while (reg & ((1 << 7) | (1 << 3)));
+
+                /*
+                 * 3. DMC1 refresh count for 133Mhz if (index == L4) is
+                 * true refresh counter is already programed in upper
+                 * code. 0x287@83Mhz
+                 */
+                if (!bus_speed_changing)
+                        s5pv210_set_refresh(DMC1, 133000);
+
+                /* 4. SCLKAPLL -> SCLKMPLL */
+                reg = __raw_readl(S5P_CLK_SRC0);
+                reg &= ~(S5P_CLKSRC0_MUX200_MASK);
+                reg |= (0x1 << S5P_CLKSRC0_MUX200_SHIFT);
+                __raw_writel(reg, S5P_CLK_SRC0);
+
+                do {
+                        reg = __raw_readl(S5P_CLKMUX_STAT0);
+                } while (reg & (0x1 << 18));
+
+        }
+
+        /* Change divider */
+        reg = __raw_readl(S5P_CLK_DIV0);
+
+        reg &= ~(S5P_CLKDIV0_APLL_MASK | S5P_CLKDIV0_A2M_MASK |
+                S5P_CLKDIV0_HCLK200_MASK | S5P_CLKDIV0_PCLK100_MASK |
+                S5P_CLKDIV0_HCLK166_MASK | S5P_CLKDIV0_PCLK83_MASK |
+                S5P_CLKDIV0_HCLK133_MASK | S5P_CLKDIV0_PCLK66_MASK);
+
+        reg |= ((clkdiv_val[index][0] << S5P_CLKDIV0_APLL_SHIFT) |
+                (clkdiv_val[index][1] << S5P_CLKDIV0_A2M_SHIFT) |
+                (clkdiv_val[index][2] << S5P_CLKDIV0_HCLK200_SHIFT) |
+                (clkdiv_val[index][3] << S5P_CLKDIV0_PCLK100_SHIFT) |
+                (clkdiv_val[index][4] << S5P_CLKDIV0_HCLK166_SHIFT) |
+                (clkdiv_val[index][5] << S5P_CLKDIV0_PCLK83_SHIFT) |
+                (clkdiv_val[index][6] << S5P_CLKDIV0_HCLK133_SHIFT) |
+                (clkdiv_val[index][7] << S5P_CLKDIV0_PCLK66_SHIFT));
+
+        __raw_writel(reg, S5P_CLK_DIV0);
+
+        do {
+                reg = __raw_readl(S5P_CLKDIV_STAT0);
+        } while (reg & 0xff);
+
+        /* ARM MCS value changed */
+        reg = __raw_readl(S5P_ARM_MCS_CON);
+        reg &= ~0x3;
+        if (index >= L3)
+                reg |= 0x3;
+        else
+                reg |= 0x1;
+
+        __raw_writel(reg, S5P_ARM_MCS_CON);
+
+        if (pll_changing) {
+                /* 5. Set Lock time = 30us*24Mhz = 0x2cf */
+                __raw_writel(0x2cf, S5P_APLL_LOCK);
+
+                /*
+                 * 6. Turn on APLL
+                 * 6-1. Set PMS values
+                 * 6-2. Wait untile the PLL is locked
+                 */
+                if (index == L0)
+                        __raw_writel(APLL_VAL_1000, S5P_APLL_CON);
+                else
+                        __raw_writel(APLL_VAL_800, S5P_APLL_CON);
+
+                do {
+                        reg = __raw_readl(S5P_APLL_CON);
+                } while (!(reg & (0x1 << 29)));
+
+                /*
+                 * 7. Change souce clock from SCLKMPLL(667Mhz)
+                 * to SCLKA2M(200Mhz) in MFC_MUX and G3D MUX
+                 * (667/4=166)->(200/4=50)Mhz
+                 */
+                reg = __raw_readl(S5P_CLK_SRC2);
+                reg &= ~(S5P_CLKSRC2_G3D_MASK | S5P_CLKSRC2_MFC_MASK);
+                reg |= (0 << S5P_CLKSRC2_G3D_SHIFT) |
+                        (0 << S5P_CLKSRC2_MFC_SHIFT);
+                __raw_writel(reg, S5P_CLK_SRC2);
+
+                do {
+                        reg = __raw_readl(S5P_CLKMUX_STAT1);
+                } while (reg & ((1 << 7) | (1 << 3)));
+
+                /*
+                 * 8. Change divider for MFC and G3D
+                 * (200/4=50)->(200/1=200)Mhz
+                 */
+                reg = __raw_readl(S5P_CLK_DIV2);
+                reg &= ~(S5P_CLKDIV2_G3D_MASK | S5P_CLKDIV2_MFC_MASK);
+                reg |= (clkdiv_val[index][10] << S5P_CLKDIV2_G3D_SHIFT) |
+                        (clkdiv_val[index][9] << S5P_CLKDIV2_MFC_SHIFT);
+                __raw_writel(reg, S5P_CLK_DIV2);
+
+                /* For MFC, G3D dividing */
+                do {
+                        reg = __raw_readl(S5P_CLKDIV_STAT0);
+                } while (reg & ((1 << 16) | (1 << 17)));
+
+                /* 9. Change MPLL to APLL in MSYS_MUX */
+                reg = __raw_readl(S5P_CLK_SRC0);
+                reg &= ~(S5P_CLKSRC0_MUX200_MASK);
+                reg |= (0x0 << S5P_CLKSRC0_MUX200_SHIFT);
+                __raw_writel(reg, S5P_CLK_SRC0);
+
+                do {
+                        reg = __raw_readl(S5P_CLKMUX_STAT0);
+                } while (reg & (0x1 << 18));
+
+                /*
+                 * 10. DMC1 refresh counter
+                 * L4 : DMC1 = 100Mhz 7.8us/(1/100) = 0x30c
+                 * Others : DMC1 = 200Mhz 7.8us/(1/200) = 0x618
+                 */
+                if (!bus_speed_changing)
+                        s5pv210_set_refresh(DMC1, 200000);
+        }
+
+        /*
+         * L4 level need to change memory bus speed, hence onedram clock divier
+         * and memory refresh parameter should be changed
+         */
+        if (bus_speed_changing) {
+                reg = __raw_readl(S5P_CLK_DIV6);
+                reg &= ~S5P_CLKDIV6_ONEDRAM_MASK;
+                reg |= (clkdiv_val[index][8] << S5P_CLKDIV6_ONEDRAM_SHIFT);
+                __raw_writel(reg, S5P_CLK_DIV6);
+
+                do {
+                        reg = __raw_readl(S5P_CLKDIV_STAT1);
+                } while (reg & (1 << 15));
+
+                /* Reconfigure DRAM refresh counter value */
+                if (index != L4) {
+                        /*
+                         * DMC0 : 166Mhz
+                         * DMC1 : 200Mhz
+                         */
+                        s5pv210_set_refresh(DMC0, 166000);
+                        s5pv210_set_refresh(DMC1, 200000);
+                } else {
+                        /*
+                         * DMC0 : 83Mhz
+                         * DMC1 : 100Mhz
+                         */
+                        s5pv210_set_refresh(DMC0, 83000);
+                        s5pv210_set_refresh(DMC1, 100000);
+                }
+        }
+
+        if (freqs.new < freqs.old) {
+                /* Voltage down: will be implemented */
+        }
+
+        cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+        printk(KERN_DEBUG "Perf changed[L%d]\n", index);
+
+        return 0;
+}
+
+#ifdef CONFIG_PM
+static int s5pv210_cpufreq_suspend(struct cpufreq_policy *policy,
+                                   pm_message_t pmsg)
+{
+        return 0;
+}
+
+static int s5pv210_cpufreq_resume(struct cpufreq_policy *policy)
+{
+        return 0;
+}
+#endif
+
+static int check_mem_type(void __iomem *dmc_reg)
+{
+        unsigned long val;
+
+        val = __raw_readl(dmc_reg + 0x4);
+        val = (val & (0xf << 8));
+
+        return val >> 8;
+}
+
+static int __init s5pv210_cpu_init(struct cpufreq_policy *policy)
+{
+        unsigned long mem_type;
+
+        cpu_clk = clk_get(NULL, "armclk");
+        if (IS_ERR(cpu_clk))
+                return PTR_ERR(cpu_clk);
+
+        dmc0_clk = clk_get(NULL, "sclk_dmc0");
+        if (IS_ERR(dmc0_clk)) {
+                clk_put(cpu_clk);
+                return PTR_ERR(dmc0_clk);
+        }
+
+        dmc1_clk = clk_get(NULL, "hclk_msys");
+        if (IS_ERR(dmc1_clk)) {
+                clk_put(dmc0_clk);
+                clk_put(cpu_clk);
+                return PTR_ERR(dmc1_clk);
+        }
+
+        if (policy->cpu != 0)
+                return -EINVAL;
+
+        /*
+         * check_mem_type : This driver only support LPDDR & LPDDR2.
+         * other memory type is not supported.
+         */
+        mem_type = check_mem_type(S5P_VA_DMC0);
+
+        if ((mem_type != LPDDR) && (mem_type != LPDDR2)) {
+                printk(KERN_ERR "CPUFreq doesn't support this memory type\n");
+                return -EINVAL;
+        }
+
+        /* Find current refresh counter and frequency each DMC */
+        s5pv210_dram_conf[0].refresh = (__raw_readl(S5P_VA_DMC0 + 0x30) * 1000);
+        s5pv210_dram_conf[0].freq = clk_get_rate(dmc0_clk);
+
+        s5pv210_dram_conf[1].refresh = (__raw_readl(S5P_VA_DMC1 + 0x30) * 1000);
+        s5pv210_dram_conf[1].freq = clk_get_rate(dmc1_clk);
+
+        policy->cur = policy->min = policy->max = s5pv210_getspeed(0);
+
+        cpufreq_frequency_table_get_attr(s5pv210_freq_table, policy->cpu);
+
+        policy->cpuinfo.transition_latency = 40000;
+
+        return cpufreq_frequency_table_cpuinfo(policy, s5pv210_freq_table);
+}
+
+static struct cpufreq_driver s5pv210_driver = {
+        .flags          = CPUFREQ_STICKY,
+        .verify         = s5pv210_verify_speed,
+        .target         = s5pv210_target,
+        .get            = s5pv210_getspeed,
+        .init           = s5pv210_cpu_init,
+        .name           = "s5pv210",
+#ifdef CONFIG_PM
+        .suspend        = s5pv210_cpufreq_suspend,
+        .resume         = s5pv210_cpufreq_resume,
+#endif
+};
+
+static int __init s5pv210_cpufreq_init(void)
+{
+        return cpufreq_register_driver(&s5pv210_driver);
+}
+
+late_initcall(s5pv210_cpufreq_init);
diff --git a/arch/arm/mach-s5pv210/dev-audio.c b/arch/arm/mach-s5pv210/dev-audio.c
index 21dc6cf955c3..1303fcb12b51 100644
--- a/arch/arm/mach-s5pv210/dev-audio.c
+++ b/arch/arm/mach-s5pv210/dev-audio.c
@@ -24,29 +24,15 @@ static int s5pv210_cfg_i2s(struct platform_device *pdev)
         /* configure GPIO for i2s port */
         switch (pdev->id) {
         case 1:
-                s3c_gpio_cfgpin(S5PV210_GPC0(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPC0(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPC0(2), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPC0(3), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPC0(4), S3C_GPIO_SFN(2));
+                s3c_gpio_cfgpin_range(S5PV210_GPC0(0), 5, S3C_GPIO_SFN(2));
                 break;
 
         case 2:
-                s3c_gpio_cfgpin(S5PV210_GPC1(0), S3C_GPIO_SFN(4));
-                s3c_gpio_cfgpin(S5PV210_GPC1(1), S3C_GPIO_SFN(4));
-                s3c_gpio_cfgpin(S5PV210_GPC1(2), S3C_GPIO_SFN(4));
-                s3c_gpio_cfgpin(S5PV210_GPC1(3), S3C_GPIO_SFN(4));
-                s3c_gpio_cfgpin(S5PV210_GPC1(4), S3C_GPIO_SFN(4));
+                s3c_gpio_cfgpin_range(S5PV210_GPC1(0), 5, S3C_GPIO_SFN(4));
                 break;
 
         case -1:
-                s3c_gpio_cfgpin(S5PV210_GPI(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPI(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPI(2), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPI(3), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPI(4), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPI(5), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPI(6), S3C_GPIO_SFN(2));
+                s3c_gpio_cfgpin_range(S5PV210_GPI(0), 7, S3C_GPIO_SFN(2));
                 break;
 
         default:
@@ -151,25 +137,13 @@ static int s5pv210_pcm_cfg_gpio(struct platform_device *pdev)
 {
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S5PV210_GPI(0), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PV210_GPI(1), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PV210_GPI(2), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PV210_GPI(3), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PV210_GPI(4), S3C_GPIO_SFN(3));
+                s3c_gpio_cfgpin_range(S5PV210_GPI(0), 5, S3C_GPIO_SFN(3));
                 break;
         case 1:
-                s3c_gpio_cfgpin(S5PV210_GPC0(0), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PV210_GPC0(1), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PV210_GPC0(2), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PV210_GPC0(3), S3C_GPIO_SFN(3));
-                s3c_gpio_cfgpin(S5PV210_GPC0(4), S3C_GPIO_SFN(3));
+                s3c_gpio_cfgpin_range(S5PV210_GPC0(0), 5, S3C_GPIO_SFN(3));
                 break;
         case 2:
-                s3c_gpio_cfgpin(S5PV210_GPC1(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPC1(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPC1(2), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPC1(3), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPC1(4), S3C_GPIO_SFN(2));
+                s3c_gpio_cfgpin_range(S5PV210_GPC1(0), 5, S3C_GPIO_SFN(2));
                 break;
         default:
                 printk(KERN_DEBUG "Invalid PCM Controller number!");
@@ -271,13 +245,7 @@ struct platform_device s5pv210_device_pcm2 = {
 
 static int s5pv210_ac97_cfg_gpio(struct platform_device *pdev)
 {
-        s3c_gpio_cfgpin(S5PV210_GPC0(0), S3C_GPIO_SFN(4));
-        s3c_gpio_cfgpin(S5PV210_GPC0(1), S3C_GPIO_SFN(4));
-        s3c_gpio_cfgpin(S5PV210_GPC0(2), S3C_GPIO_SFN(4));
-        s3c_gpio_cfgpin(S5PV210_GPC0(3), S3C_GPIO_SFN(4));
-        s3c_gpio_cfgpin(S5PV210_GPC0(4), S3C_GPIO_SFN(4));
-
-        return 0;
+        return s3c_gpio_cfgpin_range(S5PV210_GPC0(0), 5, S3C_GPIO_SFN(4));
 }
 
 static struct resource s5pv210_ac97_resource[] = {
@@ -325,3 +293,43 @@ struct platform_device s5pv210_device_ac97 = {
                 .coherent_dma_mask = DMA_BIT_MASK(32),
         },
 };
+
+/* S/PDIF Controller platform_device */
+
+static int s5pv210_spdif_cfg_gpio(struct platform_device *pdev)
+{
+        s3c_gpio_cfgpin_range(S5PV210_GPC1(0), 2, S3C_GPIO_SFN(3));
+
+        return 0;
+}
+
+static struct resource s5pv210_spdif_resource[] = {
+        [0] = {
+                .start  = S5PV210_PA_SPDIF,
+                .end    = S5PV210_PA_SPDIF + 0x100 - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = DMACH_SPDIF,
+                .end    = DMACH_SPDIF,
+                .flags  = IORESOURCE_DMA,
+        },
+};
+
+static struct s3c_audio_pdata samsung_spdif_pdata = {
+        .cfg_gpio = s5pv210_spdif_cfg_gpio,
+};
+
+static u64 s5pv210_spdif_dmamask = DMA_BIT_MASK(32);
+
+struct platform_device s5pv210_device_spdif = {
+        .name           = "samsung-spdif",
+        .id             = -1,
+        .num_resources  = ARRAY_SIZE(s5pv210_spdif_resource),
+        .resource       = s5pv210_spdif_resource,
+        .dev = {
+                .platform_data = &samsung_spdif_pdata,
+                .dma_mask = &s5pv210_spdif_dmamask,
+                .coherent_dma_mask = DMA_BIT_MASK(32),
+        },
+};
diff --git a/arch/arm/mach-s5pv210/dev-spi.c b/arch/arm/mach-s5pv210/dev-spi.c
index 826cdbc43e20..e3249a47e3b1 100644
--- a/arch/arm/mach-s5pv210/dev-spi.c
+++ b/arch/arm/mach-s5pv210/dev-spi.c
@@ -35,23 +35,15 @@ static char *spi_src_clks[] = {
  */
 static int s5pv210_spi_cfg_gpio(struct platform_device *pdev)
 {
+        unsigned int base;
+
         switch (pdev->id) {
         case 0:
-                s3c_gpio_cfgpin(S5PV210_GPB(0), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPB(1), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPB(2), S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(S5PV210_GPB(0), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PV210_GPB(1), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PV210_GPB(2), S3C_GPIO_PULL_UP);
+                base = S5PV210_GPB(0);
                 break;
 
         case 1:
-                s3c_gpio_cfgpin(S5PV210_GPB(4), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPB(5), S3C_GPIO_SFN(2));
-                s3c_gpio_cfgpin(S5PV210_GPB(6), S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(S5PV210_GPB(4), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PV210_GPB(5), S3C_GPIO_PULL_UP);
-                s3c_gpio_setpull(S5PV210_GPB(6), S3C_GPIO_PULL_UP);
+                base = S5PV210_GPB(4);
                 break;
 
         default:
@@ -59,6 +51,9 @@ static int s5pv210_spi_cfg_gpio(struct platform_device *pdev)
                 return -EINVAL;
         }
 
+        s3c_gpio_cfgall_range(base, 3,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
+
         return 0;
 }
 
diff --git a/arch/arm/mach-s5pv210/dma.c b/arch/arm/mach-s5pv210/dma.c
index 778ad5fe231a..497d3439a142 100644
--- a/arch/arm/mach-s5pv210/dma.c
+++ b/arch/arm/mach-s5pv210/dma.c
@@ -82,7 +82,7 @@ static struct s3c_pl330_platdata s5pv210_pdma0_pdata = {
 
 static struct platform_device s5pv210_device_pdma0 = {
         .name           = "s3c-pl330",
-        .id             = 1,
+        .id             = 0,
         .num_resources  = ARRAY_SIZE(s5pv210_pdma0_resource),
         .resource       = s5pv210_pdma0_resource,
         .dev            = {
@@ -144,7 +144,7 @@ static struct s3c_pl330_platdata s5pv210_pdma1_pdata = {
 
 static struct platform_device s5pv210_device_pdma1 = {
         .name           = "s3c-pl330",
-        .id             = 2,
+        .id             = 1,
         .num_resources  = ARRAY_SIZE(s5pv210_pdma1_resource),
         .resource       = s5pv210_pdma1_resource,
         .dev            = {
diff --git a/arch/arm/mach-s5pv210/gpiolib.c b/arch/arm/mach-s5pv210/gpiolib.c
index 0d459112d039..ab673effd767 100644
--- a/arch/arm/mach-s5pv210/gpiolib.c
+++ b/arch/arm/mach-s5pv210/gpiolib.c
@@ -150,6 +150,7 @@ static struct s3c_gpio_chip s5pv210_gpio_4bit[] = {
                         .label  = "GPG3",
                 },
         }, {
+                .config = &gpio_cfg_noint,
                 .chip   = {
                         .base   = S5PV210_GPI(0),
                         .ngpio  = S5PV210_GPIO_I_NR,
@@ -223,34 +224,42 @@ static struct s3c_gpio_chip s5pv210_gpio_4bit[] = {
         }, {
                 .base   = (S5P_VA_GPIO + 0xC00),
                 .config = &gpio_cfg_noint,
+                .irq_base = IRQ_EINT(0),
                 .chip   = {
                         .base   = S5PV210_GPH0(0),
                         .ngpio  = S5PV210_GPIO_H0_NR,
                         .label  = "GPH0",
+                        .to_irq = samsung_gpiolib_to_irq,
                 },
         }, {
                 .base   = (S5P_VA_GPIO + 0xC20),
                 .config = &gpio_cfg_noint,
+                .irq_base = IRQ_EINT(8),
                 .chip   = {
                         .base   = S5PV210_GPH1(0),
                         .ngpio  = S5PV210_GPIO_H1_NR,
                         .label  = "GPH1",
+                        .to_irq = samsung_gpiolib_to_irq,
                 },
         }, {
                 .base   = (S5P_VA_GPIO + 0xC40),
                 .config = &gpio_cfg_noint,
+                .irq_base = IRQ_EINT(16),
                 .chip   = {
                         .base   = S5PV210_GPH2(0),
                         .ngpio  = S5PV210_GPIO_H2_NR,
                         .label  = "GPH2",
+                        .to_irq = samsung_gpiolib_to_irq,
                 },
         }, {
                 .base   = (S5P_VA_GPIO + 0xC60),
                 .config = &gpio_cfg_noint,
+                .irq_base = IRQ_EINT(24),
                 .chip   = {
                         .base   = S5PV210_GPH3(0),
                         .ngpio  = S5PV210_GPIO_H3_NR,
                         .label  = "GPH3",
+                        .to_irq = samsung_gpiolib_to_irq,
                 },
         },
 };
@@ -259,11 +268,14 @@ static __init int s5pv210_gpiolib_init(void)
 {
         struct s3c_gpio_chip *chip = s5pv210_gpio_4bit;
         int nr_chips = ARRAY_SIZE(s5pv210_gpio_4bit);
+        int gpioint_group = 0;
         int i = 0;
 
         for (i = 0; i < nr_chips; i++, chip++) {
-                if (chip->config == NULL)
+                if (chip->config == NULL) {
                         chip->config = &gpio_cfg;
+                        chip->group = gpioint_group++;
+                }
                 if (chip->base == NULL)
                         chip->base = S5PV210_BANK_BASE(i);
         }
diff --git a/arch/arm/mach-s5pv210/include/mach/irqs.h b/arch/arm/mach-s5pv210/include/mach/irqs.h
index e1c020e5a49b..119b95fdc3ce 100644
--- a/arch/arm/mach-s5pv210/include/mach/irqs.h
+++ b/arch/arm/mach-s5pv210/include/mach/irqs.h
@@ -55,8 +55,8 @@
 #define IRQ_SPI1                S5P_IRQ_VIC1(16)
 #define IRQ_SPI2                S5P_IRQ_VIC1(17)
 #define IRQ_IRDA                S5P_IRQ_VIC1(18)
-#define IRQ_CAN0                S5P_IRQ_VIC1(19)
-#define IRQ_CAN1                S5P_IRQ_VIC1(20)
+#define IRQ_IIC2                S5P_IRQ_VIC1(19)
+#define IRQ_IIC3                S5P_IRQ_VIC1(20)
 #define IRQ_HSIRX               S5P_IRQ_VIC1(21)
 #define IRQ_HSITX               S5P_IRQ_VIC1(22)
 #define IRQ_UHOST               S5P_IRQ_VIC1(23)
@@ -109,7 +109,7 @@
 
 #define IRQ_IPC                 S5P_IRQ_VIC3(0)
 #define IRQ_HOSTIF              S5P_IRQ_VIC3(1)
-#define IRQ_MMC3                S5P_IRQ_VIC3(2)
+#define IRQ_HSMMC3              S5P_IRQ_VIC3(2)
 #define IRQ_CEC                 S5P_IRQ_VIC3(3)
 #define IRQ_TSI                 S5P_IRQ_VIC3(4)
 #define IRQ_MDNIE0              S5P_IRQ_VIC3(5)
@@ -121,8 +121,12 @@
 #define S5P_EINT_BASE1          (S5P_IRQ_VIC0(0))
 #define S5P_EINT_BASE2          (IRQ_VIC_END + 1)
 
+/* GPIO interrupt */
+#define S5P_GPIOINT_BASE        (IRQ_EINT(31) + 1)
+#define S5P_GPIOINT_GROUP_MAXNR 22
+
 /* Set the default NR_IRQS */
-#define NR_IRQS                 (IRQ_EINT(31) + 1)
+#define NR_IRQS                 (IRQ_EINT(31) + S5P_GPIOINT_COUNT + 1)
 
 /* Compatibility */
 #define IRQ_LCD_FIFO            IRQ_LCD0
diff --git a/arch/arm/mach-s5pv210/include/mach/map.h b/arch/arm/mach-s5pv210/include/mach/map.h
index bd9afd52466a..861d7fe11fc9 100644
--- a/arch/arm/mach-s5pv210/include/mach/map.h
+++ b/arch/arm/mach-s5pv210/include/mach/map.h
@@ -57,6 +57,8 @@
 
 #define S5P_SZ_UART             SZ_256
 
+#define S3C_VA_UARTx(x)         (S3C_VA_UART + ((x) * S3C_UART_OFFSET))
+
 #define S5PV210_PA_SROMC        (0xE8000000)
 
 #define S5PV210_PA_CFCON        (0xE8200000)
@@ -73,6 +75,9 @@
 
 #define S5PV210_PA_HSMMC(x)     (0xEB000000 + ((x) * 0x100000))
 
+#define S5PV210_PA_HSOTG        (0xEC000000)
+#define S5PV210_PA_HSPHY        (0xEC100000)
+
 #define S5PV210_PA_VIC0         (0xF2000000)
 #define S5PV210_PA_VIC1         (0xF2100000)
 #define S5PV210_PA_VIC2         (0xF2200000)
@@ -81,6 +86,9 @@
 #define S5PV210_PA_SDRAM        (0x20000000)
 #define S5P_PA_SDRAM            S5PV210_PA_SDRAM
 
+/* S/PDIF */
+#define S5PV210_PA_SPDIF        0xE1100000
+
 /* I2S */
 #define S5PV210_PA_IIS0         0xEEE30000
 #define S5PV210_PA_IIS1         0xE2100000
@@ -96,6 +104,9 @@
 
 #define S5PV210_PA_ADC          (0xE1700000)
 
+#define S5PV210_PA_DMC0         (0xF0000000)
+#define S5PV210_PA_DMC1         (0xF1400000)
+
 /* compatibiltiy defines. */
 #define S3C_PA_UART             S5PV210_PA_UART
 #define S3C_PA_HSMMC0           S5PV210_PA_HSMMC(0)
@@ -108,6 +119,7 @@
 #define S3C_PA_FB               S5PV210_PA_FB
 #define S3C_PA_RTC              S5PV210_PA_RTC
 #define S3C_PA_WDT              S5PV210_PA_WATCHDOG
+#define S3C_PA_USB_HSOTG        S5PV210_PA_HSOTG
 #define S5P_PA_FIMC0            S5PV210_PA_FIMC0
 #define S5P_PA_FIMC1            S5PV210_PA_FIMC1
 #define S5P_PA_FIMC2            S5PV210_PA_FIMC2
diff --git a/arch/arm/mach-s5pv210/include/mach/pm-core.h b/arch/arm/mach-s5pv210/include/mach/pm-core.h
new file mode 100644
index 000000000000..e8d394f8b057
--- /dev/null
+++ b/arch/arm/mach-s5pv210/include/mach/pm-core.h
@@ -0,0 +1,43 @@
+/* linux/arch/arm/mach-s5pv210/include/mach/pm-core.h
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * Based on arch/arm/mach-s3c2410/include/mach/pm-core.h,
+ * Copyright 2008 Simtec Electronics
+ *      Ben Dooks <ben@simtec.co.uk>
+ *      http://armlinux.simtec.co.uk/
+ *
+ * S5PV210 - PM core support for arch/arm/plat-s5p/pm.c
+ *
+ * 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.
+*/
+
+static inline void s3c_pm_debug_init_uart(void)
+{
+        /* nothing here yet */
+}
+
+static inline void s3c_pm_arch_prepare_irqs(void)
+{
+        __raw_writel(s3c_irqwake_intmask, S5P_WAKEUP_MASK);
+        __raw_writel(s3c_irqwake_eintmask, S5P_EINT_WAKEUP_MASK);
+}
+
+static inline void s3c_pm_arch_stop_clocks(void)
+{
+        /* nothing here yet */
+}
+
+static inline void s3c_pm_arch_show_resume_irqs(void)
+{
+        /* nothing here yet */
+}
+
+static inline void s3c_pm_arch_update_uart(void __iomem *regs,
+                                           struct pm_uart_save *save)
+{
+        /* nothing here yet */
+}
diff --git a/arch/arm/mach-s5pv210/include/mach/regs-clock.h b/arch/arm/mach-s5pv210/include/mach/regs-clock.h
index 499aef737476..ebaabe021af9 100644
--- a/arch/arm/mach-s5pv210/include/mach/regs-clock.h
+++ b/arch/arm/mach-s5pv210/include/mach/regs-clock.h
@@ -25,6 +25,7 @@
 #define S5P_APLL_CON            S5P_CLKREG(0x100)
 #define S5P_MPLL_CON            S5P_CLKREG(0x108)
 #define S5P_EPLL_CON            S5P_CLKREG(0x110)
+#define S5P_EPLL_CON1           S5P_CLKREG(0x114)
 #define S5P_VPLL_CON            S5P_CLKREG(0x120)
 
 #define S5P_CLK_SRC0            S5P_CLKREG(0x200)
@@ -67,11 +68,28 @@
 #define S5P_CLKGATE_BUS1        S5P_CLKREG(0x488)
 #define S5P_CLK_OUT             S5P_CLKREG(0x500)
 
+/* DIV/MUX STATUS */
+#define S5P_CLKDIV_STAT0        S5P_CLKREG(0x1000)
+#define S5P_CLKDIV_STAT1        S5P_CLKREG(0x1004)
+#define S5P_CLKMUX_STAT0        S5P_CLKREG(0x1100)
+#define S5P_CLKMUX_STAT1        S5P_CLKREG(0x1104)
+
 /* CLKSRC0 */
-#define S5P_CLKSRC0_MUX200_MASK         (0x1<<16)
+#define S5P_CLKSRC0_MUX200_SHIFT        (16)
+#define S5P_CLKSRC0_MUX200_MASK         (0x1 << S5P_CLKSRC0_MUX200_SHIFT)
 #define S5P_CLKSRC0_MUX166_MASK         (0x1<<20)
 #define S5P_CLKSRC0_MUX133_MASK         (0x1<<24)
 
+/* CLKSRC2 */
+#define S5P_CLKSRC2_G3D_SHIFT           (0)
+#define S5P_CLKSRC2_G3D_MASK            (0x3 << S5P_CLKSRC2_G3D_SHIFT)
+#define S5P_CLKSRC2_MFC_SHIFT           (4)
+#define S5P_CLKSRC2_MFC_MASK            (0x3 << S5P_CLKSRC2_MFC_SHIFT)
+
+/* CLKSRC6*/
+#define S5P_CLKSRC6_ONEDRAM_SHIFT       (24)
+#define S5P_CLKSRC6_ONEDRAM_MASK        (0x3 << S5P_CLKSRC6_ONEDRAM_SHIFT)
+
 /* CLKDIV0 */
 #define S5P_CLKDIV0_APLL_SHIFT          (0)
 #define S5P_CLKDIV0_APLL_MASK           (0x7 << S5P_CLKDIV0_APLL_SHIFT)
@@ -90,12 +108,24 @@
 #define S5P_CLKDIV0_PCLK66_SHIFT        (28)
 #define S5P_CLKDIV0_PCLK66_MASK         (0x7 << S5P_CLKDIV0_PCLK66_SHIFT)
 
+/* CLKDIV2 */
+#define S5P_CLKDIV2_G3D_SHIFT           (0)
+#define S5P_CLKDIV2_G3D_MASK            (0xF << S5P_CLKDIV2_G3D_SHIFT)
+#define S5P_CLKDIV2_MFC_SHIFT           (4)
+#define S5P_CLKDIV2_MFC_MASK            (0xF << S5P_CLKDIV2_MFC_SHIFT)
+
+/* CLKDIV6 */
+#define S5P_CLKDIV6_ONEDRAM_SHIFT       (28)
+#define S5P_CLKDIV6_ONEDRAM_MASK        (0xF << S5P_CLKDIV6_ONEDRAM_SHIFT)
+
 #define S5P_SWRESET             S5P_CLKREG(0x2000)
 
+#define S5P_ARM_MCS_CON         S5P_CLKREG(0x6100)
+
 /* Registers related to power management */
 #define S5P_PWR_CFG             S5P_CLKREG(0xC000)
 #define S5P_EINT_WAKEUP_MASK    S5P_CLKREG(0xC004)
-#define S5P_WAKEUP_MASK         S5P_CLKREG(0xC008)
+#define S5P_WAKEUP_MASK         S5P_CLKREG(0xC008)
 #define S5P_PWR_MODE            S5P_CLKREG(0xC00C)
 #define S5P_NORMAL_CFG          S5P_CLKREG(0xC010)
 #define S5P_IDLE_CFG            S5P_CLKREG(0xC020)
@@ -159,8 +189,11 @@
 #define S5P_SLEEP_CFG_USBOSC_EN         (1 << 1)
 
 /* OTHERS Resgister */
+#define S5P_OTHERS_RET_IO               (1 << 31)
+#define S5P_OTHERS_RET_CF               (1 << 30)
+#define S5P_OTHERS_RET_MMC              (1 << 29)
+#define S5P_OTHERS_RET_UART             (1 << 28)
 #define S5P_OTHERS_USB_SIG_MASK         (1 << 16)
-#define S5P_OTHERS_MIPI_DPHY_EN         (1 << 28)
 
 /* MIPI */
 #define S5P_MIPI_DPHY_EN                (3)
diff --git a/arch/arm/mach-s5pv210/include/mach/regs-gpio.h b/arch/arm/mach-s5pv210/include/mach/regs-gpio.h
index 49e029b4978a..de0c89976078 100644
--- a/arch/arm/mach-s5pv210/include/mach/regs-gpio.h
+++ b/arch/arm/mach-s5pv210/include/mach/regs-gpio.h
@@ -31,13 +31,6 @@
 
 #define eint_irq_to_bit(irq)            (1 << (EINT_OFFSET(irq) & 0x7))
 
-/* values for S5P_EXTINT0 */
-#define S5P_EXTINT_LOWLEV               (0x00)
-#define S5P_EXTINT_HILEV                (0x01)
-#define S5P_EXTINT_FALLEDGE             (0x02)
-#define S5P_EXTINT_RISEEDGE             (0x03)
-#define S5P_EXTINT_BOTHEDGE             (0x04)
-
 #define EINT_MODE               S3C_GPIO_SFN(0xf)
 
 #define EINT_GPIO_0(x)          S5PV210_GPH0(x)
diff --git a/arch/arm/mach-s5pv210/include/mach/regs-sys.h b/arch/arm/mach-s5pv210/include/mach/regs-sys.h
new file mode 100644
index 000000000000..26691d39d0f4
--- /dev/null
+++ b/arch/arm/mach-s5pv210/include/mach/regs-sys.h
@@ -0,0 +1,19 @@
+/* arch/arm/mach-s5pv210/include/mach/regs-sys.h
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S5PV210 - System registers definitions
+ *
+ * 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.
+*/
+
+#define S5PV210_USB_PHY_CON     (S3C_VA_SYS + 0xE80C)
+#define S5PV210_USB_PHY0_EN     (1 << 0)
+#define S5PV210_USB_PHY1_EN     (1 << 1)
+
+/* compatibility defines for s3c-hsotg driver */
+#define S3C64XX_OTHERS          S5PV210_USB_PHY_CON
+#define S3C64XX_OTHERS_USBMASK  S5PV210_USB_PHY0_EN
diff --git a/arch/arm/mach-s5pv210/include/mach/vmalloc.h b/arch/arm/mach-s5pv210/include/mach/vmalloc.h
index df9a28808323..a6c659d68a5d 100644
--- a/arch/arm/mach-s5pv210/include/mach/vmalloc.h
+++ b/arch/arm/mach-s5pv210/include/mach/vmalloc.h
@@ -17,6 +17,6 @@
 #ifndef __ASM_ARCH_VMALLOC_H
 #define __ASM_ARCH_VMALLOC_H __FILE__
 
-#define VMALLOC_END     (0xE0000000UL)
+#define VMALLOC_END     0xF6000000UL
 
 #endif /* __ASM_ARCH_VMALLOC_H */
diff --git a/arch/arm/mach-s5pv210/mach-aquila.c b/arch/arm/mach-s5pv210/mach-aquila.c
index 00883087363c..28677caf3613 100644
--- a/arch/arm/mach-s5pv210/mach-aquila.c
+++ b/arch/arm/mach-s5pv210/mach-aquila.c
@@ -16,6 +16,8 @@
 #include <linux/i2c.h>
 #include <linux/i2c-gpio.h>
 #include <linux/mfd/max8998.h>
+#include <linux/mfd/wm8994/pdata.h>
+#include <linux/regulator/fixed.h>
 #include <linux/gpio_keys.h>
 #include <linux/input.h>
 #include <linux/gpio.h>
@@ -379,6 +381,119 @@ static struct max8998_platform_data aquila_max8998_pdata = {
 };
 #endif
 
+static struct regulator_consumer_supply wm8994_fixed_voltage0_supplies[] = {
+        {
+                .dev_name       = "5-001a",
+                .supply         = "DBVDD",
+        }, {
+                .dev_name       = "5-001a",
+                .supply         = "AVDD2",
+        }, {
+                .dev_name       = "5-001a",
+                .supply         = "CPVDD",
+        },
+};
+
+static struct regulator_consumer_supply wm8994_fixed_voltage1_supplies[] = {
+        {
+                .dev_name       = "5-001a",
+                .supply         = "SPKVDD1",
+        }, {
+                .dev_name       = "5-001a",
+                .supply         = "SPKVDD2",
+        },
+};
+
+static struct regulator_init_data wm8994_fixed_voltage0_init_data = {
+        .constraints = {
+                .always_on = 1,
+        },
+        .num_consumer_supplies  = ARRAY_SIZE(wm8994_fixed_voltage0_supplies),
+        .consumer_supplies      = wm8994_fixed_voltage0_supplies,
+};
+
+static struct regulator_init_data wm8994_fixed_voltage1_init_data = {
+        .constraints = {
+                .always_on = 1,
+        },
+        .num_consumer_supplies  = ARRAY_SIZE(wm8994_fixed_voltage1_supplies),
+        .consumer_supplies      = wm8994_fixed_voltage1_supplies,
+};
+
+static struct fixed_voltage_config wm8994_fixed_voltage0_config = {
+        .supply_name    = "VCC_1.8V_PDA",
+        .microvolts     = 1800000,
+        .gpio           = -EINVAL,
+        .init_data      = &wm8994_fixed_voltage0_init_data,
+};
+
+static struct fixed_voltage_config wm8994_fixed_voltage1_config = {
+        .supply_name    = "V_BAT",
+        .microvolts     = 3700000,
+        .gpio           = -EINVAL,
+        .init_data      = &wm8994_fixed_voltage1_init_data,
+};
+
+static struct platform_device wm8994_fixed_voltage0 = {
+        .name           = "reg-fixed-voltage",
+        .id             = 0,
+        .dev            = {
+                .platform_data  = &wm8994_fixed_voltage0_config,
+        },
+};
+
+static struct platform_device wm8994_fixed_voltage1 = {
+        .name           = "reg-fixed-voltage",
+        .id             = 1,
+        .dev            = {
+                .platform_data  = &wm8994_fixed_voltage1_config,
+        },
+};
+
+static struct regulator_consumer_supply wm8994_avdd1_supply = {
+        .dev_name       = "5-001a",
+        .supply         = "AVDD1",
+};
+
+static struct regulator_consumer_supply wm8994_dcvdd_supply = {
+        .dev_name       = "5-001a",
+        .supply         = "DCVDD",
+};
+
+static struct regulator_init_data wm8994_ldo1_data = {
+        .constraints    = {
+                .name           = "AVDD1_3.0V",
+                .valid_ops_mask = REGULATOR_CHANGE_STATUS,
+        },
+        .num_consumer_supplies  = 1,
+        .consumer_supplies      = &wm8994_avdd1_supply,
+};
+
+static struct regulator_init_data wm8994_ldo2_data = {
+        .constraints    = {
+                .name           = "DCVDD_1.0V",
+        },
+        .num_consumer_supplies  = 1,
+        .consumer_supplies      = &wm8994_dcvdd_supply,
+};
+
+static struct wm8994_pdata wm8994_platform_data = {
+        /* configure gpio1 function: 0x0001(Logic level input/output) */
+        .gpio_defaults[0] = 0x0001,
+        /* configure gpio3/4/5/7 function for AIF2 voice */
+        .gpio_defaults[2] = 0x8100,
+        .gpio_defaults[3] = 0x8100,
+        .gpio_defaults[4] = 0x8100,
+        .gpio_defaults[6] = 0x0100,
+        /* configure gpio8/9/10/11 function for AIF3 BT */
+        .gpio_defaults[7] = 0x8100,
+        .gpio_defaults[8] = 0x0100,
+        .gpio_defaults[9] = 0x0100,
+        .gpio_defaults[10] = 0x0100,
+        .ldo[0] = { S5PV210_MP03(6), NULL, &wm8994_ldo1_data }, /* XM0FRNB_2 */
+        .ldo[1] = { 0, NULL, &wm8994_ldo2_data },
+};
+
 /* GPIO I2C PMIC */
 #define AP_I2C_GPIO_PMIC_BUS_4  4
 static struct i2c_gpio_platform_data aquila_i2c_gpio_pmic_data = {
@@ -404,6 +519,29 @@ static struct i2c_board_info i2c_gpio_pmic_devs[] __initdata = {
 #endif
 };
 
+/* GPIO I2C AP 1.8V */
+#define AP_I2C_GPIO_BUS_5       5
+static struct i2c_gpio_platform_data aquila_i2c_gpio5_data = {
+        .sda_pin        = S5PV210_MP05(3),      /* XM0ADDR_11 */
+        .scl_pin        = S5PV210_MP05(2),      /* XM0ADDR_10 */
+};
+
+static struct platform_device aquila_i2c_gpio5 = {
+        .name           = "i2c-gpio",
+        .id             = AP_I2C_GPIO_BUS_5,
+        .dev            = {
+                .platform_data  = &aquila_i2c_gpio5_data,
+        },
+};
+
+static struct i2c_board_info i2c_gpio5_devs[] __initdata = {
+        {
+                /* CS/ADDR = low 0x34 (FYI: high = 0x36) */
+                I2C_BOARD_INFO("wm8994", 0x1a),
+                .platform_data  = &wm8994_platform_data,
+        },
+};
+
 /* PMIC Power button */
 static struct gpio_keys_button aquila_gpio_keys_table[] = {
         {
@@ -475,6 +613,7 @@ static void aquila_setup_sdhci(void)
 
 static struct platform_device *aquila_devices[] __initdata = {
         &aquila_i2c_gpio_pmic,
+        &aquila_i2c_gpio5,
         &aquila_device_gpiokeys,
         &s3c_device_fb,
         &s5p_device_onenand,
@@ -484,8 +623,33 @@ static struct platform_device *aquila_devices[] __initdata = {
         &s5p_device_fimc0,
         &s5p_device_fimc1,
         &s5p_device_fimc2,
+        &s5pv210_device_iis0,
+        &wm8994_fixed_voltage0,
+        &wm8994_fixed_voltage1,
 };
 
+static void __init aquila_sound_init(void)
+{
+        unsigned int gpio;
+
+        /* CODEC_XTAL_EN
+         *
+         * The Aquila board have a oscillator which provide main clock
+         * to WM8994 codec. The oscillator provide 24MHz clock to WM8994
+         * clock. Set gpio setting of "CODEC_XTAL_EN" to enable a oscillator.
+         * */
+        gpio = S5PV210_GPH3(2);         /* XEINT_26 */
+        gpio_request(gpio, "CODEC_XTAL_EN");
+        s3c_gpio_cfgpin(gpio, S3C_GPIO_OUTPUT);
+        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
+
+        /* Ths main clock of WM8994 codec uses the output of CLKOUT pin.
+         * The CLKOUT[9:8] set to 0x3(XUSBXTI) of 0xE010E000(OTHERS)
+         * because it needs 24MHz clock to operate WM8994 codec.
+         */
+        __raw_writel(__raw_readl(S5P_OTHERS) | (0x3 << 8), S5P_OTHERS);
+}
+
 static void __init aquila_map_io(void)
 {
         s5p_init_io(NULL, 0, S5P_VA_CHIPID);
@@ -506,6 +670,11 @@ static void __init aquila_machine_init(void)
         s3c_fimc_setname(1, "s5p-fimc");
         s3c_fimc_setname(2, "s5p-fimc");
 
+        /* SOUND */
+        aquila_sound_init();
+        i2c_register_board_info(AP_I2C_GPIO_BUS_5, i2c_gpio5_devs,
+                        ARRAY_SIZE(i2c_gpio5_devs));
+
         /* FB */
         s3c_fb_set_platdata(&aquila_lcd_pdata);
 
diff --git a/arch/arm/mach-s5pv210/mach-goni.c b/arch/arm/mach-s5pv210/mach-goni.c
index d9ecf57fc2a5..b1dcf964a768 100644
--- a/arch/arm/mach-s5pv210/mach-goni.c
+++ b/arch/arm/mach-s5pv210/mach-goni.c
@@ -15,7 +15,13 @@
 #include <linux/fb.h>
 #include <linux/i2c.h>
 #include <linux/i2c-gpio.h>
+#include <linux/i2c/qt602240_ts.h>
 #include <linux/mfd/max8998.h>
+#include <linux/mfd/wm8994/pdata.h>
+#include <linux/regulator/fixed.h>
+#include <linux/spi/spi.h>
+#include <linux/spi/spi_gpio.h>
+#include <linux/lcd.h>
 #include <linux/gpio_keys.h>
 #include <linux/input.h>
 #include <linux/gpio.h>
@@ -35,7 +41,10 @@
 #include <plat/devs.h>
 #include <plat/cpu.h>
 #include <plat/fb.h>
+#include <plat/iic.h>
+#include <plat/keypad.h>
 #include <plat/sdhci.h>
+#include <plat/clock.h>
 
 /* Following are default values for UCON, ULCON and UFCON UART registers */
 #define GONI_UCON_DEFAULT       (S3C2410_UCON_TXILEVEL |        \
@@ -87,13 +96,12 @@ static struct s3c2410_uartcfg goni_uartcfgs[] __initdata = {
 /* Frame Buffer */
 static struct s3c_fb_pd_win goni_fb_win0 = {
         .win_mode = {
-                .pixclock = 1000000000000ULL / ((16+16+2+480)*(28+3+2+800)*55),
                 .left_margin    = 16,
                 .right_margin   = 16,
-                .upper_margin   = 3,
+                .upper_margin   = 2,
                 .lower_margin   = 28,
                 .hsync_len      = 2,
-                .vsync_len      = 2,
+                .vsync_len      = 1,
                 .xres           = 480,
                 .yres           = 800,
                 .refresh        = 55,
@@ -111,9 +119,160 @@ static struct s3c_fb_platdata goni_lcd_pdata __initdata = {
         .setup_gpio     = s5pv210_fb_gpio_setup_24bpp,
 };
 
+static int lcd_power_on(struct lcd_device *ld, int enable)
+{
+        return 1;
+}
+
+static int reset_lcd(struct lcd_device *ld)
+{
+        static unsigned int first = 1;
+        int reset_gpio = -1;
+
+        reset_gpio = S5PV210_MP05(5);
+
+        if (first) {
+                gpio_request(reset_gpio, "MLCD_RST");
+                first = 0;
+        }
+
+        gpio_direction_output(reset_gpio, 1);
+        return 1;
+}
+
+static struct lcd_platform_data goni_lcd_platform_data = {
+        .reset                  = reset_lcd,
+        .power_on               = lcd_power_on,
+        .lcd_enabled            = 0,
+        .reset_delay            = 120,  /* 120ms */
+        .power_on_delay         = 25,   /* 25ms */
+        .power_off_delay        = 200,  /* 200ms */
+};
+
+#define LCD_BUS_NUM     3
+static struct spi_board_info spi_board_info[] __initdata = {
+        {
+                .modalias       = "s6e63m0",
+                .platform_data  = &goni_lcd_platform_data,
+                .max_speed_hz   = 1200000,
+                .bus_num        = LCD_BUS_NUM,
+                .chip_select    = 0,
+                .mode           = SPI_MODE_3,
+                .controller_data = (void *)S5PV210_MP01(1), /* DISPLAY_CS */
+        },
+};
+
+static struct spi_gpio_platform_data lcd_spi_gpio_data = {
+        .sck    = S5PV210_MP04(1), /* DISPLAY_CLK */
+        .mosi   = S5PV210_MP04(3), /* DISPLAY_SI */
+        .miso   = SPI_GPIO_NO_MISO,
+        .num_chipselect = 1,
+};
+
+static struct platform_device goni_spi_gpio = {
+        .name   = "spi_gpio",
+        .id     = LCD_BUS_NUM,
+        .dev    = {
+                .parent         = &s3c_device_fb.dev,
+                .platform_data  = &lcd_spi_gpio_data,
+        },
+};
+
+/* KEYPAD */
+static uint32_t keymap[] __initdata = {
+        /* KEY(row, col, keycode) */
+        KEY(0, 1, KEY_MENU),            /* Send */
+        KEY(0, 2, KEY_BACK),            /* End */
+        KEY(1, 1, KEY_CONFIG),          /* Half shot */
+        KEY(1, 2, KEY_VOLUMEUP),
+        KEY(2, 1, KEY_CAMERA),          /* Full shot */
+        KEY(2, 2, KEY_VOLUMEDOWN),
+};
+
+static struct matrix_keymap_data keymap_data __initdata = {
+        .keymap         = keymap,
+        .keymap_size    = ARRAY_SIZE(keymap),
+};
+
+static struct samsung_keypad_platdata keypad_data __initdata = {
+        .keymap_data    = &keymap_data,
+        .rows           = 3,
+        .cols           = 3,
+};
+
+/* Radio */
+static struct i2c_board_info i2c1_devs[] __initdata = {
+        {
+                I2C_BOARD_INFO("si470x", 0x10),
+        },
+};
+
+static void __init goni_radio_init(void)
+{
+        int gpio;
+
+        gpio = S5PV210_GPJ2(4);                 /* XMSMDATA_4 */
+        gpio_request(gpio, "FM_INT");
+        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(0xf));
+        i2c1_devs[0].irq = gpio_to_irq(gpio);
+
+        gpio = S5PV210_GPJ2(5);                 /* XMSMDATA_5 */
+        gpio_request(gpio, "FM_RST");
+        gpio_direction_output(gpio, 1);
+}
+
+/* TSP */
+static struct qt602240_platform_data qt602240_platform_data = {
+        .x_line         = 17,
+        .y_line         = 11,
+        .x_size         = 800,
+        .y_size         = 480,
+        .blen           = 0x21,
+        .threshold      = 0x28,
+        .voltage        = 2800000,              /* 2.8V */
+        .orient         = QT602240_DIAGONAL,
+};
+
+static struct s3c2410_platform_i2c i2c2_data __initdata = {
+        .flags          = 0,
+        .bus_num        = 2,
+        .slave_addr     = 0x10,
+        .frequency      = 400 * 1000,
+        .sda_delay      = 100,
+};
+
+static struct i2c_board_info i2c2_devs[] __initdata = {
+        {
+                I2C_BOARD_INFO("qt602240_ts", 0x4a),
+                .platform_data = &qt602240_platform_data,
+        },
+};
+
+static void __init goni_tsp_init(void)
+{
+        int gpio;
+
+        gpio = S5PV210_GPJ1(3);         /* XMSMADDR_11 */
+        gpio_request(gpio, "TSP_LDO_ON");
+        gpio_direction_output(gpio, 1);
+        gpio_export(gpio, 0);
+
+        gpio = S5PV210_GPJ0(5);         /* XMSMADDR_5 */
+        gpio_request(gpio, "TSP_INT");
+
+        s5p_register_gpio_interrupt(gpio);
+        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(0xf));
+        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_UP);
+        i2c2_devs[0].irq = gpio_to_irq(gpio);
+}
+
 /* MAX8998 regulators */
 #if defined(CONFIG_REGULATOR_MAX8998) || defined(CONFIG_REGULATOR_MAX8998_MODULE)
 
+static struct regulator_consumer_supply goni_ldo5_consumers[] = {
+        REGULATOR_SUPPLY("vmmc", "s3c-sdhci.0"),
+};
+
 static struct regulator_init_data goni_ldo2_data = {
         .constraints    = {
                 .name           = "VALIVE_1.1V",
@@ -153,6 +312,8 @@ static struct regulator_init_data goni_ldo5_data = {
                 .max_uV         = 2800000,
                 .apply_uV       = 1,
         },
+        .num_consumer_supplies = ARRAY_SIZE(goni_ldo5_consumers),
+        .consumer_supplies = goni_ldo5_consumers,
 };
 
 static struct regulator_init_data goni_ldo6_data = {
@@ -360,6 +521,119 @@ static struct max8998_platform_data goni_max8998_pdata = {
 };
 #endif
 
+static struct regulator_consumer_supply wm8994_fixed_voltage0_supplies[] = {
+        {
+                .dev_name       = "5-001a",
+                .supply         = "DBVDD",
+        }, {
+                .dev_name       = "5-001a",
+                .supply         = "AVDD2",
+        }, {
+                .dev_name       = "5-001a",
+                .supply         = "CPVDD",
+        },
+};
+
+static struct regulator_consumer_supply wm8994_fixed_voltage1_supplies[] = {
+        {
+                .dev_name       = "5-001a",
+                .supply         = "SPKVDD1",
+        }, {
+                .dev_name       = "5-001a",
+                .supply         = "SPKVDD2",
+        },
+};
+
+static struct regulator_init_data wm8994_fixed_voltage0_init_data = {
+        .constraints = {
+                .always_on = 1,
+        },
+        .num_consumer_supplies  = ARRAY_SIZE(wm8994_fixed_voltage0_supplies),
+        .consumer_supplies      = wm8994_fixed_voltage0_supplies,
+};
+
+static struct regulator_init_data wm8994_fixed_voltage1_init_data = {
+        .constraints = {
+                .always_on = 1,
+        },
+        .num_consumer_supplies  = ARRAY_SIZE(wm8994_fixed_voltage1_supplies),
+        .consumer_supplies      = wm8994_fixed_voltage1_supplies,
+};
+
+static struct fixed_voltage_config wm8994_fixed_voltage0_config = {
+        .supply_name    = "VCC_1.8V_PDA",
+        .microvolts     = 1800000,
+        .gpio           = -EINVAL,
+        .init_data      = &wm8994_fixed_voltage0_init_data,
+};
+
+static struct fixed_voltage_config wm8994_fixed_voltage1_config = {
+        .supply_name    = "V_BAT",
+        .microvolts     = 3700000,
+        .gpio           = -EINVAL,
+        .init_data      = &wm8994_fixed_voltage1_init_data,
+};
+
+static struct platform_device wm8994_fixed_voltage0 = {
+        .name           = "reg-fixed-voltage",
+        .id             = 0,
+        .dev            = {
+                .platform_data  = &wm8994_fixed_voltage0_config,
+        },
+};
+
+static struct platform_device wm8994_fixed_voltage1 = {
+        .name           = "reg-fixed-voltage",
+        .id             = 1,
+        .dev            = {
+                .platform_data  = &wm8994_fixed_voltage1_config,
+        },
+};
+
+static struct regulator_consumer_supply wm8994_avdd1_supply = {
+        .dev_name       = "5-001a",
+        .supply         = "AVDD1",
+};
+
+static struct regulator_consumer_supply wm8994_dcvdd_supply = {
+        .dev_name       = "5-001a",
+        .supply         = "DCVDD",
+};
+
+static struct regulator_init_data wm8994_ldo1_data = {
+        .constraints    = {
+                .name           = "AVDD1_3.0V",
+                .valid_ops_mask = REGULATOR_CHANGE_STATUS,
+        },
+        .num_consumer_supplies  = 1,
+        .consumer_supplies      = &wm8994_avdd1_supply,
+};
+
+static struct regulator_init_data wm8994_ldo2_data = {
+        .constraints    = {
+                .name           = "DCVDD_1.0V",
+        },
+        .num_consumer_supplies  = 1,
+        .consumer_supplies      = &wm8994_dcvdd_supply,
+};
+
+static struct wm8994_pdata wm8994_platform_data = {
+        /* configure gpio1 function: 0x0001(Logic level input/output) */
+        .gpio_defaults[0] = 0x0001,
+        /* configure gpio3/4/5/7 function for AIF2 voice */
+        .gpio_defaults[2] = 0x8100,
+        .gpio_defaults[3] = 0x8100,
+        .gpio_defaults[4] = 0x8100,
+        .gpio_defaults[6] = 0x0100,
+        /* configure gpio8/9/10/11 function for AIF3 BT */
+        .gpio_defaults[7] = 0x8100,
+        .gpio_defaults[8] = 0x0100,
+        .gpio_defaults[9] = 0x0100,
+        .gpio_defaults[10] = 0x0100,
+        .ldo[0] = { S5PV210_MP03(6), NULL, &wm8994_ldo1_data }, /* XM0FRNB_2 */
+        .ldo[1] = { 0, NULL, &wm8994_ldo2_data },
+};
+
 /* GPIO I2C PMIC */
 #define AP_I2C_GPIO_PMIC_BUS_4  4
 static struct i2c_gpio_platform_data goni_i2c_gpio_pmic_data = {
@@ -385,6 +659,29 @@ static struct i2c_board_info i2c_gpio_pmic_devs[] __initdata = {
 #endif
 };
 
+/* GPIO I2C AP 1.8V */
+#define AP_I2C_GPIO_BUS_5       5
+static struct i2c_gpio_platform_data goni_i2c_gpio5_data = {
+        .sda_pin        = S5PV210_MP05(3),      /* XM0ADDR_11 */
+        .scl_pin        = S5PV210_MP05(2),      /* XM0ADDR_10 */
+};
+
+static struct platform_device goni_i2c_gpio5 = {
+        .name           = "i2c-gpio",
+        .id             = AP_I2C_GPIO_BUS_5,
+        .dev            = {
+                .platform_data  = &goni_i2c_gpio5_data,
+        },
+};
+
+static struct i2c_board_info i2c_gpio5_devs[] __initdata = {
+        {
+                /* CS/ADDR = low 0x34 (FYI: high = 0x36) */
+                I2C_BOARD_INFO("wm8994", 0x1a),
+                .platform_data  = &wm8994_platform_data,
+        },
+};
+
 /* PMIC Power button */
 static struct gpio_keys_button goni_gpio_keys_table[] = {
         {
@@ -444,11 +741,37 @@ static struct s3c_sdhci_platdata goni_hsmmc2_data __initdata = {
         .ext_cd_gpio_invert     = 1,
 };
 
+static struct regulator_consumer_supply mmc2_supplies[] = {
+        REGULATOR_SUPPLY("vmmc", "s3c-sdhci.2"),
+};
+
+static struct regulator_init_data mmc2_fixed_voltage_init_data = {
+        .constraints            = {
+                .name           = "V_TF_2.8V",
+                .valid_ops_mask = REGULATOR_CHANGE_STATUS,
+        },
+        .num_consumer_supplies  = ARRAY_SIZE(mmc2_supplies),
+        .consumer_supplies      = mmc2_supplies,
+};
+
+static struct fixed_voltage_config mmc2_fixed_voltage_config = {
+        .supply_name            = "EXT_FLASH_EN",
+        .microvolts             = 2800000,
+        .gpio                   = GONI_EXT_FLASH_EN,
+        .enable_high            = true,
+        .init_data              = &mmc2_fixed_voltage_init_data,
+};
+
+static struct platform_device mmc2_fixed_voltage = {
+        .name           = "reg-fixed-voltage",
+        .id             = 2,
+        .dev            = {
+                .platform_data  = &mmc2_fixed_voltage_config,
+        },
+};
+
 static void goni_setup_sdhci(void)
 {
-        gpio_request(GONI_EXT_FLASH_EN, "FLASH_EN");
-        gpio_direction_output(GONI_EXT_FLASH_EN, 1);
-
         s3c_sdhci0_set_platdata(&goni_hsmmc0_data);
         s3c_sdhci1_set_platdata(&goni_hsmmc1_data);
         s3c_sdhci2_set_platdata(&goni_hsmmc2_data);
@@ -457,7 +780,10 @@ static void goni_setup_sdhci(void)
 static struct platform_device *goni_devices[] __initdata = {
         &s3c_device_fb,
         &s5p_device_onenand,
+        &goni_spi_gpio,
         &goni_i2c_gpio_pmic,
+        &goni_i2c_gpio5,
+        &mmc2_fixed_voltage,
         &goni_device_gpiokeys,
         &s5p_device_fimc0,
         &s5p_device_fimc1,
@@ -465,8 +791,24 @@ static struct platform_device *goni_devices[] __initdata = {
         &s3c_device_hsmmc0,
         &s3c_device_hsmmc1,
         &s3c_device_hsmmc2,
+        &s5pv210_device_iis0,
+        &s3c_device_usb_hsotg,
+        &samsung_device_keypad,
+        &s3c_device_i2c1,
+        &s3c_device_i2c2,
+        &wm8994_fixed_voltage0,
+        &wm8994_fixed_voltage1,
 };
 
+static void __init goni_sound_init(void)
+{
+        /* Ths main clock of WM8994 codec uses the output of CLKOUT pin.
+         * The CLKOUT[9:8] set to 0x3(XUSBXTI) of 0xE010E000(OTHERS)
+         * because it needs 24MHz clock to operate WM8994 codec.
+         */
+        __raw_writel(__raw_readl(S5P_OTHERS) | (0x3 << 8), S5P_OTHERS);
+}
+
 static void __init goni_map_io(void)
 {
         s5p_init_io(NULL, 0, S5P_VA_CHIPID);
@@ -476,6 +818,20 @@ static void __init goni_map_io(void)
 
 static void __init goni_machine_init(void)
 {
+        /* Radio: call before I2C 1 registeration */
+        goni_radio_init();
+
+        /* I2C1 */
+        s3c_i2c1_set_platdata(NULL);
+        i2c_register_board_info(1, i2c1_devs, ARRAY_SIZE(i2c1_devs));
+
+        /* TSP: call before I2C 2 registeration */
+        goni_tsp_init();
+
+        /* I2C2 */
+        s3c_i2c2_set_platdata(&i2c2_data);
+        i2c_register_board_info(2, i2c2_devs, ARRAY_SIZE(i2c2_devs));
+
         /* PMIC */
         goni_pmic_init();
         i2c_register_board_info(AP_I2C_GPIO_PMIC_BUS_4, i2c_gpio_pmic_devs,
@@ -483,9 +839,22 @@ static void __init goni_machine_init(void)
         /* SDHCI */
         goni_setup_sdhci();
 
+        /* SOUND */
+        goni_sound_init();
+        i2c_register_board_info(AP_I2C_GPIO_BUS_5, i2c_gpio5_devs,
+                        ARRAY_SIZE(i2c_gpio5_devs));
+
         /* FB */
         s3c_fb_set_platdata(&goni_lcd_pdata);
 
+        /* SPI */
+        spi_register_board_info(spi_board_info, ARRAY_SIZE(spi_board_info));
+
+        /* KEYPAD */
+        samsung_keypad_set_platdata(&keypad_data);
+
+        clk_xusbxti.rate = 24000000;
+
         platform_add_devices(goni_devices, ARRAY_SIZE(goni_devices));
 }
 
diff --git a/arch/arm/mach-s5pv210/mach-smdkc110.c b/arch/arm/mach-s5pv210/mach-smdkc110.c
index cea9bca79d88..0ad7924fe62e 100644
--- a/arch/arm/mach-s5pv210/mach-smdkc110.c
+++ b/arch/arm/mach-s5pv210/mach-smdkc110.c
@@ -28,6 +28,7 @@
 #include <plat/cpu.h>
 #include <plat/ata.h>
 #include <plat/iic.h>
+#include <plat/pm.h>
 
 /* Following are default values for UCON, ULCON and UFCON UART registers */
 #define SMDKC110_UCON_DEFAULT   (S3C2410_UCON_TXILEVEL |        \
@@ -81,6 +82,7 @@ static struct s3c_ide_platdata smdkc110_ide_pdata __initdata = {
 static struct platform_device *smdkc110_devices[] __initdata = {
         &s5pv210_device_iis0,
         &s5pv210_device_ac97,
+        &s5pv210_device_spdif,
         &s3c_device_cfcon,
         &s3c_device_i2c0,
         &s3c_device_i2c1,
@@ -110,6 +112,8 @@ static void __init smdkc110_map_io(void)
 
 static void __init smdkc110_machine_init(void)
 {
+        s3c_pm_init();
+
         s3c_i2c0_set_platdata(NULL);
         s3c_i2c1_set_platdata(NULL);
         s3c_i2c2_set_platdata(NULL);
diff --git a/arch/arm/mach-s5pv210/mach-smdkv210.c b/arch/arm/mach-s5pv210/mach-smdkv210.c
index 83189ae9da9a..bcd7a5d53401 100644
--- a/arch/arm/mach-s5pv210/mach-smdkv210.c
+++ b/arch/arm/mach-s5pv210/mach-smdkv210.c
@@ -31,6 +31,7 @@
 #include <plat/ata.h>
 #include <plat/iic.h>
 #include <plat/keypad.h>
+#include <plat/pm.h>
 
 /* Following are default values for UCON, ULCON and UFCON UART registers */
 #define SMDKV210_UCON_DEFAULT   (S3C2410_UCON_TXILEVEL |        \
@@ -103,6 +104,7 @@ static struct samsung_keypad_platdata smdkv210_keypad_data __initdata = {
 static struct platform_device *smdkv210_devices[] __initdata = {
         &s5pv210_device_iis0,
         &s5pv210_device_ac97,
+        &s5pv210_device_spdif,
         &s3c_device_adc,
         &s3c_device_cfcon,
         &s3c_device_hsmmc0,
@@ -145,6 +147,8 @@ static void __init smdkv210_map_io(void)
 
 static void __init smdkv210_machine_init(void)
 {
+        s3c_pm_init();
+
         samsung_keypad_set_platdata(&smdkv210_keypad_data);
         s3c24xx_ts_set_platdata(&s3c_ts_platform);
 
diff --git a/arch/arm/mach-s5pv210/mach-torbreck.c b/arch/arm/mach-s5pv210/mach-torbreck.c
new file mode 100644
index 000000000000..043c938806b0
--- /dev/null
+++ b/arch/arm/mach-s5pv210/mach-torbreck.c
@@ -0,0 +1,131 @@
+/* linux/arch/arm/mach-s5pv210/mach-torbreck.c
+ *
+ * Copyright (c) 2010 aESOP Community
+ *              http://www.aesop.or.kr/
+ *
+ * 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/kernel.h>
+#include <linux/types.h>
+#include <linux/i2c.h>
+#include <linux/init.h>
+#include <linux/serial_core.h>
+
+#include <asm/mach/arch.h>
+#include <asm/mach/map.h>
+#include <asm/setup.h>
+#include <asm/mach-types.h>
+
+#include <mach/map.h>
+#include <mach/regs-clock.h>
+
+#include <plat/regs-serial.h>
+#include <plat/s5pv210.h>
+#include <plat/devs.h>
+#include <plat/cpu.h>
+#include <plat/iic.h>
+
+/* Following are default values for UCON, ULCON and UFCON UART registers */
+#define TORBRECK_UCON_DEFAULT   (S3C2410_UCON_TXILEVEL |        \
+                                 S3C2410_UCON_RXILEVEL |        \
+                                 S3C2410_UCON_TXIRQMODE |       \
+                                 S3C2410_UCON_RXIRQMODE |       \
+                                 S3C2410_UCON_RXFIFO_TOI |      \
+                                 S3C2443_UCON_RXERR_IRQEN)
+
+#define TORBRECK_ULCON_DEFAULT  S3C2410_LCON_CS8
+
+#define TORBRECK_UFCON_DEFAULT  (S3C2410_UFCON_FIFOMODE |       \
+                                 S5PV210_UFCON_TXTRIG4 |        \
+                                 S5PV210_UFCON_RXTRIG4)
+
+static struct s3c2410_uartcfg torbreck_uartcfgs[] __initdata = {
+        [0] = {
+                .hwport         = 0,
+                .flags          = 0,
+                .ucon           = TORBRECK_UCON_DEFAULT,
+                .ulcon          = TORBRECK_ULCON_DEFAULT,
+                .ufcon          = TORBRECK_UFCON_DEFAULT,
+        },
+        [1] = {
+                .hwport         = 1,
+                .flags          = 0,
+                .ucon           = TORBRECK_UCON_DEFAULT,
+                .ulcon          = TORBRECK_ULCON_DEFAULT,
+                .ufcon          = TORBRECK_UFCON_DEFAULT,
+        },
+        [2] = {
+                .hwport         = 2,
+                .flags          = 0,
+                .ucon           = TORBRECK_UCON_DEFAULT,
+                .ulcon          = TORBRECK_ULCON_DEFAULT,
+                .ufcon          = TORBRECK_UFCON_DEFAULT,
+        },
+        [3] = {
+                .hwport         = 3,
+                .flags          = 0,
+                .ucon           = TORBRECK_UCON_DEFAULT,
+                .ulcon          = TORBRECK_ULCON_DEFAULT,
+                .ufcon          = TORBRECK_UFCON_DEFAULT,
+        },
+};
+
+static struct platform_device *torbreck_devices[] __initdata = {
+        &s5pv210_device_iis0,
+        &s3c_device_cfcon,
+        &s3c_device_hsmmc0,
+        &s3c_device_hsmmc1,
+        &s3c_device_hsmmc2,
+        &s3c_device_hsmmc3,
+        &s3c_device_i2c0,
+        &s3c_device_i2c1,
+        &s3c_device_i2c2,
+        &s3c_device_rtc,
+        &s3c_device_wdt,
+};
+
+static struct i2c_board_info torbreck_i2c_devs0[] __initdata = {
+        /* To Be Updated */
+};
+
+static struct i2c_board_info torbreck_i2c_devs1[] __initdata = {
+        /* To Be Updated */
+};
+
+static struct i2c_board_info torbreck_i2c_devs2[] __initdata = {
+        /* To Be Updated */
+};
+
+static void __init torbreck_map_io(void)
+{
+        s5p_init_io(NULL, 0, S5P_VA_CHIPID);
+        s3c24xx_init_clocks(24000000);
+        s3c24xx_init_uarts(torbreck_uartcfgs, ARRAY_SIZE(torbreck_uartcfgs));
+}
+
+static void __init torbreck_machine_init(void)
+{
+        s3c_i2c0_set_platdata(NULL);
+        s3c_i2c1_set_platdata(NULL);
+        s3c_i2c2_set_platdata(NULL);
+        i2c_register_board_info(0, torbreck_i2c_devs0,
+                        ARRAY_SIZE(torbreck_i2c_devs0));
+        i2c_register_board_info(1, torbreck_i2c_devs1,
+                        ARRAY_SIZE(torbreck_i2c_devs1));
+        i2c_register_board_info(2, torbreck_i2c_devs2,
+                        ARRAY_SIZE(torbreck_i2c_devs2));
+
+        platform_add_devices(torbreck_devices, ARRAY_SIZE(torbreck_devices));
+}
+
+MACHINE_START(TORBRECK, "TORBRECK")
+        /* Maintainer: Hyunchul Ko <ghcstop@gmail.com> */
+        .boot_params    = S5P_PA_SDRAM + 0x100,
+        .init_irq       = s5pv210_init_irq,
+        .map_io         = torbreck_map_io,
+        .init_machine   = torbreck_machine_init,
+        .timer          = &s3c24xx_timer,
+MACHINE_END
diff --git a/arch/arm/mach-s5pv210/pm.c b/arch/arm/mach-s5pv210/pm.c
new file mode 100644
index 000000000000..549d7924fd4c
--- /dev/null
+++ b/arch/arm/mach-s5pv210/pm.c
@@ -0,0 +1,166 @@
+/* linux/arch/arm/mach-s5pv210/pm.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * S5PV210 - Power Management support
+ *
+ * Based on arch/arm/mach-s3c2410/pm.c
+ * Copyright (c) 2006 Simtec Electronics
+ *      Ben Dooks <ben@simtec.co.uk>
+ *
+ * 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/init.h>
+#include <linux/suspend.h>
+#include <linux/io.h>
+
+#include <plat/cpu.h>
+#include <plat/pm.h>
+#include <plat/regs-timer.h>
+
+#include <mach/regs-irq.h>
+#include <mach/regs-clock.h>
+
+static struct sleep_save s5pv210_core_save[] = {
+        /* Clock source */
+        SAVE_ITEM(S5P_CLK_SRC0),
+        SAVE_ITEM(S5P_CLK_SRC1),
+        SAVE_ITEM(S5P_CLK_SRC2),
+        SAVE_ITEM(S5P_CLK_SRC3),
+        SAVE_ITEM(S5P_CLK_SRC4),
+        SAVE_ITEM(S5P_CLK_SRC5),
+        SAVE_ITEM(S5P_CLK_SRC6),
+
+        /* Clock source Mask */
+        SAVE_ITEM(S5P_CLK_SRC_MASK0),
+        SAVE_ITEM(S5P_CLK_SRC_MASK1),
+
+        /* Clock Divider */
+        SAVE_ITEM(S5P_CLK_DIV0),
+        SAVE_ITEM(S5P_CLK_DIV1),
+        SAVE_ITEM(S5P_CLK_DIV2),
+        SAVE_ITEM(S5P_CLK_DIV3),
+        SAVE_ITEM(S5P_CLK_DIV4),
+        SAVE_ITEM(S5P_CLK_DIV5),
+        SAVE_ITEM(S5P_CLK_DIV6),
+        SAVE_ITEM(S5P_CLK_DIV7),
+
+        /* Clock Main Gate */
+        SAVE_ITEM(S5P_CLKGATE_MAIN0),
+        SAVE_ITEM(S5P_CLKGATE_MAIN1),
+        SAVE_ITEM(S5P_CLKGATE_MAIN2),
+
+        /* Clock source Peri Gate */
+        SAVE_ITEM(S5P_CLKGATE_PERI0),
+        SAVE_ITEM(S5P_CLKGATE_PERI1),
+
+        /* Clock source SCLK Gate */
+        SAVE_ITEM(S5P_CLKGATE_SCLK0),
+        SAVE_ITEM(S5P_CLKGATE_SCLK1),
+
+        /* Clock IP Clock gate */
+        SAVE_ITEM(S5P_CLKGATE_IP0),
+        SAVE_ITEM(S5P_CLKGATE_IP1),
+        SAVE_ITEM(S5P_CLKGATE_IP2),
+        SAVE_ITEM(S5P_CLKGATE_IP3),
+        SAVE_ITEM(S5P_CLKGATE_IP4),
+
+        /* Clock Blcok and Bus gate */
+        SAVE_ITEM(S5P_CLKGATE_BLOCK),
+        SAVE_ITEM(S5P_CLKGATE_BUS0),
+
+        /* Clock ETC */
+        SAVE_ITEM(S5P_CLK_OUT),
+        SAVE_ITEM(S5P_MDNIE_SEL),
+
+        /* PWM Register */
+        SAVE_ITEM(S3C2410_TCFG0),
+        SAVE_ITEM(S3C2410_TCFG1),
+        SAVE_ITEM(S3C64XX_TINT_CSTAT),
+        SAVE_ITEM(S3C2410_TCON),
+        SAVE_ITEM(S3C2410_TCNTB(0)),
+        SAVE_ITEM(S3C2410_TCMPB(0)),
+        SAVE_ITEM(S3C2410_TCNTO(0)),
+};
+
+void s5pv210_cpu_suspend(void)
+{
+        unsigned long tmp;
+
+        /* issue the standby signal into the pm unit. Note, we
+         * issue a write-buffer drain just in case */
+
+        tmp = 0;
+
+        asm("b 1f\n\t"
+            ".align 5\n\t"
+            "1:\n\t"
+            "mcr p15, 0, %0, c7, c10, 5\n\t"
+            "mcr p15, 0, %0, c7, c10, 4\n\t"
+            "wfi" : : "r" (tmp));
+
+        /* we should never get past here */
+        panic("sleep resumed to originator?");
+}
+
+static void s5pv210_pm_prepare(void)
+{
+        unsigned int tmp;
+
+        /* ensure at least INFORM0 has the resume address */
+        __raw_writel(virt_to_phys(s3c_cpu_resume), S5P_INFORM0);
+
+        tmp = __raw_readl(S5P_SLEEP_CFG);
+        tmp &= ~(S5P_SLEEP_CFG_OSC_EN | S5P_SLEEP_CFG_USBOSC_EN);
+        __raw_writel(tmp, S5P_SLEEP_CFG);
+
+        /* WFI for SLEEP mode configuration by SYSCON */
+        tmp = __raw_readl(S5P_PWR_CFG);
+        tmp &= S5P_CFG_WFI_CLEAN;
+        tmp |= S5P_CFG_WFI_SLEEP;
+        __raw_writel(tmp, S5P_PWR_CFG);
+
+        /* SYSCON interrupt handling disable */
+        tmp = __raw_readl(S5P_OTHERS);
+        tmp |= S5P_OTHER_SYSC_INTOFF;
+        __raw_writel(tmp, S5P_OTHERS);
+
+        s3c_pm_do_save(s5pv210_core_save, ARRAY_SIZE(s5pv210_core_save));
+}
+
+static int s5pv210_pm_add(struct sys_device *sysdev)
+{
+        pm_cpu_prep = s5pv210_pm_prepare;
+        pm_cpu_sleep = s5pv210_cpu_suspend;
+
+        return 0;
+}
+
+static int s5pv210_pm_resume(struct sys_device *dev)
+{
+        u32 tmp;
+
+        tmp = __raw_readl(S5P_OTHERS);
+        tmp |= (S5P_OTHERS_RET_IO | S5P_OTHERS_RET_CF |\
+                S5P_OTHERS_RET_MMC | S5P_OTHERS_RET_UART);
+        __raw_writel(tmp , S5P_OTHERS);
+
+        s3c_pm_do_restore_core(s5pv210_core_save, ARRAY_SIZE(s5pv210_core_save));
+
+        return 0;
+}
+
+static struct sysdev_driver s5pv210_pm_driver = {
+        .add            = s5pv210_pm_add,
+        .resume         = s5pv210_pm_resume,
+};
+
+static __init int s5pv210_pm_drvinit(void)
+{
+        return sysdev_driver_register(&s5pv210_sysclass, &s5pv210_pm_driver);
+}
+arch_initcall(s5pv210_pm_drvinit);
diff --git a/arch/arm/mach-s5pv210/setup-fb-24bpp.c b/arch/arm/mach-s5pv210/setup-fb-24bpp.c
index 928cf1f125fa..e932ebfac56d 100644
--- a/arch/arm/mach-s5pv210/setup-fb-24bpp.c
+++ b/arch/arm/mach-s5pv210/setup-fb-24bpp.c
@@ -21,33 +21,21 @@
 #include <mach/regs-clock.h>
 #include <plat/gpio-cfg.h>
 
-void s5pv210_fb_gpio_setup_24bpp(void)
+static void s5pv210_fb_cfg_gpios(unsigned int base, unsigned int nr)
 {
-        unsigned int gpio = 0;
-
-        for (gpio = S5PV210_GPF0(0); gpio <= S5PV210_GPF0(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
+        s3c_gpio_cfgrange_nopull(base, nr, S3C_GPIO_SFN(2));
 
-        for (gpio = S5PV210_GPF1(0); gpio <= S5PV210_GPF1(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
+        for (; nr > 0; nr--, base++)
+                s5p_gpio_set_drvstr(base, S5P_GPIO_DRVSTR_LV4);
+}
 
-        for (gpio = S5PV210_GPF2(0); gpio <= S5PV210_GPF2(7); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
 
-        for (gpio = S5PV210_GPF3(0); gpio <= S5PV210_GPF3(3); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
+void s5pv210_fb_gpio_setup_24bpp(void)
+{
+        s5pv210_fb_cfg_gpios(S5PV210_GPF0(0), 8);
+        s5pv210_fb_cfg_gpios(S5PV210_GPF1(0), 8);
+        s5pv210_fb_cfg_gpios(S5PV210_GPF2(0), 8);
+        s5pv210_fb_cfg_gpios(S5PV210_GPF3(0), 4);
 
         /* Set DISPLAY_CONTROL register for Display path selection.
          *
diff --git a/arch/arm/mach-s5pv210/setup-i2c0.c b/arch/arm/mach-s5pv210/setup-i2c0.c
index d38f7cb7e662..0f1cc3a1c1e8 100644
--- a/arch/arm/mach-s5pv210/setup-i2c0.c
+++ b/arch/arm/mach-s5pv210/setup-i2c0.c
@@ -23,8 +23,6 @@ struct platform_device; /* don't need the contents */
 
 void s3c_i2c0_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5PV210_GPD1(0), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV210_GPD1(0), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5PV210_GPD1(1), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV210_GPD1(1), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5PV210_GPD1(0), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
diff --git a/arch/arm/mach-s5pv210/setup-i2c1.c b/arch/arm/mach-s5pv210/setup-i2c1.c
index 148bb7857d89..f61365a34c56 100644
--- a/arch/arm/mach-s5pv210/setup-i2c1.c
+++ b/arch/arm/mach-s5pv210/setup-i2c1.c
@@ -23,8 +23,6 @@ struct platform_device; /* don't need the contents */
 
 void s3c_i2c1_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5PV210_GPD1(2), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV210_GPD1(2), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5PV210_GPD1(3), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV210_GPD1(3), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5PV210_GPD1(2), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
diff --git a/arch/arm/mach-s5pv210/setup-i2c2.c b/arch/arm/mach-s5pv210/setup-i2c2.c
index 2396cb8c373e..2f91b5cefbc6 100644
--- a/arch/arm/mach-s5pv210/setup-i2c2.c
+++ b/arch/arm/mach-s5pv210/setup-i2c2.c
@@ -23,8 +23,6 @@ struct platform_device; /* don't need the contents */
 
 void s3c_i2c2_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5PV210_GPD1(4), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV210_GPD1(4), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5PV210_GPD1(5), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV210_GPD1(5), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5PV210_GPD1(4), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
diff --git a/arch/arm/mach-s5pv210/setup-ide.c b/arch/arm/mach-s5pv210/setup-ide.c
index b558b1cc8d60..ea123d546bd2 100644
--- a/arch/arm/mach-s5pv210/setup-ide.c
+++ b/arch/arm/mach-s5pv210/setup-ide.c
@@ -15,36 +15,25 @@
 
 #include <plat/gpio-cfg.h>
 
+static void s5pv210_ide_cfg_gpios(unsigned int base, unsigned int nr)
+{
+        s3c_gpio_cfgrange_nopull(base, nr, S3C_GPIO_SFN(4));
+
+        for (; nr > 0; nr--, base++)
+                s5p_gpio_set_drvstr(base, S5P_GPIO_DRVSTR_LV4);
+}
+
 void s5pv210_ide_setup_gpio(void)
 {
-        unsigned int gpio = 0;
-
-        for (gpio = S5PV210_GPJ0(0); gpio <= S5PV210_GPJ0(7); gpio++) {
-                /* CF_Add[0 - 2], CF_IORDY, CF_INTRQ, CF_DMARQ, CF_DMARST,
-                        CF_DMACK */
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(4));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
-
-        for (gpio = S5PV210_GPJ2(0); gpio <= S5PV210_GPJ2(7); gpio++) {
-                /*CF_Data[0 - 7] */
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(4));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
-
-        for (gpio = S5PV210_GPJ3(0); gpio <= S5PV210_GPJ3(7); gpio++) {
-                /* CF_Data[8 - 15] */
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(4));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
-
-        for (gpio = S5PV210_GPJ4(0); gpio <= S5PV210_GPJ4(3); gpio++) {
-                /* CF_CS0, CF_CS1, CF_IORD, CF_IOWR */
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(4));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
-        }
+        /* CF_Add[0 - 2], CF_IORDY, CF_INTRQ, CF_DMARQ, CF_DMARST, CF_DMACK */
+        s5pv210_ide_cfg_gpios(S5PV210_GPJ0(0), 8);
+
+        /* CF_Data[0 - 7] */
+        s5pv210_ide_cfg_gpios(S5PV210_GPJ2(0), 8);
+
+        /* CF_Data[8 - 15] */
+        s5pv210_ide_cfg_gpios(S5PV210_GPJ3(0), 8);
+
+        /* CF_CS0, CF_CS1, CF_IORD, CF_IOWR */
+        s5pv210_ide_cfg_gpios(S5PV210_GPJ4(0), 4);
 }
diff --git a/arch/arm/mach-s5pv210/setup-keypad.c b/arch/arm/mach-s5pv210/setup-keypad.c
index 37b2790aafc3..c56420a52f48 100644
--- a/arch/arm/mach-s5pv210/setup-keypad.c
+++ b/arch/arm/mach-s5pv210/setup-keypad.c
@@ -16,19 +16,9 @@
 
 void samsung_keypad_cfg_gpio(unsigned int rows, unsigned int cols)
 {
-        unsigned int gpio, end;
-
         /* Set all the necessary GPH3 pins to special-function 3: KP_ROW[x] */
-        end = S5PV210_GPH3(rows);
-        for (gpio = S5PV210_GPH3(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PV210_GPH3(0), rows, S3C_GPIO_SFN(3));
 
         /* Set all the necessary GPH2 pins to special-function 3: KP_COL[x] */
-        end = S5PV210_GPH2(cols);
-        for (gpio = S5PV210_GPH2(0); gpio < end; gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PV210_GPH2(0), cols, S3C_GPIO_SFN(3));
 }
diff --git a/arch/arm/mach-s5pv210/setup-sdhci-gpio.c b/arch/arm/mach-s5pv210/setup-sdhci-gpio.c
index b18587b1ec58..746777d56df9 100644
--- a/arch/arm/mach-s5pv210/setup-sdhci-gpio.c
+++ b/arch/arm/mach-s5pv210/setup-sdhci-gpio.c
@@ -26,26 +26,17 @@
 void s5pv210_setup_sdhci0_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
 
         /* Set all the necessary GPG0/GPG1 pins to special-function 2 */
-        for (gpio = S5PV210_GPG0(0); gpio < S5PV210_GPG0(2); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PV210_GPG0(0), 2, S3C_GPIO_SFN(2));
+
         switch (width) {
         case 8:
                 /* GPG1[3:6] special-funtion 3 */
-                for (gpio = S5PV210_GPG1(3); gpio <= S5PV210_GPG1(6); gpio++) {
-                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                }
+                s3c_gpio_cfgrange_nopull(S5PV210_GPG1(3), 4, S3C_GPIO_SFN(3));
         case 4:
                 /* GPG0[3:6] special-funtion 2 */
-                for (gpio = S5PV210_GPG0(3); gpio <= S5PV210_GPG0(6); gpio++) {
-                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                }
+                s3c_gpio_cfgrange_nopull(S5PV210_GPG0(3), 4, S3C_GPIO_SFN(2));
         default:
                 break;
         }
@@ -59,19 +50,12 @@ void s5pv210_setup_sdhci0_cfg_gpio(struct platform_device *dev, int width)
 void s5pv210_setup_sdhci1_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
 
         /* Set all the necessary GPG1[0:1] pins to special-function 2 */
-        for (gpio = S5PV210_GPG1(0); gpio < S5PV210_GPG1(2); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PV210_GPG1(0), 2, S3C_GPIO_SFN(2));
 
         /* Data pin GPG1[3:6] to special-function 2 */
-        for (gpio = S5PV210_GPG1(3); gpio <= S5PV210_GPG1(6); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PV210_GPG1(3), 4, S3C_GPIO_SFN(2));
 
         if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
                 s3c_gpio_setpull(S5PV210_GPG1(2), S3C_GPIO_PULL_UP);
@@ -82,27 +66,17 @@ void s5pv210_setup_sdhci1_cfg_gpio(struct platform_device *dev, int width)
 void s5pv210_setup_sdhci2_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
 
         /* Set all the necessary GPG2[0:1] pins to special-function 2 */
-        for (gpio = S5PV210_GPG2(0); gpio < S5PV210_GPG2(2); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PV210_GPG2(0), 2, S3C_GPIO_SFN(2));
 
         switch (width) {
         case 8:
                 /* Data pin GPG3[3:6] to special-function 3 */
-                for (gpio = S5PV210_GPG3(3); gpio <= S5PV210_GPG3(6); gpio++) {
-                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
-                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                }
+                s3c_gpio_cfgrange_nopull(S5PV210_GPG3(3), 4, S3C_GPIO_SFN(3));
         case 4:
                 /* Data pin GPG2[3:6] to special-function 2 */
-                for (gpio = S5PV210_GPG2(3); gpio <= S5PV210_GPG2(6); gpio++) {
-                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-                }
+                s3c_gpio_cfgrange_nopull(S5PV210_GPG2(3), 4, S3C_GPIO_SFN(2));
         default:
                 break;
         }
@@ -116,19 +90,12 @@ void s5pv210_setup_sdhci2_cfg_gpio(struct platform_device *dev, int width)
 void s5pv210_setup_sdhci3_cfg_gpio(struct platform_device *dev, int width)
 {
         struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
-        unsigned int gpio;
 
-        /* Set all the necessary GPG3[0:2] pins to special-function 2 */
-        for (gpio = S5PV210_GPG3(0); gpio < S5PV210_GPG3(2); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        /* Set all the necessary GPG3[0:1] pins to special-function 2 */
+        s3c_gpio_cfgrange_nopull(S5PV210_GPG3(0), 2, S3C_GPIO_SFN(2));
 
         /* Data pin GPG3[3:6] to special-function 2 */
-        for (gpio = S5PV210_GPG3(3); gpio <= S5PV210_GPG3(6); gpio++) {
-                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
-                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
-        }
+        s3c_gpio_cfgrange_nopull(S5PV210_GPG3(3), 4, S3C_GPIO_SFN(2));
 
         if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
                 s3c_gpio_setpull(S5PV210_GPG3(2), S3C_GPIO_PULL_UP);
diff --git a/arch/arm/mach-s5pv210/sleep.S b/arch/arm/mach-s5pv210/sleep.S
new file mode 100644
index 000000000000..d4d222b716b4
--- /dev/null
+++ b/arch/arm/mach-s5pv210/sleep.S
@@ -0,0 +1,170 @@
+/* linux/arch/arm/plat-s5p/sleep.S
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * S5PV210 power Manager (Suspend-To-RAM) support
+ * Based on S3C2410 sleep code by:
+ *      Ben Dooks, (c) 2004 Simtec Electronics
+ *
+ * Based on PXA/SA1100 sleep code by:
+ *      Nicolas Pitre, (c) 2002 Monta Vista Software Inc
+ *      Cliff Brake, (c) 2001
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
+*/
+
+#include <linux/linkage.h>
+#include <asm/assembler.h>
+#include <asm/memory.h>
+
+        .text
+
+        /* s3c_cpu_save
+         *
+         * entry:
+         *      r0 = save address (virtual addr of s3c_sleep_save_phys)
+        */
+
+ENTRY(s3c_cpu_save)
+
+        stmfd   sp!, { r3 - r12, lr }
+
+        mrc     p15, 0, r4, c13, c0, 0  @ FCSE/PID
+        mrc     p15, 0, r5, c3, c0, 0   @ Domain ID
+        mrc     p15, 0, r6, c2, c0, 0   @ Translation Table BASE0
+        mrc     p15, 0, r7, c2, c0, 1   @ Translation Table BASE1
+        mrc     p15, 0, r8, c2, c0, 2   @ Translation Table Control
+        mrc     p15, 0, r9, c1, c0, 0   @ Control register
+        mrc     p15, 0, r10, c1, c0, 1  @ Auxiliary control register
+        mrc     p15, 0, r11, c1, c0, 2  @ Co-processor access controls
+        mrc     p15, 0, r12, c10, c2, 0 @ Read PRRR
+        mrc     p15, 0, r3, c10, c2, 1  @ READ NMRR
+
+        stmia   r0, { r3 - r13 }
+
+        bl      s3c_pm_cb_flushcache
+
+        ldr     r0, =pm_cpu_sleep
+        ldr     r0, [ r0 ]
+        mov     pc, r0
+
+resume_with_mmu:
+        /*
+         * After MMU is turned on, restore the previous MMU table.
+         */
+        ldr     r9 , =(PAGE_OFFSET - PHYS_OFFSET)
+        add     r4, r4, r9
+        str     r12, [r4]
+
+        ldmfd   sp!, { r3 - r12, pc }
+
+        .ltorg
+
+        .data
+
+        .global s3c_sleep_save_phys
+s3c_sleep_save_phys:
+        .word   0
+
+        /* sleep magic, to allow the bootloader to check for an valid
+         * image to resume to. Must be the first word before the
+         * s3c_cpu_resume entry.
+        */
+
+        .word   0x2bedf00d
+
+        /* s3c_cpu_resume
+         *
+         * resume code entry for bootloader to call
+         *
+         * we must put this code here in the data segment as we have no
+         * other way of restoring the stack pointer after sleep, and we
+         * must not write to the code segment (code is read-only)
+        */
+
+ENTRY(s3c_cpu_resume)
+        mov     r0, #PSR_I_BIT | PSR_F_BIT | SVC_MODE
+        msr     cpsr_c, r0
+
+        mov     r1, #0
+        mcr     p15, 0, r1, c8, c7, 0           @ invalidate TLBs
+        mcr     p15, 0, r1, c7, c5, 0           @ invalidate I Cache
+
+        ldr     r0, s3c_sleep_save_phys         @ address of restore block
+        ldmia   r0, { r3 - r13 }
+
+        mcr     p15, 0, r4, c13, c0, 0          @ FCSE/PID
+        mcr     p15, 0, r5, c3, c0, 0           @ Domain ID
+
+        mcr     p15, 0, r8, c2, c0, 2           @ Translation Table Control
+        mcr     p15, 0, r7, c2, c0, 1           @ Translation Table BASE1
+        mcr     p15, 0, r6, c2, c0, 0           @ Translation Table BASE0
+
+        mcr     p15, 0, r10, c1, c0, 1          @ Auxiliary control register
+
+        mov     r0, #0
+        mcr     p15, 0, r0, c8, c7, 0           @ Invalidate I & D TLB
+
+        mov     r0, #0                          @ restore copro access
+        mcr     p15, 0, r11, c1, c0, 2          @ Co-processor access
+        mcr     p15, 0, r0, c7, c5, 4
+
+        mcr     p15, 0, r12, c10, c2, 0         @ write PRRR
+        mcr     p15, 0, r3, c10, c2, 1          @ write NMRR
+
+        /*
+         * In Cortex-A8, when MMU is turned on, the pipeline is flushed.
+         * And there are no valid entries in the MMU table at this point.
+         * So before turning on the MMU, the MMU entry for the DRAM address
+         * range is added. After the MMU is turned on, the other entries
+         * in the MMU table will be restored.
+        */
+
+        /* r6 = Translation Table BASE0 */
+        mov     r4, r6
+        mov     r4, r4, LSR #14
+        mov     r4, r4, LSL #14
+
+        /* Load address for adding to MMU table list */
+        ldr     r11, =0xE010F000                @ INFORM0 reg.
+        ldr     r10, [r11, #0]
+        mov     r10, r10, LSR #18
+        bic     r10, r10, #0x3
+        orr     r4, r4, r10
+
+        /* Calculate MMU table entry */
+        mov     r10, r10, LSL #18
+        ldr     r5, =0x40E
+        orr     r10, r10, r5
+
+        /* Back up originally data */
+        ldr     r12, [r4]
+
+        /* Add calculated MMU table entry into MMU table list */
+        str     r10, [r4]
+
+        ldr     r2, =resume_with_mmu
+        mcr     p15, 0, r9, c1, c0, 0           @ turn on MMU, etc
+
+        nop
+        nop
+        nop
+        nop
+        nop                                     @ second-to-last before mmu
+
+        mov     pc, r2                          @ go back to virtual address
+
+        .ltorg
diff --git a/arch/arm/mach-s5pv310/Kconfig b/arch/arm/mach-s5pv310/Kconfig
index 331b5bd97aba..1150b360f38c 100644
--- a/arch/arm/mach-s5pv310/Kconfig
+++ b/arch/arm/mach-s5pv310/Kconfig
@@ -11,7 +11,6 @@ if ARCH_S5PV310
 
 config CPU_S5PV310
         bool
-        select PLAT_S5P
         help
           Enable S5PV310 CPU support
 
@@ -25,21 +24,105 @@ config S5PV310_SETUP_I2C2
         help
           Common setup code for i2c bus 2.
 
+config S5PV310_SETUP_I2C3
+        bool
+        help
+          Common setup code for i2c bus 3.
+
+config S5PV310_SETUP_I2C4
+        bool
+        help
+          Common setup code for i2c bus 4.
+
+config S5PV310_SETUP_I2C5
+        bool
+        help
+          Common setup code for i2c bus 5.
+
+config S5PV310_SETUP_I2C6
+        bool
+        help
+          Common setup code for i2c bus 6.
+
+config S5PV310_SETUP_I2C7
+        bool
+        help
+          Common setup code for i2c bus 7.
+
+config S5PV310_SETUP_SDHCI
+        bool
+        select S5PV310_SETUP_SDHCI_GPIO
+        help
+          Internal helper functions for S5PV310 based SDHCI systems.
+
+config S5PV310_SETUP_SDHCI_GPIO
+        bool
+        help
+          Common setup code for SDHCI gpio.
+
 # machine support
 
-config MACH_SMDKV310
-        bool "SMDKV310"
+menu "S5PC210 Machines"
+
+config MACH_SMDKC210
+        bool "SMDKC210"
         select CPU_S5PV310
-        select ARCH_SPARSEMEM_ENABLE
+        select S3C_DEV_RTC
+        select S3C_DEV_WDT
+        select S3C_DEV_HSMMC
+        select S3C_DEV_HSMMC1
+        select S3C_DEV_HSMMC2
+        select S3C_DEV_HSMMC3
+        select S5PV310_SETUP_SDHCI
         help
-          Machine support for Samsung SMDKV310
+          Machine support for Samsung SMDKC210
+          S5PC210(MCP) is one of package option of S5PV310
 
 config MACH_UNIVERSAL_C210
         bool "Mobile UNIVERSAL_C210 Board"
         select CPU_S5PV310
-        select ARCH_SPARSEMEM_ENABLE
+        select S5P_DEV_ONENAND
+        select S3C_DEV_I2C1
+        select S5PV310_SETUP_I2C1
         help
           Machine support for Samsung Mobile Universal S5PC210 Reference
           Board. S5PC210(MCP) is one of package option of S5PV310
 
+endmenu
+
+menu "S5PV310 Machines"
+
+config MACH_SMDKV310
+        bool "SMDKV310"
+        select CPU_S5PV310
+        select S3C_DEV_RTC
+        select S3C_DEV_WDT
+        select S3C_DEV_HSMMC
+        select S3C_DEV_HSMMC1
+        select S3C_DEV_HSMMC2
+        select S3C_DEV_HSMMC3
+        select S5PV310_SETUP_SDHCI
+        help
+          Machine support for Samsung SMDKV310
+
+endmenu
+
+comment "Configuration for HSMMC bus width"
+
+menu "Use 8-bit bus width"
+
+config S5PV310_SDHCI_CH0_8BIT
+        bool "Channel 0 with 8-bit bus"
+        help
+          Support HSMMC Channel 0 8-bit bus.
+          If selected, Channel 1 is disabled.
+
+config S5PV310_SDHCI_CH2_8BIT
+        bool "Channel 2 with 8-bit bus"
+        help
+          Support HSMMC Channel 2 8-bit bus.
+          If selected, Channel 3 is disabled.
+
+endmenu
+
 endif
diff --git a/arch/arm/mach-s5pv310/Makefile b/arch/arm/mach-s5pv310/Makefile
index d5b51c72340f..84afc64e7c01 100644
--- a/arch/arm/mach-s5pv310/Makefile
+++ b/arch/arm/mach-s5pv310/Makefile
@@ -13,7 +13,7 @@ obj-				:=
 # Core support for S5PV310 system
 
 obj-$(CONFIG_CPU_S5PV310)       += cpu.o init.o clock.o irq-combiner.o
-obj-$(CONFIG_CPU_S5PV310)       += setup-i2c0.o time.o
+obj-$(CONFIG_CPU_S5PV310)       += setup-i2c0.o time.o gpiolib.o irq-eint.o
 
 obj-$(CONFIG_SMP)               += platsmp.o headsmp.o
 obj-$(CONFIG_LOCAL_TIMERS)      += localtimer.o
@@ -21,6 +21,7 @@ obj-$(CONFIG_HOTPLUG_CPU)	+= hotplug.o
 
 # machine support
 
+obj-$(CONFIG_MACH_SMDKC210)     += mach-smdkc210.o
 obj-$(CONFIG_MACH_SMDKV310)     += mach-smdkv310.o
 obj-$(CONFIG_MACH_UNIVERSAL_C210)       += mach-universal_c210.o
 
@@ -28,3 +29,10 @@ obj-$(CONFIG_MACH_UNIVERSAL_C210)	+= mach-universal_c210.o
 
 obj-$(CONFIG_S5PV310_SETUP_I2C1)        += setup-i2c1.o
 obj-$(CONFIG_S5PV310_SETUP_I2C2)        += setup-i2c2.o
+obj-$(CONFIG_S5PV310_SETUP_I2C3)        += setup-i2c3.o
+obj-$(CONFIG_S5PV310_SETUP_I2C4)        += setup-i2c4.o
+obj-$(CONFIG_S5PV310_SETUP_I2C5)        += setup-i2c5.o
+obj-$(CONFIG_S5PV310_SETUP_I2C6)        += setup-i2c6.o
+obj-$(CONFIG_S5PV310_SETUP_I2C7)        += setup-i2c7.o
+obj-$(CONFIG_S5PV310_SETUP_SDHCI)       += setup-sdhci.o
+obj-$(CONFIG_S5PV310_SETUP_SDHCI_GPIO)  += setup-sdhci-gpio.o
diff --git a/arch/arm/mach-s5pv310/clock.c b/arch/arm/mach-s5pv310/clock.c
index 26a0f03df8ea..58c9d33f36fe 100644
--- a/arch/arm/mach-s5pv310/clock.c
+++ b/arch/arm/mach-s5pv310/clock.c
@@ -30,16 +30,92 @@ static struct clk clk_sclk_hdmi27m = {
         .rate           = 27000000,
 };
 
+static struct clk clk_sclk_hdmiphy = {
+        .name           = "sclk_hdmiphy",
+        .id             = -1,
+};
+
+static struct clk clk_sclk_usbphy0 = {
+        .name           = "sclk_usbphy0",
+        .id             = -1,
+        .rate           = 27000000,
+};
+
+static struct clk clk_sclk_usbphy1 = {
+        .name           = "sclk_usbphy1",
+        .id             = -1,
+};
+
+static int s5pv310_clksrc_mask_top_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKSRC_MASK_TOP, clk, enable);
+}
+
+static int s5pv310_clksrc_mask_cam_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKSRC_MASK_CAM, clk, enable);
+}
+
+static int s5pv310_clksrc_mask_lcd0_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKSRC_MASK_LCD0, clk, enable);
+}
+
+static int s5pv310_clksrc_mask_lcd1_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKSRC_MASK_LCD1, clk, enable);
+}
+
+static int s5pv310_clksrc_mask_fsys_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKSRC_MASK_FSYS, clk, enable);
+}
+
 static int s5pv310_clksrc_mask_peril0_ctrl(struct clk *clk, int enable)
 {
         return s5p_gatectrl(S5P_CLKSRC_MASK_PERIL0, clk, enable);
 }
 
+static int s5pv310_clksrc_mask_peril1_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKSRC_MASK_PERIL1, clk, enable);
+}
+
+static int s5pv310_clk_ip_cam_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKGATE_IP_CAM, clk, enable);
+}
+
+static int s5pv310_clk_ip_image_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKGATE_IP_IMAGE, clk, enable);
+}
+
+static int s5pv310_clk_ip_lcd0_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKGATE_IP_LCD0, clk, enable);
+}
+
+static int s5pv310_clk_ip_lcd1_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKGATE_IP_LCD1, clk, enable);
+}
+
+static int s5pv310_clk_ip_fsys_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKGATE_IP_FSYS, clk, enable);
+}
+
 static int s5pv310_clk_ip_peril_ctrl(struct clk *clk, int enable)
 {
         return s5p_gatectrl(S5P_CLKGATE_IP_PERIL, clk, enable);
 }
 
+static int s5pv310_clk_ip_perir_ctrl(struct clk *clk, int enable)
+{
+        return s5p_gatectrl(S5P_CLKGATE_IP_PERIR, clk, enable);
+}
+
 /* Core list of CMU_CPU side */
 
 static struct clksrc_clk clk_mout_apll = {
@@ -79,7 +155,7 @@ static struct clksrc_clk clk_mout_mpll = {
 };
 
 static struct clk *clkset_moutcore_list[] = {
-        [0] = &clk_sclk_apll.clk,
+        [0] = &clk_mout_apll.clk,
         [1] = &clk_mout_mpll.clk,
 };
 
@@ -150,24 +226,6 @@ static struct clksrc_clk clk_periphclk = {
         .reg_div        = { .reg = S5P_CLKDIV_CPU, .shift = 12, .size = 3 },
 };
 
-static struct clksrc_clk clk_atclk = {
-        .clk    = {
-                .name           = "atclk",
-                .id             = -1,
-                .parent         = &clk_moutcore.clk,
-        },
-        .reg_div        = { .reg = S5P_CLKDIV_CPU, .shift = 16, .size = 3 },
-};
-
-static struct clksrc_clk clk_pclk_dbg = {
-        .clk    = {
-                .name           = "pclk_dbg",
-                .id             = -1,
-                .parent         = &clk_atclk.clk,
-        },
-        .reg_div        = { .reg = S5P_CLKDIV_CPU, .shift = 20, .size = 3 },
-};
-
 /* Core list of CMU_CORE side */
 
 static struct clk *clkset_corebus_list[] = {
@@ -241,7 +299,7 @@ static struct clk *clkset_aclk_top_list[] = {
         [1] = &clk_sclk_apll.clk,
 };
 
-static struct clksrc_sources clkset_aclk_200 = {
+static struct clksrc_sources clkset_aclk = {
         .sources        = clkset_aclk_top_list,
         .nr_sources     = ARRAY_SIZE(clkset_aclk_top_list),
 };
@@ -251,52 +309,37 @@ static struct clksrc_clk clk_aclk_200 = {
                 .name           = "aclk_200",
                 .id             = -1,
         },
-        .sources        = &clkset_aclk_200,
+        .sources        = &clkset_aclk,
         .reg_src        = { .reg = S5P_CLKSRC_TOP0, .shift = 12, .size = 1 },
         .reg_div        = { .reg = S5P_CLKDIV_TOP, .shift = 0, .size = 3 },
 };
 
-static struct clksrc_sources clkset_aclk_100 = {
-        .sources        = clkset_aclk_top_list,
-        .nr_sources     = ARRAY_SIZE(clkset_aclk_top_list),
-};
-
 static struct clksrc_clk clk_aclk_100 = {
         .clk    = {
                 .name           = "aclk_100",
                 .id             = -1,
         },
-        .sources        = &clkset_aclk_100,
+        .sources        = &clkset_aclk,
         .reg_src        = { .reg = S5P_CLKSRC_TOP0, .shift = 16, .size = 1 },
         .reg_div        = { .reg = S5P_CLKDIV_TOP, .shift = 4, .size = 4 },
 };
 
-static struct clksrc_sources clkset_aclk_160 = {
-        .sources        = clkset_aclk_top_list,
-        .nr_sources     = ARRAY_SIZE(clkset_aclk_top_list),
-};
-
 static struct clksrc_clk clk_aclk_160 = {
         .clk    = {
                 .name           = "aclk_160",
                 .id             = -1,
         },
-        .sources        = &clkset_aclk_160,
+        .sources        = &clkset_aclk,
         .reg_src        = { .reg = S5P_CLKSRC_TOP0, .shift = 20, .size = 1 },
         .reg_div        = { .reg = S5P_CLKDIV_TOP, .shift = 8, .size = 3 },
 };
 
-static struct clksrc_sources clkset_aclk_133 = {
-        .sources        = clkset_aclk_top_list,
-        .nr_sources     = ARRAY_SIZE(clkset_aclk_top_list),
-};
-
 static struct clksrc_clk clk_aclk_133 = {
         .clk    = {
                 .name           = "aclk_133",
                 .id             = -1,
         },
-        .sources        = &clkset_aclk_133,
+        .sources        = &clkset_aclk,
         .reg_src        = { .reg = S5P_CLKSRC_TOP0, .shift = 24, .size = 1 },
         .reg_div        = { .reg = S5P_CLKDIV_TOP, .shift = 12, .size = 3 },
 };
@@ -315,6 +358,8 @@ static struct clksrc_clk clk_vpllsrc = {
         .clk    = {
                 .name           = "vpll_src",
                 .id             = -1,
+                .enable         = s5pv310_clksrc_mask_top_ctrl,
+                .ctrlbit        = (1 << 0),
         },
         .sources        = &clkset_vpllsrc,
         .reg_src        = { .reg = S5P_CLKSRC_TOP1, .shift = 0, .size = 1 },
@@ -346,7 +391,175 @@ static struct clk init_clocks_disable[] = {
                 .parent         = &clk_aclk_100.clk,
                 .enable         = s5pv310_clk_ip_peril_ctrl,
                 .ctrlbit        = (1<<24),
-        }
+        }, {
+                .name           = "csis",
+                .id             = 0,
+                .enable         = s5pv310_clk_ip_cam_ctrl,
+                .ctrlbit        = (1 << 4),
+        }, {
+                .name           = "csis",
+                .id             = 1,
+                .enable         = s5pv310_clk_ip_cam_ctrl,
+                .ctrlbit        = (1 << 5),
+        }, {
+                .name           = "fimc",
+                .id             = 0,
+                .enable         = s5pv310_clk_ip_cam_ctrl,
+                .ctrlbit        = (1 << 0),
+        }, {
+                .name           = "fimc",
+                .id             = 1,
+                .enable         = s5pv310_clk_ip_cam_ctrl,
+                .ctrlbit        = (1 << 1),
+        }, {
+                .name           = "fimc",
+                .id             = 2,
+                .enable         = s5pv310_clk_ip_cam_ctrl,
+                .ctrlbit        = (1 << 2),
+        }, {
+                .name           = "fimc",
+                .id             = 3,
+                .enable         = s5pv310_clk_ip_cam_ctrl,
+                .ctrlbit        = (1 << 3),
+        }, {
+                .name           = "fimd",
+                .id             = 0,
+                .enable         = s5pv310_clk_ip_lcd0_ctrl,
+                .ctrlbit        = (1 << 0),
+        }, {
+                .name           = "fimd",
+                .id             = 1,
+                .enable         = s5pv310_clk_ip_lcd1_ctrl,
+                .ctrlbit        = (1 << 0),
+        }, {
+                .name           = "hsmmc",
+                .id             = 0,
+                .parent         = &clk_aclk_133.clk,
+                .enable         = s5pv310_clk_ip_fsys_ctrl,
+                .ctrlbit        = (1 << 5),
+        }, {
+                .name           = "hsmmc",
+                .id             = 1,
+                .parent         = &clk_aclk_133.clk,
+                .enable         = s5pv310_clk_ip_fsys_ctrl,
+                .ctrlbit        = (1 << 6),
+        }, {
+                .name           = "hsmmc",
+                .id             = 2,
+                .parent         = &clk_aclk_133.clk,
+                .enable         = s5pv310_clk_ip_fsys_ctrl,
+                .ctrlbit        = (1 << 7),
+        }, {
+                .name           = "hsmmc",
+                .id             = 3,
+                .parent         = &clk_aclk_133.clk,
+                .enable         = s5pv310_clk_ip_fsys_ctrl,
+                .ctrlbit        = (1 << 8),
+        }, {
+                .name           = "hsmmc",
+                .id             = 4,
+                .parent         = &clk_aclk_133.clk,
+                .enable         = s5pv310_clk_ip_fsys_ctrl,
+                .ctrlbit        = (1 << 9),
+        }, {
+                .name           = "sata",
+                .id             = -1,
+                .enable         = s5pv310_clk_ip_fsys_ctrl,
+                .ctrlbit        = (1 << 10),
+        }, {
+                .name           = "adc",
+                .id             = -1,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 15),
+        }, {
+                .name           = "rtc",
+                .id             = -1,
+                .enable         = s5pv310_clk_ip_perir_ctrl,
+                .ctrlbit        = (1 << 15),
+        }, {
+                .name           = "watchdog",
+                .id             = -1,
+                .enable         = s5pv310_clk_ip_perir_ctrl,
+                .ctrlbit        = (1 << 14),
+        }, {
+                .name           = "usbhost",
+                .id             = -1,
+                .enable         = s5pv310_clk_ip_fsys_ctrl ,
+                .ctrlbit        = (1 << 12),
+        }, {
+                .name           = "otg",
+                .id             = -1,
+                .enable         = s5pv310_clk_ip_fsys_ctrl,
+                .ctrlbit        = (1 << 13),
+        }, {
+                .name           = "spi",
+                .id             = 0,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 16),
+        }, {
+                .name           = "spi",
+                .id             = 1,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 17),
+        }, {
+                .name           = "spi",
+                .id             = 2,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 18),
+        }, {
+                .name           = "fimg2d",
+                .id             = -1,
+                .enable         = s5pv310_clk_ip_image_ctrl,
+                .ctrlbit        = (1 << 0),
+        }, {
+                .name           = "i2c",
+                .id             = 0,
+                .parent         = &clk_aclk_100.clk,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 6),
+        }, {
+                .name           = "i2c",
+                .id             = 1,
+                .parent         = &clk_aclk_100.clk,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 7),
+        }, {
+                .name           = "i2c",
+                .id             = 2,
+                .parent         = &clk_aclk_100.clk,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 8),
+        }, {
+                .name           = "i2c",
+                .id             = 3,
+                .parent         = &clk_aclk_100.clk,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 9),
+        }, {
+                .name           = "i2c",
+                .id             = 4,
+                .parent         = &clk_aclk_100.clk,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 10),
+        }, {
+                .name           = "i2c",
+                .id             = 5,
+                .parent         = &clk_aclk_100.clk,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 11),
+        }, {
+                .name           = "i2c",
+                .id             = 6,
+                .parent         = &clk_aclk_100.clk,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 12),
+        }, {
+                .name           = "i2c",
+                .id             = 7,
+                .parent         = &clk_aclk_100.clk,
+                .enable         = s5pv310_clk_ip_peril_ctrl,
+                .ctrlbit        = (1 << 13),
+        },
 };
 
 static struct clk init_clocks[] = {
@@ -387,6 +600,9 @@ static struct clk *clkset_group_list[] = {
         [0] = &clk_ext_xtal_mux,
         [1] = &clk_xusbxti,
         [2] = &clk_sclk_hdmi27m,
+        [3] = &clk_sclk_usbphy0,
+        [4] = &clk_sclk_usbphy1,
+        [5] = &clk_sclk_hdmiphy,
         [6] = &clk_mout_mpll.clk,
         [7] = &clk_mout_epll.clk,
         [8] = &clk_sclk_vpll.clk,
@@ -397,6 +613,104 @@ static struct clksrc_sources clkset_group = {
         .nr_sources     = ARRAY_SIZE(clkset_group_list),
 };
 
+static struct clk *clkset_mout_g2d0_list[] = {
+        [0] = &clk_mout_mpll.clk,
+        [1] = &clk_sclk_apll.clk,
+};
+
+static struct clksrc_sources clkset_mout_g2d0 = {
+        .sources        = clkset_mout_g2d0_list,
+        .nr_sources     = ARRAY_SIZE(clkset_mout_g2d0_list),
+};
+
+static struct clksrc_clk clk_mout_g2d0 = {
+        .clk    = {
+                .name           = "mout_g2d0",
+                .id             = -1,
+        },
+        .sources        = &clkset_mout_g2d0,
+        .reg_src        = { .reg = S5P_CLKSRC_IMAGE, .shift = 0, .size = 1 },
+};
+
+static struct clk *clkset_mout_g2d1_list[] = {
+        [0] = &clk_mout_epll.clk,
+        [1] = &clk_sclk_vpll.clk,
+};
+
+static struct clksrc_sources clkset_mout_g2d1 = {
+        .sources        = clkset_mout_g2d1_list,
+        .nr_sources     = ARRAY_SIZE(clkset_mout_g2d1_list),
+};
+
+static struct clksrc_clk clk_mout_g2d1 = {
+        .clk    = {
+                .name           = "mout_g2d1",
+                .id             = -1,
+        },
+        .sources        = &clkset_mout_g2d1,
+        .reg_src        = { .reg = S5P_CLKSRC_IMAGE, .shift = 4, .size = 1 },
+};
+
+static struct clk *clkset_mout_g2d_list[] = {
+        [0] = &clk_mout_g2d0.clk,
+        [1] = &clk_mout_g2d1.clk,
+};
+
+static struct clksrc_sources clkset_mout_g2d = {
+        .sources        = clkset_mout_g2d_list,
+        .nr_sources     = ARRAY_SIZE(clkset_mout_g2d_list),
+};
+
+static struct clksrc_clk clk_dout_mmc0 = {
+        .clk            = {
+                .name           = "dout_mmc0",
+                .id             = -1,
+        },
+        .sources = &clkset_group,
+        .reg_src = { .reg = S5P_CLKSRC_FSYS, .shift = 0, .size = 4 },
+        .reg_div = { .reg = S5P_CLKDIV_FSYS1, .shift = 0, .size = 4 },
+};
+
+static struct clksrc_clk clk_dout_mmc1 = {
+        .clk            = {
+                .name           = "dout_mmc1",
+                .id             = -1,
+        },
+        .sources = &clkset_group,
+        .reg_src = { .reg = S5P_CLKSRC_FSYS, .shift = 4, .size = 4 },
+        .reg_div = { .reg = S5P_CLKDIV_FSYS1, .shift = 16, .size = 4 },
+};
+
+static struct clksrc_clk clk_dout_mmc2 = {
+        .clk            = {
+                .name           = "dout_mmc2",
+                .id             = -1,
+        },
+        .sources = &clkset_group,
+        .reg_src = { .reg = S5P_CLKSRC_FSYS, .shift = 8, .size = 4 },
+        .reg_div = { .reg = S5P_CLKDIV_FSYS2, .shift = 0, .size = 4 },
+};
+
+static struct clksrc_clk clk_dout_mmc3 = {
+        .clk            = {
+                .name           = "dout_mmc3",
+                .id             = -1,
+        },
+        .sources = &clkset_group,
+        .reg_src = { .reg = S5P_CLKSRC_FSYS, .shift = 12, .size = 4 },
+        .reg_div = { .reg = S5P_CLKDIV_FSYS2, .shift = 16, .size = 4 },
+};
+
+static struct clksrc_clk clk_dout_mmc4 = {
+        .clk            = {
+                .name           = "dout_mmc4",
+                .id             = -1,
+        },
+        .sources = &clkset_group,
+        .reg_src = { .reg = S5P_CLKSRC_FSYS, .shift = 16, .size = 4 },
+        .reg_div = { .reg = S5P_CLKDIV_FSYS3, .shift = 0, .size = 4 },
+};
+
 static struct clksrc_clk clksrcs[] = {
         {
                 .clk    = {
@@ -448,7 +762,200 @@ static struct clksrc_clk clksrcs[] = {
                 .sources = &clkset_group,
                 .reg_src = { .reg = S5P_CLKSRC_PERIL0, .shift = 24, .size = 4 },
                 .reg_div = { .reg = S5P_CLKDIV_PERIL3, .shift = 0, .size = 4 },
-        },
+        }, {
+                .clk            = {
+                        .name           = "sclk_csis",
+                        .id             = 0,
+                        .enable         = s5pv310_clksrc_mask_cam_ctrl,
+                        .ctrlbit        = (1 << 24),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_CAM, .shift = 24, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_CAM, .shift = 24, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_csis",
+                        .id             = 1,
+                        .enable         = s5pv310_clksrc_mask_cam_ctrl,
+                        .ctrlbit        = (1 << 28),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_CAM, .shift = 28, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_CAM, .shift = 28, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_cam",
+                        .id             = 0,
+                        .enable         = s5pv310_clksrc_mask_cam_ctrl,
+                        .ctrlbit        = (1 << 16),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_CAM, .shift = 16, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_CAM, .shift = 16, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_cam",
+                        .id             = 1,
+                        .enable         = s5pv310_clksrc_mask_cam_ctrl,
+                        .ctrlbit        = (1 << 20),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_CAM, .shift = 20, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_CAM, .shift = 20, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_fimc",
+                        .id             = 0,
+                        .enable         = s5pv310_clksrc_mask_cam_ctrl,
+                        .ctrlbit        = (1 << 0),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_CAM, .shift = 0, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_CAM, .shift = 0, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_fimc",
+                        .id             = 1,
+                        .enable         = s5pv310_clksrc_mask_cam_ctrl,
+                        .ctrlbit        = (1 << 4),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_CAM, .shift = 4, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_CAM, .shift = 4, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_fimc",
+                        .id             = 2,
+                        .enable         = s5pv310_clksrc_mask_cam_ctrl,
+                        .ctrlbit        = (1 << 8),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_CAM, .shift = 8, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_CAM, .shift = 8, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_fimc",
+                        .id             = 3,
+                        .enable         = s5pv310_clksrc_mask_cam_ctrl,
+                        .ctrlbit        = (1 << 12),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_CAM, .shift = 12, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_CAM, .shift = 12, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_fimd",
+                        .id             = 0,
+                        .enable         = s5pv310_clksrc_mask_lcd0_ctrl,
+                        .ctrlbit        = (1 << 0),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_LCD0, .shift = 0, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_LCD0, .shift = 0, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_fimd",
+                        .id             = 1,
+                        .enable         = s5pv310_clksrc_mask_lcd1_ctrl,
+                        .ctrlbit        = (1 << 0),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_LCD1, .shift = 0, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_LCD1, .shift = 0, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_sata",
+                        .id             = -1,
+                        .enable         = s5pv310_clksrc_mask_fsys_ctrl,
+                        .ctrlbit        = (1 << 24),
+                },
+                .sources = &clkset_mout_corebus,
+                .reg_src = { .reg = S5P_CLKSRC_FSYS, .shift = 24, .size = 1 },
+                .reg_div = { .reg = S5P_CLKDIV_FSYS0, .shift = 20, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_spi",
+                        .id             = 0,
+                        .enable         = s5pv310_clksrc_mask_peril1_ctrl,
+                        .ctrlbit        = (1 << 16),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_PERIL1, .shift = 16, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_PERIL1, .shift = 0, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_spi",
+                        .id             = 1,
+                        .enable         = s5pv310_clksrc_mask_peril1_ctrl,
+                        .ctrlbit        = (1 << 20),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_PERIL1, .shift = 20, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_PERIL1, .shift = 16, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_spi",
+                        .id             = 2,
+                        .enable         = s5pv310_clksrc_mask_peril1_ctrl,
+                        .ctrlbit        = (1 << 24),
+                },
+                .sources = &clkset_group,
+                .reg_src = { .reg = S5P_CLKSRC_PERIL1, .shift = 24, .size = 4 },
+                .reg_div = { .reg = S5P_CLKDIV_PERIL2, .shift = 0, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_fimg2d",
+                        .id             = -1,
+                },
+                .sources = &clkset_mout_g2d,
+                .reg_src = { .reg = S5P_CLKSRC_IMAGE, .shift = 8, .size = 1 },
+                .reg_div = { .reg = S5P_CLKDIV_IMAGE, .shift = 0, .size = 4 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_mmc",
+                        .id             = 0,
+                        .parent         = &clk_dout_mmc0.clk,
+                        .enable         = s5pv310_clksrc_mask_fsys_ctrl,
+                        .ctrlbit        = (1 << 0),
+                },
+                .reg_div = { .reg = S5P_CLKDIV_FSYS1, .shift = 8, .size = 8 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_mmc",
+                        .id             = 1,
+                        .parent         = &clk_dout_mmc1.clk,
+                        .enable         = s5pv310_clksrc_mask_fsys_ctrl,
+                        .ctrlbit        = (1 << 4),
+                },
+                .reg_div = { .reg = S5P_CLKDIV_FSYS1, .shift = 24, .size = 8 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_mmc",
+                        .id             = 2,
+                        .parent         = &clk_dout_mmc2.clk,
+                        .enable         = s5pv310_clksrc_mask_fsys_ctrl,
+                        .ctrlbit        = (1 << 8),
+                },
+                .reg_div = { .reg = S5P_CLKDIV_FSYS2, .shift = 8, .size = 8 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_mmc",
+                        .id             = 3,
+                        .parent         = &clk_dout_mmc3.clk,
+                        .enable         = s5pv310_clksrc_mask_fsys_ctrl,
+                        .ctrlbit        = (1 << 12),
+                },
+                .reg_div = { .reg = S5P_CLKDIV_FSYS2, .shift = 24, .size = 8 },
+        }, {
+                .clk            = {
+                        .name           = "sclk_mmc",
+                        .id             = 4,
+                        .parent         = &clk_dout_mmc4.clk,
+                        .enable         = s5pv310_clksrc_mask_fsys_ctrl,
+                        .ctrlbit        = (1 << 16),
+                },
+                .reg_div = { .reg = S5P_CLKDIV_FSYS3, .shift = 8, .size = 8 },
+        }
 };
 
 /* Clock initialization code */
@@ -464,8 +971,6 @@ static struct clksrc_clk *sysclks[] = {
         &clk_aclk_cores,
         &clk_aclk_corem1,
         &clk_periphclk,
-        &clk_atclk,
-        &clk_pclk_dbg,
         &clk_mout_corebus,
         &clk_sclk_dmc,
         &clk_aclk_cored,
@@ -478,6 +983,11 @@ static struct clksrc_clk *sysclks[] = {
         &clk_aclk_100,
         &clk_aclk_160,
         &clk_aclk_133,
+        &clk_dout_mmc0,
+        &clk_dout_mmc1,
+        &clk_dout_mmc2,
+        &clk_dout_mmc3,
+        &clk_dout_mmc4,
 };
 
 void __init_or_cpufreq s5pv310_setup_clocks(void)
@@ -490,15 +1000,11 @@ void __init_or_cpufreq s5pv310_setup_clocks(void)
         unsigned long vpllsrc;
         unsigned long xtal;
         unsigned long armclk;
-        unsigned long aclk_corem0;
-        unsigned long aclk_cores;
-        unsigned long aclk_corem1;
-        unsigned long periphclk;
         unsigned long sclk_dmc;
-        unsigned long aclk_cored;
-        unsigned long aclk_corep;
-        unsigned long aclk_acp;
-        unsigned long pclk_acp;
+        unsigned long aclk_200;
+        unsigned long aclk_100;
+        unsigned long aclk_160;
+        unsigned long aclk_133;
         unsigned int ptr;
 
         printk(KERN_DEBUG "%s: registering clocks\n", __func__);
@@ -529,26 +1035,21 @@ void __init_or_cpufreq s5pv310_setup_clocks(void)
                         apll, mpll, epll, vpll);
 
         armclk = clk_get_rate(&clk_armclk.clk);
-        aclk_corem0 = clk_get_rate(&clk_aclk_corem0.clk);
-        aclk_cores = clk_get_rate(&clk_aclk_cores.clk);
-        aclk_corem1 = clk_get_rate(&clk_aclk_corem1.clk);
-        periphclk = clk_get_rate(&clk_periphclk.clk);
         sclk_dmc = clk_get_rate(&clk_sclk_dmc.clk);
-        aclk_cored = clk_get_rate(&clk_aclk_cored.clk);
-        aclk_corep = clk_get_rate(&clk_aclk_corep.clk);
-        aclk_acp = clk_get_rate(&clk_aclk_acp.clk);
-        pclk_acp = clk_get_rate(&clk_pclk_acp.clk);
-
-        printk(KERN_INFO "S5PV310: ARMCLK=%ld, COREM0=%ld, CORES=%ld\n"
-                         "COREM1=%ld, PERI=%ld, DMC=%ld, CORED=%ld\n"
-                         "COREP=%ld, ACLK_ACP=%ld, PCLK_ACP=%ld",
-                        armclk, aclk_corem0, aclk_cores, aclk_corem1,
-                        periphclk, sclk_dmc, aclk_cored, aclk_corep,
-                        aclk_acp, pclk_acp);
+
+        aclk_200 = clk_get_rate(&clk_aclk_200.clk);
+        aclk_100 = clk_get_rate(&clk_aclk_100.clk);
+        aclk_160 = clk_get_rate(&clk_aclk_160.clk);
+        aclk_133 = clk_get_rate(&clk_aclk_133.clk);
+
+        printk(KERN_INFO "S5PV310: ARMCLK=%ld, DMC=%ld, ACLK200=%ld\n"
+                         "ACLK100=%ld, ACLK160=%ld, ACLK133=%ld\n",
+                        armclk, sclk_dmc, aclk_200,
+                        aclk_100, aclk_160, aclk_133);
 
         clk_f.rate = armclk;
         clk_h.rate = sclk_dmc;
-        clk_p.rate = periphclk;
+        clk_p.rate = aclk_100;
 
         for (ptr = 0; ptr < ARRAY_SIZE(clksrcs); ptr++)
                 s3c_set_clksrc(&clksrcs[ptr], true);
diff --git a/arch/arm/mach-s5pv310/cpu.c b/arch/arm/mach-s5pv310/cpu.c
index 4add39853ff9..82ce4aa6d61a 100644
--- a/arch/arm/mach-s5pv310/cpu.c
+++ b/arch/arm/mach-s5pv310/cpu.c
@@ -15,10 +15,12 @@
 #include <asm/mach/irq.h>
 
 #include <asm/proc-fns.h>
+#include <asm/hardware/cache-l2x0.h>
 
 #include <plat/cpu.h>
 #include <plat/clock.h>
 #include <plat/s5pv310.h>
+#include <plat/sdhci.h>
 
 #include <mach/regs-irq.h>
 
@@ -56,15 +58,30 @@ static struct map_desc s5pv310_iodesc[] __initdata = {
                 .length         = SZ_4K,
                 .type           = MT_DEVICE,
         }, {
-                .virtual        = (unsigned long)S5P_VA_GPIO,
+                .virtual        = (unsigned long)S5P_VA_GPIO1,
                 .pfn            = __phys_to_pfn(S5PV310_PA_GPIO1),
                 .length         = SZ_4K,
                 .type           = MT_DEVICE,
         }, {
+                .virtual        = (unsigned long)S5P_VA_GPIO2,
+                .pfn            = __phys_to_pfn(S5PV310_PA_GPIO2),
+                .length         = SZ_4K,
+                .type           = MT_DEVICE,
+        }, {
+                .virtual        = (unsigned long)S5P_VA_GPIO3,
+                .pfn            = __phys_to_pfn(S5PV310_PA_GPIO3),
+                .length         = SZ_256,
+                .type           = MT_DEVICE,
+        }, {
                 .virtual        = (unsigned long)S3C_VA_UART,
                 .pfn            = __phys_to_pfn(S3C_PA_UART),
                 .length         = SZ_512K,
                 .type           = MT_DEVICE,
+        }, {
+                .virtual        = (unsigned long)S5P_VA_SROMC,
+                .pfn            = __phys_to_pfn(S5PV310_PA_SROMC),
+                .length         = SZ_4K,
+                .type           = MT_DEVICE,
         },
 };
 
@@ -83,6 +100,12 @@ static void s5pv310_idle(void)
 void __init s5pv310_map_io(void)
 {
         iotable_init(s5pv310_iodesc, ARRAY_SIZE(s5pv310_iodesc));
+
+        /* initialize device information early */
+        s5pv310_default_sdhci0();
+        s5pv310_default_sdhci1();
+        s5pv310_default_sdhci2();
+        s5pv310_default_sdhci3();
 }
 
 void __init s5pv310_init_clocks(int xtal)
@@ -131,6 +154,28 @@ static int __init s5pv310_core_init(void)
 
 core_initcall(s5pv310_core_init);
 
+#ifdef CONFIG_CACHE_L2X0
+static int __init s5pv310_l2x0_cache_init(void)
+{
+        /* TAG, Data Latency Control: 2cycle */
+        __raw_writel(0x110, S5P_VA_L2CC + L2X0_TAG_LATENCY_CTRL);
+        __raw_writel(0x110, S5P_VA_L2CC + L2X0_DATA_LATENCY_CTRL);
+
+        /* L2X0 Prefetch Control */
+        __raw_writel(0x30000007, S5P_VA_L2CC + L2X0_PREFETCH_CTRL);
+
+        /* L2X0 Power Control */
+        __raw_writel(L2X0_DYNAMIC_CLK_GATING_EN | L2X0_STNDBY_MODE_EN,
+                     S5P_VA_L2CC + L2X0_POWER_CTRL);
+
+        l2x0_init(S5P_VA_L2CC, 0x7C070001, 0xC200ffff);
+
+        return 0;
+}
+
+early_initcall(s5pv310_l2x0_cache_init);
+#endif
+
 int __init s5pv310_init(void)
 {
         printk(KERN_INFO "S5PV310: Initializing architecture\n");
diff --git a/arch/arm/mach-s5pv310/gpiolib.c b/arch/arm/mach-s5pv310/gpiolib.c
new file mode 100644
index 000000000000..55217b8923ec
--- /dev/null
+++ b/arch/arm/mach-s5pv310/gpiolib.c
@@ -0,0 +1,304 @@
+/* linux/arch/arm/mach-s5pv310/gpiolib.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * S5PV310 - GPIOlib support
+ *
+ * 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/kernel.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+
+#include <mach/map.h>
+
+#include <plat/gpio-core.h>
+#include <plat/gpio-cfg.h>
+#include <plat/gpio-cfg-helpers.h>
+
+static struct s3c_gpio_cfg gpio_cfg = {
+        .set_config     = s3c_gpio_setcfg_s3c64xx_4bit,
+        .set_pull       = s3c_gpio_setpull_updown,
+        .get_pull       = s3c_gpio_getpull_updown,
+};
+
+static struct s3c_gpio_cfg gpio_cfg_noint = {
+        .set_config     = s3c_gpio_setcfg_s3c64xx_4bit,
+        .set_pull       = s3c_gpio_setpull_updown,
+        .get_pull       = s3c_gpio_getpull_updown,
+};
+
+/*
+ * Following are the gpio banks in v310.
+ *
+ * The 'config' member when left to NULL, is initialized to the default
+ * structure gpio_cfg in the init function below.
+ *
+ * The 'base' member is also initialized in the init function below.
+ * Note: The initialization of 'base' member of s3c_gpio_chip structure
+ * uses the above macro and depends on the banks being listed in order here.
+ */
+static struct s3c_gpio_chip s5pv310_gpio_part1_4bit[] = {
+        {
+                .chip   = {
+                        .base   = S5PV310_GPA0(0),
+                        .ngpio  = S5PV310_GPIO_A0_NR,
+                        .label  = "GPA0",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPA1(0),
+                        .ngpio  = S5PV310_GPIO_A1_NR,
+                        .label  = "GPA1",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPB(0),
+                        .ngpio  = S5PV310_GPIO_B_NR,
+                        .label  = "GPB",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPC0(0),
+                        .ngpio  = S5PV310_GPIO_C0_NR,
+                        .label  = "GPC0",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPC1(0),
+                        .ngpio  = S5PV310_GPIO_C1_NR,
+                        .label  = "GPC1",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPD0(0),
+                        .ngpio  = S5PV310_GPIO_D0_NR,
+                        .label  = "GPD0",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPD1(0),
+                        .ngpio  = S5PV310_GPIO_D1_NR,
+                        .label  = "GPD1",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPE0(0),
+                        .ngpio  = S5PV310_GPIO_E0_NR,
+                        .label  = "GPE0",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPE1(0),
+                        .ngpio  = S5PV310_GPIO_E1_NR,
+                        .label  = "GPE1",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPE2(0),
+                        .ngpio  = S5PV310_GPIO_E2_NR,
+                        .label  = "GPE2",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPE3(0),
+                        .ngpio  = S5PV310_GPIO_E3_NR,
+                        .label  = "GPE3",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPE4(0),
+                        .ngpio  = S5PV310_GPIO_E4_NR,
+                        .label  = "GPE4",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPF0(0),
+                        .ngpio  = S5PV310_GPIO_F0_NR,
+                        .label  = "GPF0",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPF1(0),
+                        .ngpio  = S5PV310_GPIO_F1_NR,
+                        .label  = "GPF1",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPF2(0),
+                        .ngpio  = S5PV310_GPIO_F2_NR,
+                        .label  = "GPF2",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPF3(0),
+                        .ngpio  = S5PV310_GPIO_F3_NR,
+                        .label  = "GPF3",
+                },
+        },
+};
+
+static struct s3c_gpio_chip s5pv310_gpio_part2_4bit[] = {
+        {
+                .chip   = {
+                        .base   = S5PV310_GPJ0(0),
+                        .ngpio  = S5PV310_GPIO_J0_NR,
+                        .label  = "GPJ0",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPJ1(0),
+                        .ngpio  = S5PV310_GPIO_J1_NR,
+                        .label  = "GPJ1",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPK0(0),
+                        .ngpio  = S5PV310_GPIO_K0_NR,
+                        .label  = "GPK0",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPK1(0),
+                        .ngpio  = S5PV310_GPIO_K1_NR,
+                        .label  = "GPK1",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPK2(0),
+                        .ngpio  = S5PV310_GPIO_K2_NR,
+                        .label  = "GPK2",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPK3(0),
+                        .ngpio  = S5PV310_GPIO_K3_NR,
+                        .label  = "GPK3",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPL0(0),
+                        .ngpio  = S5PV310_GPIO_L0_NR,
+                        .label  = "GPL0",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPL1(0),
+                        .ngpio  = S5PV310_GPIO_L1_NR,
+                        .label  = "GPL1",
+                },
+        }, {
+                .chip   = {
+                        .base   = S5PV310_GPL2(0),
+                        .ngpio  = S5PV310_GPIO_L2_NR,
+                        .label  = "GPL2",
+                },
+        }, {
+                .base   = (S5P_VA_GPIO2 + 0xC00),
+                .config = &gpio_cfg_noint,
+                .irq_base = IRQ_EINT(0),
+                .chip   = {
+                        .base   = S5PV310_GPX0(0),
+                        .ngpio  = S5PV310_GPIO_X0_NR,
+                        .label  = "GPX0",
+                        .to_irq = samsung_gpiolib_to_irq,
+                },
+        }, {
+                .base   = (S5P_VA_GPIO2 + 0xC20),
+                .config = &gpio_cfg_noint,
+                .irq_base = IRQ_EINT(8),
+                .chip   = {
+                        .base   = S5PV310_GPX1(0),
+                        .ngpio  = S5PV310_GPIO_X1_NR,
+                        .label  = "GPX1",
+                        .to_irq = samsung_gpiolib_to_irq,
+                },
+        }, {
+                .base   = (S5P_VA_GPIO2 + 0xC40),
+                .config = &gpio_cfg_noint,
+                .irq_base = IRQ_EINT(16),
+                .chip   = {
+                        .base   = S5PV310_GPX2(0),
+                        .ngpio  = S5PV310_GPIO_X2_NR,
+                        .label  = "GPX2",
+                        .to_irq = samsung_gpiolib_to_irq,
+                },
+        }, {
+                .base   = (S5P_VA_GPIO2 + 0xC60),
+                .config = &gpio_cfg_noint,
+                .irq_base = IRQ_EINT(24),
+                .chip   = {
+                        .base   = S5PV310_GPX3(0),
+                        .ngpio  = S5PV310_GPIO_X3_NR,
+                        .label  = "GPX3",
+                        .to_irq = samsung_gpiolib_to_irq,
+                },
+        },
+};
+
+static struct s3c_gpio_chip s5pv310_gpio_part3_4bit[] = {
+        {
+                .chip   = {
+                        .base   = S5PV310_GPZ(0),
+                        .ngpio  = S5PV310_GPIO_Z_NR,
+                        .label  = "GPZ",
+                },
+        },
+};
+
+static __init int s5pv310_gpiolib_init(void)
+{
+        struct s3c_gpio_chip *chip;
+        int i;
+        int nr_chips;
+
+        /* GPIO part 1 */
+
+        chip = s5pv310_gpio_part1_4bit;
+        nr_chips = ARRAY_SIZE(s5pv310_gpio_part1_4bit);
+
+        for (i = 0; i < nr_chips; i++, chip++) {
+                if (chip->config == NULL)
+                        chip->config = &gpio_cfg;
+                if (chip->base == NULL)
+                        chip->base = S5P_VA_GPIO1 + (i) * 0x20;
+        }
+
+        samsung_gpiolib_add_4bit_chips(s5pv310_gpio_part1_4bit, nr_chips);
+
+        /* GPIO part 2 */
+
+        chip = s5pv310_gpio_part2_4bit;
+        nr_chips = ARRAY_SIZE(s5pv310_gpio_part2_4bit);
+
+        for (i = 0; i < nr_chips; i++, chip++) {
+                if (chip->config == NULL)
+                        chip->config = &gpio_cfg;
+                if (chip->base == NULL)
+                        chip->base = S5P_VA_GPIO2 + (i) * 0x20;
+        }
+
+        samsung_gpiolib_add_4bit_chips(s5pv310_gpio_part2_4bit, nr_chips);
+
+        /* GPIO part 3 */
+
+        chip = s5pv310_gpio_part3_4bit;
+        nr_chips = ARRAY_SIZE(s5pv310_gpio_part3_4bit);
+
+        for (i = 0; i < nr_chips; i++, chip++) {
+                if (chip->config == NULL)
+                        chip->config = &gpio_cfg;
+                if (chip->base == NULL)
+                        chip->base = S5P_VA_GPIO3 + (i) * 0x20;
+        }
+
+        samsung_gpiolib_add_4bit_chips(s5pv310_gpio_part3_4bit, nr_chips);
+
+        return 0;
+}
+core_initcall(s5pv310_gpiolib_init);
diff --git a/arch/arm/mach-s5pv310/hotplug.c b/arch/arm/mach-s5pv310/hotplug.c
new file mode 100644
index 000000000000..03652c3605f6
--- /dev/null
+++ b/arch/arm/mach-s5pv310/hotplug.c
@@ -0,0 +1,144 @@
+/* linux arch/arm/mach-s5pv310/hotplug.c
+ *
+ *  Cloned from linux/arch/arm/mach-realview/hotplug.c
+ *
+ *  Copyright (C) 2002 ARM Ltd.
+ *  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/kernel.h>
+#include <linux/errno.h>
+#include <linux/smp.h>
+#include <linux/completion.h>
+
+#include <asm/cacheflush.h>
+
+extern volatile int pen_release;
+
+static DECLARE_COMPLETION(cpu_killed);
+
+static inline void cpu_enter_lowpower(void)
+{
+        unsigned int v;
+
+        flush_cache_all();
+        asm volatile(
+        "       mcr     p15, 0, %1, c7, c5, 0\n"
+        "       mcr     p15, 0, %1, c7, c10, 4\n"
+        /*
+         * Turn off coherency
+         */
+        "       mrc     p15, 0, %0, c1, c0, 1\n"
+        "       bic     %0, %0, #0x20\n"
+        "       mcr     p15, 0, %0, c1, c0, 1\n"
+        "       mrc     p15, 0, %0, c1, c0, 0\n"
+        "       bic     %0, %0, #0x04\n"
+        "       mcr     p15, 0, %0, c1, c0, 0\n"
+          : "=&r" (v)
+          : "r" (0)
+          : "cc");
+}
+
+static inline void cpu_leave_lowpower(void)
+{
+        unsigned int v;
+
+        asm volatile(
+        "mrc    p15, 0, %0, c1, c0, 0\n"
+        "       orr     %0, %0, #0x04\n"
+        "       mcr     p15, 0, %0, c1, c0, 0\n"
+        "       mrc     p15, 0, %0, c1, c0, 1\n"
+        "       orr     %0, %0, #0x20\n"
+        "       mcr     p15, 0, %0, c1, c0, 1\n"
+          : "=&r" (v)
+          :
+          : "cc");
+}
+
+static inline void platform_do_lowpower(unsigned int cpu)
+{
+        /*
+         * there is no power-control hardware on this platform, so all
+         * we can do is put the core into WFI; this is safe as the calling
+         * code will have already disabled interrupts
+         */
+        for (;;) {
+                /*
+                 * here's the WFI
+                 */
+                asm(".word      0xe320f003\n"
+                    :
+                    :
+                    : "memory", "cc");
+
+                if (pen_release == cpu) {
+                        /*
+                         * OK, proper wakeup, we're done
+                         */
+                        break;
+                }
+
+                /*
+                 * getting here, means that we have come out of WFI without
+                 * having been woken up - this shouldn't happen
+                 *
+                 * The trouble is, letting people know about this is not really
+                 * possible, since we are currently running incoherently, and
+                 * therefore cannot safely call printk() or anything else
+                 */
+#ifdef DEBUG
+                printk(KERN_WARN "CPU%u: spurious wakeup call\n", cpu);
+#endif
+        }
+}
+
+int platform_cpu_kill(unsigned int cpu)
+{
+        return wait_for_completion_timeout(&cpu_killed, 5000);
+}
+
+/*
+ * platform-specific code to shutdown a CPU
+ *
+ * Called with IRQs disabled
+ */
+void platform_cpu_die(unsigned int cpu)
+{
+#ifdef DEBUG
+        unsigned int this_cpu = hard_smp_processor_id();
+
+        if (cpu != this_cpu) {
+                printk(KERN_CRIT "Eek! platform_cpu_die running on %u, should be %u\n",
+                           this_cpu, cpu);
+                BUG();
+        }
+#endif
+
+        printk(KERN_NOTICE "CPU%u: shutdown\n", cpu);
+        complete(&cpu_killed);
+
+        /*
+         * we're ready for shutdown now, so do it
+         */
+        cpu_enter_lowpower();
+        platform_do_lowpower(cpu);
+
+        /*
+         * bring this CPU back into the world of cache
+         * coherency, and then restore interrupts
+         */
+        cpu_leave_lowpower();
+}
+
+int platform_cpu_disable(unsigned int cpu)
+{
+        /*
+         * we don't allow CPU 0 to be shutdown (it is still too special
+         * e.g. clock tick interrupts)
+         */
+        return cpu == 0 ? -EPERM : 0;
+}
diff --git a/arch/arm/mach-s5pv310/include/mach/irqs.h b/arch/arm/mach-s5pv310/include/mach/irqs.h
index 471fc3bb199a..99e7dad8a85a 100644
--- a/arch/arm/mach-s5pv310/include/mach/irqs.h
+++ b/arch/arm/mach-s5pv310/include/mach/irqs.h
@@ -3,7 +3,7 @@
  * Copyright (c) 2010 Samsung Electronics Co., Ltd.
  *              http://www.samsung.com/
  *
- * S5PV210 - IRQ definitions
+ * S5PV310 - IRQ definitions
  *
  * 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
@@ -60,6 +60,9 @@
 #define IRQ_TIMER3_VIC          COMBINER_IRQ(22, 3)
 #define IRQ_TIMER4_VIC          COMBINER_IRQ(22, 4)
 
+#define IRQ_RTC_ALARM           COMBINER_IRQ(23, 0)
+#define IRQ_RTC_TIC             COMBINER_IRQ(23, 1)
+
 #define IRQ_UART0               COMBINER_IRQ(26, 0)
 #define IRQ_UART1               COMBINER_IRQ(26, 1)
 #define IRQ_UART2               COMBINER_IRQ(26, 2)
@@ -67,13 +70,46 @@
 #define IRQ_UART4               COMBINER_IRQ(26, 4)
 
 #define IRQ_IIC                 COMBINER_IRQ(27, 0)
+#define IRQ_IIC1                COMBINER_IRQ(27, 1)
+#define IRQ_IIC2                COMBINER_IRQ(27, 2)
+#define IRQ_IIC3                COMBINER_IRQ(27, 3)
+#define IRQ_IIC4                COMBINER_IRQ(27, 4)
+#define IRQ_IIC5                COMBINER_IRQ(27, 5)
+#define IRQ_IIC6                COMBINER_IRQ(27, 6)
+#define IRQ_IIC7                COMBINER_IRQ(27, 7)
+
+#define IRQ_HSMMC0              COMBINER_IRQ(29, 0)
+#define IRQ_HSMMC1              COMBINER_IRQ(29, 1)
+#define IRQ_HSMMC2              COMBINER_IRQ(29, 2)
+#define IRQ_HSMMC3              COMBINER_IRQ(29, 3)
 
 #define IRQ_ONENAND_AUDI        COMBINER_IRQ(34, 0)
 
-/* Set the default NR_IRQS */
+#define IRQ_EINT4               COMBINER_IRQ(37, 0)
+#define IRQ_EINT5               COMBINER_IRQ(37, 1)
+#define IRQ_EINT6               COMBINER_IRQ(37, 2)
+#define IRQ_EINT7               COMBINER_IRQ(37, 3)
+#define IRQ_EINT8               COMBINER_IRQ(38, 0)
+
+#define IRQ_EINT9               COMBINER_IRQ(38, 1)
+#define IRQ_EINT10              COMBINER_IRQ(38, 2)
+#define IRQ_EINT11              COMBINER_IRQ(38, 3)
+#define IRQ_EINT12              COMBINER_IRQ(38, 4)
+#define IRQ_EINT13              COMBINER_IRQ(38, 5)
+#define IRQ_EINT14              COMBINER_IRQ(38, 6)
+#define IRQ_EINT15              COMBINER_IRQ(38, 7)
+
+#define IRQ_EINT16_31           COMBINER_IRQ(39, 0)
 
-#define NR_IRQS                 COMBINER_IRQ(MAX_COMBINER_NR, 0)
+#define MAX_COMBINER_NR         40
+
+#define S5P_IRQ_EINT_BASE       COMBINER_IRQ(MAX_COMBINER_NR, 0)
+
+#define S5P_EINT_BASE1          (S5P_IRQ_EINT_BASE + 0)
+#define S5P_EINT_BASE2          (S5P_IRQ_EINT_BASE + 16)
+
+/* Set the default NR_IRQS */
 
-#define MAX_COMBINER_NR         39
+#define NR_IRQS                 (S5P_IRQ_EINT_BASE + 32)
 
 #endif /* __ASM_ARCH_IRQS_H */
diff --git a/arch/arm/mach-s5pv310/include/mach/map.h b/arch/arm/mach-s5pv310/include/mach/map.h
index aff6d23624bb..7acf4e77e92e 100644
--- a/arch/arm/mach-s5pv310/include/mach/map.h
+++ b/arch/arm/mach-s5pv310/include/mach/map.h
@@ -25,6 +25,8 @@
 
 #define S5PV310_PA_SYSRAM               (0x02025000)
 
+#define S5PV310_PA_SROM_BANK(x)         (0x04000000 + ((x) * 0x01000000))
+
 #define S5PC210_PA_ONENAND              (0x0C000000)
 #define S5P_PA_ONENAND                  S5PC210_PA_ONENAND
 
@@ -34,12 +36,13 @@
 #define S5PV310_PA_CHIPID               (0x10000000)
 #define S5P_PA_CHIPID                   S5PV310_PA_CHIPID
 
-#define S5PV310_PA_SYSCON               (0x10020000)
+#define S5PV310_PA_SYSCON               (0x10010000)
 #define S5P_PA_SYSCON                   S5PV310_PA_SYSCON
 
 #define S5PV310_PA_CMU                  (0x10030000)
 
 #define S5PV310_PA_WATCHDOG             (0x10060000)
+#define S5PV310_PA_RTC                  (0x10070000)
 
 #define S5PV310_PA_COMBINER             (0x10448000)
 
@@ -55,6 +58,8 @@
 
 #define S5PV310_PA_HSMMC(x)             (0x12510000 + ((x) * 0x10000))
 
+#define S5PV310_PA_SROMC                (0x12570000)
+
 #define S5PV310_PA_UART                 (0x13800000)
 
 #define S5P_PA_UART(x)                  (S5PV310_PA_UART + ((x) * S3C_UART_OFFSET))
@@ -66,7 +71,7 @@
 
 #define S5P_SZ_UART                     SZ_256
 
-#define S5PV310_PA_IIC0                 (0x13860000)
+#define S5PV310_PA_IIC(x)               (0x13860000 + ((x) * 0x10000))
 
 #define S5PV310_PA_TIMER                (0x139D0000)
 #define S5P_PA_TIMER                    S5PV310_PA_TIMER
@@ -80,7 +85,15 @@
 #define S3C_PA_HSMMC1                   S5PV310_PA_HSMMC(1)
 #define S3C_PA_HSMMC2                   S5PV310_PA_HSMMC(2)
 #define S3C_PA_HSMMC3                   S5PV310_PA_HSMMC(3)
-#define S3C_PA_IIC                      S5PV310_PA_IIC0
+#define S3C_PA_IIC                      S5PV310_PA_IIC(0)
+#define S3C_PA_IIC1                     S5PV310_PA_IIC(1)
+#define S3C_PA_IIC2                     S5PV310_PA_IIC(2)
+#define S3C_PA_IIC3                     S5PV310_PA_IIC(3)
+#define S3C_PA_IIC4                     S5PV310_PA_IIC(4)
+#define S3C_PA_IIC5                     S5PV310_PA_IIC(5)
+#define S3C_PA_IIC6                     S5PV310_PA_IIC(6)
+#define S3C_PA_IIC7                     S5PV310_PA_IIC(7)
+#define S3C_PA_RTC                      S5PV310_PA_RTC
 #define S3C_PA_WDT                      S5PV310_PA_WATCHDOG
 
 #endif /* __ASM_ARCH_MAP_H */
diff --git a/arch/arm/mach-s5pv310/include/mach/regs-clock.h b/arch/arm/mach-s5pv310/include/mach/regs-clock.h
index 4013553cd9be..f1028cad9788 100644
--- a/arch/arm/mach-s5pv310/include/mach/regs-clock.h
+++ b/arch/arm/mach-s5pv310/include/mach/regs-clock.h
@@ -26,11 +26,23 @@
 
 #define S5P_CLKSRC_TOP0                 S5P_CLKREG(0x0C210)
 #define S5P_CLKSRC_TOP1                 S5P_CLKREG(0x0C214)
-
+#define S5P_CLKSRC_CAM                  S5P_CLKREG(0x0C220)
+#define S5P_CLKSRC_IMAGE                S5P_CLKREG(0x0C230)
+#define S5P_CLKSRC_LCD0                 S5P_CLKREG(0x0C234)
+#define S5P_CLKSRC_LCD1                 S5P_CLKREG(0x0C238)
+#define S5P_CLKSRC_FSYS                 S5P_CLKREG(0x0C240)
 #define S5P_CLKSRC_PERIL0               S5P_CLKREG(0x0C250)
+#define S5P_CLKSRC_PERIL1               S5P_CLKREG(0x0C254)
 
 #define S5P_CLKDIV_TOP                  S5P_CLKREG(0x0C510)
-
+#define S5P_CLKDIV_CAM                  S5P_CLKREG(0x0C520)
+#define S5P_CLKDIV_IMAGE                S5P_CLKREG(0x0C530)
+#define S5P_CLKDIV_LCD0                 S5P_CLKREG(0x0C534)
+#define S5P_CLKDIV_LCD1                 S5P_CLKREG(0x0C538)
+#define S5P_CLKDIV_FSYS0                S5P_CLKREG(0x0C540)
+#define S5P_CLKDIV_FSYS1                S5P_CLKREG(0x0C544)
+#define S5P_CLKDIV_FSYS2                S5P_CLKREG(0x0C548)
+#define S5P_CLKDIV_FSYS3                S5P_CLKREG(0x0C54C)
 #define S5P_CLKDIV_PERIL0               S5P_CLKREG(0x0C550)
 #define S5P_CLKDIV_PERIL1               S5P_CLKREG(0x0C554)
 #define S5P_CLKDIV_PERIL2               S5P_CLKREG(0x0C558)
@@ -38,9 +50,21 @@
 #define S5P_CLKDIV_PERIL4               S5P_CLKREG(0x0C560)
 #define S5P_CLKDIV_PERIL5               S5P_CLKREG(0x0C564)
 
+#define S5P_CLKSRC_MASK_TOP             S5P_CLKREG(0x0C310)
+#define S5P_CLKSRC_MASK_CAM             S5P_CLKREG(0x0C320)
+#define S5P_CLKSRC_MASK_LCD0            S5P_CLKREG(0x0C334)
+#define S5P_CLKSRC_MASK_LCD1            S5P_CLKREG(0x0C338)
+#define S5P_CLKSRC_MASK_FSYS            S5P_CLKREG(0x0C340)
 #define S5P_CLKSRC_MASK_PERIL0          S5P_CLKREG(0x0C350)
+#define S5P_CLKSRC_MASK_PERIL1          S5P_CLKREG(0x0C354)
 
+#define S5P_CLKGATE_IP_CAM              S5P_CLKREG(0x0C920)
+#define S5P_CLKGATE_IP_IMAGE            S5P_CLKREG(0x0C930)
+#define S5P_CLKGATE_IP_LCD0             S5P_CLKREG(0x0C934)
+#define S5P_CLKGATE_IP_LCD1             S5P_CLKREG(0x0C938)
+#define S5P_CLKGATE_IP_FSYS             S5P_CLKREG(0x0C940)
 #define S5P_CLKGATE_IP_PERIL            S5P_CLKREG(0x0C950)
+#define S5P_CLKGATE_IP_PERIR            S5P_CLKREG(0x0C960)
 
 #define S5P_CLKSRC_CORE                 S5P_CLKREG(0x10200)
 #define S5P_CLKDIV_CORE0                S5P_CLKREG(0x10500)
@@ -60,4 +84,8 @@
 
 #define S5P_CLKGATE_SCLKCPU             S5P_CLKREG(0x14800)
 
+/* Compatibility defines */
+
+#define S5P_EPLL_CON                    S5P_EPLL_CON0
+
 #endif /* __ASM_ARCH_REGS_CLOCK_H */
diff --git a/arch/arm/mach-s5pv310/include/mach/regs-gpio.h b/arch/arm/mach-s5pv310/include/mach/regs-gpio.h
new file mode 100644
index 000000000000..82e9e0c9d452
--- /dev/null
+++ b/arch/arm/mach-s5pv310/include/mach/regs-gpio.h
@@ -0,0 +1,42 @@
+/* linux/arch/arm/mach-s5pv310/include/mach/regs-gpio.h
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * S5PV310 - GPIO (including EINT) register definitions
+ *
+ * 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.
+*/
+
+#ifndef __ASM_ARCH_REGS_GPIO_H
+#define __ASM_ARCH_REGS_GPIO_H __FILE__
+
+#include <mach/map.h>
+#include <mach/irqs.h>
+
+#define S5PV310_EINT40CON               (S5P_VA_GPIO2 + 0xE00)
+#define S5P_EINT_CON(x)                 (S5PV310_EINT40CON + ((x) * 0x4))
+
+#define S5PV310_EINT40FLTCON0           (S5P_VA_GPIO2 + 0xE80)
+#define S5P_EINT_FLTCON(x)              (S5PV310_EINT40FLTCON0 + ((x) * 0x4))
+
+#define S5PV310_EINT40MASK              (S5P_VA_GPIO2 + 0xF00)
+#define S5P_EINT_MASK(x)                (S5PV310_EINT40MASK + ((x) * 0x4))
+
+#define S5PV310_EINT40PEND              (S5P_VA_GPIO2 + 0xF40)
+#define S5P_EINT_PEND(x)                (S5PV310_EINT40PEND + ((x) * 0x4))
+
+#define EINT_REG_NR(x)                  (EINT_OFFSET(x) >> 3)
+
+#define eint_irq_to_bit(irq)            (1 << (EINT_OFFSET(irq) & 0x7))
+
+#define EINT_MODE                       S3C_GPIO_SFN(0xf)
+
+#define EINT_GPIO_0(x)                  S5PV310_GPX0(x)
+#define EINT_GPIO_1(x)                  S5PV310_GPX1(x)
+#define EINT_GPIO_2(x)                  S5PV310_GPX2(x)
+#define EINT_GPIO_3(x)                  S5PV310_GPX3(x)
+
+#endif /* __ASM_ARCH_REGS_GPIO_H */
diff --git a/arch/arm/mach-s5pv310/include/mach/regs-srom.h b/arch/arm/mach-s5pv310/include/mach/regs-srom.h
new file mode 100644
index 000000000000..1898b3e10550
--- /dev/null
+++ b/arch/arm/mach-s5pv310/include/mach/regs-srom.h
@@ -0,0 +1,50 @@
+/* linux/arch/arm/mach-s5pv310/include/mach/regs-srom.h
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * S5PV310 - SROMC register definitions
+ *
+ * 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.
+*/
+
+#ifndef __ASM_ARCH_REGS_SROM_H
+#define __ASM_ARCH_REGS_SROM_H __FILE__
+
+#include <mach/map.h>
+
+#define S5PV310_SROMREG(x)      (S5P_VA_SROMC + (x))
+
+#define S5PV310_SROM_BW         S5PV310_SROMREG(0x0)
+#define S5PV310_SROM_BC0        S5PV310_SROMREG(0x4)
+#define S5PV310_SROM_BC1        S5PV310_SROMREG(0x8)
+#define S5PV310_SROM_BC2        S5PV310_SROMREG(0xc)
+#define S5PV310_SROM_BC3        S5PV310_SROMREG(0x10)
+
+/* one register BW holds 4 x 4-bit packed settings for NCS0 - NCS3 */
+
+#define S5PV310_SROM_BW__DATAWIDTH__SHIFT       0
+#define S5PV310_SROM_BW__ADDRMODE__SHIFT        1
+#define S5PV310_SROM_BW__WAITENABLE__SHIFT      2
+#define S5PV310_SROM_BW__BYTEENABLE__SHIFT      3
+
+#define S5PV310_SROM_BW__CS_MASK                0xf
+
+#define S5PV310_SROM_BW__NCS0__SHIFT            0
+#define S5PV310_SROM_BW__NCS1__SHIFT            4
+#define S5PV310_SROM_BW__NCS2__SHIFT            8
+#define S5PV310_SROM_BW__NCS3__SHIFT            12
+
+/* applies to same to BCS0 - BCS3 */
+
+#define S5PV310_SROM_BCX__PMC__SHIFT            0
+#define S5PV310_SROM_BCX__TACP__SHIFT           4
+#define S5PV310_SROM_BCX__TCAH__SHIFT           8
+#define S5PV310_SROM_BCX__TCOH__SHIFT           12
+#define S5PV310_SROM_BCX__TACC__SHIFT           16
+#define S5PV310_SROM_BCX__TCOS__SHIFT           24
+#define S5PV310_SROM_BCX__TACS__SHIFT           28
+
+#endif /* __ASM_ARCH_REGS_SROM_H */
diff --git a/arch/arm/mach-s5pv310/include/mach/vmalloc.h b/arch/arm/mach-s5pv310/include/mach/vmalloc.h
index 256f221edf3a..65759fb97581 100644
--- a/arch/arm/mach-s5pv310/include/mach/vmalloc.h
+++ b/arch/arm/mach-s5pv310/include/mach/vmalloc.h
@@ -17,6 +17,6 @@
 #ifndef __ASM_ARCH_VMALLOC_H
 #define __ASM_ARCH_VMALLOC_H __FILE__
 
-#define VMALLOC_END     (0xF0000000UL)
+#define VMALLOC_END     0xF6000000UL
 
 #endif /* __ASM_ARCH_VMALLOC_H */
diff --git a/arch/arm/mach-s5pv310/irq-combiner.c b/arch/arm/mach-s5pv310/irq-combiner.c
index 0f7052164f23..c3f88c3faf6c 100644
--- a/arch/arm/mach-s5pv310/irq-combiner.c
+++ b/arch/arm/mach-s5pv310/irq-combiner.c
@@ -66,11 +66,7 @@ static void combiner_handle_cascade_irq(unsigned int irq, struct irq_desc *desc)
         if (status == 0)
                 goto out;
 
-        for (combiner_irq = 0; combiner_irq < 32; combiner_irq++) {
-                if (status & 0x1)
-                        break;
-                status >>= 1;
-        }
+        combiner_irq = __ffs(status);
 
         cascade_irq = combiner_irq + (chip_data->irq_offset & ~31);
         if (unlikely(cascade_irq >= NR_IRQS))
diff --git a/arch/arm/mach-s5pv310/irq-eint.c b/arch/arm/mach-s5pv310/irq-eint.c
new file mode 100644
index 000000000000..5877503e92c3
--- /dev/null
+++ b/arch/arm/mach-s5pv310/irq-eint.c
@@ -0,0 +1,228 @@
+/* linux/arch/arm/mach-s5pv310/irq-eint.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * S5PV310 - IRQ EINT support
+ *
+ * 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/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/sysdev.h>
+#include <linux/gpio.h>
+
+#include <plat/pm.h>
+#include <plat/cpu.h>
+#include <plat/gpio-cfg.h>
+
+#include <mach/regs-gpio.h>
+
+static DEFINE_SPINLOCK(eint_lock);
+
+static unsigned int eint0_15_data[16];
+
+static unsigned int s5pv310_get_irq_nr(unsigned int number)
+{
+        u32 ret = 0;
+
+        switch (number) {
+        case 0 ... 3:
+                ret = (number + IRQ_EINT0);
+                break;
+        case 4 ... 7:
+                ret = (number + (IRQ_EINT4 - 4));
+                break;
+        case 8 ... 15:
+                ret = (number + (IRQ_EINT8 - 8));
+                break;
+        default:
+                printk(KERN_ERR "number available : %d\n", number);
+        }
+
+        return ret;
+}
+
+static inline void s5pv310_irq_eint_mask(unsigned int irq)
+{
+        u32 mask;
+
+        spin_lock(&eint_lock);
+        mask = __raw_readl(S5P_EINT_MASK(EINT_REG_NR(irq)));
+        mask |= eint_irq_to_bit(irq);
+        __raw_writel(mask, S5P_EINT_MASK(EINT_REG_NR(irq)));
+        spin_unlock(&eint_lock);
+}
+
+static void s5pv310_irq_eint_unmask(unsigned int irq)
+{
+        u32 mask;
+
+        spin_lock(&eint_lock);
+        mask = __raw_readl(S5P_EINT_MASK(EINT_REG_NR(irq)));
+        mask &= ~(eint_irq_to_bit(irq));
+        __raw_writel(mask, S5P_EINT_MASK(EINT_REG_NR(irq)));
+        spin_unlock(&eint_lock);
+}
+
+static inline void s5pv310_irq_eint_ack(unsigned int irq)
+{
+        __raw_writel(eint_irq_to_bit(irq), S5P_EINT_PEND(EINT_REG_NR(irq)));
+}
+
+static void s5pv310_irq_eint_maskack(unsigned int irq)
+{
+        s5pv310_irq_eint_mask(irq);
+        s5pv310_irq_eint_ack(irq);
+}
+
+static int s5pv310_irq_eint_set_type(unsigned int irq, unsigned int type)
+{
+        int offs = EINT_OFFSET(irq);
+        int shift;
+        u32 ctrl, mask;
+        u32 newvalue = 0;
+
+        switch (type) {
+        case IRQ_TYPE_EDGE_RISING:
+                newvalue = S5P_IRQ_TYPE_EDGE_RISING;
+                break;
+
+        case IRQ_TYPE_EDGE_FALLING:
+                newvalue = S5P_IRQ_TYPE_EDGE_FALLING;
+                break;
+
+        case IRQ_TYPE_EDGE_BOTH:
+                newvalue = S5P_IRQ_TYPE_EDGE_BOTH;
+                break;
+
+        case IRQ_TYPE_LEVEL_LOW:
+                newvalue = S5P_IRQ_TYPE_LEVEL_LOW;
+                break;
+
+        case IRQ_TYPE_LEVEL_HIGH:
+                newvalue = S5P_IRQ_TYPE_LEVEL_HIGH;
+                break;
+
+        default:
+                printk(KERN_ERR "No such irq type %d", type);
+                return -EINVAL;
+        }
+
+        shift = (offs & 0x7) * 4;
+        mask = 0x7 << shift;
+
+        spin_lock(&eint_lock);
+        ctrl = __raw_readl(S5P_EINT_CON(EINT_REG_NR(irq)));
+        ctrl &= ~mask;
+        ctrl |= newvalue << shift;
+        __raw_writel(ctrl, S5P_EINT_CON(EINT_REG_NR(irq)));
+        spin_unlock(&eint_lock);
+
+        switch (offs) {
+        case 0 ... 7:
+                s3c_gpio_cfgpin(EINT_GPIO_0(offs & 0x7), EINT_MODE);
+                break;
+        case 8 ... 15:
+                s3c_gpio_cfgpin(EINT_GPIO_1(offs & 0x7), EINT_MODE);
+                break;
+        case 16 ... 23:
+                s3c_gpio_cfgpin(EINT_GPIO_2(offs & 0x7), EINT_MODE);
+                break;
+        case 24 ... 31:
+                s3c_gpio_cfgpin(EINT_GPIO_3(offs & 0x7), EINT_MODE);
+                break;
+        default:
+                printk(KERN_ERR "No such irq number %d", offs);
+        }
+
+        return 0;
+}
+
+static struct irq_chip s5pv310_irq_eint = {
+        .name           = "s5pv310-eint",
+        .mask           = s5pv310_irq_eint_mask,
+        .unmask         = s5pv310_irq_eint_unmask,
+        .mask_ack       = s5pv310_irq_eint_maskack,
+        .ack            = s5pv310_irq_eint_ack,
+        .set_type       = s5pv310_irq_eint_set_type,
+#ifdef CONFIG_PM
+        .set_wake       = s3c_irqext_wake,
+#endif
+};
+
+/* s5pv310_irq_demux_eint
+ *
+ * This function demuxes the IRQ from from EINTs 16 to 31.
+ * It is designed to be inlined into the specific handler
+ * s5p_irq_demux_eintX_Y.
+ *
+ * Each EINT pend/mask registers handle eight of them.
+ */
+static inline void s5pv310_irq_demux_eint(unsigned int start)
+{
+        unsigned int irq;
+
+        u32 status = __raw_readl(S5P_EINT_PEND(EINT_REG_NR(start)));
+        u32 mask = __raw_readl(S5P_EINT_MASK(EINT_REG_NR(start)));
+
+        status &= ~mask;
+        status &= 0xff;
+
+        while (status) {
+                irq = fls(status) - 1;
+                generic_handle_irq(irq + start);
+                status &= ~(1 << irq);
+        }
+}
+
+static void s5pv310_irq_demux_eint16_31(unsigned int irq, struct irq_desc *desc)
+{
+        s5pv310_irq_demux_eint(IRQ_EINT(16));
+        s5pv310_irq_demux_eint(IRQ_EINT(24));
+}
+
+static void s5pv310_irq_eint0_15(unsigned int irq, struct irq_desc *desc)
+{
+        u32 *irq_data = get_irq_data(irq);
+        struct irq_chip *chip = get_irq_chip(irq);
+
+        chip->mask(irq);
+
+        if (chip->ack)
+                chip->ack(irq);
+
+        generic_handle_irq(*irq_data);
+
+        chip->unmask(irq);
+}
+
+int __init s5pv310_init_irq_eint(void)
+{
+        int irq;
+
+        for (irq = 0 ; irq <= 31 ; irq++) {
+                set_irq_chip(IRQ_EINT(irq), &s5pv310_irq_eint);
+                set_irq_handler(IRQ_EINT(irq), handle_level_irq);
+                set_irq_flags(IRQ_EINT(irq), IRQF_VALID);
+        }
+
+        set_irq_chained_handler(IRQ_EINT16_31, s5pv310_irq_demux_eint16_31);
+
+        for (irq = 0 ; irq <= 15 ; irq++) {
+                eint0_15_data[irq] = IRQ_EINT(irq);
+
+                set_irq_data(s5pv310_get_irq_nr(irq), &eint0_15_data[irq]);
+                set_irq_chained_handler(s5pv310_get_irq_nr(irq),
+                                        s5pv310_irq_eint0_15);
+        }
+
+        return 0;
+}
+
+arch_initcall(s5pv310_init_irq_eint);
diff --git a/arch/arm/mach-s5pv310/mach-smdkc210.c b/arch/arm/mach-s5pv310/mach-smdkc210.c
new file mode 100644
index 000000000000..2b8d4fc52d7c
--- /dev/null
+++ b/arch/arm/mach-s5pv310/mach-smdkc210.c
@@ -0,0 +1,202 @@
+/* linux/arch/arm/mach-s5pv310/mach-smdkc210.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.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
+ * published by the Free Software Foundation.
+*/
+
+#include <linux/serial_core.h>
+#include <linux/gpio.h>
+#include <linux/mmc/host.h>
+#include <linux/platform_device.h>
+#include <linux/smsc911x.h>
+#include <linux/io.h>
+
+#include <asm/mach/arch.h>
+#include <asm/mach-types.h>
+
+#include <plat/regs-serial.h>
+#include <plat/s5pv310.h>
+#include <plat/cpu.h>
+#include <plat/devs.h>
+#include <plat/sdhci.h>
+
+#include <mach/map.h>
+#include <mach/regs-srom.h>
+
+/* Following are default values for UCON, ULCON and UFCON UART registers */
+#define SMDKC210_UCON_DEFAULT   (S3C2410_UCON_TXILEVEL |        \
+                                 S3C2410_UCON_RXILEVEL |        \
+                                 S3C2410_UCON_TXIRQMODE |       \
+                                 S3C2410_UCON_RXIRQMODE |       \
+                                 S3C2410_UCON_RXFIFO_TOI |      \
+                                 S3C2443_UCON_RXERR_IRQEN)
+
+#define SMDKC210_ULCON_DEFAULT  S3C2410_LCON_CS8
+
+#define SMDKC210_UFCON_DEFAULT  (S3C2410_UFCON_FIFOMODE |       \
+                                 S5PV210_UFCON_TXTRIG4 |        \
+                                 S5PV210_UFCON_RXTRIG4)
+
+static struct s3c2410_uartcfg smdkc210_uartcfgs[] __initdata = {
+        [0] = {
+                .hwport         = 0,
+                .flags          = 0,
+                .ucon           = SMDKC210_UCON_DEFAULT,
+                .ulcon          = SMDKC210_ULCON_DEFAULT,
+                .ufcon          = SMDKC210_UFCON_DEFAULT,
+        },
+        [1] = {
+                .hwport         = 1,
+                .flags          = 0,
+                .ucon           = SMDKC210_UCON_DEFAULT,
+                .ulcon          = SMDKC210_ULCON_DEFAULT,
+                .ufcon          = SMDKC210_UFCON_DEFAULT,
+        },
+        [2] = {
+                .hwport         = 2,
+                .flags          = 0,
+                .ucon           = SMDKC210_UCON_DEFAULT,
+                .ulcon          = SMDKC210_ULCON_DEFAULT,
+                .ufcon          = SMDKC210_UFCON_DEFAULT,
+        },
+        [3] = {
+                .hwport         = 3,
+                .flags          = 0,
+                .ucon           = SMDKC210_UCON_DEFAULT,
+                .ulcon          = SMDKC210_ULCON_DEFAULT,
+                .ufcon          = SMDKC210_UFCON_DEFAULT,
+        },
+};
+
+static struct s3c_sdhci_platdata smdkc210_hsmmc0_pdata __initdata = {
+        .cd_type                = S3C_SDHCI_CD_GPIO,
+        .ext_cd_gpio            = S5PV310_GPK0(2),
+        .ext_cd_gpio_invert     = 1,
+        .clk_type               = S3C_SDHCI_CLK_DIV_EXTERNAL,
+#ifdef CONFIG_S5PV310_SDHCI_CH0_8BIT
+        .max_width              = 8,
+        .host_caps              = MMC_CAP_8_BIT_DATA,
+#endif
+};
+
+static struct s3c_sdhci_platdata smdkc210_hsmmc1_pdata __initdata = {
+        .cd_type                = S3C_SDHCI_CD_GPIO,
+        .ext_cd_gpio            = S5PV310_GPK0(2),
+        .ext_cd_gpio_invert     = 1,
+        .clk_type               = S3C_SDHCI_CLK_DIV_EXTERNAL,
+};
+
+static struct s3c_sdhci_platdata smdkc210_hsmmc2_pdata __initdata = {
+        .cd_type                = S3C_SDHCI_CD_GPIO,
+        .ext_cd_gpio            = S5PV310_GPK2(2),
+        .ext_cd_gpio_invert     = 1,
+        .clk_type               = S3C_SDHCI_CLK_DIV_EXTERNAL,
+#ifdef CONFIG_S5PV310_SDHCI_CH2_8BIT
+        .max_width              = 8,
+        .host_caps              = MMC_CAP_8_BIT_DATA,
+#endif
+};
+
+static struct s3c_sdhci_platdata smdkc210_hsmmc3_pdata __initdata = {
+        .cd_type                = S3C_SDHCI_CD_GPIO,
+        .ext_cd_gpio            = S5PV310_GPK2(2),
+        .ext_cd_gpio_invert     = 1,
+        .clk_type               = S3C_SDHCI_CLK_DIV_EXTERNAL,
+};
+
+static struct resource smdkc210_smsc911x_resources[] = {
+        [0] = {
+                .start  = S5PV310_PA_SROM_BANK(1),
+                .end    = S5PV310_PA_SROM_BANK(1) + SZ_64K - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = IRQ_EINT(5),
+                .end    = IRQ_EINT(5),
+                .flags  = IORESOURCE_IRQ | IRQF_TRIGGER_LOW,
+        },
+};
+
+static struct smsc911x_platform_config smsc9215_config = {
+        .irq_polarity   = SMSC911X_IRQ_POLARITY_ACTIVE_HIGH,
+        .irq_type       = SMSC911X_IRQ_TYPE_PUSH_PULL,
+        .flags          = SMSC911X_USE_16BIT | SMSC911X_FORCE_INTERNAL_PHY,
+        .phy_interface  = PHY_INTERFACE_MODE_MII,
+        .mac            = {0x00, 0x80, 0x00, 0x23, 0x45, 0x67},
+};
+
+static struct platform_device smdkc210_smsc911x = {
+        .name           = "smsc911x",
+        .id             = -1,
+        .num_resources  = ARRAY_SIZE(smdkc210_smsc911x_resources),
+        .resource       = smdkc210_smsc911x_resources,
+        .dev            = {
+                .platform_data  = &smsc9215_config,
+        },
+};
+
+static struct platform_device *smdkc210_devices[] __initdata = {
+        &s3c_device_hsmmc0,
+        &s3c_device_hsmmc1,
+        &s3c_device_hsmmc2,
+        &s3c_device_hsmmc3,
+        &s3c_device_rtc,
+        &s3c_device_wdt,
+        &smdkc210_smsc911x,
+};
+
+static void __init smdkc210_smsc911x_init(void)
+{
+        u32 cs1;
+
+        /* configure nCS1 width to 16 bits */
+        cs1 = __raw_readl(S5PV310_SROM_BW) &
+                    ~(S5PV310_SROM_BW__CS_MASK <<
+                                    S5PV310_SROM_BW__NCS1__SHIFT);
+        cs1 |= ((1 << S5PV310_SROM_BW__DATAWIDTH__SHIFT) |
+                (1 << S5PV310_SROM_BW__WAITENABLE__SHIFT) |
+                (1 << S5PV310_SROM_BW__BYTEENABLE__SHIFT)) <<
+                S5PV310_SROM_BW__NCS1__SHIFT;
+        __raw_writel(cs1, S5PV310_SROM_BW);
+
+        /* set timing for nCS1 suitable for ethernet chip */
+        __raw_writel((0x1 << S5PV310_SROM_BCX__PMC__SHIFT) |
+                     (0x9 << S5PV310_SROM_BCX__TACP__SHIFT) |
+                     (0xc << S5PV310_SROM_BCX__TCAH__SHIFT) |
+                     (0x1 << S5PV310_SROM_BCX__TCOH__SHIFT) |
+                     (0x6 << S5PV310_SROM_BCX__TACC__SHIFT) |
+                     (0x1 << S5PV310_SROM_BCX__TCOS__SHIFT) |
+                     (0x1 << S5PV310_SROM_BCX__TACS__SHIFT), S5PV310_SROM_BC1);
+}
+
+static void __init smdkc210_map_io(void)
+{
+        s5p_init_io(NULL, 0, S5P_VA_CHIPID);
+        s3c24xx_init_clocks(24000000);
+        s3c24xx_init_uarts(smdkc210_uartcfgs, ARRAY_SIZE(smdkc210_uartcfgs));
+}
+
+static void __init smdkc210_machine_init(void)
+{
+        smdkc210_smsc911x_init();
+
+        s3c_sdhci0_set_platdata(&smdkc210_hsmmc0_pdata);
+        s3c_sdhci1_set_platdata(&smdkc210_hsmmc1_pdata);
+        s3c_sdhci2_set_platdata(&smdkc210_hsmmc2_pdata);
+        s3c_sdhci3_set_platdata(&smdkc210_hsmmc3_pdata);
+
+        platform_add_devices(smdkc210_devices, ARRAY_SIZE(smdkc210_devices));
+}
+
+MACHINE_START(SMDKC210, "SMDKC210")
+        /* Maintainer: Kukjin Kim <kgene.kim@samsung.com> */
+        .boot_params    = S5P_PA_SDRAM + 0x100,
+        .init_irq       = s5pv310_init_irq,
+        .map_io         = smdkc210_map_io,
+        .init_machine   = smdkc210_machine_init,
+        .timer          = &s5pv310_timer,
+MACHINE_END
diff --git a/arch/arm/mach-s5pv310/mach-smdkv310.c b/arch/arm/mach-s5pv310/mach-smdkv310.c
index 46215a14b3bb..35826d66632c 100644
--- a/arch/arm/mach-s5pv310/mach-smdkv310.c
+++ b/arch/arm/mach-s5pv310/mach-smdkv310.c
@@ -9,16 +9,23 @@
 */
 
 #include <linux/serial_core.h>
+#include <linux/gpio.h>
+#include <linux/mmc/host.h>
+#include <linux/platform_device.h>
+#include <linux/smsc911x.h>
+#include <linux/io.h>
 
 #include <asm/mach/arch.h>
 #include <asm/mach-types.h>
-#include <asm/hardware/cache-l2x0.h>
 
 #include <plat/regs-serial.h>
 #include <plat/s5pv310.h>
 #include <plat/cpu.h>
+#include <plat/devs.h>
+#include <plat/sdhci.h>
 
 #include <mach/map.h>
+#include <mach/regs-srom.h>
 
 /* Following are default values for UCON, ULCON and UFCON UART registers */
 #define SMDKV310_UCON_DEFAULT   (S3C2410_UCON_TXILEVEL |        \
@@ -65,6 +72,107 @@ static struct s3c2410_uartcfg smdkv310_uartcfgs[] __initdata = {
         },
 };
 
+static struct s3c_sdhci_platdata smdkv310_hsmmc0_pdata __initdata = {
+        .cd_type                = S3C_SDHCI_CD_GPIO,
+        .ext_cd_gpio            = S5PV310_GPK0(2),
+        .ext_cd_gpio_invert     = 1,
+        .clk_type               = S3C_SDHCI_CLK_DIV_EXTERNAL,
+#ifdef CONFIG_S5PV310_SDHCI_CH0_8BIT
+        .max_width              = 8,
+        .host_caps              = MMC_CAP_8_BIT_DATA,
+#endif
+};
+
+static struct s3c_sdhci_platdata smdkv310_hsmmc1_pdata __initdata = {
+        .cd_type                = S3C_SDHCI_CD_GPIO,
+        .ext_cd_gpio            = S5PV310_GPK0(2),
+        .ext_cd_gpio_invert     = 1,
+        .clk_type               = S3C_SDHCI_CLK_DIV_EXTERNAL,
+};
+
+static struct s3c_sdhci_platdata smdkv310_hsmmc2_pdata __initdata = {
+        .cd_type                = S3C_SDHCI_CD_GPIO,
+        .ext_cd_gpio            = S5PV310_GPK2(2),
+        .ext_cd_gpio_invert     = 1,
+        .clk_type               = S3C_SDHCI_CLK_DIV_EXTERNAL,
+#ifdef CONFIG_S5PV310_SDHCI_CH2_8BIT
+        .max_width              = 8,
+        .host_caps              = MMC_CAP_8_BIT_DATA,
+#endif
+};
+
+static struct s3c_sdhci_platdata smdkv310_hsmmc3_pdata __initdata = {
+        .cd_type                = S3C_SDHCI_CD_GPIO,
+        .ext_cd_gpio            = S5PV310_GPK2(2),
+        .ext_cd_gpio_invert     = 1,
+        .clk_type               = S3C_SDHCI_CLK_DIV_EXTERNAL,
+};
+
+static struct resource smdkv310_smsc911x_resources[] = {
+        [0] = {
+                .start  = S5PV310_PA_SROM_BANK(1),
+                .end    = S5PV310_PA_SROM_BANK(1) + SZ_64K - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = IRQ_EINT(5),
+                .end    = IRQ_EINT(5),
+                .flags  = IORESOURCE_IRQ | IRQF_TRIGGER_LOW,
+        },
+};
+
+static struct smsc911x_platform_config smsc9215_config = {
+        .irq_polarity   = SMSC911X_IRQ_POLARITY_ACTIVE_HIGH,
+        .irq_type       = SMSC911X_IRQ_TYPE_PUSH_PULL,
+        .flags          = SMSC911X_USE_16BIT | SMSC911X_FORCE_INTERNAL_PHY,
+        .phy_interface  = PHY_INTERFACE_MODE_MII,
+        .mac            = {0x00, 0x80, 0x00, 0x23, 0x45, 0x67},
+};
+
+static struct platform_device smdkv310_smsc911x = {
+        .name           = "smsc911x",
+        .id             = -1,
+        .num_resources  = ARRAY_SIZE(smdkv310_smsc911x_resources),
+        .resource       = smdkv310_smsc911x_resources,
+        .dev            = {
+                .platform_data  = &smsc9215_config,
+        },
+};
+
+static struct platform_device *smdkv310_devices[] __initdata = {
+        &s3c_device_hsmmc0,
+        &s3c_device_hsmmc1,
+        &s3c_device_hsmmc2,
+        &s3c_device_hsmmc3,
+        &s3c_device_rtc,
+        &s3c_device_wdt,
+        &smdkv310_smsc911x,
+};
+
+static void __init smdkv310_smsc911x_init(void)
+{
+        u32 cs1;
+
+        /* configure nCS1 width to 16 bits */
+        cs1 = __raw_readl(S5PV310_SROM_BW) &
+                    ~(S5PV310_SROM_BW__CS_MASK <<
+                                    S5PV310_SROM_BW__NCS1__SHIFT);
+        cs1 |= ((1 << S5PV310_SROM_BW__DATAWIDTH__SHIFT) |
+                (1 << S5PV310_SROM_BW__WAITENABLE__SHIFT) |
+                (1 << S5PV310_SROM_BW__BYTEENABLE__SHIFT)) <<
+                S5PV310_SROM_BW__NCS1__SHIFT;
+        __raw_writel(cs1, S5PV310_SROM_BW);
+
+        /* set timing for nCS1 suitable for ethernet chip */
+        __raw_writel((0x1 << S5PV310_SROM_BCX__PMC__SHIFT) |
+                     (0x9 << S5PV310_SROM_BCX__TACP__SHIFT) |
+                     (0xc << S5PV310_SROM_BCX__TCAH__SHIFT) |
+                     (0x1 << S5PV310_SROM_BCX__TCOH__SHIFT) |
+                     (0x6 << S5PV310_SROM_BCX__TACC__SHIFT) |
+                     (0x1 << S5PV310_SROM_BCX__TCOS__SHIFT) |
+                     (0x1 << S5PV310_SROM_BCX__TACS__SHIFT), S5PV310_SROM_BC1);
+}
+
 static void __init smdkv310_map_io(void)
 {
         s5p_init_io(NULL, 0, S5P_VA_CHIPID);
@@ -74,9 +182,14 @@ static void __init smdkv310_map_io(void)
 
 static void __init smdkv310_machine_init(void)
 {
-#ifdef CONFIG_CACHE_L2X0
-        l2x0_init(S5P_VA_L2CC, 1 << 28, 0xffffffff);
-#endif
+        smdkv310_smsc911x_init();
+
+        s3c_sdhci0_set_platdata(&smdkv310_hsmmc0_pdata);
+        s3c_sdhci1_set_platdata(&smdkv310_hsmmc1_pdata);
+        s3c_sdhci2_set_platdata(&smdkv310_hsmmc2_pdata);
+        s3c_sdhci3_set_platdata(&smdkv310_hsmmc3_pdata);
+
+        platform_add_devices(smdkv310_devices, ARRAY_SIZE(smdkv310_devices));
 }
 
 MACHINE_START(SMDKV310, "SMDKV310")
diff --git a/arch/arm/mach-s5pv310/mach-universal_c210.c b/arch/arm/mach-s5pv310/mach-universal_c210.c
index d7c2ec770f88..16d8fc00cafd 100644
--- a/arch/arm/mach-s5pv310/mach-universal_c210.c
+++ b/arch/arm/mach-s5pv310/mach-universal_c210.c
@@ -7,15 +7,20 @@
  * published by the Free Software Foundation.
 */
 
+#include <linux/platform_device.h>
 #include <linux/serial_core.h>
+#include <linux/input.h>
+#include <linux/i2c.h>
+#include <linux/gpio_keys.h>
+#include <linux/gpio.h>
 
 #include <asm/mach/arch.h>
 #include <asm/mach-types.h>
-#include <asm/hardware/cache-l2x0.h>
 
 #include <plat/regs-serial.h>
 #include <plat/s5pv310.h>
 #include <plat/cpu.h>
+#include <plat/devs.h>
 
 #include <mach/map.h>
 
@@ -60,6 +65,72 @@ static struct s3c2410_uartcfg universal_uartcfgs[] __initdata = {
         },
 };
 
+static struct gpio_keys_button universal_gpio_keys_tables[] = {
+        {
+                .code                   = KEY_VOLUMEUP,
+                .gpio                   = S5PV310_GPX2(0),      /* XEINT16 */
+                .desc                   = "gpio-keys: KEY_VOLUMEUP",
+                .type                   = EV_KEY,
+                .active_low             = 1,
+                .debounce_interval      = 1,
+        }, {
+                .code                   = KEY_VOLUMEDOWN,
+                .gpio                   = S5PV310_GPX2(1),      /* XEINT17 */
+                .desc                   = "gpio-keys: KEY_VOLUMEDOWN",
+                .type                   = EV_KEY,
+                .active_low             = 1,
+                .debounce_interval      = 1,
+        }, {
+                .code                   = KEY_CONFIG,
+                .gpio                   = S5PV310_GPX2(2),      /* XEINT18 */
+                .desc                   = "gpio-keys: KEY_CONFIG",
+                .type                   = EV_KEY,
+                .active_low             = 1,
+                .debounce_interval      = 1,
+        }, {
+                .code                   = KEY_CAMERA,
+                .gpio                   = S5PV310_GPX2(3),      /* XEINT19 */
+                .desc                   = "gpio-keys: KEY_CAMERA",
+                .type                   = EV_KEY,
+                .active_low             = 1,
+                .debounce_interval      = 1,
+        }, {
+                .code                   = KEY_OK,
+                .gpio                   = S5PV310_GPX3(5),      /* XEINT29 */
+                .desc                   = "gpio-keys: KEY_OK",
+                .type                   = EV_KEY,
+                .active_low             = 1,
+                .debounce_interval      = 1,
+        },
+};
+
+static struct gpio_keys_platform_data universal_gpio_keys_data = {
+        .buttons        = universal_gpio_keys_tables,
+        .nbuttons       = ARRAY_SIZE(universal_gpio_keys_tables),
+};
+
+static struct platform_device universal_gpio_keys = {
+        .name                   = "gpio-keys",
+        .dev                    = {
+                .platform_data  = &universal_gpio_keys_data,
+        },
+};
+
+/* I2C0 */
+static struct i2c_board_info i2c0_devs[] __initdata = {
+        /* Camera, To be updated */
+};
+
+/* I2C1 */
+static struct i2c_board_info i2c1_devs[] __initdata = {
+        /* Gyro, To be updated */
+};
+
+static struct platform_device *universal_devices[] __initdata = {
+        &universal_gpio_keys,
+        &s5p_device_onenand,
+};
+
 static void __init universal_map_io(void)
 {
         s5p_init_io(NULL, 0, S5P_VA_CHIPID);
@@ -69,9 +140,11 @@ static void __init universal_map_io(void)
 
 static void __init universal_machine_init(void)
 {
-#ifdef CONFIG_CACHE_L2X0
-        l2x0_init(S5P_VA_L2CC, 1 << 28, 0xffffffff);
-#endif
+        i2c_register_board_info(0, i2c0_devs, ARRAY_SIZE(i2c0_devs));
+        i2c_register_board_info(1, i2c1_devs, ARRAY_SIZE(i2c1_devs));
+
+        /* Last */
+        platform_add_devices(universal_devices, ARRAY_SIZE(universal_devices));
 }
 
 MACHINE_START(UNIVERSAL_C210, "UNIVERSAL_C210")
diff --git a/arch/arm/mach-s5pv310/setup-i2c0.c b/arch/arm/mach-s5pv310/setup-i2c0.c
index 436712807383..f47f8f3152ec 100644
--- a/arch/arm/mach-s5pv310/setup-i2c0.c
+++ b/arch/arm/mach-s5pv310/setup-i2c0.c
@@ -21,8 +21,6 @@ struct platform_device; /* don't need the contents */
 
 void s3c_i2c0_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5PV310_GPD1(0), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV310_GPD1(0), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5PV310_GPD1(1), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV310_GPD1(1), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5PV310_GPD1(0), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
diff --git a/arch/arm/mach-s5pv310/setup-i2c1.c b/arch/arm/mach-s5pv310/setup-i2c1.c
index 1ecd5bc35b5a..9d07e4e2f14c 100644
--- a/arch/arm/mach-s5pv310/setup-i2c1.c
+++ b/arch/arm/mach-s5pv310/setup-i2c1.c
@@ -18,8 +18,6 @@ struct platform_device; /* don't need the contents */
 
 void s3c_i2c1_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5PV310_GPD1(2), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV310_GPD1(2), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5PV310_GPD1(3), S3C_GPIO_SFN(2));
-        s3c_gpio_setpull(S5PV310_GPD1(3), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5PV310_GPD1(2), 2,
+                              S3C_GPIO_SFN(2), S3C_GPIO_PULL_UP);
 }
diff --git a/arch/arm/mach-s5pv310/setup-i2c2.c b/arch/arm/mach-s5pv310/setup-i2c2.c
index 4c0d8def660a..4163b1233daf 100644
--- a/arch/arm/mach-s5pv310/setup-i2c2.c
+++ b/arch/arm/mach-s5pv310/setup-i2c2.c
@@ -18,8 +18,6 @@ struct platform_device; /* don't need the contents */
 
 void s3c_i2c2_cfg_gpio(struct platform_device *dev)
 {
-        s3c_gpio_cfgpin(S5PV310_GPA0(6), S3C_GPIO_SFN(3));
-        s3c_gpio_setpull(S5PV310_GPA0(6), S3C_GPIO_PULL_UP);
-        s3c_gpio_cfgpin(S5PV310_GPA0(7), S3C_GPIO_SFN(3));
-        s3c_gpio_setpull(S5PV310_GPA0(7), S3C_GPIO_PULL_UP);
+        s3c_gpio_cfgall_range(S5PV310_GPA0(6), 2,
+                              S3C_GPIO_SFN(3), S3C_GPIO_PULL_UP);
 }
diff --git a/arch/arm/mach-s5pv310/setup-i2c3.c b/arch/arm/mach-s5pv310/setup-i2c3.c
new file mode 100644
index 000000000000..180f153d2a20
--- /dev/null
+++ b/arch/arm/mach-s5pv310/setup-i2c3.c
@@ -0,0 +1,23 @@
+/*
+ * linux/arch/arm/mach-s5pv310/setup-i2c3.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *
+ * I2C3 GPIO configuration.
+ *
+ * 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.
+*/
+
+struct platform_device; /* don't need the contents */
+
+#include <linux/gpio.h>
+#include <plat/iic.h>
+#include <plat/gpio-cfg.h>
+
+void s3c_i2c3_cfg_gpio(struct platform_device *dev)
+{
+        s3c_gpio_cfgall_range(S5PV310_GPA1(2), 2,
+                              S3C_GPIO_SFN(3), S3C_GPIO_PULL_UP);
+}
diff --git a/arch/arm/mach-s5pv310/setup-i2c4.c b/arch/arm/mach-s5pv310/setup-i2c4.c
new file mode 100644
index 000000000000..909e8dfc5316
--- /dev/null
+++ b/arch/arm/mach-s5pv310/setup-i2c4.c
@@ -0,0 +1,23 @@
+/*
+ * linux/arch/arm/mach-s5pv310/setup-i2c4.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *
+ * I2C4 GPIO configuration.
+ *
+ * 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.
+*/
+
+struct platform_device; /* don't need the contents */
+
+#include <linux/gpio.h>
+#include <plat/iic.h>
+#include <plat/gpio-cfg.h>
+
+void s3c_i2c4_cfg_gpio(struct platform_device *dev)
+{
+        s3c_gpio_cfgall_range(S5PV310_GPB(2), 2,
+                              S3C_GPIO_SFN(3), S3C_GPIO_PULL_UP);
+}
diff --git a/arch/arm/mach-s5pv310/setup-i2c5.c b/arch/arm/mach-s5pv310/setup-i2c5.c
new file mode 100644
index 000000000000..5d0fa4ac0283
--- /dev/null
+++ b/arch/arm/mach-s5pv310/setup-i2c5.c
@@ -0,0 +1,23 @@
+/*
+ * linux/arch/arm/mach-s5pv310/setup-i2c5.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *
+ * I2C5 GPIO configuration.
+ *
+ * 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.
+*/
+
+struct platform_device; /* don't need the contents */
+
+#include <linux/gpio.h>
+#include <plat/iic.h>
+#include <plat/gpio-cfg.h>
+
+void s3c_i2c5_cfg_gpio(struct platform_device *dev)
+{
+        s3c_gpio_cfgall_range(S5PV310_GPB(6), 2,
+                              S3C_GPIO_SFN(3), S3C_GPIO_PULL_UP);
+}
diff --git a/arch/arm/mach-s5pv310/setup-i2c6.c b/arch/arm/mach-s5pv310/setup-i2c6.c
new file mode 100644
index 000000000000..34aafab92ac4
--- /dev/null
+++ b/arch/arm/mach-s5pv310/setup-i2c6.c
@@ -0,0 +1,23 @@
+/*
+ * linux/arch/arm/mach-s5pv310/setup-i2c6.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *
+ * I2C6 GPIO configuration.
+ *
+ * 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.
+*/
+
+struct platform_device; /* don't need the contents */
+
+#include <linux/gpio.h>
+#include <plat/iic.h>
+#include <plat/gpio-cfg.h>
+
+void s3c_i2c6_cfg_gpio(struct platform_device *dev)
+{
+        s3c_gpio_cfgall_range(S5PV310_GPC1(3), 2,
+                              S3C_GPIO_SFN(4), S3C_GPIO_PULL_UP);
+}
diff --git a/arch/arm/mach-s5pv310/setup-i2c7.c b/arch/arm/mach-s5pv310/setup-i2c7.c
new file mode 100644
index 000000000000..9b25b8d18920
--- /dev/null
+++ b/arch/arm/mach-s5pv310/setup-i2c7.c
@@ -0,0 +1,23 @@
+/*
+ * linux/arch/arm/mach-s5pv310/setup-i2c7.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *
+ * I2C7 GPIO configuration.
+ *
+ * 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.
+*/
+
+struct platform_device; /* don't need the contents */
+
+#include <linux/gpio.h>
+#include <plat/iic.h>
+#include <plat/gpio-cfg.h>
+
+void s3c_i2c7_cfg_gpio(struct platform_device *dev)
+{
+        s3c_gpio_cfgall_range(S5PV310_GPD0(2), 2,
+                              S3C_GPIO_SFN(3), S3C_GPIO_PULL_UP);
+}
diff --git a/arch/arm/mach-s5pv310/setup-sdhci-gpio.c b/arch/arm/mach-s5pv310/setup-sdhci-gpio.c
new file mode 100644
index 000000000000..86d38cc49135
--- /dev/null
+++ b/arch/arm/mach-s5pv310/setup-sdhci-gpio.c
@@ -0,0 +1,152 @@
+/* linux/arch/arm/mach-s5pv310/setup-sdhci-gpio.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S5PV310 - Helper functions for setting up SDHCI device(s) GPIO (HSMMC)
+ *
+ * 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/kernel.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+#include <linux/mmc/host.h>
+#include <linux/mmc/card.h>
+
+#include <plat/gpio-cfg.h>
+#include <plat/regs-sdhci.h>
+#include <plat/sdhci.h>
+
+void s5pv310_setup_sdhci0_cfg_gpio(struct platform_device *dev, int width)
+{
+        struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
+        unsigned int gpio;
+
+        /* Set all the necessary GPK0[0:1] pins to special-function 2 */
+        for (gpio = S5PV310_GPK0(0); gpio < S5PV310_GPK0(2); gpio++) {
+                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+
+        switch (width) {
+        case 8:
+                for (gpio = S5PV310_GPK1(3); gpio <= S5PV310_GPK1(6); gpio++) {
+                        /* Data pin GPK1[3:6] to special-funtion 3 */
+                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
+                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_UP);
+                        s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+                }
+        case 4:
+                for (gpio = S5PV310_GPK0(3); gpio <= S5PV310_GPK0(6); gpio++) {
+                        /* Data pin GPK0[3:6] to special-funtion 2 */
+                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
+                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_UP);
+                        s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+                }
+        default:
+                break;
+        }
+
+        if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
+                s3c_gpio_cfgpin(S5PV310_GPK0(2), S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(S5PV310_GPK0(2), S3C_GPIO_PULL_UP);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+}
+
+void s5pv310_setup_sdhci1_cfg_gpio(struct platform_device *dev, int width)
+{
+        struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
+        unsigned int gpio;
+
+        /* Set all the necessary GPK1[0:1] pins to special-function 2 */
+        for (gpio = S5PV310_GPK1(0); gpio < S5PV310_GPK1(2); gpio++) {
+                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+
+        for (gpio = S5PV310_GPK1(3); gpio <= S5PV310_GPK1(6); gpio++) {
+                /* Data pin GPK1[3:6] to special-function 2 */
+                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_UP);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+
+        if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
+                s3c_gpio_cfgpin(S5PV310_GPK1(2), S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(S5PV310_GPK1(2), S3C_GPIO_PULL_UP);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+}
+
+void s5pv310_setup_sdhci2_cfg_gpio(struct platform_device *dev, int width)
+{
+        struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
+        unsigned int gpio;
+
+        /* Set all the necessary GPK2[0:1] pins to special-function 2 */
+        for (gpio = S5PV310_GPK2(0); gpio < S5PV310_GPK2(2); gpio++) {
+                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+
+        switch (width) {
+        case 8:
+                for (gpio = S5PV310_GPK3(3); gpio <= S5PV310_GPK3(6); gpio++) {
+                        /* Data pin GPK3[3:6] to special-function 3 */
+                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(3));
+                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_UP);
+                        s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+                }
+        case 4:
+                for (gpio = S5PV310_GPK2(3); gpio <= S5PV310_GPK2(6); gpio++) {
+                        /* Data pin GPK2[3:6] to special-function 2 */
+                        s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
+                        s3c_gpio_setpull(gpio, S3C_GPIO_PULL_UP);
+                        s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+                }
+        default:
+                break;
+        }
+
+        if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
+                s3c_gpio_cfgpin(S5PV310_GPK2(2), S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(S5PV310_GPK2(2), S3C_GPIO_PULL_UP);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+}
+
+void s5pv310_setup_sdhci3_cfg_gpio(struct platform_device *dev, int width)
+{
+        struct s3c_sdhci_platdata *pdata = dev->dev.platform_data;
+        unsigned int gpio;
+
+        /* Set all the necessary GPK3[0:1] pins to special-function 2 */
+        for (gpio = S5PV310_GPK3(0); gpio < S5PV310_GPK3(2); gpio++) {
+                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_NONE);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+
+        for (gpio = S5PV310_GPK3(3); gpio <= S5PV310_GPK3(6); gpio++) {
+                /* Data pin GPK3[3:6] to special-function 2 */
+                s3c_gpio_cfgpin(gpio, S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(gpio, S3C_GPIO_PULL_UP);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+
+        if (pdata->cd_type == S3C_SDHCI_CD_INTERNAL) {
+                s3c_gpio_cfgpin(S5PV310_GPK3(2), S3C_GPIO_SFN(2));
+                s3c_gpio_setpull(S5PV310_GPK3(2), S3C_GPIO_PULL_UP);
+                s5p_gpio_set_drvstr(gpio, S5P_GPIO_DRVSTR_LV4);
+        }
+}
diff --git a/arch/arm/mach-s5pv310/setup-sdhci.c b/arch/arm/mach-s5pv310/setup-sdhci.c
new file mode 100644
index 000000000000..db8358fc4662
--- /dev/null
+++ b/arch/arm/mach-s5pv310/setup-sdhci.c
@@ -0,0 +1,69 @@
+/* linux/arch/arm/mach-s5pv310/setup-sdhci.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S5PV310 - Helper functions for settign up SDHCI device(s) (HSMMC)
+ *
+ * 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/kernel.h>
+#include <linux/types.h>
+#include <linux/interrupt.h>
+#include <linux/platform_device.h>
+#include <linux/io.h>
+
+#include <linux/mmc/card.h>
+#include <linux/mmc/host.h>
+
+#include <plat/regs-sdhci.h>
+
+/* clock sources for the mmc bus clock, order as for the ctrl2[5..4] */
+
+char *s5pv310_hsmmc_clksrcs[4] = {
+        [0] = NULL,
+        [1] = NULL,
+        [2] = "sclk_mmc",       /* mmc_bus */
+        [3] = NULL,
+};
+
+void s5pv310_setup_sdhci_cfg_card(struct platform_device *dev, void __iomem *r,
+                                  struct mmc_ios *ios, struct mmc_card *card)
+{
+        u32 ctrl2, ctrl3;
+
+        /* don't need to alter anything acording to card-type */
+
+        ctrl2 = readl(r + S3C_SDHCI_CONTROL2);
+
+        /* select base clock source to HCLK */
+
+        ctrl2 &= S3C_SDHCI_CTRL2_SELBASECLK_MASK;
+
+        /*
+         * clear async mode, enable conflict mask, rx feedback ctrl, SD
+         * clk hold and no use debounce count
+         */
+
+        ctrl2 |= (S3C64XX_SDHCI_CTRL2_ENSTAASYNCCLR |
+                  S3C64XX_SDHCI_CTRL2_ENCMDCNFMSK |
+                  S3C_SDHCI_CTRL2_ENFBCLKRX |
+                  S3C_SDHCI_CTRL2_DFCNT_NONE |
+                  S3C_SDHCI_CTRL2_ENCLKOUTHOLD);
+
+        /* Tx and Rx feedback clock delay control */
+
+        if (ios->clock < 25 * 1000000)
+                ctrl3 = (S3C_SDHCI_CTRL3_FCSEL3 |
+                         S3C_SDHCI_CTRL3_FCSEL2 |
+                         S3C_SDHCI_CTRL3_FCSEL1 |
+                         S3C_SDHCI_CTRL3_FCSEL0);
+        else
+                ctrl3 = (S3C_SDHCI_CTRL3_FCSEL1 | S3C_SDHCI_CTRL3_FCSEL0);
+
+        writel(ctrl2, r + S3C_SDHCI_CONTROL2);
+        writel(ctrl3, r + S3C_SDHCI_CONTROL3);
+}
diff --git a/arch/arm/mach-sa1100/cpu-sa1100.c b/arch/arm/mach-sa1100/cpu-sa1100.c
index c0a13ef5436f..96f7dc103b59 100644
--- a/arch/arm/mach-sa1100/cpu-sa1100.c
+++ b/arch/arm/mach-sa1100/cpu-sa1100.c
@@ -184,16 +184,15 @@ static int sa1100_target(struct cpufreq_policy *policy,
 {
         unsigned int cur = sa11x0_getspeed(0);
         unsigned int new_ppcr;
-
         struct cpufreq_freqs freqs;
+
+        new_ppcr = sa11x0_freq_to_ppcr(target_freq);
         switch(relation){
         case CPUFREQ_RELATION_L:
-                new_ppcr = sa11x0_freq_to_ppcr(target_freq);
                 if (sa11x0_ppcr_to_freq(new_ppcr) > policy->max)
                         new_ppcr--;
                 break;
         case CPUFREQ_RELATION_H:
-                new_ppcr = sa11x0_freq_to_ppcr(target_freq);
                 if ((sa11x0_ppcr_to_freq(new_ppcr) > target_freq) &&
                     (sa11x0_ppcr_to_freq(new_ppcr - 1) >= policy->min))
                         new_ppcr--;
diff --git a/arch/arm/mach-shmobile/board-ap4evb.c b/arch/arm/mach-shmobile/board-ap4evb.c
index 22a2b44ddb7b..46ca4d4abf91 100644
--- a/arch/arm/mach-shmobile/board-ap4evb.c
+++ b/arch/arm/mach-shmobile/board-ap4evb.c
@@ -30,7 +30,6 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/partitions.h>
 #include <linux/mtd/physmap.h>
-#include <linux/mmc/host.h>
 #include <linux/mmc/sh_mmcif.h>
 #include <linux/i2c.h>
 #include <linux/i2c/tsc2007.h>
@@ -44,6 +43,10 @@
 #include <linux/input/sh_keysc.h>
 #include <linux/usb/r8a66597.h>
 
+#include <media/sh_mobile_ceu.h>
+#include <media/sh_mobile_csi2.h>
+#include <media/soc_camera.h>
+
 #include <sound/sh_fsi.h>
 
 #include <video/sh_mobile_hdmi.h>
@@ -250,7 +253,7 @@ static int slot_cn7_get_cd(struct platform_device *pdev)
 /* SH_MMCIF */
 static struct resource sh_mmcif_resources[] = {
         [0] = {
-                .name   = "SH_MMCIF",
+                .name   = "MMCIF",
                 .start  = 0xE6BD0000,
                 .end    = 0xE6BD00FF,
                 .flags  = IORESOURCE_MEM,
@@ -390,10 +393,40 @@ static struct platform_device usb1_host_device = {
         .resource       = usb1_host_resources,
 };
 
+const static struct fb_videomode ap4evb_lcdc_modes[] = {
+        {
+#ifdef CONFIG_AP4EVB_QHD
+                .name           = "R63302(QHD)",
+                .xres           = 544,
+                .yres           = 961,
+                .left_margin    = 72,
+                .right_margin   = 600,
+                .hsync_len      = 16,
+                .upper_margin   = 8,
+                .lower_margin   = 8,
+                .vsync_len      = 2,
+                .sync           = FB_SYNC_VERT_HIGH_ACT | FB_SYNC_HOR_HIGH_ACT,
+#else
+                .name           = "WVGA Panel",
+                .xres           = 800,
+                .yres           = 480,
+                .left_margin    = 220,
+                .right_margin   = 110,
+                .hsync_len      = 70,
+                .upper_margin   = 20,
+                .lower_margin   = 5,
+                .vsync_len      = 5,
+                .sync           = 0,
+#endif
+        },
+};
+
 static struct sh_mobile_lcdc_info lcdc_info = {
         .ch[0] = {
                 .chan = LCDC_CHAN_MAINLCD,
                 .bpp = 16,
+                .lcd_cfg = ap4evb_lcdc_modes,
+                .num_cfg = ARRAY_SIZE(ap4evb_lcdc_modes),
         }
 };
 
@@ -532,27 +565,6 @@ static struct platform_device *qhd_devices[] __initdata = {
 
 /* FSI */
 #define IRQ_FSI         evt2irq(0x1840)
-#define FSIACKCR        0xE6150018
-static void fsiackcr_init(struct clk *clk)
-{
-        u32 status = __raw_readl(clk->enable_reg);
-
-        /* use external clock */
-        status &= ~0x000000ff;
-        status |= 0x00000080;
-        __raw_writel(status, clk->enable_reg);
-}
-
-static struct clk_ops fsiackcr_clk_ops = {
-        .init = fsiackcr_init,
-};
-
-static struct clk fsiackcr_clk = {
-        .ops            = &fsiackcr_clk_ops,
-        .enable_reg     = (void __iomem *)FSIACKCR,
-        .rate           = 0, /* unknown */
-};
-
 static struct sh_fsi_platform_info fsi_info = {
         .porta_flags = SH_FSI_BRS_INV |
                        SH_FSI_OUT_SLAVE_MODE |
@@ -592,26 +604,6 @@ static struct sh_mobile_lcdc_info sh_mobile_lcdc1_info = {
                 .interface_type = RGB24,
                 .clock_divider = 1,
                 .flags = LCDC_FLAGS_DWPOL,
-                .lcd_cfg = {
-                        .name = "HDMI",
-                        /* So far only 720p is supported */
-                        .xres = 1280,
-                        .yres = 720,
-                        /*
-                         * If left and right margins are not multiples of 8,
-                         * LDHAJR will be adjusted accordingly by the LCDC
-                         * driver. Until we start using EDID, these values
-                         * might have to be adjusted for different monitors.
-                         */
-                        .left_margin = 200,
-                        .right_margin = 88,
-                        .hsync_len = 48,
-                        .upper_margin = 20,
-                        .lower_margin = 5,
-                        .vsync_len = 5,
-                        .pixclock = 13468,
-                        .sync = FB_SYNC_VERT_HIGH_ACT | FB_SYNC_HOR_HIGH_ACT,
-                },
         }
 };
 
@@ -623,7 +615,7 @@ static struct resource lcdc1_resources[] = {
                 .flags  = IORESOURCE_MEM,
         },
         [1] = {
-                .start  = intcs_evt2irq(0x17a0),
+                .start  = intcs_evt2irq(0x1780),
                 .flags  = IORESOURCE_IRQ,
         },
 };
@@ -704,6 +696,95 @@ static struct platform_device leds_device = {
         },
 };
 
+static struct i2c_board_info imx074_info = {
+        I2C_BOARD_INFO("imx074", 0x1a),
+};
+
+struct soc_camera_link imx074_link = {
+        .bus_id         = 0,
+        .board_info     = &imx074_info,
+        .i2c_adapter_id = 0,
+        .module_name    = "imx074",
+};
+
+static struct platform_device ap4evb_camera = {
+        .name   = "soc-camera-pdrv",
+        .id     = 0,
+        .dev    = {
+                .platform_data = &imx074_link,
+        },
+};
+
+static struct sh_csi2_client_config csi2_clients[] = {
+        {
+                .phy            = SH_CSI2_PHY_MAIN,
+                .lanes          = 3,
+                .channel        = 0,
+                .pdev           = &ap4evb_camera,
+        },
+};
+
+static struct sh_csi2_pdata csi2_info = {
+        .type           = SH_CSI2C,
+        .clients        = csi2_clients,
+        .num_clients    = ARRAY_SIZE(csi2_clients),
+        .flags          = SH_CSI2_ECC | SH_CSI2_CRC,
+};
+
+static struct resource csi2_resources[] = {
+        [0] = {
+                .name   = "CSI2",
+                .start  = 0xffc90000,
+                .end    = 0xffc90fff,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = intcs_evt2irq(0x17a0),
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+static struct platform_device csi2_device = {
+        .name   = "sh-mobile-csi2",
+        .id     = 0,
+        .num_resources  = ARRAY_SIZE(csi2_resources),
+        .resource       = csi2_resources,
+        .dev    = {
+                .platform_data = &csi2_info,
+        },
+};
+
+static struct sh_mobile_ceu_info sh_mobile_ceu_info = {
+        .flags = SH_CEU_FLAG_USE_8BIT_BUS,
+        .csi2_dev = &csi2_device.dev,
+};
+
+static struct resource ceu_resources[] = {
+        [0] = {
+                .name   = "CEU",
+                .start  = 0xfe910000,
+                .end    = 0xfe91009f,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = intcs_evt2irq(0x880),
+                .flags  = IORESOURCE_IRQ,
+        },
+        [2] = {
+                /* place holder for contiguous memory */
+        },
+};
+
+static struct platform_device ceu_device = {
+        .name           = "sh_mobile_ceu",
+        .id             = 0, /* "ceu0" clock */
+        .num_resources  = ARRAY_SIZE(ceu_resources),
+        .resource       = ceu_resources,
+        .dev    = {
+                .platform_data  = &sh_mobile_ceu_info,
+        },
+};
+
 static struct platform_device *ap4evb_devices[] __initdata = {
         &leds_device,
         &nor_flash_device,
@@ -716,6 +797,9 @@ static struct platform_device *ap4evb_devices[] __initdata = {
         &lcdc1_device,
         &lcdc_device,
         &hdmi_device,
+        &csi2_device,
+        &ceu_device,
+        &ap4evb_camera,
 };
 
 static int __init hdmi_init_pm_clock(void)
@@ -730,22 +814,22 @@ static int __init hdmi_init_pm_clock(void)
                 goto out;
         }
 
-        ret = clk_set_parent(&pllc2_clk, &dv_clki_div2_clk);
+        ret = clk_set_parent(&sh7372_pllc2_clk, &sh7372_dv_clki_div2_clk);
         if (ret < 0) {
-                pr_err("Cannot set PLLC2 parent: %d, %d users\n", ret, pllc2_clk.usecount);
+                pr_err("Cannot set PLLC2 parent: %d, %d users\n", ret, sh7372_pllc2_clk.usecount);
                 goto out;
         }
 
-        pr_debug("PLLC2 initial frequency %lu\n", clk_get_rate(&pllc2_clk));
+        pr_debug("PLLC2 initial frequency %lu\n", clk_get_rate(&sh7372_pllc2_clk));
 
-        rate = clk_round_rate(&pllc2_clk, 594000000);
+        rate = clk_round_rate(&sh7372_pllc2_clk, 594000000);
         if (rate < 0) {
                 pr_err("Cannot get suitable rate: %ld\n", rate);
                 ret = rate;
                 goto out;
         }
 
-        ret = clk_set_rate(&pllc2_clk, rate);
+        ret = clk_set_rate(&sh7372_pllc2_clk, rate);
         if (ret < 0) {
                 pr_err("Cannot set rate %ld: %d\n", rate, ret);
                 goto out;
@@ -753,7 +837,7 @@ static int __init hdmi_init_pm_clock(void)
 
         pr_debug("PLLC2 set frequency %lu\n", rate);
 
-        ret = clk_set_parent(hdmi_ick, &pllc2_clk);
+        ret = clk_set_parent(hdmi_ick, &sh7372_pllc2_clk);
         if (ret < 0) {
                 pr_err("Cannot set HDMI parent: %d\n", ret);
                 goto out;
@@ -767,11 +851,51 @@ out:
 
 device_initcall(hdmi_init_pm_clock);
 
+#define FSIACK_DUMMY_RATE 48000
+static int __init fsi_init_pm_clock(void)
+{
+        struct clk *fsia_ick;
+        int ret;
+
+        /*
+         * FSIACK is connected to AK4642,
+         * and the rate is depend on playing sound rate.
+         * So, set dummy rate (= 48k) here
+         */
+        ret = clk_set_rate(&sh7372_fsiack_clk, FSIACK_DUMMY_RATE);
+        if (ret < 0) {
+                pr_err("Cannot set FSIACK dummy rate: %d\n", ret);
+                return ret;
+        }
+
+        fsia_ick = clk_get(&fsi_device.dev, "icka");
+        if (IS_ERR(fsia_ick)) {
+                ret = PTR_ERR(fsia_ick);
+                pr_err("Cannot get FSI ICK: %d\n", ret);
+                return ret;
+        }
+
+        ret = clk_set_parent(fsia_ick, &sh7372_fsiack_clk);
+        if (ret < 0) {
+                pr_err("Cannot set FSI-A parent: %d\n", ret);
+                goto out;
+        }
+
+        ret = clk_set_rate(fsia_ick, FSIACK_DUMMY_RATE);
+        if (ret < 0)
+                pr_err("Cannot set FSI-A rate: %d\n", ret);
+
+out:
+        clk_put(fsia_ick);
+
+        return ret;
+}
+device_initcall(fsi_init_pm_clock);
+
 /*
  * FIXME !!
  *
  * gpio_no_direction
- * gpio_pull_up
  * are quick_hack.
  *
  * current gpio frame work doesn't have
@@ -783,49 +907,37 @@ static void __init gpio_no_direction(u32 addr)
         __raw_writeb(0x00, addr);
 }
 
-static void __init gpio_pull_up(u32 addr)
-{
-        u8 data = __raw_readb(addr);
-
-        data &= 0x0F;
-        data |= 0xC0;
-        __raw_writeb(data, addr);
-}
-
 /* TouchScreen */
+#ifdef CONFIG_AP4EVB_QHD
+# define GPIO_TSC_IRQ   GPIO_FN_IRQ28_123
+# define GPIO_TSC_PORT  GPIO_PORT123
+#else /* WVGA */
+# define GPIO_TSC_IRQ   GPIO_FN_IRQ7_40
+# define GPIO_TSC_PORT  GPIO_PORT40
+#endif
+
 #define IRQ28   evt2irq(0x3380) /* IRQ28A */
 #define IRQ7    evt2irq(0x02e0) /* IRQ7A */
 static int ts_get_pendown_state(void)
 {
-        int val1, val2;
+        int val;
 
-        gpio_free(GPIO_FN_IRQ28_123);
-        gpio_free(GPIO_FN_IRQ7_40);
+        gpio_free(GPIO_TSC_IRQ);
 
-        gpio_request(GPIO_PORT123, NULL);
-        gpio_request(GPIO_PORT40, NULL);
+        gpio_request(GPIO_TSC_PORT, NULL);
 
-        gpio_direction_input(GPIO_PORT123);
-        gpio_direction_input(GPIO_PORT40);
+        gpio_direction_input(GPIO_TSC_PORT);
 
-        val1 = gpio_get_value(GPIO_PORT123);
-        val2 = gpio_get_value(GPIO_PORT40);
+        val = gpio_get_value(GPIO_TSC_PORT);
 
-        gpio_request(GPIO_FN_IRQ28_123, NULL);  /* for QHD */
-        gpio_request(GPIO_FN_IRQ7_40, NULL);    /* for WVGA */
+        gpio_request(GPIO_TSC_IRQ, NULL);
 
-        return val1 ^ val2;
+        return !val;
 }
 
-#define PORT40CR        0xE6051028
-#define PORT123CR       0xE605007B
 static int ts_init(void)
 {
-        gpio_request(GPIO_FN_IRQ28_123, NULL);  /* for QHD */
-        gpio_request(GPIO_FN_IRQ7_40, NULL);    /* for WVGA */
-
-        gpio_pull_up(PORT40CR);
-        gpio_pull_up(PORT123CR);
+        gpio_request(GPIO_TSC_IRQ, NULL);
 
         return 0;
 }
@@ -974,14 +1086,6 @@ static void __init ap4evb_init(void)
                 clk_put(clk);
         }
 
-        /* change parent of FSI A */
-        clk = clk_get(NULL, "fsia_clk");
-        if (!IS_ERR(clk)) {
-                clk_register(&fsiackcr_clk);
-                clk_set_parent(clk, &fsiackcr_clk);
-                clk_put(clk);
-        }
-
         /*
          * set irq priority, to avoid sound chopping
          * when NFS rootfs is used
@@ -996,8 +1100,10 @@ static void __init ap4evb_init(void)
                                 ARRAY_SIZE(i2c1_devices));
 
 #ifdef CONFIG_AP4EVB_QHD
+
         /*
-         * QHD
+         * For QHD Panel (MIPI-DSI, CONFIG_AP4EVB_QHD=y) and
+         * IRQ28 for Touch Panel, set dip switches S3, S43 as OFF, ON.
          */
 
         /* enable KEYSC */
@@ -1023,17 +1129,6 @@ static void __init ap4evb_init(void)
         lcdc_info.ch[0].interface_type          = RGB24;
         lcdc_info.ch[0].clock_divider           = 1;
         lcdc_info.ch[0].flags                   = LCDC_FLAGS_DWPOL;
-        lcdc_info.ch[0].lcd_cfg.name            = "R63302(QHD)";
-        lcdc_info.ch[0].lcd_cfg.xres            = 544;
-        lcdc_info.ch[0].lcd_cfg.yres            = 961;
-        lcdc_info.ch[0].lcd_cfg.left_margin     = 72;
-        lcdc_info.ch[0].lcd_cfg.right_margin    = 600;
-        lcdc_info.ch[0].lcd_cfg.hsync_len       = 16;
-        lcdc_info.ch[0].lcd_cfg.upper_margin    = 8;
-        lcdc_info.ch[0].lcd_cfg.lower_margin    = 8;
-        lcdc_info.ch[0].lcd_cfg.vsync_len       = 2;
-        lcdc_info.ch[0].lcd_cfg.sync            = FB_SYNC_VERT_HIGH_ACT |
-                                                  FB_SYNC_HOR_HIGH_ACT;
         lcdc_info.ch[0].lcd_size_cfg.width      = 44;
         lcdc_info.ch[0].lcd_size_cfg.height     = 79;
 
@@ -1041,8 +1136,10 @@ static void __init ap4evb_init(void)
 
 #else
         /*
-         * WVGA
+         * For WVGA Panel (18-bit RGB, CONFIG_AP4EVB_WVGA=y) and
+         * IRQ7 for Touch Panel, set dip switches S3, S43 to ON, OFF.
          */
+
         gpio_request(GPIO_FN_LCDD17,   NULL);
         gpio_request(GPIO_FN_LCDD16,   NULL);
         gpio_request(GPIO_FN_LCDD15,   NULL);
@@ -1074,16 +1171,6 @@ static void __init ap4evb_init(void)
         lcdc_info.ch[0].interface_type          = RGB18;
         lcdc_info.ch[0].clock_divider           = 2;
         lcdc_info.ch[0].flags                   = 0;
-        lcdc_info.ch[0].lcd_cfg.name            = "WVGA Panel";
-        lcdc_info.ch[0].lcd_cfg.xres            = 800;
-        lcdc_info.ch[0].lcd_cfg.yres            = 480;
-        lcdc_info.ch[0].lcd_cfg.left_margin     = 220;
-        lcdc_info.ch[0].lcd_cfg.right_margin    = 110;
-        lcdc_info.ch[0].lcd_cfg.hsync_len       = 70;
-        lcdc_info.ch[0].lcd_cfg.upper_margin    = 20;
-        lcdc_info.ch[0].lcd_cfg.lower_margin    = 5;
-        lcdc_info.ch[0].lcd_cfg.vsync_len       = 5;
-        lcdc_info.ch[0].lcd_cfg.sync            = 0;
         lcdc_info.ch[0].lcd_size_cfg.width      = 152;
         lcdc_info.ch[0].lcd_size_cfg.height     = 91;
 
@@ -1094,6 +1181,23 @@ static void __init ap4evb_init(void)
         i2c_register_board_info(0, &tsc_device, 1);
 #endif /* CONFIG_AP4EVB_QHD */
 
+        /* CEU */
+
+        /*
+         * TODO: reserve memory for V4L2 DMA buffers, when a suitable API
+         * becomes available
+         */
+
+        /* MIPI-CSI stuff */
+        gpio_request(GPIO_FN_VIO_CKO, NULL);
+
+        clk = clk_get(NULL, "vck1_clk");
+        if (!IS_ERR(clk)) {
+                clk_set_rate(clk, clk_round_rate(clk, 13000000));
+                clk_enable(clk);
+                clk_put(clk);
+        }
+
         sh7372_add_standard_devices();
 
         /* HDMI */
@@ -1116,7 +1220,7 @@ static void __init ap4evb_timer_init(void)
         shmobile_timer.init();
 
         /* External clock source */
-        clk_set_rate(&dv_clki_clk, 27000000);
+        clk_set_rate(&sh7372_dv_clki_clk, 27000000);
 }
 
 static struct sys_timer ap4evb_timer = {
diff --git a/arch/arm/mach-shmobile/clock-sh7367.c b/arch/arm/mach-shmobile/clock-sh7367.c
index b6454c9f2abb..9f78729098f2 100644
--- a/arch/arm/mach-shmobile/clock-sh7367.c
+++ b/arch/arm/mach-shmobile/clock-sh7367.c
@@ -321,7 +321,7 @@ static struct clk_lookup lookups[] = {
         CLKDEV_DEV_ID("sh-sci.3", &mstp_clks[SYMSTP001]), /* SCIFA3 */
         CLKDEV_DEV_ID("sh-sci.4", &mstp_clks[SYMSTP000]), /* SCIFA4 */
         CLKDEV_DEV_ID("sh_siu", &mstp_clks[SYMSTP231]), /* SIU */
-        CLKDEV_CON_ID("cmt1", &mstp_clks[SYMSTP229]), /* CMT10 */
+        CLKDEV_DEV_ID("sh_cmt.10", &mstp_clks[SYMSTP229]), /* CMT10 */
         CLKDEV_DEV_ID("sh_irda", &mstp_clks[SYMSTP225]), /* IRDA */
         CLKDEV_DEV_ID("i2c-sh_mobile.1", &mstp_clks[SYMSTP223]), /* IIC1 */
         CLKDEV_DEV_ID("r8a66597_hcd.0", &mstp_clks[SYMSTP222]), /* USBHS */
diff --git a/arch/arm/mach-shmobile/clock-sh7372.c b/arch/arm/mach-shmobile/clock-sh7372.c
index 759468992ad2..8565aefa21fd 100644
--- a/arch/arm/mach-shmobile/clock-sh7372.c
+++ b/arch/arm/mach-shmobile/clock-sh7372.c
@@ -51,7 +51,7 @@
 #define SMSTPCR4        0xe6150140
 
 /* Platforms must set frequency on their DV_CLKI pin */
-struct clk dv_clki_clk = {
+struct clk sh7372_dv_clki_clk = {
 };
 
 /* Fixed 32 KHz root clock from EXTALR pin */
@@ -86,9 +86,9 @@ static struct clk_ops div2_clk_ops = {
 };
 
 /* Divide dv_clki by two */
-struct clk dv_clki_div2_clk = {
+struct clk sh7372_dv_clki_div2_clk = {
         .ops            = &div2_clk_ops,
-        .parent         = &dv_clki_clk,
+        .parent         = &sh7372_dv_clki_clk,
 };
 
 /* Divide extal1 by two */
@@ -150,7 +150,7 @@ static struct clk pllc1_div2_clk = {
 static struct clk *pllc2_parent[] = {
         [0] = &extal1_div2_clk,
         [1] = &extal2_div2_clk,
-        [2] = &dv_clki_div2_clk,
+        [2] = &sh7372_dv_clki_div2_clk,
 };
 
 /* Only multipliers 20 * 2 to 46 * 2 are valid, last entry for CPUFREQ_TABLE_END */
@@ -284,7 +284,7 @@ static struct clk_ops pllc2_clk_ops = {
         .set_parent     = pllc2_set_parent,
 };
 
-struct clk pllc2_clk = {
+struct clk sh7372_pllc2_clk = {
         .ops            = &pllc2_clk_ops,
         .parent         = &extal1_div2_clk,
         .freq_table     = pllc2_freq_table,
@@ -292,19 +292,28 @@ struct clk pllc2_clk = {
         .parent_num     = ARRAY_SIZE(pllc2_parent),
 };
 
+/* External input clock (pin name: FSIACK/FSIBCK ) */
+struct clk sh7372_fsiack_clk = {
+};
+
+struct clk sh7372_fsibck_clk = {
+};
+
 static struct clk *main_clks[] = {
-        &dv_clki_clk,
+        &sh7372_dv_clki_clk,
         &r_clk,
         &sh7372_extal1_clk,
         &sh7372_extal2_clk,
-        &dv_clki_div2_clk,
+        &sh7372_dv_clki_div2_clk,
         &extal1_div2_clk,
         &extal2_div2_clk,
         &extal2_div4_clk,
         &pllc0_clk,
         &pllc1_clk,
         &pllc1_div2_clk,
-        &pllc2_clk,
+        &sh7372_pllc2_clk,
+        &sh7372_fsiack_clk,
+        &sh7372_fsibck_clk,
 };
 
 static void div4_kick(struct clk *clk)
@@ -357,7 +366,7 @@ static struct clk div4_clks[DIV4_NR] = {
 };
 
 enum { DIV6_VCK1, DIV6_VCK2, DIV6_VCK3, DIV6_FMSI, DIV6_FMSO,
-       DIV6_FSIA, DIV6_FSIB, DIV6_SUB, DIV6_SPU,
+       DIV6_SUB, DIV6_SPU,
        DIV6_VOU, DIV6_DSIT, DIV6_DSI0P, DIV6_DSI1P,
        DIV6_NR };
 
@@ -367,8 +376,6 @@ static struct clk div6_clks[DIV6_NR] = {
         [DIV6_VCK3] = SH_CLK_DIV6(&pllc1_div2_clk, VCLKCR3, 0),
         [DIV6_FMSI] = SH_CLK_DIV6(&pllc1_div2_clk, FMSICKCR, 0),
         [DIV6_FMSO] = SH_CLK_DIV6(&pllc1_div2_clk, FMSOCKCR, 0),
-        [DIV6_FSIA] = SH_CLK_DIV6(&pllc1_div2_clk, FSIACKCR, 0),
-        [DIV6_FSIB] = SH_CLK_DIV6(&pllc1_div2_clk, FSIBCKCR, 0),
         [DIV6_SUB] = SH_CLK_DIV6(&sh7372_extal2_clk, SUBCKCR, 0),
         [DIV6_SPU] = SH_CLK_DIV6(&pllc1_div2_clk, SPUCKCR, 0),
         [DIV6_VOU] = SH_CLK_DIV6(&pllc1_div2_clk, VOUCKCR, 0),
@@ -377,24 +384,42 @@ static struct clk div6_clks[DIV6_NR] = {
         [DIV6_DSI1P] = SH_CLK_DIV6(&pllc1_div2_clk, DSI1PCKCR, 0),
 };
 
-enum { DIV6_HDMI, DIV6_REPARENT_NR };
+enum { DIV6_HDMI, DIV6_FSIA, DIV6_FSIB, DIV6_REPARENT_NR };
 
 /* Indices are important - they are the actual src selecting values */
 static struct clk *hdmi_parent[] = {
         [0] = &pllc1_div2_clk,
-        [1] = &pllc2_clk,
-        [2] = &dv_clki_clk,
+        [1] = &sh7372_pllc2_clk,
+        [2] = &sh7372_dv_clki_clk,
         [3] = NULL,     /* pllc2_div4 not implemented yet */
 };
 
+static struct clk *fsiackcr_parent[] = {
+        [0] = &pllc1_div2_clk,
+        [1] = &sh7372_pllc2_clk,
+        [2] = &sh7372_fsiack_clk, /* external input for FSI A */
+        [3] = NULL,     /* setting prohibited */
+};
+
+static struct clk *fsibckcr_parent[] = {
+        [0] = &pllc1_div2_clk,
+        [1] = &sh7372_pllc2_clk,
+        [2] = &sh7372_fsibck_clk, /* external input for FSI B */
+        [3] = NULL,     /* setting prohibited */
+};
+
 static struct clk div6_reparent_clks[DIV6_REPARENT_NR] = {
         [DIV6_HDMI] = SH_CLK_DIV6_EXT(&pllc1_div2_clk, HDMICKCR, 0,
                                       hdmi_parent, ARRAY_SIZE(hdmi_parent), 6, 2),
+        [DIV6_FSIA] = SH_CLK_DIV6_EXT(&pllc1_div2_clk, FSIACKCR, 0,
+                                      fsiackcr_parent, ARRAY_SIZE(fsiackcr_parent), 6, 2),
+        [DIV6_FSIB] = SH_CLK_DIV6_EXT(&pllc1_div2_clk, FSIBCKCR, 0,
+                                      fsibckcr_parent, ARRAY_SIZE(fsibckcr_parent), 6, 2),
 };
 
 enum { MSTP001,
        MSTP131, MSTP130,
-       MSTP129, MSTP128, MSTP127, MSTP126,
+       MSTP129, MSTP128, MSTP127, MSTP126, MSTP125,
        MSTP118, MSTP117, MSTP116,
        MSTP106, MSTP101, MSTP100,
        MSTP223,
@@ -414,6 +439,7 @@ static struct clk mstp_clks[MSTP_NR] = {
         [MSTP128] = MSTP(&div4_clks[DIV4_B], SMSTPCR1, 28, 0), /* VEU0 */
         [MSTP127] = MSTP(&div4_clks[DIV4_B], SMSTPCR1, 27, 0), /* CEU */
         [MSTP126] = MSTP(&div4_clks[DIV4_B], SMSTPCR1, 26, 0), /* CSI2 */
+        [MSTP125] = MSTP(&div6_clks[DIV6_SUB], SMSTPCR1, 25, 0), /* TMU0 */
         [MSTP118] = MSTP(&div4_clks[DIV4_B], SMSTPCR1, 18, 0), /* DSITX */
         [MSTP117] = MSTP(&div4_clks[DIV4_B], SMSTPCR1, 17, 0), /* LCDC1 */
         [MSTP116] = MSTP(&div6_clks[DIV6_SUB], SMSTPCR1, 16, 0), /* IIC0 */
@@ -429,7 +455,7 @@ static struct clk mstp_clks[MSTP_NR] = {
         [MSTP201] = MSTP(&div6_clks[DIV6_SUB], SMSTPCR2, 1, 0), /* SCIFA3 */
         [MSTP200] = MSTP(&div6_clks[DIV6_SUB], SMSTPCR2, 0, 0), /* SCIFA4 */
         [MSTP329] = MSTP(&r_clk, SMSTPCR3, 29, 0), /* CMT10 */
-        [MSTP328] = MSTP(&div6_clks[DIV6_SPU], SMSTPCR3, 28, 0), /* FSIA */
+        [MSTP328] = MSTP(&div6_clks[DIV6_SPU], SMSTPCR3, 28, 0), /* FSI2 */
         [MSTP323] = MSTP(&div6_clks[DIV6_SUB], SMSTPCR3, 23, 0), /* IIC1 */
         [MSTP322] = MSTP(&div6_clks[DIV6_SUB], SMSTPCR3, 22, 0), /* USB0 */
         [MSTP314] = MSTP(&div4_clks[DIV4_HP], SMSTPCR3, 14, 0), /* SDHI0 */
@@ -445,10 +471,11 @@ static struct clk mstp_clks[MSTP_NR] = {
 
 #define CLKDEV_CON_ID(_id, _clk) { .con_id = _id, .clk = _clk }
 #define CLKDEV_DEV_ID(_id, _clk) { .dev_id = _id, .clk = _clk }
+#define CLKDEV_ICK_ID(_cid, _did, _clk) { .con_id = _cid, .dev_id = _did, .clk = _clk }
 
 static struct clk_lookup lookups[] = {
         /* main clocks */
-        CLKDEV_CON_ID("dv_clki_div2_clk", &dv_clki_div2_clk),
+        CLKDEV_CON_ID("dv_clki_div2_clk", &sh7372_dv_clki_div2_clk),
         CLKDEV_CON_ID("r_clk", &r_clk),
         CLKDEV_CON_ID("extal1", &sh7372_extal1_clk),
         CLKDEV_CON_ID("extal2", &sh7372_extal2_clk),
@@ -458,7 +485,7 @@ static struct clk_lookup lookups[] = {
         CLKDEV_CON_ID("pllc0_clk", &pllc0_clk),
         CLKDEV_CON_ID("pllc1_clk", &pllc1_clk),
         CLKDEV_CON_ID("pllc1_div2_clk", &pllc1_div2_clk),
-        CLKDEV_CON_ID("pllc2_clk", &pllc2_clk),
+        CLKDEV_CON_ID("pllc2_clk", &sh7372_pllc2_clk),
 
         /* DIV4 clocks */
         CLKDEV_CON_ID("i_clk", &div4_clks[DIV4_I]),
@@ -483,8 +510,8 @@ static struct clk_lookup lookups[] = {
         CLKDEV_CON_ID("vck3_clk", &div6_clks[DIV6_VCK3]),
         CLKDEV_CON_ID("fmsi_clk", &div6_clks[DIV6_FMSI]),
         CLKDEV_CON_ID("fmso_clk", &div6_clks[DIV6_FMSO]),
-        CLKDEV_CON_ID("fsia_clk", &div6_clks[DIV6_FSIA]),
-        CLKDEV_CON_ID("fsib_clk", &div6_clks[DIV6_FSIB]),
+        CLKDEV_CON_ID("fsia_clk", &div6_reparent_clks[DIV6_FSIA]),
+        CLKDEV_CON_ID("fsib_clk", &div6_reparent_clks[DIV6_FSIB]),
         CLKDEV_CON_ID("sub_clk", &div6_clks[DIV6_SUB]),
         CLKDEV_CON_ID("spu_clk", &div6_clks[DIV6_SPU]),
         CLKDEV_CON_ID("vou_clk", &div6_clks[DIV6_VOU]),
@@ -501,6 +528,8 @@ static struct clk_lookup lookups[] = {
         CLKDEV_DEV_ID("uio_pdrv_genirq.1", &mstp_clks[MSTP128]), /* VEU0 */
         CLKDEV_DEV_ID("sh_mobile_ceu.0", &mstp_clks[MSTP127]), /* CEU */
         CLKDEV_DEV_ID("sh-mobile-csi2.0", &mstp_clks[MSTP126]), /* CSI2 */
+        CLKDEV_DEV_ID("sh_tmu.0", &mstp_clks[MSTP125]), /* TMU00 */
+        CLKDEV_DEV_ID("sh_tmu.1", &mstp_clks[MSTP125]), /* TMU01 */
         CLKDEV_DEV_ID("sh-mipi-dsi.0", &mstp_clks[MSTP118]), /* DSITX */
         CLKDEV_DEV_ID("sh_mobile_lcdc_fb.1", &mstp_clks[MSTP117]), /* LCDC1 */
         CLKDEV_DEV_ID("i2c-sh_mobile.0", &mstp_clks[MSTP116]), /* IIC0 */
@@ -516,7 +545,7 @@ static struct clk_lookup lookups[] = {
         CLKDEV_DEV_ID("sh-sci.2", &mstp_clks[MSTP202]), /* SCIFA2 */
         CLKDEV_DEV_ID("sh-sci.3", &mstp_clks[MSTP201]), /* SCIFA3 */
         CLKDEV_DEV_ID("sh-sci.4", &mstp_clks[MSTP200]), /* SCIFA4 */
-        CLKDEV_CON_ID("cmt1", &mstp_clks[MSTP329]), /* CMT10 */
+        CLKDEV_DEV_ID("sh_cmt.10", &mstp_clks[MSTP329]), /* CMT10 */
         CLKDEV_DEV_ID("sh_fsi2", &mstp_clks[MSTP328]), /* FSI2 */
         CLKDEV_DEV_ID("i2c-sh_mobile.1", &mstp_clks[MSTP323]), /* IIC1 */
         CLKDEV_DEV_ID("r8a66597_hcd.0", &mstp_clks[MSTP323]), /* USB0 */
@@ -531,7 +560,10 @@ static struct clk_lookup lookups[] = {
         CLKDEV_DEV_ID("r8a66597_hcd.1", &mstp_clks[MSTP406]), /* USB1 */
         CLKDEV_DEV_ID("r8a66597_udc.1", &mstp_clks[MSTP406]), /* USB1 */
         CLKDEV_DEV_ID("sh_keysc.0", &mstp_clks[MSTP403]), /* KEYSC */
-        {.con_id = "ick", .dev_id = "sh-mobile-hdmi", .clk = &div6_reparent_clks[DIV6_HDMI]},
+
+        CLKDEV_ICK_ID("ick", "sh-mobile-hdmi", &div6_reparent_clks[DIV6_HDMI]),
+        CLKDEV_ICK_ID("icka", "sh_fsi2", &div6_reparent_clks[DIV6_FSIA]),
+        CLKDEV_ICK_ID("ickb", "sh_fsi2", &div6_reparent_clks[DIV6_FSIB]),
 };
 
 void __init sh7372_clock_init(void)
@@ -548,7 +580,7 @@ void __init sh7372_clock_init(void)
                 ret = sh_clk_div6_register(div6_clks, DIV6_NR);
 
         if (!ret)
-                ret = sh_clk_div6_reparent_register(div6_reparent_clks, DIV6_NR);
+                ret = sh_clk_div6_reparent_register(div6_reparent_clks, DIV6_REPARENT_NR);
 
         if (!ret)
                 ret = sh_clk_mstp32_register(mstp_clks, MSTP_NR);
diff --git a/arch/arm/mach-shmobile/clock-sh7377.c b/arch/arm/mach-shmobile/clock-sh7377.c
index e007c28cf0a8..f91395aeb9ab 100644
--- a/arch/arm/mach-shmobile/clock-sh7377.c
+++ b/arch/arm/mach-shmobile/clock-sh7377.c
@@ -333,7 +333,7 @@ static struct clk_lookup lookups[] = {
         CLKDEV_DEV_ID("sh-sci.3", &mstp_clks[MSTP201]), /* SCIFA3 */
         CLKDEV_DEV_ID("sh-sci.4", &mstp_clks[MSTP200]), /* SCIFA4 */
         CLKDEV_DEV_ID("sh-sci.6", &mstp_clks[MSTP331]), /* SCIFA6 */
-        CLKDEV_CON_ID("cmt1", &mstp_clks[MSTP329]), /* CMT10 */
+        CLKDEV_DEV_ID("sh_cmt.10", &mstp_clks[MSTP329]), /* CMT10 */
         CLKDEV_DEV_ID("sh_irda", &mstp_clks[MSTP325]), /* IRDA */
         CLKDEV_DEV_ID("i2c-sh_mobile.1", &mstp_clks[MSTP323]), /* IIC1 */
         CLKDEV_DEV_ID("r8a66597_hcd.0", &mstp_clks[MSTP322]), /* USBHS */
diff --git a/arch/arm/mach-shmobile/include/mach/sh7372.h b/arch/arm/mach-shmobile/include/mach/sh7372.h
index 33e9700ded7e..147775a94bce 100644
--- a/arch/arm/mach-shmobile/include/mach/sh7372.h
+++ b/arch/arm/mach-shmobile/include/mach/sh7372.h
@@ -457,8 +457,12 @@ enum {
         SHDMA_SLAVE_SDHI2_TX,
 };
 
-extern struct clk dv_clki_clk;
-extern struct clk dv_clki_div2_clk;
-extern struct clk pllc2_clk;
+extern struct clk sh7372_extal1_clk;
+extern struct clk sh7372_extal2_clk;
+extern struct clk sh7372_dv_clki_clk;
+extern struct clk sh7372_dv_clki_div2_clk;
+extern struct clk sh7372_pllc2_clk;
+extern struct clk sh7372_fsiack_clk;
+extern struct clk sh7372_fsibck_clk;
 
 #endif /* __ASM_SH7372_H__ */
diff --git a/arch/arm/mach-shmobile/intc-sh7372.c b/arch/arm/mach-shmobile/intc-sh7372.c
index e3551b56cd03..4cd3cae38e72 100644
--- a/arch/arm/mach-shmobile/intc-sh7372.c
+++ b/arch/arm/mach-shmobile/intc-sh7372.c
@@ -369,9 +369,13 @@ enum {
         INTCS,
 
         /* interrupt sources INTCS */
+
+        /* IRQ0S - IRQ31S */
         VEU_VEU0, VEU_VEU1, VEU_VEU2, VEU_VEU3,
         RTDMAC_1_DEI0, RTDMAC_1_DEI1, RTDMAC_1_DEI2, RTDMAC_1_DEI3,
         CEU, BEU_BEU0, BEU_BEU1, BEU_BEU2,
+        /* MFI */
+        /* BBIF2 */
         VPU,
         TSIF1,
         _3DG_SGX530,
@@ -379,13 +383,17 @@ enum {
         IIC2_ALI2, IIC2_TACKI2, IIC2_WAITI2, IIC2_DTEI2,
         IPMMU_IPMMUR, IPMMU_IPMMUR2,
         RTDMAC_2_DEI4, RTDMAC_2_DEI5, RTDMAC_2_DADERR,
+        /* KEYSC */
+        /* TTI20 */
         MSIOF,
         IIC0_ALI0, IIC0_TACKI0, IIC0_WAITI0, IIC0_DTEI0,
         TMU_TUNI0, TMU_TUNI1, TMU_TUNI2,
         CMT0,
         TSIF0,
+        /* CMT2 */
         LMB,
         CTI,
+        /* RWDT0 */
         ICB,
         JPU_JPEG,
         LCDC,
@@ -397,11 +405,17 @@ enum {
         CSIRX,
         DSITX_DSITX0,
         DSITX_DSITX1,
+        /* SPU2 */
+        /* FSI */
+        /* FMSI */
+        /* HDMI */
         TMU1_TUNI0, TMU1_TUNI1, TMU1_TUNI2,
         CMT4,
         DSITX1_DSITX1_0,
         DSITX1_DSITX1_1,
+        /* MFIS2 */
         CPORTS2R,
+        /* CEC */
         JPU6E,
 
         /* interrupt groups INTCS */
@@ -410,12 +424,15 @@ enum {
 };
 
 static struct intc_vect intcs_vectors[] = {
+        /* IRQ0S - IRQ31S */
         INTCS_VECT(VEU_VEU0, 0x700), INTCS_VECT(VEU_VEU1, 0x720),
         INTCS_VECT(VEU_VEU2, 0x740), INTCS_VECT(VEU_VEU3, 0x760),
         INTCS_VECT(RTDMAC_1_DEI0, 0x800), INTCS_VECT(RTDMAC_1_DEI1, 0x820),
         INTCS_VECT(RTDMAC_1_DEI2, 0x840), INTCS_VECT(RTDMAC_1_DEI3, 0x860),
         INTCS_VECT(CEU, 0x880), INTCS_VECT(BEU_BEU0, 0x8a0),
         INTCS_VECT(BEU_BEU1, 0x8c0), INTCS_VECT(BEU_BEU2, 0x8e0),
+        /* MFI */
+        /* BBIF2 */
         INTCS_VECT(VPU, 0x980),
         INTCS_VECT(TSIF1, 0x9a0),
         INTCS_VECT(_3DG_SGX530, 0x9e0),
@@ -425,14 +442,19 @@ static struct intc_vect intcs_vectors[] = {
         INTCS_VECT(IPMMU_IPMMUR, 0xb00), INTCS_VECT(IPMMU_IPMMUR2, 0xb20),
         INTCS_VECT(RTDMAC_2_DEI4, 0xb80), INTCS_VECT(RTDMAC_2_DEI5, 0xba0),
         INTCS_VECT(RTDMAC_2_DADERR, 0xbc0),
+        /* KEYSC */
+        /* TTI20 */
+        INTCS_VECT(MSIOF, 0x0d20),
         INTCS_VECT(IIC0_ALI0, 0xe00), INTCS_VECT(IIC0_TACKI0, 0xe20),
         INTCS_VECT(IIC0_WAITI0, 0xe40), INTCS_VECT(IIC0_DTEI0, 0xe60),
         INTCS_VECT(TMU_TUNI0, 0xe80), INTCS_VECT(TMU_TUNI1, 0xea0),
         INTCS_VECT(TMU_TUNI2, 0xec0),
         INTCS_VECT(CMT0, 0xf00),
         INTCS_VECT(TSIF0, 0xf20),
+        /* CMT2 */
         INTCS_VECT(LMB, 0xf60),
         INTCS_VECT(CTI, 0x400),
+        /* RWDT0 */
         INTCS_VECT(ICB, 0x480),
         INTCS_VECT(JPU_JPEG, 0x560),
         INTCS_VECT(LCDC, 0x580),
@@ -446,12 +468,18 @@ static struct intc_vect intcs_vectors[] = {
         INTCS_VECT(CSIRX, 0x17a0),
         INTCS_VECT(DSITX_DSITX0, 0x17c0),
         INTCS_VECT(DSITX_DSITX1, 0x17e0),
+        /* SPU2 */
+        /* FSI */
+        /* FMSI */
+        /* HDMI */
         INTCS_VECT(TMU1_TUNI0, 0x1900), INTCS_VECT(TMU1_TUNI1, 0x1920),
         INTCS_VECT(TMU1_TUNI2, 0x1940),
         INTCS_VECT(CMT4, 0x1980),
         INTCS_VECT(DSITX1_DSITX1_0, 0x19a0),
         INTCS_VECT(DSITX1_DSITX1_1, 0x19c0),
+        /* MFIS2 */
         INTCS_VECT(CPORTS2R, 0x1a20),
+        /* CEC */
         INTCS_VECT(JPU6E, 0x1a80),
 
         INTC_VECT(INTCS, 0xf80),
diff --git a/arch/arm/mach-shmobile/pfc-sh7372.c b/arch/arm/mach-shmobile/pfc-sh7372.c
index ec420353f8e3..9c265dae138a 100644
--- a/arch/arm/mach-shmobile/pfc-sh7372.c
+++ b/arch/arm/mach-shmobile/pfc-sh7372.c
@@ -166,12 +166,12 @@ enum {
         MSIOF2_TSYNC_MARK,      MSIOF2_TSCK_MARK,       MSIOF2_RXD_MARK,
         MSIOF2_TXD_MARK,
 
-        /* MSIOF3 */
+        /* BBIF1 */
         BBIF1_RXD_MARK,         BBIF1_TSYNC_MARK,       BBIF1_TSCK_MARK,
         BBIF1_TXD_MARK,         BBIF1_RSCK_MARK,        BBIF1_RSYNC_MARK,
         BBIF1_FLOW_MARK,        BB_RX_FLOW_N_MARK,
 
-        /* MSIOF4 */
+        /* BBIF2 */
         BBIF2_TSCK1_MARK,       BBIF2_TSYNC1_MARK,
         BBIF2_TXD1_MARK,        BBIF2_RXD_MARK,
 
@@ -976,12 +976,12 @@ static struct pinmux_gpio pinmux_gpios[] = {
         GPIO_FN(MSIOF2_TSYNC),  GPIO_FN(MSIOF2_TSCK),   GPIO_FN(MSIOF2_RXD),
         GPIO_FN(MSIOF2_TXD),
 
-        /* MSIOF3 */
+        /* BBIF1 */
         GPIO_FN(BBIF1_RXD),     GPIO_FN(BBIF1_TSYNC),   GPIO_FN(BBIF1_TSCK),
         GPIO_FN(BBIF1_TXD),     GPIO_FN(BBIF1_RSCK),    GPIO_FN(BBIF1_RSYNC),
         GPIO_FN(BBIF1_FLOW),    GPIO_FN(BB_RX_FLOW_N),
 
-        /* MSIOF4 */
+        /* BBIF2 */
         GPIO_FN(BBIF2_TSCK1),   GPIO_FN(BBIF2_TSYNC1),
         GPIO_FN(BBIF2_TXD1),    GPIO_FN(BBIF2_RXD),
 
diff --git a/arch/arm/mach-shmobile/setup-sh7367.c b/arch/arm/mach-shmobile/setup-sh7367.c
index 3148c11a550e..003008c18360 100644
--- a/arch/arm/mach-shmobile/setup-sh7367.c
+++ b/arch/arm/mach-shmobile/setup-sh7367.c
@@ -154,7 +154,6 @@ static struct sh_timer_config cmt10_platform_data = {
         .name = "CMT10",
         .channel_offset = 0x10,
         .timer_bit = 0,
-        .clk = "r_clk",
         .clockevent_rating = 125,
         .clocksource_rating = 125,
 };
diff --git a/arch/arm/mach-shmobile/setup-sh7372.c b/arch/arm/mach-shmobile/setup-sh7372.c
index e26686c9d0b6..564a6d0be473 100644
--- a/arch/arm/mach-shmobile/setup-sh7372.c
+++ b/arch/arm/mach-shmobile/setup-sh7372.c
@@ -158,7 +158,6 @@ static struct sh_timer_config cmt10_platform_data = {
         .name = "CMT10",
         .channel_offset = 0x10,
         .timer_bit = 0,
-        .clk = "cmt1",
         .clockevent_rating = 125,
         .clocksource_rating = 125,
 };
@@ -186,6 +185,67 @@ static struct platform_device cmt10_device = {
         .num_resources  = ARRAY_SIZE(cmt10_resources),
 };
 
+/* TMU */
+static struct sh_timer_config tmu00_platform_data = {
+        .name = "TMU00",
+        .channel_offset = 0x4,
+        .timer_bit = 0,
+        .clockevent_rating = 200,
+};
+
+static struct resource tmu00_resources[] = {
+        [0] = {
+                .name   = "TMU00",
+                .start  = 0xfff60008,
+                .end    = 0xfff60013,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = intcs_evt2irq(0xe80), /* TMU_TUNI0 */
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+static struct platform_device tmu00_device = {
+        .name           = "sh_tmu",
+        .id             = 0,
+        .dev = {
+                .platform_data  = &tmu00_platform_data,
+        },
+        .resource       = tmu00_resources,
+        .num_resources  = ARRAY_SIZE(tmu00_resources),
+};
+
+static struct sh_timer_config tmu01_platform_data = {
+        .name = "TMU01",
+        .channel_offset = 0x10,
+        .timer_bit = 1,
+        .clocksource_rating = 200,
+};
+
+static struct resource tmu01_resources[] = {
+        [0] = {
+                .name   = "TMU01",
+                .start  = 0xfff60014,
+                .end    = 0xfff6001f,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = intcs_evt2irq(0xea0), /* TMU_TUNI1 */
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+static struct platform_device tmu01_device = {
+        .name           = "sh_tmu",
+        .id             = 1,
+        .dev = {
+                .platform_data  = &tmu01_platform_data,
+        },
+        .resource       = tmu01_resources,
+        .num_resources  = ARRAY_SIZE(tmu01_resources),
+};
+
 /* I2C */
 static struct resource iic0_resources[] = {
         [0] = {
@@ -419,14 +479,14 @@ static struct resource sh7372_dmae0_resources[] = {
         },
         {
                 /* DMA error IRQ */
-                .start  = 246,
-                .end    = 246,
+                .start  = evt2irq(0x20c0),
+                .end    = evt2irq(0x20c0),
                 .flags  = IORESOURCE_IRQ,
         },
         {
                 /* IRQ for channels 0-5 */
-                .start  = 240,
-                .end    = 245,
+                .start  = evt2irq(0x2000),
+                .end    = evt2irq(0x20a0),
                 .flags  = IORESOURCE_IRQ,
         },
 };
@@ -447,14 +507,14 @@ static struct resource sh7372_dmae1_resources[] = {
         },
         {
                 /* DMA error IRQ */
-                .start  = 254,
-                .end    = 254,
+                .start  = evt2irq(0x21c0),
+                .end    = evt2irq(0x21c0),
                 .flags  = IORESOURCE_IRQ,
         },
         {
                 /* IRQ for channels 0-5 */
-                .start  = 248,
-                .end    = 253,
+                .start  = evt2irq(0x2100),
+                .end    = evt2irq(0x21a0),
                 .flags  = IORESOURCE_IRQ,
         },
 };
@@ -475,14 +535,14 @@ static struct resource sh7372_dmae2_resources[] = {
         },
         {
                 /* DMA error IRQ */
-                .start  = 262,
-                .end    = 262,
+                .start  = evt2irq(0x22c0),
+                .end    = evt2irq(0x22c0),
                 .flags  = IORESOURCE_IRQ,
         },
         {
                 /* IRQ for channels 0-5 */
-                .start  = 256,
-                .end    = 261,
+                .start  = evt2irq(0x2200),
+                .end    = evt2irq(0x22a0),
                 .flags  = IORESOURCE_IRQ,
         },
 };
@@ -526,6 +586,11 @@ static struct platform_device *sh7372_early_devices[] __initdata = {
         &scif5_device,
         &scif6_device,
         &cmt10_device,
+        &tmu00_device,
+        &tmu01_device,
+};
+
+static struct platform_device *sh7372_late_devices[] __initdata = {
         &iic0_device,
         &iic1_device,
         &dma0_device,
@@ -537,6 +602,9 @@ void __init sh7372_add_standard_devices(void)
 {
         platform_add_devices(sh7372_early_devices,
                             ARRAY_SIZE(sh7372_early_devices));
+
+        platform_add_devices(sh7372_late_devices,
+                            ARRAY_SIZE(sh7372_late_devices));
 }
 
 void __init sh7372_add_early_devices(void)
diff --git a/arch/arm/mach-shmobile/setup-sh7377.c b/arch/arm/mach-shmobile/setup-sh7377.c
index bb4adf17dbf4..575dbd6c2f1d 100644
--- a/arch/arm/mach-shmobile/setup-sh7377.c
+++ b/arch/arm/mach-shmobile/setup-sh7377.c
@@ -172,7 +172,6 @@ static struct sh_timer_config cmt10_platform_data = {
         .name = "CMT10",
         .channel_offset = 0x10,
         .timer_bit = 0,
-        .clk = "r_clk",
         .clockevent_rating = 125,
         .clocksource_rating = 125,
 };
diff --git a/arch/arm/mach-ux500/cpu.c b/arch/arm/mach-ux500/cpu.c
index e0fd747e447a..73fb1a551ec6 100644
--- a/arch/arm/mach-ux500/cpu.c
+++ b/arch/arm/mach-ux500/cpu.c
@@ -10,6 +10,7 @@
 #include <linux/io.h>
 #include <linux/clk.h>
 
+#include <asm/cacheflush.h>
 #include <asm/hardware/cache-l2x0.h>
 #include <asm/hardware/gic.h>
 #include <asm/mach/map.h>
@@ -71,6 +72,46 @@ void __init ux500_init_irq(void)
 }
 
 #ifdef CONFIG_CACHE_L2X0
+static inline void ux500_cache_wait(void __iomem *reg, unsigned long mask)
+{
+        /* wait for the operation to complete */
+        while (readl(reg) & mask)
+                ;
+}
+
+static inline void ux500_cache_sync(void)
+{
+        void __iomem *base = __io_address(UX500_L2CC_BASE);
+        writel(0, base + L2X0_CACHE_SYNC);
+        ux500_cache_wait(base + L2X0_CACHE_SYNC, 1);
+}
+
+/*
+ * The L2 cache cannot be turned off in the non-secure world.
+ * Dummy until a secure service is in place.
+ */
+static void ux500_l2x0_disable(void)
+{
+}
+
+/*
+ * This is only called when doing a kexec, just after turning off the L2
+ * and L1 cache, and it is surrounded by a spinlock in the generic version.
+ * However, we're not really turning off the L2 cache right now and the
+ * PL310 does not support exclusive accesses (used to implement the spinlock).
+ * So, the invalidation needs to be done without the spinlock.
+ */
+static void ux500_l2x0_inv_all(void)
+{
+        void __iomem *l2x0_base = __io_address(UX500_L2CC_BASE);
+        uint32_t l2x0_way_mask = (1<<16) - 1;   /* Bitmask of active ways */
+
+        /* invalidate all ways */
+        writel(l2x0_way_mask, l2x0_base + L2X0_INV_WAY);
+        ux500_cache_wait(l2x0_base + L2X0_INV_WAY, l2x0_way_mask);
+        ux500_cache_sync();
+}
+
 static int ux500_l2x0_init(void)
 {
         void __iomem *l2x0_base;
@@ -80,6 +121,10 @@ static int ux500_l2x0_init(void)
         /* 64KB way size, 8 way associativity, force WA */
         l2x0_init(l2x0_base, 0x3e060000, 0xc0000fff);
 
+        /* Override invalidate function */
+        outer_cache.disable = ux500_l2x0_disable;
+        outer_cache.inv_all = ux500_l2x0_inv_all;
+
         return 0;
 }
 early_initcall(ux500_l2x0_init);
diff --git a/arch/arm/mm/Kconfig b/arch/arm/mm/Kconfig
index a0a2928ae4dd..4414a01e1e8a 100644
--- a/arch/arm/mm/Kconfig
+++ b/arch/arm/mm/Kconfig
@@ -779,6 +779,14 @@ config CACHE_L2X0
         help
           This option enables the L2x0 PrimeCell.
 
+config CACHE_PL310
+        bool
+        depends on CACHE_L2X0
+        default y if CPU_V7 && !CPU_V6
+        help
+          This option enables optimisations for the PL310 cache
+          controller.
+
 config CACHE_TAUROS2
         bool "Enable the Tauros2 L2 cache controller"
         depends on (ARCH_DOVE || ARCH_MMP)
diff --git a/arch/arm/mm/cache-fa.S b/arch/arm/mm/cache-fa.S
index 7148e53e6078..1fa6f71470de 100644
--- a/arch/arm/mm/cache-fa.S
+++ b/arch/arm/mm/cache-fa.S
@@ -38,6 +38,17 @@
 #define CACHE_DLIMIT    (CACHE_DSIZE * 2)
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(fa_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(fa_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Clean and invalidate all cache entries in a particular address
@@ -233,6 +244,7 @@ ENDPROC(fa_dma_unmap_area)
 
         .type   fa_cache_fns, #object
 ENTRY(fa_cache_fns)
+        .long   fa_flush_icache_all
         .long   fa_flush_kern_cache_all
         .long   fa_flush_user_cache_all
         .long   fa_flush_user_cache_range
diff --git a/arch/arm/mm/cache-l2x0.c b/arch/arm/mm/cache-l2x0.c
index 9982eb385c0f..170c9bb95866 100644
--- a/arch/arm/mm/cache-l2x0.c
+++ b/arch/arm/mm/cache-l2x0.c
@@ -28,14 +28,24 @@
 static void __iomem *l2x0_base;
 static DEFINE_SPINLOCK(l2x0_lock);
 static uint32_t l2x0_way_mask;  /* Bitmask of active ways */
+static uint32_t l2x0_size;
 
-static inline void cache_wait(void __iomem *reg, unsigned long mask)
+static inline void cache_wait_way(void __iomem *reg, unsigned long mask)
 {
-        /* wait for the operation to complete */
+        /* wait for cache operation by line or way to complete */
         while (readl_relaxed(reg) & mask)
                 ;
 }
 
+#ifdef CONFIG_CACHE_PL310
+static inline void cache_wait(void __iomem *reg, unsigned long mask)
+{
+        /* cache operations by line are atomic on PL310 */
+}
+#else
+#define cache_wait      cache_wait_way
+#endif
+
 static inline void cache_sync(void)
 {
         void __iomem *base = l2x0_base;
@@ -103,14 +113,40 @@ static void l2x0_cache_sync(void)
         spin_unlock_irqrestore(&l2x0_lock, flags);
 }
 
-static inline void l2x0_inv_all(void)
+static void l2x0_flush_all(void)
+{
+        unsigned long flags;
+
+        /* clean all ways */
+        spin_lock_irqsave(&l2x0_lock, flags);
+        writel_relaxed(l2x0_way_mask, l2x0_base + L2X0_CLEAN_INV_WAY);
+        cache_wait_way(l2x0_base + L2X0_CLEAN_INV_WAY, l2x0_way_mask);
+        cache_sync();
+        spin_unlock_irqrestore(&l2x0_lock, flags);
+}
+
+static void l2x0_clean_all(void)
+{
+        unsigned long flags;
+
+        /* clean all ways */
+        spin_lock_irqsave(&l2x0_lock, flags);
+        writel_relaxed(l2x0_way_mask, l2x0_base + L2X0_CLEAN_WAY);
+        cache_wait_way(l2x0_base + L2X0_CLEAN_WAY, l2x0_way_mask);
+        cache_sync();
+        spin_unlock_irqrestore(&l2x0_lock, flags);
+}
+
+static void l2x0_inv_all(void)
 {
         unsigned long flags;
 
         /* invalidate all ways */
         spin_lock_irqsave(&l2x0_lock, flags);
+        /* Invalidating when L2 is enabled is a nono */
+        BUG_ON(readl(l2x0_base + L2X0_CTRL) & 1);
         writel_relaxed(l2x0_way_mask, l2x0_base + L2X0_INV_WAY);
-        cache_wait(l2x0_base + L2X0_INV_WAY, l2x0_way_mask);
+        cache_wait_way(l2x0_base + L2X0_INV_WAY, l2x0_way_mask);
         cache_sync();
         spin_unlock_irqrestore(&l2x0_lock, flags);
 }
@@ -159,6 +195,11 @@ static void l2x0_clean_range(unsigned long start, unsigned long end)
         void __iomem *base = l2x0_base;
         unsigned long flags;
 
+        if ((end - start) >= l2x0_size) {
+                l2x0_clean_all();
+                return;
+        }
+
         spin_lock_irqsave(&l2x0_lock, flags);
         start &= ~(CACHE_LINE_SIZE - 1);
         while (start < end) {
@@ -184,6 +225,11 @@ static void l2x0_flush_range(unsigned long start, unsigned long end)
         void __iomem *base = l2x0_base;
         unsigned long flags;
 
+        if ((end - start) >= l2x0_size) {
+                l2x0_flush_all();
+                return;
+        }
+
         spin_lock_irqsave(&l2x0_lock, flags);
         start &= ~(CACHE_LINE_SIZE - 1);
         while (start < end) {
@@ -206,10 +252,20 @@ static void l2x0_flush_range(unsigned long start, unsigned long end)
         spin_unlock_irqrestore(&l2x0_lock, flags);
 }
 
+static void l2x0_disable(void)
+{
+        unsigned long flags;
+
+        spin_lock_irqsave(&l2x0_lock, flags);
+        writel(0, l2x0_base + L2X0_CTRL);
+        spin_unlock_irqrestore(&l2x0_lock, flags);
+}
+
 void __init l2x0_init(void __iomem *base, __u32 aux_val, __u32 aux_mask)
 {
         __u32 aux;
         __u32 cache_id;
+        __u32 way_size = 0;
         int ways;
         const char *type;
 
@@ -244,6 +300,13 @@ void __init l2x0_init(void __iomem *base, __u32 aux_val, __u32 aux_mask)
         l2x0_way_mask = (1 << ways) - 1;
 
         /*
+         * L2 cache Size =  Way size * Number of ways
+         */
+        way_size = (aux & L2X0_AUX_CTRL_WAY_SIZE_MASK) >> 17;
+        way_size = 1 << (way_size + 3);
+        l2x0_size = ways * way_size * SZ_1K;
+
+        /*
          * Check if l2x0 controller is already enabled.
          * If you are booting from non-secure mode
          * accessing the below registers will fault.
@@ -263,8 +326,11 @@ void __init l2x0_init(void __iomem *base, __u32 aux_val, __u32 aux_mask)
         outer_cache.clean_range = l2x0_clean_range;
         outer_cache.flush_range = l2x0_flush_range;
         outer_cache.sync = l2x0_cache_sync;
+        outer_cache.flush_all = l2x0_flush_all;
+        outer_cache.inv_all = l2x0_inv_all;
+        outer_cache.disable = l2x0_disable;
 
         printk(KERN_INFO "%s cache controller enabled\n", type);
-        printk(KERN_INFO "l2x0: %d ways, CACHE_ID 0x%08x, AUX_CTRL 0x%08x\n",
-                         ways, cache_id, aux);
+        printk(KERN_INFO "l2x0: %d ways, CACHE_ID 0x%08x, AUX_CTRL 0x%08x, Cache size: %d B\n",
+                        ways, cache_id, aux, l2x0_size);
 }
diff --git a/arch/arm/mm/cache-v3.S b/arch/arm/mm/cache-v3.S
index c2ff3c599fee..2e2bc406a18d 100644
--- a/arch/arm/mm/cache-v3.S
+++ b/arch/arm/mm/cache-v3.S
@@ -13,6 +13,15 @@
 #include "proc-macros.S"
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(v3_flush_icache_all)
+        mov     pc, lr
+ENDPROC(v3_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Invalidate all cache entries in a particular address
@@ -122,6 +131,7 @@ ENDPROC(v3_dma_map_area)
 
         .type   v3_cache_fns, #object
 ENTRY(v3_cache_fns)
+        .long   v3_flush_icache_all
         .long   v3_flush_kern_cache_all
         .long   v3_flush_user_cache_all
         .long   v3_flush_user_cache_range
diff --git a/arch/arm/mm/cache-v4.S b/arch/arm/mm/cache-v4.S
index 4810f7e3e813..a8fefb523f19 100644
--- a/arch/arm/mm/cache-v4.S
+++ b/arch/arm/mm/cache-v4.S
@@ -13,6 +13,15 @@
 #include "proc-macros.S"
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(v4_flush_icache_all)
+        mov     pc, lr
+ENDPROC(v4_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Invalidate all cache entries in a particular address
@@ -134,6 +143,7 @@ ENDPROC(v4_dma_map_area)
 
         .type   v4_cache_fns, #object
 ENTRY(v4_cache_fns)
+        .long   v4_flush_icache_all
         .long   v4_flush_kern_cache_all
         .long   v4_flush_user_cache_all
         .long   v4_flush_user_cache_range
diff --git a/arch/arm/mm/cache-v4wb.S b/arch/arm/mm/cache-v4wb.S
index df8368afa102..d3644db467b7 100644
--- a/arch/arm/mm/cache-v4wb.S
+++ b/arch/arm/mm/cache-v4wb.S
@@ -51,6 +51,17 @@ flush_base:
         .text
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(v4wb_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(v4wb_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Clean and invalidate all cache entries in a particular address
@@ -244,6 +255,7 @@ ENDPROC(v4wb_dma_unmap_area)
 
         .type   v4wb_cache_fns, #object
 ENTRY(v4wb_cache_fns)
+        .long   v4wb_flush_icache_all
         .long   v4wb_flush_kern_cache_all
         .long   v4wb_flush_user_cache_all
         .long   v4wb_flush_user_cache_range
diff --git a/arch/arm/mm/cache-v4wt.S b/arch/arm/mm/cache-v4wt.S
index 45c70312f43b..49c2b66cf3dd 100644
--- a/arch/arm/mm/cache-v4wt.S
+++ b/arch/arm/mm/cache-v4wt.S
@@ -41,6 +41,17 @@
 #define CACHE_DLIMIT    16384
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(v4wt_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(v4wt_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Invalidate all cache entries in a particular address
@@ -188,6 +199,7 @@ ENDPROC(v4wt_dma_map_area)
 
         .type   v4wt_cache_fns, #object
 ENTRY(v4wt_cache_fns)
+        .long   v4wt_flush_icache_all
         .long   v4wt_flush_kern_cache_all
         .long   v4wt_flush_user_cache_all
         .long   v4wt_flush_user_cache_range
diff --git a/arch/arm/mm/fault-armv.c b/arch/arm/mm/fault-armv.c
index c493d7244d3d..83e59f870426 100644
--- a/arch/arm/mm/fault-armv.c
+++ b/arch/arm/mm/fault-armv.c
@@ -66,6 +66,30 @@ static int do_adjust_pte(struct vm_area_struct *vma, unsigned long address,
         return ret;
 }
 
+#if USE_SPLIT_PTLOCKS
+/*
+ * If we are using split PTE locks, then we need to take the page
+ * lock here.  Otherwise we are using shared mm->page_table_lock
+ * which is already locked, thus cannot take it.
+ */
+static inline void do_pte_lock(spinlock_t *ptl)
+{
+        /*
+         * Use nested version here to indicate that we are already
+         * holding one similar spinlock.
+         */
+        spin_lock_nested(ptl, SINGLE_DEPTH_NESTING);
+}
+
+static inline void do_pte_unlock(spinlock_t *ptl)
+{
+        spin_unlock(ptl);
+}
+#else /* !USE_SPLIT_PTLOCKS */
+static inline void do_pte_lock(spinlock_t *ptl) {}
+static inline void do_pte_unlock(spinlock_t *ptl) {}
+#endif /* USE_SPLIT_PTLOCKS */
+
 static int adjust_pte(struct vm_area_struct *vma, unsigned long address,
         unsigned long pfn)
 {
@@ -90,11 +114,11 @@ static int adjust_pte(struct vm_area_struct *vma, unsigned long address,
          */
         ptl = pte_lockptr(vma->vm_mm, pmd);
         pte = pte_offset_map(pmd, address);
-        spin_lock(ptl);
+        do_pte_lock(ptl);
 
         ret = do_adjust_pte(vma, address, pfn, pte);
 
-        spin_unlock(ptl);
+        do_pte_unlock(ptl);
         pte_unmap(pte);
 
         return ret;
diff --git a/arch/arm/mm/init.c b/arch/arm/mm/init.c
index 7fd9b5eb177f..5164069ced42 100644
--- a/arch/arm/mm/init.c
+++ b/arch/arm/mm/init.c
@@ -18,6 +18,7 @@
 #include <linux/highmem.h>
 #include <linux/gfp.h>
 #include <linux/memblock.h>
+#include <linux/sort.h>
 
 #include <asm/mach-types.h>
 #include <asm/sections.h>
@@ -121,9 +122,10 @@ void show_mem(void)
         printk("%d pages swap cached\n", cached);
 }
 
-static void __init find_limits(struct meminfo *mi,
-        unsigned long *min, unsigned long *max_low, unsigned long *max_high)
+static void __init find_limits(unsigned long *min, unsigned long *max_low,
+        unsigned long *max_high)
 {
+        struct meminfo *mi = &meminfo;
         int i;
 
         *min = -1UL;
@@ -147,14 +149,13 @@ static void __init find_limits(struct meminfo *mi,
         }
 }
 
-static void __init arm_bootmem_init(struct meminfo *mi,
-        unsigned long start_pfn, unsigned long end_pfn)
+static void __init arm_bootmem_init(unsigned long start_pfn,
+        unsigned long end_pfn)
 {
         struct memblock_region *reg;
         unsigned int boot_pages;
         phys_addr_t bitmap;
         pg_data_t *pgdat;
-        int i;
 
         /*
          * Allocate the bootmem bitmap page.  This must be in a region
@@ -172,30 +173,39 @@ static void __init arm_bootmem_init(struct meminfo *mi,
         pgdat = NODE_DATA(0);
         init_bootmem_node(pgdat, __phys_to_pfn(bitmap), start_pfn, end_pfn);
 
-        for_each_bank(i, mi) {
-                struct membank *bank = &mi->bank[i];
-                if (!bank->highmem)
-                        free_bootmem(bank_phys_start(bank), bank_phys_size(bank));
+        /* Free the lowmem regions from memblock into bootmem. */
+        for_each_memblock(memory, reg) {
+                unsigned long start = memblock_region_memory_base_pfn(reg);
+                unsigned long end = memblock_region_memory_end_pfn(reg);
+
+                if (end >= end_pfn)
+                        end = end_pfn;
+                if (start >= end)
+                        break;
+
+                free_bootmem(__pfn_to_phys(start), (end - start) << PAGE_SHIFT);
         }
 
-        /*
-         * Reserve the memblock reserved regions in bootmem.
-         */
+        /* Reserve the lowmem memblock reserved regions in bootmem. */
         for_each_memblock(reserved, reg) {
-                phys_addr_t start = memblock_region_reserved_base_pfn(reg);
-                phys_addr_t end = memblock_region_reserved_end_pfn(reg);
-                if (start >= start_pfn && end <= end_pfn)
-                        reserve_bootmem_node(pgdat, __pfn_to_phys(start),
-                                             (end - start) << PAGE_SHIFT,
-                                             BOOTMEM_DEFAULT);
+                unsigned long start = memblock_region_reserved_base_pfn(reg);
+                unsigned long end = memblock_region_reserved_end_pfn(reg);
+
+                if (end >= end_pfn)
+                        end = end_pfn;
+                if (start >= end)
+                        break;
+
+                reserve_bootmem(__pfn_to_phys(start),
+                                (end - start) << PAGE_SHIFT, BOOTMEM_DEFAULT);
         }
 }
 
-static void __init arm_bootmem_free(struct meminfo *mi, unsigned long min,
-        unsigned long max_low, unsigned long max_high)
+static void __init arm_bootmem_free(unsigned long min, unsigned long max_low,
+        unsigned long max_high)
 {
         unsigned long zone_size[MAX_NR_ZONES], zhole_size[MAX_NR_ZONES];
-        int i;
+        struct memblock_region *reg;
 
         /*
          * initialise the zones.
@@ -217,13 +227,20 @@ static void __init arm_bootmem_free(struct meminfo *mi, unsigned long min,
          *  holes = node_size - sum(bank_sizes)
          */
         memcpy(zhole_size, zone_size, sizeof(zhole_size));
-        for_each_bank(i, mi) {
-                int idx = 0;
+        for_each_memblock(memory, reg) {
+                unsigned long start = memblock_region_memory_base_pfn(reg);
+                unsigned long end = memblock_region_memory_end_pfn(reg);
+
+                if (start < max_low) {
+                        unsigned long low_end = min(end, max_low);
+                        zhole_size[0] -= low_end - start;
+                }
 #ifdef CONFIG_HIGHMEM
-                if (mi->bank[i].highmem)
-                        idx = ZONE_HIGHMEM;
+                if (end > max_low) {
+                        unsigned long high_start = max(start, max_low);
+                        zhole_size[ZONE_HIGHMEM] -= end - high_start;
+                }
 #endif
-                zhole_size[idx] -= bank_pfn_size(&mi->bank[i]);
         }
 
         /*
@@ -256,10 +273,19 @@ static void arm_memory_present(void)
 }
 #endif
 
+static int __init meminfo_cmp(const void *_a, const void *_b)
+{
+        const struct membank *a = _a, *b = _b;
+        long cmp = bank_pfn_start(a) - bank_pfn_start(b);
+        return cmp < 0 ? -1 : cmp > 0 ? 1 : 0;
+}
+
 void __init arm_memblock_init(struct meminfo *mi, struct machine_desc *mdesc)
 {
         int i;
 
+        sort(&meminfo.bank, meminfo.nr_banks, sizeof(meminfo.bank[0]), meminfo_cmp, NULL);
+
         memblock_init();
         for (i = 0; i < mi->nr_banks; i++)
                 memblock_add(mi->bank[i].start, mi->bank[i].size);
@@ -292,14 +318,13 @@ void __init arm_memblock_init(struct meminfo *mi, struct machine_desc *mdesc)
 
 void __init bootmem_init(void)
 {
-        struct meminfo *mi = &meminfo;
         unsigned long min, max_low, max_high;
 
         max_low = max_high = 0;
 
-        find_limits(mi, &min, &max_low, &max_high);
+        find_limits(&min, &max_low, &max_high);
 
-        arm_bootmem_init(mi, min, max_low);
+        arm_bootmem_init(min, max_low);
 
         /*
          * Sparsemem tries to allocate bootmem in memory_present(),
@@ -317,7 +342,7 @@ void __init bootmem_init(void)
          * the sparse mem_map arrays initialized by sparse_init()
          * for memmap_init_zone(), otherwise all PFNs are invalid.
          */
-        arm_bootmem_free(mi, min, max_low, max_high);
+        arm_bootmem_free(min, max_low, max_high);
 
         high_memory = __va((max_low << PAGE_SHIFT) - 1) + 1;
 
@@ -411,6 +436,56 @@ static void __init free_unused_memmap(struct meminfo *mi)
         }
 }
 
+static void __init free_highpages(void)
+{
+#ifdef CONFIG_HIGHMEM
+        unsigned long max_low = max_low_pfn + PHYS_PFN_OFFSET;
+        struct memblock_region *mem, *res;
+
+        /* set highmem page free */
+        for_each_memblock(memory, mem) {
+                unsigned long start = memblock_region_memory_base_pfn(mem);
+                unsigned long end = memblock_region_memory_end_pfn(mem);
+
+                /* Ignore complete lowmem entries */
+                if (end <= max_low)
+                        continue;
+
+                /* Truncate partial highmem entries */
+                if (start < max_low)
+                        start = max_low;
+
+                /* Find and exclude any reserved regions */
+                for_each_memblock(reserved, res) {
+                        unsigned long res_start, res_end;
+
+                        res_start = memblock_region_reserved_base_pfn(res);
+                        res_end = memblock_region_reserved_end_pfn(res);
+
+                        if (res_end < start)
+                                continue;
+                        if (res_start < start)
+                                res_start = start;
+                        if (res_start > end)
+                                res_start = end;
+                        if (res_end > end)
+                                res_end = end;
+                        if (res_start != start)
+                                totalhigh_pages += free_area(start, res_start,
+                                                             NULL);
+                        start = res_end;
+                        if (start == end)
+                                break;
+                }
+
+                /* And now free anything which remains */
+                if (start < end)
+                        totalhigh_pages += free_area(start, end, NULL);
+        }
+        totalram_pages += totalhigh_pages;
+#endif
+}
+
 /*
  * mem_init() marks the free areas in the mem_map and tells us how much
  * memory is free.  This is done after various parts of the system have
@@ -419,6 +494,7 @@ static void __init free_unused_memmap(struct meminfo *mi)
 void __init mem_init(void)
 {
         unsigned long reserved_pages, free_pages;
+        struct memblock_region *reg;
         int i;
 #ifdef CONFIG_HAVE_TCM
         /* These pointers are filled in on TCM detection */
@@ -439,16 +515,7 @@ void __init mem_init(void)
                                     __phys_to_pfn(__pa(swapper_pg_dir)), NULL);
 #endif
 
-#ifdef CONFIG_HIGHMEM
-        /* set highmem page free */
-        for_each_bank (i, &meminfo) {
-                unsigned long start = bank_pfn_start(&meminfo.bank[i]);
-                unsigned long end = bank_pfn_end(&meminfo.bank[i]);
-                if (start >= max_low_pfn + PHYS_PFN_OFFSET)
-                        totalhigh_pages += free_area(start, end, NULL);
-        }
-        totalram_pages += totalhigh_pages;
-#endif
+        free_highpages();
 
         reserved_pages = free_pages = 0;
 
@@ -478,9 +545,11 @@ void __init mem_init(void)
          */
         printk(KERN_INFO "Memory:");
         num_physpages = 0;
-        for (i = 0; i < meminfo.nr_banks; i++) {
-                num_physpages += bank_pfn_size(&meminfo.bank[i]);
-                printk(" %ldMB", bank_phys_size(&meminfo.bank[i]) >> 20);
+        for_each_memblock(memory, reg) {
+                unsigned long pages = memblock_region_memory_end_pfn(reg) -
+                        memblock_region_memory_base_pfn(reg);
+                num_physpages += pages;
+                printk(" %ldMB", pages >> (20 - PAGE_SHIFT));
         }
         printk(" = %luMB total\n", num_physpages >> (20 - PAGE_SHIFT));
 
diff --git a/arch/arm/mm/mmu.c b/arch/arm/mm/mmu.c
index c32f731d56d3..72ad3e1f56cf 100644
--- a/arch/arm/mm/mmu.c
+++ b/arch/arm/mm/mmu.c
@@ -14,7 +14,6 @@
 #include <linux/mman.h>
 #include <linux/nodemask.h>
 #include <linux/memblock.h>
-#include <linux/sort.h>
 #include <linux/fs.h>
 
 #include <asm/cputype.h>
@@ -265,17 +264,17 @@ static struct mem_type mem_types[] = {
                 .domain    = DOMAIN_KERNEL,
         },
         [MT_MEMORY_DTCM] = {
-                .prot_pte       = L_PTE_PRESENT | L_PTE_YOUNG |
-                                  L_PTE_DIRTY | L_PTE_WRITE,
-                .prot_l1        = PMD_TYPE_TABLE,
-                .prot_sect      = PMD_TYPE_SECT | PMD_SECT_XN,
-                .domain         = DOMAIN_KERNEL,
+                .prot_pte  = L_PTE_PRESENT | L_PTE_YOUNG | L_PTE_DIRTY |
+                                L_PTE_WRITE,
+                .prot_l1   = PMD_TYPE_TABLE,
+                .prot_sect = PMD_TYPE_SECT | PMD_SECT_XN,
+                .domain    = DOMAIN_KERNEL,
         },
         [MT_MEMORY_ITCM] = {
                 .prot_pte  = L_PTE_PRESENT | L_PTE_YOUNG | L_PTE_DIRTY |
-                                L_PTE_USER | L_PTE_EXEC,
+                                L_PTE_WRITE | L_PTE_EXEC,
                 .prot_l1   = PMD_TYPE_TABLE,
-                .domain    = DOMAIN_IO,
+                .domain    = DOMAIN_KERNEL,
         },
 };
 
@@ -745,13 +744,14 @@ static int __init early_vmalloc(char *arg)
 }
 early_param("vmalloc", early_vmalloc);
 
-phys_addr_t lowmem_end_addr;
+static phys_addr_t lowmem_limit __initdata = 0;
 
 static void __init sanity_check_meminfo(void)
 {
         int i, j, highmem = 0;
 
-        lowmem_end_addr = __pa(vmalloc_min - 1) + 1;
+        lowmem_limit = __pa(vmalloc_min - 1) + 1;
+        memblock_set_current_limit(lowmem_limit);
 
         for (i = 0, j = 0; i < meminfo.nr_banks; i++) {
                 struct membank *bank = &meminfo.bank[j];
@@ -852,6 +852,7 @@ static void __init sanity_check_meminfo(void)
 static inline void prepare_page_table(void)
 {
         unsigned long addr;
+        phys_addr_t end;
 
         /*
          * Clear out all the mappings below the kernel image.
@@ -867,10 +868,17 @@ static inline void prepare_page_table(void)
                 pmd_clear(pmd_off_k(addr));
 
         /*
+         * Find the end of the first block of lowmem.
+         */
+        end = memblock.memory.regions[0].base + memblock.memory.regions[0].size;
+        if (end >= lowmem_limit)
+                end = lowmem_limit;
+
+        /*
          * Clear out all the kernel space mappings, except for the first
          * memory bank, up to the end of the vmalloc region.
          */
-        for (addr = __phys_to_virt(bank_phys_end(&meminfo.bank[0]));
+        for (addr = __phys_to_virt(end);
              addr < VMALLOC_END; addr += PGDIR_SIZE)
                 pmd_clear(pmd_off_k(addr));
 }
@@ -987,37 +995,28 @@ static void __init kmap_init(void)
 #endif
 }
 
-static inline void map_memory_bank(struct membank *bank)
-{
-        struct map_desc map;
-
-        map.pfn = bank_pfn_start(bank);
-        map.virtual = __phys_to_virt(bank_phys_start(bank));
-        map.length = bank_phys_size(bank);
-        map.type = MT_MEMORY;
-
-        create_mapping(&map);
-}
-
 static void __init map_lowmem(void)
 {
-        struct meminfo *mi = &meminfo;
-        int i;
+        struct memblock_region *reg;
 
         /* Map all the lowmem memory banks. */
-        for (i = 0; i < mi->nr_banks; i++) {
-                struct membank *bank = &mi->bank[i];
+        for_each_memblock(memory, reg) {
+                phys_addr_t start = reg->base;
+                phys_addr_t end = start + reg->size;
+                struct map_desc map;
+
+                if (end > lowmem_limit)
+                        end = lowmem_limit;
+                if (start >= end)
+                        break;
 
-                if (!bank->highmem)
-                        map_memory_bank(bank);
-        }
-}
+                map.pfn = __phys_to_pfn(start);
+                map.virtual = __phys_to_virt(start);
+                map.length = end - start;
+                map.type = MT_MEMORY;
 
-static int __init meminfo_cmp(const void *_a, const void *_b)
-{
-        const struct membank *a = _a, *b = _b;
-        long cmp = bank_pfn_start(a) - bank_pfn_start(b);
-        return cmp < 0 ? -1 : cmp > 0 ? 1 : 0;
+                create_mapping(&map);
+        }
 }
 
 /*
@@ -1028,8 +1027,6 @@ void __init paging_init(struct machine_desc *mdesc)
 {
         void *zero_page;
 
-        sort(&meminfo.bank, meminfo.nr_banks, sizeof(meminfo.bank[0]), meminfo_cmp, NULL);
-
         build_mem_type_table();
         sanity_check_meminfo();
         prepare_page_table();
diff --git a/arch/arm/mm/proc-arm1020.S b/arch/arm/mm/proc-arm1020.S
index a6f5f8475b96..bcf748d9f4e2 100644
--- a/arch/arm/mm/proc-arm1020.S
+++ b/arch/arm/mm/proc-arm1020.S
@@ -119,6 +119,20 @@ ENTRY(cpu_arm1020_do_idle)
 /* ================================= CACHE ================================ */
 
         .align  5
+
+/*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm1020_flush_icache_all)
+#ifndef CONFIG_CPU_ICACHE_DISABLE
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+#endif
+        mov     pc, lr
+ENDPROC(arm1020_flush_icache_all)
+
 /*
  *      flush_user_cache_all()
  *
@@ -351,6 +365,7 @@ ENTRY(arm1020_dma_unmap_area)
 ENDPROC(arm1020_dma_unmap_area)
 
 ENTRY(arm1020_cache_fns)
+        .long   arm1020_flush_icache_all
         .long   arm1020_flush_kern_cache_all
         .long   arm1020_flush_user_cache_all
         .long   arm1020_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm1020e.S b/arch/arm/mm/proc-arm1020e.S
index afc06b9c3133..ab7ec26657ea 100644
--- a/arch/arm/mm/proc-arm1020e.S
+++ b/arch/arm/mm/proc-arm1020e.S
@@ -119,6 +119,20 @@ ENTRY(cpu_arm1020e_do_idle)
 /* ================================= CACHE ================================ */
 
         .align  5
+
+/*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm1020e_flush_icache_all)
+#ifndef CONFIG_CPU_ICACHE_DISABLE
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+#endif
+        mov     pc, lr
+ENDPROC(arm1020e_flush_icache_all)
+
 /*
  *      flush_user_cache_all()
  *
@@ -337,6 +351,7 @@ ENTRY(arm1020e_dma_unmap_area)
 ENDPROC(arm1020e_dma_unmap_area)
 
 ENTRY(arm1020e_cache_fns)
+        .long   arm1020e_flush_icache_all
         .long   arm1020e_flush_kern_cache_all
         .long   arm1020e_flush_user_cache_all
         .long   arm1020e_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm1022.S b/arch/arm/mm/proc-arm1022.S
index 8915e0ba3fe5..831c5e54e22f 100644
--- a/arch/arm/mm/proc-arm1022.S
+++ b/arch/arm/mm/proc-arm1022.S
@@ -108,6 +108,20 @@ ENTRY(cpu_arm1022_do_idle)
 /* ================================= CACHE ================================ */
 
         .align  5
+
+/*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm1022_flush_icache_all)
+#ifndef CONFIG_CPU_ICACHE_DISABLE
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+#endif
+        mov     pc, lr
+ENDPROC(arm1022_flush_icache_all)
+
 /*
  *      flush_user_cache_all()
  *
@@ -326,6 +340,7 @@ ENTRY(arm1022_dma_unmap_area)
 ENDPROC(arm1022_dma_unmap_area)
 
 ENTRY(arm1022_cache_fns)
+        .long   arm1022_flush_icache_all
         .long   arm1022_flush_kern_cache_all
         .long   arm1022_flush_user_cache_all
         .long   arm1022_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm1026.S b/arch/arm/mm/proc-arm1026.S
index ff446c5d476f..e3f7e9a166bf 100644
--- a/arch/arm/mm/proc-arm1026.S
+++ b/arch/arm/mm/proc-arm1026.S
@@ -108,6 +108,20 @@ ENTRY(cpu_arm1026_do_idle)
 /* ================================= CACHE ================================ */
 
         .align  5
+
+/*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm1026_flush_icache_all)
+#ifndef CONFIG_CPU_ICACHE_DISABLE
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+#endif
+        mov     pc, lr
+ENDPROC(arm1026_flush_icache_all)
+
 /*
  *      flush_user_cache_all()
  *
@@ -320,6 +334,7 @@ ENTRY(arm1026_dma_unmap_area)
 ENDPROC(arm1026_dma_unmap_area)
 
 ENTRY(arm1026_cache_fns)
+        .long   arm1026_flush_icache_all
         .long   arm1026_flush_kern_cache_all
         .long   arm1026_flush_user_cache_all
         .long   arm1026_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm920.S b/arch/arm/mm/proc-arm920.S
index fecf570939f3..6109f278a904 100644
--- a/arch/arm/mm/proc-arm920.S
+++ b/arch/arm/mm/proc-arm920.S
@@ -110,6 +110,17 @@ ENTRY(cpu_arm920_do_idle)
 #ifndef CONFIG_CPU_DCACHE_WRITETHROUGH
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm920_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(arm920_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Invalidate all cache entries in a particular address
@@ -305,6 +316,7 @@ ENTRY(arm920_dma_unmap_area)
 ENDPROC(arm920_dma_unmap_area)
 
 ENTRY(arm920_cache_fns)
+        .long   arm920_flush_icache_all
         .long   arm920_flush_kern_cache_all
         .long   arm920_flush_user_cache_all
         .long   arm920_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm922.S b/arch/arm/mm/proc-arm922.S
index e3cbf87c9480..bb2f0f46a5e6 100644
--- a/arch/arm/mm/proc-arm922.S
+++ b/arch/arm/mm/proc-arm922.S
@@ -112,6 +112,17 @@ ENTRY(cpu_arm922_do_idle)
 #ifndef CONFIG_CPU_DCACHE_WRITETHROUGH
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm922_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(arm922_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Clean and invalidate all cache entries in a particular
@@ -307,6 +318,7 @@ ENTRY(arm922_dma_unmap_area)
 ENDPROC(arm922_dma_unmap_area)
 
 ENTRY(arm922_cache_fns)
+        .long   arm922_flush_icache_all
         .long   arm922_flush_kern_cache_all
         .long   arm922_flush_user_cache_all
         .long   arm922_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm925.S b/arch/arm/mm/proc-arm925.S
index 572424c867b5..c13e01accfe2 100644
--- a/arch/arm/mm/proc-arm925.S
+++ b/arch/arm/mm/proc-arm925.S
@@ -145,6 +145,17 @@ ENTRY(cpu_arm925_do_idle)
         mov     pc, lr
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm925_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(arm925_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Clean and invalidate all cache entries in a particular
@@ -362,6 +373,7 @@ ENTRY(arm925_dma_unmap_area)
 ENDPROC(arm925_dma_unmap_area)
 
 ENTRY(arm925_cache_fns)
+        .long   arm925_flush_icache_all
         .long   arm925_flush_kern_cache_all
         .long   arm925_flush_user_cache_all
         .long   arm925_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm926.S b/arch/arm/mm/proc-arm926.S
index 63d168b4ebe6..42eb4315740b 100644
--- a/arch/arm/mm/proc-arm926.S
+++ b/arch/arm/mm/proc-arm926.S
@@ -111,6 +111,17 @@ ENTRY(cpu_arm926_do_idle)
         mov     pc, lr
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm926_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(arm926_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Clean and invalidate all cache entries in a particular
@@ -325,6 +336,7 @@ ENTRY(arm926_dma_unmap_area)
 ENDPROC(arm926_dma_unmap_area)
 
 ENTRY(arm926_cache_fns)
+        .long   arm926_flush_icache_all
         .long   arm926_flush_kern_cache_all
         .long   arm926_flush_user_cache_all
         .long   arm926_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm940.S b/arch/arm/mm/proc-arm940.S
index f6a62822418e..7b11cdb9935f 100644
--- a/arch/arm/mm/proc-arm940.S
+++ b/arch/arm/mm/proc-arm940.S
@@ -68,6 +68,17 @@ ENTRY(cpu_arm940_do_idle)
         mov     pc, lr
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm940_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(arm940_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  */
 ENTRY(arm940_flush_user_cache_all)
@@ -254,6 +265,7 @@ ENTRY(arm940_dma_unmap_area)
 ENDPROC(arm940_dma_unmap_area)
 
 ENTRY(arm940_cache_fns)
+        .long   arm940_flush_icache_all
         .long   arm940_flush_kern_cache_all
         .long   arm940_flush_user_cache_all
         .long   arm940_flush_user_cache_range
diff --git a/arch/arm/mm/proc-arm946.S b/arch/arm/mm/proc-arm946.S
index ea2e7f2eb95b..1a5bbf080342 100644
--- a/arch/arm/mm/proc-arm946.S
+++ b/arch/arm/mm/proc-arm946.S
@@ -75,6 +75,17 @@ ENTRY(cpu_arm946_do_idle)
         mov     pc, lr
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(arm946_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(arm946_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  */
 ENTRY(arm946_flush_user_cache_all)
@@ -296,6 +307,7 @@ ENTRY(arm946_dma_unmap_area)
 ENDPROC(arm946_dma_unmap_area)
 
 ENTRY(arm946_cache_fns)
+        .long   arm946_flush_icache_all
         .long   arm946_flush_kern_cache_all
         .long   arm946_flush_user_cache_all
         .long   arm946_flush_user_cache_range
diff --git a/arch/arm/mm/proc-feroceon.S b/arch/arm/mm/proc-feroceon.S
index 578da69200cf..b4597edbff97 100644
--- a/arch/arm/mm/proc-feroceon.S
+++ b/arch/arm/mm/proc-feroceon.S
@@ -124,6 +124,17 @@ ENTRY(cpu_feroceon_do_idle)
         mov     pc, lr
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(feroceon_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(feroceon_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Clean and invalidate all cache entries in a particular
@@ -401,6 +412,7 @@ ENTRY(feroceon_dma_unmap_area)
 ENDPROC(feroceon_dma_unmap_area)
 
 ENTRY(feroceon_cache_fns)
+        .long   feroceon_flush_icache_all
         .long   feroceon_flush_kern_cache_all
         .long   feroceon_flush_user_cache_all
         .long   feroceon_flush_user_cache_range
@@ -412,6 +424,7 @@ ENTRY(feroceon_cache_fns)
         .long   feroceon_dma_flush_range
 
 ENTRY(feroceon_range_cache_fns)
+        .long   feroceon_flush_icache_all
         .long   feroceon_flush_kern_cache_all
         .long   feroceon_flush_user_cache_all
         .long   feroceon_flush_user_cache_range
diff --git a/arch/arm/mm/proc-xsc3.S b/arch/arm/mm/proc-xsc3.S
index cad07e403044..ec26355cb7c2 100644
--- a/arch/arm/mm/proc-xsc3.S
+++ b/arch/arm/mm/proc-xsc3.S
@@ -141,6 +141,17 @@ ENTRY(cpu_xsc3_do_idle)
 /* ================================= CACHE ================================ */
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(xsc3_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(xsc3_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Invalidate all cache entries in a particular address
@@ -325,6 +336,7 @@ ENTRY(xsc3_dma_unmap_area)
 ENDPROC(xsc3_dma_unmap_area)
 
 ENTRY(xsc3_cache_fns)
+        .long   xsc3_flush_icache_all
         .long   xsc3_flush_kern_cache_all
         .long   xsc3_flush_user_cache_all
         .long   xsc3_flush_user_cache_range
diff --git a/arch/arm/mm/proc-xscale.S b/arch/arm/mm/proc-xscale.S
index cb245edb2c2b..523408c0bb38 100644
--- a/arch/arm/mm/proc-xscale.S
+++ b/arch/arm/mm/proc-xscale.S
@@ -181,6 +181,17 @@ ENTRY(cpu_xscale_do_idle)
 /* ================================= CACHE ================================ */
 
 /*
+ *      flush_icache_all()
+ *
+ *      Unconditionally clean and invalidate the entire icache.
+ */
+ENTRY(xscale_flush_icache_all)
+        mov     r0, #0
+        mcr     p15, 0, r0, c7, c5, 0           @ invalidate I cache
+        mov     pc, lr
+ENDPROC(xscale_flush_icache_all)
+
+/*
  *      flush_user_cache_all()
  *
  *      Invalidate all cache entries in a particular address
@@ -397,6 +408,7 @@ ENTRY(xscale_dma_unmap_area)
 ENDPROC(xscale_dma_unmap_area)
 
 ENTRY(xscale_cache_fns)
+        .long   xscale_flush_icache_all
         .long   xscale_flush_kern_cache_all
         .long   xscale_flush_user_cache_all
         .long   xscale_flush_user_cache_range
diff --git a/arch/arm/plat-mxc/Makefile b/arch/arm/plat-mxc/Makefile
index 06875b4dd70f..372670952789 100644
--- a/arch/arm/plat-mxc/Makefile
+++ b/arch/arm/plat-mxc/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_MXC_USE_EPIT) += epit.o
 obj-$(CONFIG_ARCH_MXC_AUDMUX_V1) += audmux-v1.o
 obj-$(CONFIG_ARCH_MXC_AUDMUX_V2) += audmux-v2.o
 obj-$(CONFIG_MXC_DEBUG_BOARD) += 3ds_debugboard.o
+obj-$(CONFIG_CPU_FREQ_IMX)    += cpufreq.o
 ifdef CONFIG_SND_IMX_SOC
 obj-y += ssi-fiq.o
 obj-y += ssi-fiq-ksym.o
diff --git a/arch/arm/plat-mxc/cpufreq.c b/arch/arm/plat-mxc/cpufreq.c
new file mode 100644
index 000000000000..039538e68793
--- /dev/null
+++ b/arch/arm/plat-mxc/cpufreq.c
@@ -0,0 +1,206 @@
+/*
+ * Copyright (C) 2010 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
+ */
+
+/*
+ * A driver for the Freescale Semiconductor i.MXC CPUfreq module.
+ * The CPUFREQ driver is for controling CPU frequency. It allows you to change
+ * the CPU clock speed on the fly.
+ */
+
+#include <linux/cpufreq.h>
+#include <linux/clk.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <mach/hardware.h>
+#include <mach/clock.h>
+
+#define CLK32_FREQ      32768
+#define NANOSECOND      (1000 * 1000 * 1000)
+
+struct cpu_op *(*get_cpu_op)(int *op);
+
+static int cpu_freq_khz_min;
+static int cpu_freq_khz_max;
+
+static struct clk *cpu_clk;
+static struct cpufreq_frequency_table *imx_freq_table;
+
+static int cpu_op_nr;
+static struct cpu_op *cpu_op_tbl;
+
+static int set_cpu_freq(int freq)
+{
+        int ret = 0;
+        int org_cpu_rate;
+
+        org_cpu_rate = clk_get_rate(cpu_clk);
+        if (org_cpu_rate == freq)
+                return ret;
+
+        ret = clk_set_rate(cpu_clk, freq);
+        if (ret != 0) {
+                printk(KERN_DEBUG "cannot set CPU clock rate\n");
+                return ret;
+        }
+
+        return ret;
+}
+
+static int mxc_verify_speed(struct cpufreq_policy *policy)
+{
+        if (policy->cpu != 0)
+                return -EINVAL;
+
+        return cpufreq_frequency_table_verify(policy, imx_freq_table);
+}
+
+static unsigned int mxc_get_speed(unsigned int cpu)
+{
+        if (cpu)
+                return 0;
+
+        return clk_get_rate(cpu_clk) / 1000;
+}
+
+static int mxc_set_target(struct cpufreq_policy *policy,
+                          unsigned int target_freq, unsigned int relation)
+{
+        struct cpufreq_freqs freqs;
+        int freq_Hz;
+        int ret = 0;
+        unsigned int index;
+
+        cpufreq_frequency_table_target(policy, imx_freq_table,
+                        target_freq, relation, &index);
+        freq_Hz = imx_freq_table[index].frequency * 1000;
+
+        freqs.old = clk_get_rate(cpu_clk) / 1000;
+        freqs.new = freq_Hz / 1000;
+        freqs.cpu = 0;
+        freqs.flags = 0;
+        cpufreq_notify_transition(&freqs, CPUFREQ_PRECHANGE);
+
+        ret = set_cpu_freq(freq_Hz);
+
+        cpufreq_notify_transition(&freqs, CPUFREQ_POSTCHANGE);
+
+        return ret;
+}
+
+static int __init mxc_cpufreq_init(struct cpufreq_policy *policy)
+{
+        int ret;
+        int i;
+
+        printk(KERN_INFO "i.MXC CPU frequency driver\n");
+
+        if (policy->cpu != 0)
+                return -EINVAL;
+
+        if (!get_cpu_op)
+                return -EINVAL;
+
+        cpu_clk = clk_get(NULL, "cpu_clk");
+        if (IS_ERR(cpu_clk)) {
+                printk(KERN_ERR "%s: failed to get cpu clock\n", __func__);
+                return PTR_ERR(cpu_clk);
+        }
+
+        cpu_op_tbl = get_cpu_op(&cpu_op_nr);
+
+        cpu_freq_khz_min = cpu_op_tbl[0].cpu_rate / 1000;
+        cpu_freq_khz_max = cpu_op_tbl[0].cpu_rate / 1000;
+
+        imx_freq_table = kmalloc(
+                sizeof(struct cpufreq_frequency_table) * (cpu_op_nr + 1),
+                        GFP_KERNEL);
+        if (!imx_freq_table) {
+                ret = -ENOMEM;
+                goto err1;
+        }
+
+        for (i = 0; i < cpu_op_nr; i++) {
+                imx_freq_table[i].index = i;
+                imx_freq_table[i].frequency = cpu_op_tbl[i].cpu_rate / 1000;
+
+                if ((cpu_op_tbl[i].cpu_rate / 1000) < cpu_freq_khz_min)
+                        cpu_freq_khz_min = cpu_op_tbl[i].cpu_rate / 1000;
+
+                if ((cpu_op_tbl[i].cpu_rate / 1000) > cpu_freq_khz_max)
+                        cpu_freq_khz_max = cpu_op_tbl[i].cpu_rate / 1000;
+        }
+
+        imx_freq_table[i].index = i;
+        imx_freq_table[i].frequency = CPUFREQ_TABLE_END;
+
+        policy->cur = clk_get_rate(cpu_clk) / 1000;
+        policy->governor = CPUFREQ_DEFAULT_GOVERNOR;
+        policy->min = policy->cpuinfo.min_freq = cpu_freq_khz_min;
+        policy->max = policy->cpuinfo.max_freq = cpu_freq_khz_max;
+
+        /* Manual states, that PLL stabilizes in two CLK32 periods */
+        policy->cpuinfo.transition_latency = 2 * NANOSECOND / CLK32_FREQ;
+
+        ret = cpufreq_frequency_table_cpuinfo(policy, imx_freq_table);
+
+        if (ret < 0) {
+                printk(KERN_ERR "%s: failed to register i.MXC CPUfreq \
+                                with error code %d\n", __func__, ret);
+                goto err;
+        }
+
+        cpufreq_frequency_table_get_attr(imx_freq_table, policy->cpu);
+        return 0;
+err:
+        kfree(imx_freq_table);
+err1:
+        clk_put(cpu_clk);
+        return ret;
+}
+
+static int mxc_cpufreq_exit(struct cpufreq_policy *policy)
+{
+        cpufreq_frequency_table_put_attr(policy->cpu);
+
+        set_cpu_freq(cpu_freq_khz_max * 1000);
+        clk_put(cpu_clk);
+        kfree(imx_freq_table);
+        return 0;
+}
+
+static struct cpufreq_driver mxc_driver = {
+        .flags = CPUFREQ_STICKY,
+        .verify = mxc_verify_speed,
+        .target = mxc_set_target,
+        .get = mxc_get_speed,
+        .init = mxc_cpufreq_init,
+        .exit = mxc_cpufreq_exit,
+        .name = "imx",
+};
+
+static int __devinit mxc_cpufreq_driver_init(void)
+{
+        return cpufreq_register_driver(&mxc_driver);
+}
+
+static void mxc_cpufreq_driver_exit(void)
+{
+        cpufreq_unregister_driver(&mxc_driver);
+}
+
+module_init(mxc_cpufreq_driver_init);
+module_exit(mxc_cpufreq_driver_exit);
+
+MODULE_AUTHOR("Freescale Semiconductor Inc. Yong Shen <yong.shen@linaro.org>");
+MODULE_DESCRIPTION("CPUfreq driver for i.MX");
+MODULE_LICENSE("GPL");
diff --git a/arch/arm/plat-mxc/devices/Kconfig b/arch/arm/plat-mxc/devices/Kconfig
index 404799487f17..9aa6f3ea9012 100644
--- a/arch/arm/plat-mxc/devices/Kconfig
+++ b/arch/arm/plat-mxc/devices/Kconfig
@@ -6,9 +6,13 @@ config IMX_HAVE_PLATFORM_FEC
         default y if ARCH_MX25 || SOC_IMX27 || ARCH_MX35 || ARCH_MX51
 
 config IMX_HAVE_PLATFORM_FLEXCAN
-        select HAVE_CAN_FLEXCAN
+        select HAVE_CAN_FLEXCAN if CAN
         bool
 
+config IMX_HAVE_PLATFORM_GPIO_KEYS
+        bool
+        default y if ARCH_MX51
+        
 config IMX_HAVE_PLATFORM_IMX_I2C
         bool
 
diff --git a/arch/arm/plat-mxc/devices/Makefile b/arch/arm/plat-mxc/devices/Makefile
index 0a3c1f089413..45aefeb283ba 100644
--- a/arch/arm/plat-mxc/devices/Makefile
+++ b/arch/arm/plat-mxc/devices/Makefile
@@ -1,6 +1,7 @@
 obj-$(CONFIG_IMX_HAVE_PLATFORM_ESDHC) += platform-esdhc.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_FEC) += platform-fec.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_FLEXCAN) += platform-flexcan.o
+obj-$(CONFIG_IMX_HAVE_PLATFORM_GPIO_KEYS) += platform-gpio_keys.o
 obj-y += platform-imx-dma.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_IMX_I2C) += platform-imx-i2c.o
 obj-$(CONFIG_IMX_HAVE_PLATFORM_IMX_SSI) += platform-imx-ssi.o
diff --git a/arch/arm/plat-mxc/devices/platform-gpio_keys.c b/arch/arm/plat-mxc/devices/platform-gpio_keys.c
new file mode 100644
index 000000000000..1c53a532ea0e
--- /dev/null
+++ b/arch/arm/plat-mxc/devices/platform-gpio_keys.c
@@ -0,0 +1,27 @@
+/*
+ * Copyright (C) 2010 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
+ * as published by the Free Software Foundation; either version 2
+ * of the License, or (at your option) any later version.
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor,
+ * Boston, MA  02110-1301, USA.
+ */
+#include <asm/sizes.h>
+#include <mach/hardware.h>
+#include <mach/devices-common.h>
+
+struct platform_device *__init imx_add_gpio_keys(
+                const struct gpio_keys_platform_data *pdata)
+{
+        return imx_add_platform_device("gpio-keys", -1, NULL,
+                 0, pdata, sizeof(*pdata));
+}
diff --git a/arch/arm/plat-mxc/gpio.c b/arch/arm/plat-mxc/gpio.c
index 9d38da077edb..9c3e36232b5b 100644
--- a/arch/arm/plat-mxc/gpio.c
+++ b/arch/arm/plat-mxc/gpio.c
@@ -20,6 +20,7 @@
  */
 
 #include <linux/init.h>
+#include <linux/interrupt.h>
 #include <linux/io.h>
 #include <linux/irq.h>
 #include <linux/gpio.h>
@@ -201,11 +202,42 @@ static void mx2_gpio_irq_handler(u32 irq, struct irq_desc *desc)
         }
 }
 
+/*
+ * Set interrupt number "irq" in the GPIO as a wake-up source.
+ * While system is running, all registered GPIO interrupts need to have
+ * wake-up enabled. When system is suspended, only selected GPIO interrupts
+ * need to have wake-up enabled.
+ * @param  irq          interrupt source number
+ * @param  enable       enable as wake-up if equal to non-zero
+ * @return       This function returns 0 on success.
+ */
+static int gpio_set_wake_irq(u32 irq, u32 enable)
+{
+        u32 gpio = irq_to_gpio(irq);
+        u32 gpio_idx = gpio & 0x1F;
+        struct mxc_gpio_port *port = &mxc_gpio_ports[gpio / 32];
+
+        if (enable) {
+                if (port->irq_high && (gpio_idx >= 16))
+                        enable_irq_wake(port->irq_high);
+                else
+                        enable_irq_wake(port->irq);
+        } else {
+                if (port->irq_high && (gpio_idx >= 16))
+                        disable_irq_wake(port->irq_high);
+                else
+                        disable_irq_wake(port->irq);
+        }
+
+        return 0;
+}
+
 static struct irq_chip gpio_irq_chip = {
         .ack = gpio_ack_irq,
         .mask = gpio_mask_irq,
         .unmask = gpio_unmask_irq,
         .set_type = gpio_set_irq_type,
+        .set_wake = gpio_set_wake_irq,
 };
 
 static void _set_gpio_direction(struct gpio_chip *chip, unsigned offset,
diff --git a/arch/arm/plat-mxc/include/mach/devices-common.h b/arch/arm/plat-mxc/include/mach/devices-common.h
index 86d7575a564d..8c6896fd1e5f 100644
--- a/arch/arm/plat-mxc/include/mach/devices-common.h
+++ b/arch/arm/plat-mxc/include/mach/devices-common.h
@@ -29,6 +29,10 @@ struct platform_device *__init imx_add_flexcan(int id,
                 resource_size_t irq,
                 const struct flexcan_platform_data *pdata);
 
+#include <linux/gpio_keys.h>
+struct platform_device *__init imx_add_gpio_keys(
+                const struct gpio_keys_platform_data *pdata);
+
 #include <mach/i2c.h>
 struct imx_imx_i2c_data {
         int id;
diff --git a/arch/arm/plat-mxc/include/mach/iomux-mx51.h b/arch/arm/plat-mxc/include/mach/iomux-mx51.h
index e46b1c2836d4..d7a41e9a2605 100644
--- a/arch/arm/plat-mxc/include/mach/iomux-mx51.h
+++ b/arch/arm/plat-mxc/include/mach/iomux-mx51.h
@@ -45,6 +45,8 @@ typedef enum iomux_config {
                                 PAD_CTL_PKE | PAD_CTL_HYS)
 #define MX51_GPIO_PAD_CTRL              (PAD_CTL_DSE_HIGH | PAD_CTL_PKE | \
                                 PAD_CTL_SRE_FAST)
+#define MX51_GPIO_PAD_CTRL_2    (PAD_CTL_SRE_FAST | PAD_CTL_DSE_HIGH | \
+                                        PAD_CTL_PUS_100K_UP)
 #define MX51_ECSPI_PAD_CTRL     (PAD_CTL_HYS | PAD_CTL_PKE | PAD_CTL_DSE_HIGH | \
                                 PAD_CTL_SRE_FAST)
 #define MX51_SDHCI_PAD_CTRL     (PAD_CTL_DSE_HIGH | PAD_CTL_PUS_47K_UP | \
diff --git a/arch/arm/plat-mxc/include/mach/mx31.h b/arch/arm/plat-mxc/include/mach/mx31.h
index 03e2afabc9fc..61cfe827498b 100644
--- a/arch/arm/plat-mxc/include/mach/mx31.h
+++ b/arch/arm/plat-mxc/include/mach/mx31.h
@@ -240,7 +240,6 @@ static inline void mx31_setup_weimcs(size_t cs,
 #define MPEG4_ENC_BASE_ADDR MX31_MPEG4_ENC_BASE_ADDR
 #define MXC_INT_MPEG4_ENCODER MX31_INT_MPEG4_ENCODER
 #define MXC_INT_FIRI MX31_INT_FIRI
-#define MXC_INT_MMC_SDHC1 MX31_INT_MMC_SDHC1
 #define MXC_INT_MBX MX31_INT_MBX
 #define MXC_INT_CSPI3 MX31_INT_CSPI3
 #define MXC_INT_SIM2 MX31_INT_SIM2
diff --git a/arch/arm/plat-mxc/include/mach/mx35.h b/arch/arm/plat-mxc/include/mach/mx35.h
index ff905cb32458..6267cff6035d 100644
--- a/arch/arm/plat-mxc/include/mach/mx35.h
+++ b/arch/arm/plat-mxc/include/mach/mx35.h
@@ -197,8 +197,6 @@
 /* these should go away */
 #define MXC_FEC_BASE_ADDR MX35_FEC_BASE_ADDR
 #define MXC_INT_OWIRE MX35_INT_OWIRE
-#define MXC_INT_MMC_SDHC2 MX35_INT_MMC_SDHC2
-#define MXC_INT_MMC_SDHC3 MX35_INT_MMC_SDHC3
 #define MXC_INT_GPU2D MX35_INT_GPU2D
 #define MXC_INT_ASRC MX35_INT_ASRC
 #define MXC_INT_USBHS MX35_INT_USBHS
diff --git a/arch/arm/plat-mxc/include/mach/mxc.h b/arch/arm/plat-mxc/include/mach/mxc.h
index a790bf212972..a42c7207082d 100644
--- a/arch/arm/plat-mxc/include/mach/mxc.h
+++ b/arch/arm/plat-mxc/include/mach/mxc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright 2004-2007 Freescale Semiconductor, Inc. All Rights Reserved.
+ * Copyright 2004-2007, 2010 Freescale Semiconductor, Inc. All Rights Reserved.
  * Copyright (C) 2008 Juergen Beisert (kernel@pengutronix.de)
  *
  * This program is free software; you can redistribute it and/or
@@ -20,6 +20,8 @@
 #ifndef __ASM_ARCH_MXC_H__
 #define __ASM_ARCH_MXC_H__
 
+#include <linux/types.h>
+
 #ifndef __ASM_ARCH_MXC_HARDWARE_H__
 #error "Do not include directly."
 #endif
@@ -133,6 +135,15 @@ extern unsigned int __mxc_cpu_type;
 # define cpu_is_mxc91231()      (0)
 #endif
 
+#ifndef __ASSEMBLY__
+
+struct cpu_op {
+        u32 cpu_rate;
+};
+
+extern struct cpu_op *(*get_cpu_op)(int *op);
+#endif
+
 #if defined(CONFIG_ARCH_MX3) || defined(CONFIG_ARCH_MX2)
 /* These are deprecated, use mx[23][157]_setup_weimcs instead. */
 #define CSCR_U(n) (IO_ADDRESS(WEIM_BASE_ADDR + n * 0x10))
diff --git a/arch/arm/plat-s3c24xx/common-smdk.c b/arch/arm/plat-s3c24xx/common-smdk.c
index 7b44d0c592b5..bcc43f346272 100644
--- a/arch/arm/plat-s3c24xx/common-smdk.c
+++ b/arch/arm/plat-s3c24xx/common-smdk.c
@@ -147,7 +147,7 @@ static struct mtd_partition smdk_default_nand_part[] = {
         [7] = {
                 .name   = "S3C2410 flash partition 7",
                 .offset = SZ_1M * 48,
-                .size   = SZ_16M,
+                .size   = MTDPART_SIZ_FULL,
         }
 };
 
diff --git a/arch/arm/plat-s3c24xx/gpiolib.c b/arch/arm/plat-s3c24xx/gpiolib.c
index ae9d3c2403f0..24c6f5a30596 100644
--- a/arch/arm/plat-s3c24xx/gpiolib.c
+++ b/arch/arm/plat-s3c24xx/gpiolib.c
@@ -74,11 +74,6 @@ static int s3c24xx_gpiolib_bankf_toirq(struct gpio_chip *chip, unsigned offset)
         return -EINVAL;
 }
 
-static int s3c24xx_gpiolib_bankg_toirq(struct gpio_chip *chip, unsigned offset)
-{
-        return IRQ_EINT8 + offset;
-}
-
 static struct s3c_gpio_cfg s3c24xx_gpiocfg_banka = {
         .set_config     = s3c_gpio_setcfg_s3c24xx_a,
         .get_config     = s3c_gpio_getcfg_s3c24xx_a,
@@ -159,12 +154,13 @@ struct s3c_gpio_chip s3c24xx_gpios[] = {
         [6] = {
                 .base   = S3C2410_GPGCON,
                 .pm     = __gpio_pm(&s3c_gpio_pm_2bit),
+                .irq_base = IRQ_EINT8,
                 .chip   = {
                         .base                   = S3C2410_GPG(0),
                         .owner                  = THIS_MODULE,
                         .label                  = "GPIOG",
                         .ngpio                  = 16,
-                        .to_irq                 = s3c24xx_gpiolib_bankg_toirq,
+                        .to_irq                 = samsung_gpiolib_to_irq,
                 },
         }, {
                 .base   = S3C2410_GPHCON,
diff --git a/arch/arm/plat-s5p/Kconfig b/arch/arm/plat-s5p/Kconfig
index 25960966af7c..65dbfa8e0a86 100644
--- a/arch/arm/plat-s5p/Kconfig
+++ b/arch/arm/plat-s5p/Kconfig
@@ -32,6 +32,11 @@ config S5P_EXT_INT
           Use the external interrupts (other than GPIO interrupts.)
           Note: Do not choose this for S5P6440 and S5P6450.
 
+config S5P_GPIO_INT
+        bool
+        help
+          Common code for the GPIO interrupts (other than external interrupts.)
+
 config S5P_DEV_FIMC0
         bool
         help
diff --git a/arch/arm/plat-s5p/Makefile b/arch/arm/plat-s5p/Makefile
index f3e917e27da8..de65238a7aef 100644
--- a/arch/arm/plat-s5p/Makefile
+++ b/arch/arm/plat-s5p/Makefile
@@ -18,6 +18,9 @@ obj-y				+= cpu.o
 obj-y                           += clock.o
 obj-y                           += irq.o
 obj-$(CONFIG_S5P_EXT_INT)       += irq-eint.o
+obj-$(CONFIG_S5P_GPIO_INT)      += irq-gpioint.o
+obj-$(CONFIG_PM)                += pm.o
+obj-$(CONFIG_PM)                += irq-pm.o
 
 # devices
 
diff --git a/arch/arm/plat-s5p/clock.c b/arch/arm/plat-s5p/clock.c
index 8aaf4e6b60c3..8d081d968c58 100644
--- a/arch/arm/plat-s5p/clock.c
+++ b/arch/arm/plat-s5p/clock.c
@@ -21,6 +21,8 @@
 #include <linux/io.h>
 #include <asm/div64.h>
 
+#include <mach/regs-clock.h>
+
 #include <plat/clock.h>
 #include <plat/clock-clksrc.h>
 #include <plat/s5p-clock.h>
@@ -88,14 +90,6 @@ struct clk clk_fout_vpll = {
         .ctrlbit        = (1 << 31),
 };
 
-/* ARM clock */
-struct clk clk_arm = {
-        .name           = "armclk",
-        .id             = -1,
-        .rate           = 0,
-        .ctrlbit        = 0,
-};
-
 /* Possible clock sources for APLL Mux */
 static struct clk *clk_src_apll_list[] = {
         [0] = &clk_fin_apll,
@@ -156,6 +150,24 @@ int s5p_gatectrl(void __iomem *reg, struct clk *clk, int enable)
         return 0;
 }
 
+int s5p_epll_enable(struct clk *clk, int enable)
+{
+        unsigned int ctrlbit = clk->ctrlbit;
+        unsigned int epll_con = __raw_readl(S5P_EPLL_CON) & ~ctrlbit;
+
+        if (enable)
+                __raw_writel(epll_con | ctrlbit, S5P_EPLL_CON);
+        else
+                __raw_writel(epll_con, S5P_EPLL_CON);
+
+        return 0;
+}
+
+unsigned long s5p_epll_get_rate(struct clk *clk)
+{
+        return clk->rate;
+}
+
 static struct clk *s5p_clks[] __initdata = {
         &clk_ext_xtal_mux,
         &clk_48m,
@@ -165,7 +177,6 @@ static struct clk *s5p_clks[] __initdata = {
         &clk_fout_epll,
         &clk_fout_dpll,
         &clk_fout_vpll,
-        &clk_arm,
         &clk_vpll,
         &clk_xusbxti,
 };
diff --git a/arch/arm/plat-s5p/include/plat/irqs.h b/arch/arm/plat-s5p/include/plat/irqs.h
index 3fb3a3a17465..ba9121c60a2a 100644
--- a/arch/arm/plat-s5p/include/plat/irqs.h
+++ b/arch/arm/plat-s5p/include/plat/irqs.h
@@ -94,4 +94,22 @@
                                                 ((irq) - S5P_EINT_BASE1) : \
                                                 ((irq) + 16 - S5P_EINT_BASE2))
 
+#define IRQ_EINT_BIT(x)         EINT_OFFSET(x)
+
+/* Typically only a few gpio chips require gpio interrupt support.
+   To avoid memory waste irq descriptors are allocated only for
+   S5P_GPIOINT_GROUP_COUNT chips, each with total number of
+   S5P_GPIOINT_GROUP_SIZE pins/irqs. Each GPIOINT group can be assiged
+   to any gpio chip with the s5p_register_gpio_interrupt() function */
+#define S5P_GPIOINT_GROUP_COUNT 4
+#define S5P_GPIOINT_GROUP_SIZE  8
+#define S5P_GPIOINT_COUNT       (S5P_GPIOINT_GROUP_COUNT * S5P_GPIOINT_GROUP_SIZE)
+
+/* IRQ types common for all s5p platforms */
+#define S5P_IRQ_TYPE_LEVEL_LOW          (0x00)
+#define S5P_IRQ_TYPE_LEVEL_HIGH         (0x01)
+#define S5P_IRQ_TYPE_EDGE_FALLING       (0x02)
+#define S5P_IRQ_TYPE_EDGE_RISING        (0x03)
+#define S5P_IRQ_TYPE_EDGE_BOTH          (0x04)
+
 #endif /* __ASM_PLAT_S5P_IRQS_H */
diff --git a/arch/arm/plat-s5p/include/plat/map-s5p.h b/arch/arm/plat-s5p/include/plat/map-s5p.h
index c4ff88bf6477..fef353d44513 100644
--- a/arch/arm/plat-s5p/include/plat/map-s5p.h
+++ b/arch/arm/plat-s5p/include/plat/map-s5p.h
@@ -13,24 +13,38 @@
 #ifndef __ASM_PLAT_MAP_S5P_H
 #define __ASM_PLAT_MAP_S5P_H __FILE__
 
-#define S5P_VA_CHIPID           S3C_ADDR(0x00700000)
-#define S5P_VA_GPIO             S3C_ADDR(0x00500000)
-#define S5P_VA_SYSTIMER         S3C_ADDR(0x01200000)
-#define S5P_VA_SROMC            S3C_ADDR(0x01100000)
-#define S5P_VA_SYSRAM           S3C_ADDR(0x01180000)
-
-#define S5P_VA_COMBINER_BASE    S3C_ADDR(0x00600000)
+#define S5P_VA_CHIPID           S3C_ADDR(0x02000000)
+#define S5P_VA_CMU              S3C_ADDR(0x02100000)
+#define S5P_VA_GPIO             S3C_ADDR(0x02200000)
+#define S5P_VA_GPIO1            S5P_VA_GPIO
+#define S5P_VA_GPIO2            S3C_ADDR(0x02240000)
+#define S5P_VA_GPIO3            S3C_ADDR(0x02280000)
+
+#define S5P_VA_SYSRAM           S3C_ADDR(0x02400000)
+#define S5P_VA_DMC0             S3C_ADDR(0x02440000)
+#define S5P_VA_DMC1             S3C_ADDR(0x02480000)
+#define S5P_VA_SROMC            S3C_ADDR(0x024C0000)
+
+#define S5P_VA_SYSTIMER         S3C_ADDR(0x02500000)
+#define S5P_VA_L2CC             S3C_ADDR(0x02600000)
+
+#define S5P_VA_COMBINER_BASE    S3C_ADDR(0x02700000)
 #define S5P_VA_COMBINER(x)      (S5P_VA_COMBINER_BASE + ((x) >> 2) * 0x10)
 
-#define S5P_VA_COREPERI_BASE    S3C_ADDR(0x00800000)
+#define S5P_VA_COREPERI_BASE    S3C_ADDR(0x02800000)
 #define S5P_VA_COREPERI(x)      (S5P_VA_COREPERI_BASE + (x))
 #define S5P_VA_SCU              S5P_VA_COREPERI(0x0)
 #define S5P_VA_GIC_CPU          S5P_VA_COREPERI(0x100)
 #define S5P_VA_TWD              S5P_VA_COREPERI(0x600)
 #define S5P_VA_GIC_DIST         S5P_VA_COREPERI(0x1000)
 
-#define S5P_VA_L2CC             S3C_ADDR(0x00900000)
-#define S5P_VA_CMU              S3C_ADDR(0x00920000)
+#define S3C_VA_USB_HSPHY        S3C_ADDR(0x02900000)
+
+#define VA_VIC(x)               (S3C_VA_IRQ + ((x) * 0x10000))
+#define VA_VIC0                 VA_VIC(0)
+#define VA_VIC1                 VA_VIC(1)
+#define VA_VIC2                 VA_VIC(2)
+#define VA_VIC3                 VA_VIC(3)
 
 #define S5P_VA_UART(x)          (S3C_VA_UART + ((x) * S3C_UART_OFFSET))
 #define S5P_VA_UART0            S5P_VA_UART(0)
@@ -42,10 +56,4 @@
 #define S3C_UART_OFFSET         (0x400)
 #endif
 
-#define VA_VIC(x)               (S3C_VA_IRQ + ((x) * 0x10000))
-#define VA_VIC0                 VA_VIC(0)
-#define VA_VIC1                 VA_VIC(1)
-#define VA_VIC2                 VA_VIC(2)
-#define VA_VIC3                 VA_VIC(3)
-
 #endif /* __ASM_PLAT_MAP_S5P_H */
diff --git a/arch/arm/plat-s5p/include/plat/s5p-clock.h b/arch/arm/plat-s5p/include/plat/s5p-clock.h
index 17036c898409..2b6dcff8ab2b 100644
--- a/arch/arm/plat-s5p/include/plat/s5p-clock.h
+++ b/arch/arm/plat-s5p/include/plat/s5p-clock.h
@@ -43,4 +43,8 @@ extern struct clksrc_sources clk_src_dpll;
 
 extern int s5p_gatectrl(void __iomem *reg, struct clk *clk, int enable);
 
+/* Common EPLL operations for S5P platform */
+extern int s5p_epll_enable(struct clk *clk, int enable);
+extern unsigned long s5p_epll_get_rate(struct clk *clk);
+
 #endif /* __ASM_PLAT_S5P_CLOCK_H */
diff --git a/arch/arm/plat-s5p/irq-eint.c b/arch/arm/plat-s5p/irq-eint.c
index f36cd3327025..752f1a645f9d 100644
--- a/arch/arm/plat-s5p/irq-eint.c
+++ b/arch/arm/plat-s5p/irq-eint.c
@@ -67,23 +67,23 @@ static int s5p_irq_eint_set_type(unsigned int irq, unsigned int type)
 
         switch (type) {
         case IRQ_TYPE_EDGE_RISING:
-                newvalue = S5P_EXTINT_RISEEDGE;
+                newvalue = S5P_IRQ_TYPE_EDGE_RISING;
                 break;
 
         case IRQ_TYPE_EDGE_FALLING:
-                newvalue = S5P_EXTINT_FALLEDGE;
+                newvalue = S5P_IRQ_TYPE_EDGE_FALLING;
                 break;
 
         case IRQ_TYPE_EDGE_BOTH:
-                newvalue = S5P_EXTINT_BOTHEDGE;
+                newvalue = S5P_IRQ_TYPE_EDGE_BOTH;
                 break;
 
         case IRQ_TYPE_LEVEL_LOW:
-                newvalue = S5P_EXTINT_LOWLEV;
+                newvalue = S5P_IRQ_TYPE_LEVEL_LOW;
                 break;
 
         case IRQ_TYPE_LEVEL_HIGH:
-                newvalue = S5P_EXTINT_HILEV;
+                newvalue = S5P_IRQ_TYPE_LEVEL_HIGH;
                 break;
 
         default:
diff --git a/arch/arm/plat-s5p/irq-gpioint.c b/arch/arm/plat-s5p/irq-gpioint.c
new file mode 100644
index 000000000000..0e5dc8cbf5e3
--- /dev/null
+++ b/arch/arm/plat-s5p/irq-gpioint.c
@@ -0,0 +1,237 @@
+/* linux/arch/arm/plat-s5p/irq-gpioint.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ * Author: Kyungmin Park <kyungmin.park@samsung.com>
+ * Author: Joonyoung Shim <jy0922.shim@samsung.com>
+ * Author: Marek Szyprowski <m.szyprowski@samsung.com>
+ *
+ *  This program is free software; you can redistribute  it and/or modify it
+ *  under  the terms of  the GNU General  Public License as published by the
+ *  Free Software Foundation;  either version 2 of the  License, or (at your
+ *  option) any later version.
+ *
+ */
+
+#include <linux/kernel.h>
+#include <linux/interrupt.h>
+#include <linux/irq.h>
+#include <linux/io.h>
+#include <linux/gpio.h>
+
+#include <mach/map.h>
+#include <plat/gpio-core.h>
+#include <plat/gpio-cfg.h>
+
+#define S5P_GPIOREG(x)                  (S5P_VA_GPIO + (x))
+
+#define GPIOINT_CON_OFFSET              0x700
+#define GPIOINT_MASK_OFFSET             0x900
+#define GPIOINT_PEND_OFFSET             0xA00
+
+static struct s3c_gpio_chip *irq_chips[S5P_GPIOINT_GROUP_MAXNR];
+
+static int s5p_gpioint_get_group(unsigned int irq)
+{
+        struct gpio_chip *chip = get_irq_data(irq);
+        struct s3c_gpio_chip *s3c_chip = container_of(chip,
+                        struct s3c_gpio_chip, chip);
+        int group;
+
+        for (group = 0; group < S5P_GPIOINT_GROUP_MAXNR; group++)
+                if (s3c_chip == irq_chips[group])
+                        break;
+
+        return group;
+}
+
+static int s5p_gpioint_get_offset(unsigned int irq)
+{
+        struct gpio_chip *chip = get_irq_data(irq);
+        struct s3c_gpio_chip *s3c_chip = container_of(chip,
+                        struct s3c_gpio_chip, chip);
+
+        return irq - s3c_chip->irq_base;
+}
+
+static void s5p_gpioint_ack(unsigned int irq)
+{
+        int group, offset, pend_offset;
+        unsigned int value;
+
+        group = s5p_gpioint_get_group(irq);
+        offset = s5p_gpioint_get_offset(irq);
+        pend_offset = group << 2;
+
+        value = __raw_readl(S5P_GPIOREG(GPIOINT_PEND_OFFSET) + pend_offset);
+        value |= 1 << offset;
+        __raw_writel(value, S5P_GPIOREG(GPIOINT_PEND_OFFSET) + pend_offset);
+}
+
+static void s5p_gpioint_mask(unsigned int irq)
+{
+        int group, offset, mask_offset;
+        unsigned int value;
+
+        group = s5p_gpioint_get_group(irq);
+        offset = s5p_gpioint_get_offset(irq);
+        mask_offset = group << 2;
+
+        value = __raw_readl(S5P_GPIOREG(GPIOINT_MASK_OFFSET) + mask_offset);
+        value |= 1 << offset;
+        __raw_writel(value, S5P_GPIOREG(GPIOINT_MASK_OFFSET) + mask_offset);
+}
+
+static void s5p_gpioint_unmask(unsigned int irq)
+{
+        int group, offset, mask_offset;
+        unsigned int value;
+
+        group = s5p_gpioint_get_group(irq);
+        offset = s5p_gpioint_get_offset(irq);
+        mask_offset = group << 2;
+
+        value = __raw_readl(S5P_GPIOREG(GPIOINT_MASK_OFFSET) + mask_offset);
+        value &= ~(1 << offset);
+        __raw_writel(value, S5P_GPIOREG(GPIOINT_MASK_OFFSET) + mask_offset);
+}
+
+static void s5p_gpioint_mask_ack(unsigned int irq)
+{
+        s5p_gpioint_mask(irq);
+        s5p_gpioint_ack(irq);
+}
+
+static int s5p_gpioint_set_type(unsigned int irq, unsigned int type)
+{
+        int group, offset, con_offset;
+        unsigned int value;
+
+        group = s5p_gpioint_get_group(irq);
+        offset = s5p_gpioint_get_offset(irq);
+        con_offset = group << 2;
+
+        switch (type) {
+        case IRQ_TYPE_EDGE_RISING:
+                type = S5P_IRQ_TYPE_EDGE_RISING;
+                break;
+        case IRQ_TYPE_EDGE_FALLING:
+                type = S5P_IRQ_TYPE_EDGE_FALLING;
+                break;
+        case IRQ_TYPE_EDGE_BOTH:
+                type = S5P_IRQ_TYPE_EDGE_BOTH;
+                break;
+        case IRQ_TYPE_LEVEL_HIGH:
+                type = S5P_IRQ_TYPE_LEVEL_HIGH;
+                break;
+        case IRQ_TYPE_LEVEL_LOW:
+                type = S5P_IRQ_TYPE_LEVEL_LOW;
+                break;
+        case IRQ_TYPE_NONE:
+        default:
+                printk(KERN_WARNING "No irq type\n");
+                return -EINVAL;
+        }
+
+        value = __raw_readl(S5P_GPIOREG(GPIOINT_CON_OFFSET) + con_offset);
+        value &= ~(0x7 << (offset * 0x4));
+        value |= (type << (offset * 0x4));
+        __raw_writel(value, S5P_GPIOREG(GPIOINT_CON_OFFSET) + con_offset);
+
+        return 0;
+}
+
+struct irq_chip s5p_gpioint = {
+        .name           = "s5p_gpioint",
+        .ack            = s5p_gpioint_ack,
+        .mask           = s5p_gpioint_mask,
+        .mask_ack       = s5p_gpioint_mask_ack,
+        .unmask         = s5p_gpioint_unmask,
+        .set_type       = s5p_gpioint_set_type,
+};
+
+static void s5p_gpioint_handler(unsigned int irq, struct irq_desc *desc)
+{
+        int group, offset, pend_offset, mask_offset;
+        int real_irq;
+        unsigned int pend, mask;
+
+        for (group = 0; group < S5P_GPIOINT_GROUP_MAXNR; group++) {
+                pend_offset = group << 2;
+                pend = __raw_readl(S5P_GPIOREG(GPIOINT_PEND_OFFSET) +
+                                pend_offset);
+                if (!pend)
+                        continue;
+
+                mask_offset = group << 2;
+                mask = __raw_readl(S5P_GPIOREG(GPIOINT_MASK_OFFSET) +
+                                mask_offset);
+                pend &= ~mask;
+
+                for (offset = 0; offset < 8; offset++) {
+                        if (pend & (1 << offset)) {
+                                struct s3c_gpio_chip *chip = irq_chips[group];
+                                if (chip) {
+                                        real_irq = chip->irq_base + offset;
+                                        generic_handle_irq(real_irq);
+                                }
+                        }
+                }
+        }
+}
+
+static __init int s5p_gpioint_add(struct s3c_gpio_chip *chip)
+{
+        static int used_gpioint_groups = 0;
+        static bool handler_registered = 0;
+        int irq, group = chip->group;
+        int i;
+
+        if (used_gpioint_groups >= S5P_GPIOINT_GROUP_COUNT)
+                return -ENOMEM;
+
+        chip->irq_base = S5P_GPIOINT_BASE +
+                         used_gpioint_groups * S5P_GPIOINT_GROUP_SIZE;
+        used_gpioint_groups++;
+
+        if (!handler_registered) {
+                set_irq_chained_handler(IRQ_GPIOINT, s5p_gpioint_handler);
+                handler_registered = 1;
+        }
+
+        irq_chips[group] = chip;
+        for (i = 0; i < chip->chip.ngpio; i++) {
+                irq = chip->irq_base + i;
+                set_irq_chip(irq, &s5p_gpioint);
+                set_irq_data(irq, &chip->chip);
+                set_irq_handler(irq, handle_level_irq);
+                set_irq_flags(irq, IRQF_VALID);
+        }
+        return 0;
+}
+
+int __init s5p_register_gpio_interrupt(int pin)
+{
+        struct s3c_gpio_chip *my_chip = s3c_gpiolib_getchip(pin);
+        int offset, group;
+        int ret;
+
+        if (!my_chip)
+                return -EINVAL;
+
+        offset = pin - my_chip->chip.base;
+        group = my_chip->group;
+
+        /* check if the group has been already registered */
+        if (my_chip->irq_base)
+                return my_chip->irq_base + offset;
+
+        /* register gpio group */
+        ret = s5p_gpioint_add(my_chip);
+        if (ret == 0) {
+                my_chip->chip.to_irq = samsung_gpiolib_to_irq;
+                printk(KERN_INFO "Registered interrupt support for gpio group %d.\n",
+                       group);
+                return my_chip->irq_base + offset;
+        }
+        return ret;
+}
diff --git a/arch/arm/plat-s5p/irq-pm.c b/arch/arm/plat-s5p/irq-pm.c
new file mode 100644
index 000000000000..dc33b9ecda45
--- /dev/null
+++ b/arch/arm/plat-s5p/irq-pm.c
@@ -0,0 +1,93 @@
+/* linux/arch/arm/plat-s5p/irq-pm.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * Based on arch/arm/plat-s3c24xx/irq-pm.c,
+ * Copyright (c) 2003,2004 Simtec Electronics
+ *      Ben Dooks <ben@simtec.co.uk>
+ *      http://armlinux.simtec.co.uk/
+ *
+ * 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/init.h>
+#include <linux/module.h>
+#include <linux/interrupt.h>
+#include <linux/sysdev.h>
+
+#include <plat/cpu.h>
+#include <plat/irqs.h>
+#include <plat/pm.h>
+#include <mach/map.h>
+
+#include <mach/regs-gpio.h>
+#include <mach/regs-irq.h>
+
+/* state for IRQs over sleep */
+
+/* default is to allow for EINT0..EINT31, and IRQ_RTC_TIC, IRQ_RTC_ALARM,
+ * as wakeup sources
+ *
+ * set bit to 1 in allow bitfield to enable the wakeup settings on it
+*/
+
+unsigned long s3c_irqwake_intallow      = 0x00000006L;
+unsigned long s3c_irqwake_eintallow     = 0xffffffffL;
+
+int s3c_irq_wake(unsigned int irqno, unsigned int state)
+{
+        unsigned long irqbit;
+
+        switch (irqno) {
+        case IRQ_RTC_TIC:
+        case IRQ_RTC_ALARM:
+                irqbit = 1 << (irqno + 1 - IRQ_RTC_ALARM);
+                if (!state)
+                        s3c_irqwake_intmask |= irqbit;
+                else
+                        s3c_irqwake_intmask &= ~irqbit;
+                break;
+        default:
+                return -ENOENT;
+        }
+        return 0;
+}
+
+static struct sleep_save eint_save[] = {
+        SAVE_ITEM(S5P_EINT_CON(0)),
+        SAVE_ITEM(S5P_EINT_CON(1)),
+        SAVE_ITEM(S5P_EINT_CON(2)),
+        SAVE_ITEM(S5P_EINT_CON(3)),
+
+        SAVE_ITEM(S5P_EINT_FLTCON(0)),
+        SAVE_ITEM(S5P_EINT_FLTCON(1)),
+        SAVE_ITEM(S5P_EINT_FLTCON(2)),
+        SAVE_ITEM(S5P_EINT_FLTCON(3)),
+        SAVE_ITEM(S5P_EINT_FLTCON(4)),
+        SAVE_ITEM(S5P_EINT_FLTCON(5)),
+        SAVE_ITEM(S5P_EINT_FLTCON(6)),
+        SAVE_ITEM(S5P_EINT_FLTCON(7)),
+
+        SAVE_ITEM(S5P_EINT_MASK(0)),
+        SAVE_ITEM(S5P_EINT_MASK(1)),
+        SAVE_ITEM(S5P_EINT_MASK(2)),
+        SAVE_ITEM(S5P_EINT_MASK(3)),
+};
+
+int s3c24xx_irq_suspend(struct sys_device *dev, pm_message_t state)
+{
+        s3c_pm_do_save(eint_save, ARRAY_SIZE(eint_save));
+
+        return 0;
+}
+
+int s3c24xx_irq_resume(struct sys_device *dev)
+{
+        s3c_pm_do_restore(eint_save, ARRAY_SIZE(eint_save));
+
+        return 0;
+}
+
diff --git a/arch/arm/plat-s5p/pm.c b/arch/arm/plat-s5p/pm.c
new file mode 100644
index 000000000000..d592b6304b48
--- /dev/null
+++ b/arch/arm/plat-s5p/pm.c
@@ -0,0 +1,52 @@
+/* linux/arch/arm/plat-s5p/pm.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com
+ *
+ * S5P Power Manager (Suspend-To-RAM) support
+ *
+ * Based on arch/arm/plat-s3c24xx/pm.c
+ * Copyright (c) 2004,2006 Simtec Electronics
+ *      Ben Dooks <ben@simtec.co.uk>
+ *
+ * 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/suspend.h>
+#include <plat/pm.h>
+
+#define PFX "s5p pm: "
+
+/* s3c_pm_check_resume_pin
+ *
+ * check to see if the pin is configured correctly for sleep mode, and
+ * make any necessary adjustments if it is not
+*/
+
+static void s3c_pm_check_resume_pin(unsigned int pin, unsigned int irqoffs)
+{
+        /* nothing here yet */
+}
+
+/* s3c_pm_configure_extint
+ *
+ * configure all external interrupt pins
+*/
+
+void s3c_pm_configure_extint(void)
+{
+        /* nothing here yet */
+}
+
+void s3c_pm_restore_core(void)
+{
+        /* nothing here yet */
+}
+
+void s3c_pm_save_core(void)
+{
+        /* nothing here yet */
+}
+
diff --git a/arch/arm/plat-samsung/Kconfig b/arch/arm/plat-samsung/Kconfig
index 7c0bde781167..dcd6eff4ee53 100644
--- a/arch/arm/plat-samsung/Kconfig
+++ b/arch/arm/plat-samsung/Kconfig
@@ -180,6 +180,31 @@ config S3C_DEV_I2C2
         help
           Compile in platform device definitions for I2C channel 2
 
+config S3C_DEV_I2C3
+        bool
+        help
+          Compile in platform device definition for I2C controller 3
+
+config S3C_DEV_I2C4
+        bool
+        help
+          Compile in platform device definition for I2C controller 4
+
+config S3C_DEV_I2C5
+        bool
+        help
+          Compile in platform device definition for I2C controller 5
+
+config S3C_DEV_I2C6
+        bool
+        help
+          Compile in platform device definition for I2C controller 6
+
+config S3C_DEV_I2C7
+        bool
+        help
+          Compile in platform device definition for I2C controller 7
+
 config S3C_DEV_FB
         bool
         help
diff --git a/arch/arm/plat-samsung/Makefile b/arch/arm/plat-samsung/Makefile
index 4d8ff923207a..afcce474af8e 100644
--- a/arch/arm/plat-samsung/Makefile
+++ b/arch/arm/plat-samsung/Makefile
@@ -40,6 +40,11 @@ obj-$(CONFIG_S3C_DEV_HWMON)	+= dev-hwmon.o
 obj-y                           += dev-i2c0.o
 obj-$(CONFIG_S3C_DEV_I2C1)      += dev-i2c1.o
 obj-$(CONFIG_S3C_DEV_I2C2)      += dev-i2c2.o
+obj-$(CONFIG_S3C_DEV_I2C3)      += dev-i2c3.o
+obj-$(CONFIG_S3C_DEV_I2C4)      += dev-i2c4.o
+obj-$(CONFIG_S3C_DEV_I2C5)      += dev-i2c5.o
+obj-$(CONFIG_S3C_DEV_I2C6)      += dev-i2c6.o
+obj-$(CONFIG_S3C_DEV_I2C7)      += dev-i2c7.o
 obj-$(CONFIG_S3C_DEV_FB)        += dev-fb.o
 obj-y                           += dev-uart.o
 obj-$(CONFIG_S3C_DEV_USB_HOST)  += dev-usb.o
diff --git a/arch/arm/plat-samsung/dev-hsmmc.c b/arch/arm/plat-samsung/dev-hsmmc.c
index 9d2be0941410..db7a65c7f127 100644
--- a/arch/arm/plat-samsung/dev-hsmmc.c
+++ b/arch/arm/plat-samsung/dev-hsmmc.c
@@ -41,6 +41,7 @@ struct s3c_sdhci_platdata s3c_hsmmc0_def_platdata = {
         .max_width      = 4,
         .host_caps      = (MMC_CAP_4_BIT_DATA |
                            MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED),
+        .clk_type       = S3C_SDHCI_CLK_DIV_INTERNAL,
 };
 
 struct platform_device s3c_device_hsmmc0 = {
@@ -59,17 +60,20 @@ void s3c_sdhci0_set_platdata(struct s3c_sdhci_platdata *pd)
 {
         struct s3c_sdhci_platdata *set = &s3c_hsmmc0_def_platdata;
 
-        set->max_width = pd->max_width;
         set->cd_type = pd->cd_type;
         set->ext_cd_init = pd->ext_cd_init;
         set->ext_cd_cleanup = pd->ext_cd_cleanup;
         set->ext_cd_gpio = pd->ext_cd_gpio;
         set->ext_cd_gpio_invert = pd->ext_cd_gpio_invert;
 
+        if (pd->max_width)
+                set->max_width = pd->max_width;
         if (pd->cfg_gpio)
                 set->cfg_gpio = pd->cfg_gpio;
         if (pd->cfg_card)
                 set->cfg_card = pd->cfg_card;
         if (pd->host_caps)
-                set->host_caps = pd->host_caps;
+                set->host_caps |= pd->host_caps;
+        if (pd->clk_type)
+                set->clk_type = pd->clk_type;
 }
diff --git a/arch/arm/plat-samsung/dev-hsmmc1.c b/arch/arm/plat-samsung/dev-hsmmc1.c
index a6c8295840af..2497321f08d7 100644
--- a/arch/arm/plat-samsung/dev-hsmmc1.c
+++ b/arch/arm/plat-samsung/dev-hsmmc1.c
@@ -41,6 +41,7 @@ struct s3c_sdhci_platdata s3c_hsmmc1_def_platdata = {
         .max_width      = 4,
         .host_caps      = (MMC_CAP_4_BIT_DATA |
                            MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED),
+        .clk_type       = S3C_SDHCI_CLK_DIV_INTERNAL,
 };
 
 struct platform_device s3c_device_hsmmc1 = {
@@ -59,17 +60,20 @@ void s3c_sdhci1_set_platdata(struct s3c_sdhci_platdata *pd)
 {
         struct s3c_sdhci_platdata *set = &s3c_hsmmc1_def_platdata;
 
-        set->max_width = pd->max_width;
         set->cd_type = pd->cd_type;
         set->ext_cd_init = pd->ext_cd_init;
         set->ext_cd_cleanup = pd->ext_cd_cleanup;
         set->ext_cd_gpio = pd->ext_cd_gpio;
         set->ext_cd_gpio_invert = pd->ext_cd_gpio_invert;
 
+        if (pd->max_width)
+                set->max_width = pd->max_width;
         if (pd->cfg_gpio)
                 set->cfg_gpio = pd->cfg_gpio;
         if (pd->cfg_card)
                 set->cfg_card = pd->cfg_card;
         if (pd->host_caps)
-                set->host_caps = pd->host_caps;
+                set->host_caps |= pd->host_caps;
+        if (pd->clk_type)
+                set->clk_type = pd->clk_type;
 }
diff --git a/arch/arm/plat-samsung/dev-hsmmc2.c b/arch/arm/plat-samsung/dev-hsmmc2.c
index cb0d7143381a..f60aedba417c 100644
--- a/arch/arm/plat-samsung/dev-hsmmc2.c
+++ b/arch/arm/plat-samsung/dev-hsmmc2.c
@@ -42,6 +42,7 @@ struct s3c_sdhci_platdata s3c_hsmmc2_def_platdata = {
         .max_width      = 4,
         .host_caps      = (MMC_CAP_4_BIT_DATA |
                            MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED),
+        .clk_type       = S3C_SDHCI_CLK_DIV_INTERNAL,
 };
 
 struct platform_device s3c_device_hsmmc2 = {
@@ -60,17 +61,20 @@ void s3c_sdhci2_set_platdata(struct s3c_sdhci_platdata *pd)
 {
         struct s3c_sdhci_platdata *set = &s3c_hsmmc2_def_platdata;
 
-        set->max_width = pd->max_width;
         set->cd_type = pd->cd_type;
         set->ext_cd_init = pd->ext_cd_init;
         set->ext_cd_cleanup = pd->ext_cd_cleanup;
         set->ext_cd_gpio = pd->ext_cd_gpio;
         set->ext_cd_gpio_invert = pd->ext_cd_gpio_invert;
 
+        if (pd->max_width)
+                set->max_width = pd->max_width;
         if (pd->cfg_gpio)
                 set->cfg_gpio = pd->cfg_gpio;
         if (pd->cfg_card)
                 set->cfg_card = pd->cfg_card;
         if (pd->host_caps)
-                set->host_caps = pd->host_caps;
+                set->host_caps |= pd->host_caps;
+        if (pd->clk_type)
+                set->clk_type = pd->clk_type;
 }
diff --git a/arch/arm/plat-samsung/dev-hsmmc3.c b/arch/arm/plat-samsung/dev-hsmmc3.c
index 85aaf0f2842f..ede776f20e62 100644
--- a/arch/arm/plat-samsung/dev-hsmmc3.c
+++ b/arch/arm/plat-samsung/dev-hsmmc3.c
@@ -33,8 +33,8 @@ static struct resource s3c_hsmmc3_resource[] = {
                 .flags  = IORESOURCE_MEM,
         },
         [1] = {
-                .start  = IRQ_MMC3,
-                .end    = IRQ_MMC3,
+                .start  = IRQ_HSMMC3,
+                .end    = IRQ_HSMMC3,
                 .flags  = IORESOURCE_IRQ,
         }
 };
@@ -45,6 +45,7 @@ struct s3c_sdhci_platdata s3c_hsmmc3_def_platdata = {
         .max_width      = 4,
         .host_caps      = (MMC_CAP_4_BIT_DATA |
                            MMC_CAP_MMC_HIGHSPEED | MMC_CAP_SD_HIGHSPEED),
+        .clk_type       = S3C_SDHCI_CLK_DIV_INTERNAL,
 };
 
 struct platform_device s3c_device_hsmmc3 = {
@@ -63,15 +64,20 @@ void s3c_sdhci3_set_platdata(struct s3c_sdhci_platdata *pd)
 {
         struct s3c_sdhci_platdata *set = &s3c_hsmmc3_def_platdata;
 
-        set->max_width = pd->max_width;
         set->cd_type = pd->cd_type;
         set->ext_cd_init = pd->ext_cd_init;
         set->ext_cd_cleanup = pd->ext_cd_cleanup;
         set->ext_cd_gpio = pd->ext_cd_gpio;
         set->ext_cd_gpio_invert = pd->ext_cd_gpio_invert;
 
+        if (pd->max_width)
+                set->max_width = pd->max_width;
         if (pd->cfg_gpio)
                 set->cfg_gpio = pd->cfg_gpio;
         if (pd->cfg_card)
                 set->cfg_card = pd->cfg_card;
+        if (pd->host_caps)
+                set->host_caps |= pd->host_caps;
+        if (pd->clk_type)
+                set->clk_type = pd->clk_type;
 }
diff --git a/arch/arm/plat-samsung/dev-i2c2.c b/arch/arm/plat-samsung/dev-i2c2.c
index 07036dee09e7..ff4ba69b6830 100644
--- a/arch/arm/plat-samsung/dev-i2c2.c
+++ b/arch/arm/plat-samsung/dev-i2c2.c
@@ -32,8 +32,8 @@ static struct resource s3c_i2c_resource[] = {
                 .flags = IORESOURCE_MEM,
         },
         [1] = {
-                .start = IRQ_CAN0,
-                .end   = IRQ_CAN0,
+                .start = IRQ_IIC2,
+                .end   = IRQ_IIC2,
                 .flags = IORESOURCE_IRQ,
         },
 };
diff --git a/arch/arm/plat-samsung/dev-i2c3.c b/arch/arm/plat-samsung/dev-i2c3.c
new file mode 100644
index 000000000000..8586a10014b7
--- /dev/null
+++ b/arch/arm/plat-samsung/dev-i2c3.c
@@ -0,0 +1,68 @@
+/* linux/arch/arm/plat-samsung/dev-i2c3.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S5P series device definition for i2c device 3
+ *
+ * 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/gfp.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+
+#include <mach/irqs.h>
+#include <mach/map.h>
+
+#include <plat/regs-iic.h>
+#include <plat/iic.h>
+#include <plat/devs.h>
+#include <plat/cpu.h>
+
+static struct resource s3c_i2c_resource[] = {
+        [0] = {
+                .start  = S3C_PA_IIC3,
+                .end    = S3C_PA_IIC3 + SZ_4K - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = IRQ_IIC3,
+                .end    = IRQ_IIC3,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+struct platform_device s3c_device_i2c3 = {
+        .name           = "s3c2440-i2c",
+        .id             = 3,
+        .num_resources  = ARRAY_SIZE(s3c_i2c_resource),
+        .resource       = s3c_i2c_resource,
+};
+
+static struct s3c2410_platform_i2c default_i2c_data3 __initdata = {
+        .flags          = 0,
+        .bus_num        = 3,
+        .slave_addr     = 0x10,
+        .frequency      = 100*1000,
+        .sda_delay      = 100,
+};
+
+void __init s3c_i2c3_set_platdata(struct s3c2410_platform_i2c *pd)
+{
+        struct s3c2410_platform_i2c *npd;
+
+        if (!pd)
+                pd = &default_i2c_data3;
+
+        npd = kmemdup(pd, sizeof(struct s3c2410_platform_i2c), GFP_KERNEL);
+        if (!npd)
+                printk(KERN_ERR "%s: no memory for platform data\n", __func__);
+        else if (!npd->cfg_gpio)
+                npd->cfg_gpio = s3c_i2c3_cfg_gpio;
+
+        s3c_device_i2c3.dev.platform_data = npd;
+}
diff --git a/arch/arm/plat-samsung/dev-i2c4.c b/arch/arm/plat-samsung/dev-i2c4.c
new file mode 100644
index 000000000000..df2159e2daa6
--- /dev/null
+++ b/arch/arm/plat-samsung/dev-i2c4.c
@@ -0,0 +1,68 @@
+/* linux/arch/arm/plat-samsung/dev-i2c4.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S5P series device definition for i2c device 3
+ *
+ * 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/gfp.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+
+#include <mach/irqs.h>
+#include <mach/map.h>
+
+#include <plat/regs-iic.h>
+#include <plat/iic.h>
+#include <plat/devs.h>
+#include <plat/cpu.h>
+
+static struct resource s3c_i2c_resource[] = {
+        [0] = {
+                .start  = S3C_PA_IIC4,
+                .end    = S3C_PA_IIC4 + SZ_4K - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = IRQ_IIC4,
+                .end    = IRQ_IIC4,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+struct platform_device s3c_device_i2c4 = {
+        .name           = "s3c2440-i2c",
+        .id             = 4,
+        .num_resources  = ARRAY_SIZE(s3c_i2c_resource),
+        .resource       = s3c_i2c_resource,
+};
+
+static struct s3c2410_platform_i2c default_i2c_data4 __initdata = {
+        .flags          = 0,
+        .bus_num        = 4,
+        .slave_addr     = 0x10,
+        .frequency      = 100*1000,
+        .sda_delay      = 100,
+};
+
+void __init s3c_i2c4_set_platdata(struct s3c2410_platform_i2c *pd)
+{
+        struct s3c2410_platform_i2c *npd;
+
+        if (!pd)
+                pd = &default_i2c_data4;
+
+        npd = kmemdup(pd, sizeof(struct s3c2410_platform_i2c), GFP_KERNEL);
+        if (!npd)
+                printk(KERN_ERR "%s: no memory for platform data\n", __func__);
+        else if (!npd->cfg_gpio)
+                npd->cfg_gpio = s3c_i2c4_cfg_gpio;
+
+        s3c_device_i2c4.dev.platform_data = npd;
+}
diff --git a/arch/arm/plat-samsung/dev-i2c5.c b/arch/arm/plat-samsung/dev-i2c5.c
new file mode 100644
index 000000000000..0499c2c3877b
--- /dev/null
+++ b/arch/arm/plat-samsung/dev-i2c5.c
@@ -0,0 +1,68 @@
+/* linux/arch/arm/plat-samsung/dev-i2c3.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S5P series device definition for i2c device 3
+ *
+ * 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/gfp.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+
+#include <mach/irqs.h>
+#include <mach/map.h>
+
+#include <plat/regs-iic.h>
+#include <plat/iic.h>
+#include <plat/devs.h>
+#include <plat/cpu.h>
+
+static struct resource s3c_i2c_resource[] = {
+        [0] = {
+                .start  = S3C_PA_IIC5,
+                .end    = S3C_PA_IIC5 + SZ_4K - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = IRQ_IIC5,
+                .end    = IRQ_IIC5,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+struct platform_device s3c_device_i2c5 = {
+        .name           = "s3c2440-i2c",
+        .id             = 5,
+        .num_resources  = ARRAY_SIZE(s3c_i2c_resource),
+        .resource       = s3c_i2c_resource,
+};
+
+static struct s3c2410_platform_i2c default_i2c_data5 __initdata = {
+        .flags          = 0,
+        .bus_num        = 5,
+        .slave_addr     = 0x10,
+        .frequency      = 100*1000,
+        .sda_delay      = 100,
+};
+
+void __init s3c_i2c5_set_platdata(struct s3c2410_platform_i2c *pd)
+{
+        struct s3c2410_platform_i2c *npd;
+
+        if (!pd)
+                pd = &default_i2c_data5;
+
+        npd = kmemdup(pd, sizeof(struct s3c2410_platform_i2c), GFP_KERNEL);
+        if (!npd)
+                printk(KERN_ERR "%s: no memory for platform data\n", __func__);
+        else if (!npd->cfg_gpio)
+                npd->cfg_gpio = s3c_i2c5_cfg_gpio;
+
+        s3c_device_i2c5.dev.platform_data = npd;
+}
diff --git a/arch/arm/plat-samsung/dev-i2c6.c b/arch/arm/plat-samsung/dev-i2c6.c
new file mode 100644
index 000000000000..4083108908a8
--- /dev/null
+++ b/arch/arm/plat-samsung/dev-i2c6.c
@@ -0,0 +1,68 @@
+/* linux/arch/arm/plat-samsung/dev-i2c6.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S5P series device definition for i2c device 6
+ *
+ * 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/gfp.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+
+#include <mach/irqs.h>
+#include <mach/map.h>
+
+#include <plat/regs-iic.h>
+#include <plat/iic.h>
+#include <plat/devs.h>
+#include <plat/cpu.h>
+
+static struct resource s3c_i2c_resource[] = {
+        [0] = {
+                .start  = S3C_PA_IIC6,
+                .end    = S3C_PA_IIC6 + SZ_4K - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = IRQ_IIC6,
+                .end    = IRQ_IIC6,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+struct platform_device s3c_device_i2c6 = {
+        .name           = "s3c2440-i2c",
+        .id             = 6,
+        .num_resources  = ARRAY_SIZE(s3c_i2c_resource),
+        .resource       = s3c_i2c_resource,
+};
+
+static struct s3c2410_platform_i2c default_i2c_data6 __initdata = {
+        .flags          = 0,
+        .bus_num        = 6,
+        .slave_addr     = 0x10,
+        .frequency      = 100*1000,
+        .sda_delay      = 100,
+};
+
+void __init s3c_i2c6_set_platdata(struct s3c2410_platform_i2c *pd)
+{
+        struct s3c2410_platform_i2c *npd;
+
+        if (!pd)
+                pd = &default_i2c_data6;
+
+        npd = kmemdup(pd, sizeof(struct s3c2410_platform_i2c), GFP_KERNEL);
+        if (!npd)
+                printk(KERN_ERR "%s: no memory for platform data\n", __func__);
+        else if (!npd->cfg_gpio)
+                npd->cfg_gpio = s3c_i2c6_cfg_gpio;
+
+        s3c_device_i2c6.dev.platform_data = npd;
+}
diff --git a/arch/arm/plat-samsung/dev-i2c7.c b/arch/arm/plat-samsung/dev-i2c7.c
new file mode 100644
index 000000000000..1182451d7dce
--- /dev/null
+++ b/arch/arm/plat-samsung/dev-i2c7.c
@@ -0,0 +1,68 @@
+/* linux/arch/arm/plat-samsung/dev-i2c7.c
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S5P series device definition for i2c device 7
+ *
+ * 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/gfp.h>
+#include <linux/kernel.h>
+#include <linux/string.h>
+#include <linux/platform_device.h>
+
+#include <mach/irqs.h>
+#include <mach/map.h>
+
+#include <plat/regs-iic.h>
+#include <plat/iic.h>
+#include <plat/devs.h>
+#include <plat/cpu.h>
+
+static struct resource s3c_i2c_resource[] = {
+        [0] = {
+                .start  = S3C_PA_IIC7,
+                .end    = S3C_PA_IIC7 + SZ_4K - 1,
+                .flags  = IORESOURCE_MEM,
+        },
+        [1] = {
+                .start  = IRQ_IIC7,
+                .end    = IRQ_IIC7,
+                .flags  = IORESOURCE_IRQ,
+        },
+};
+
+struct platform_device s3c_device_i2c7 = {
+        .name           = "s3c2440-i2c",
+        .id             = 7,
+        .num_resources  = ARRAY_SIZE(s3c_i2c_resource),
+        .resource       = s3c_i2c_resource,
+};
+
+static struct s3c2410_platform_i2c default_i2c_data7 __initdata = {
+        .flags          = 0,
+        .bus_num        = 7,
+        .slave_addr     = 0x10,
+        .frequency      = 100*1000,
+        .sda_delay      = 100,
+};
+
+void __init s3c_i2c7_set_platdata(struct s3c2410_platform_i2c *pd)
+{
+        struct s3c2410_platform_i2c *npd;
+
+        if (!pd)
+                pd = &default_i2c_data7;
+
+        npd = kmemdup(pd, sizeof(struct s3c2410_platform_i2c), GFP_KERNEL);
+        if (!npd)
+                printk(KERN_ERR "%s: no memory for platform data\n", __func__);
+        else if (!npd->cfg_gpio)
+                npd->cfg_gpio = s3c_i2c7_cfg_gpio;
+
+        s3c_device_i2c7.dev.platform_data = npd;
+}
diff --git a/arch/arm/plat-samsung/gpio-config.c b/arch/arm/plat-samsung/gpio-config.c
index e3d41eaed1ff..b732b773b9af 100644
--- a/arch/arm/plat-samsung/gpio-config.c
+++ b/arch/arm/plat-samsung/gpio-config.c
@@ -41,6 +41,37 @@ int s3c_gpio_cfgpin(unsigned int pin, unsigned int config)
 }
 EXPORT_SYMBOL(s3c_gpio_cfgpin);
 
+int s3c_gpio_cfgpin_range(unsigned int start, unsigned int nr,
+                          unsigned int cfg)
+{
+        int ret;
+
+        for (; nr > 0; nr--, start++) {
+                ret = s3c_gpio_cfgpin(start, cfg);
+                if (ret != 0)
+                        return ret;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(s3c_gpio_cfgpin_range);
+
+int s3c_gpio_cfgall_range(unsigned int start, unsigned int nr,
+                          unsigned int cfg, s3c_gpio_pull_t pull)
+{
+        int ret;
+
+        for (; nr > 0; nr--, start++) {
+                s3c_gpio_setpull(start, pull);
+                ret = s3c_gpio_cfgpin(start, cfg);
+                if (ret != 0)
+                        return ret;
+        }
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(s3c_gpio_cfgall_range);
+
 unsigned s3c_gpio_getcfg(unsigned int pin)
 {
         struct s3c_gpio_chip *chip = s3c_gpiolib_getchip(pin);
@@ -80,6 +111,25 @@ int s3c_gpio_setpull(unsigned int pin, s3c_gpio_pull_t pull)
 }
 EXPORT_SYMBOL(s3c_gpio_setpull);
 
+s3c_gpio_pull_t s3c_gpio_getpull(unsigned int pin)
+{
+        struct s3c_gpio_chip *chip = s3c_gpiolib_getchip(pin);
+        unsigned long flags;
+        int offset;
+        u32 pup = 0;
+
+        if (chip) {
+                offset = pin - chip->chip.base;
+
+                s3c_gpio_lock(chip, flags);
+                pup = s3c_gpio_do_getpull(chip, offset);
+                s3c_gpio_unlock(chip, flags);
+        }
+
+        return (__force s3c_gpio_pull_t)pup;
+}
+EXPORT_SYMBOL(s3c_gpio_getpull);
+
 #ifdef CONFIG_S3C_GPIO_CFG_S3C24XX
 int s3c_gpio_setcfg_s3c24xx_a(struct s3c_gpio_chip *chip,
                               unsigned int off, unsigned int cfg)
diff --git a/arch/arm/plat-samsung/gpio.c b/arch/arm/plat-samsung/gpio.c
index b83a83351cea..7743c4b8b2fb 100644
--- a/arch/arm/plat-samsung/gpio.c
+++ b/arch/arm/plat-samsung/gpio.c
@@ -157,3 +157,11 @@ __init void s3c_gpiolib_add(struct s3c_gpio_chip *chip)
         if (ret >= 0)
                 s3c_gpiolib_track(chip);
 }
+
+int samsung_gpiolib_to_irq(struct gpio_chip *chip, unsigned int offset)
+{
+        struct s3c_gpio_chip *s3c_chip = container_of(chip,
+                        struct s3c_gpio_chip, chip);
+
+        return s3c_chip->irq_base + offset;
+}
diff --git a/arch/arm/plat-samsung/include/plat/audio.h b/arch/arm/plat-samsung/include/plat/audio.h
index e32f9edfd4b7..7712ff6336f4 100644
--- a/arch/arm/plat-samsung/include/plat/audio.h
+++ b/arch/arm/plat-samsung/include/plat/audio.h
@@ -16,6 +16,15 @@
 #define S3C64XX_AC97_GPE  1
 extern void s3c64xx_ac97_setup_gpio(int);
 
+/*
+ * The machine init code calls s5p*_spdif_setup_gpio with
+ * one of these defines in order to select appropriate bank
+ * of GPIO for S/PDIF pins
+ */
+#define S5PC100_SPDIF_GPD  0
+#define S5PC100_SPDIF_GPG3 1
+extern void s5pc100_spdif_setup_gpio(int);
+
 /**
  * struct s3c_audio_pdata - common platform data for audio device drivers
  * @cfg_gpio: Callback function to setup mux'ed pins in I2S/PCM/AC97 mode
diff --git a/arch/arm/plat-samsung/include/plat/devs.h b/arch/arm/plat-samsung/include/plat/devs.h
index c8b94279bad1..2d82a6cb1444 100644
--- a/arch/arm/plat-samsung/include/plat/devs.h
+++ b/arch/arm/plat-samsung/include/plat/devs.h
@@ -48,6 +48,11 @@ extern struct platform_device s3c_device_wdt;
 extern struct platform_device s3c_device_i2c0;
 extern struct platform_device s3c_device_i2c1;
 extern struct platform_device s3c_device_i2c2;
+extern struct platform_device s3c_device_i2c3;
+extern struct platform_device s3c_device_i2c4;
+extern struct platform_device s3c_device_i2c5;
+extern struct platform_device s3c_device_i2c6;
+extern struct platform_device s3c_device_i2c7;
 extern struct platform_device s3c_device_rtc;
 extern struct platform_device s3c_device_adc;
 extern struct platform_device s3c_device_sdi;
@@ -89,6 +94,7 @@ extern struct platform_device s5pv210_device_pcm2;
 extern struct platform_device s5pv210_device_iis0;
 extern struct platform_device s5pv210_device_iis1;
 extern struct platform_device s5pv210_device_iis2;
+extern struct platform_device s5pv210_device_spdif;
 
 extern struct platform_device s5p6442_device_pcm0;
 extern struct platform_device s5p6442_device_pcm1;
@@ -108,6 +114,7 @@ extern struct platform_device s5pc100_device_pcm1;
 extern struct platform_device s5pc100_device_iis0;
 extern struct platform_device s5pc100_device_iis1;
 extern struct platform_device s5pc100_device_iis2;
+extern struct platform_device s5pc100_device_spdif;
 
 extern struct platform_device samsung_device_keypad;
 
diff --git a/arch/arm/plat-samsung/include/plat/gpio-cfg-helpers.h b/arch/arm/plat-samsung/include/plat/gpio-cfg-helpers.h
index 3e21c75feefa..8fd65d8b5863 100644
--- a/arch/arm/plat-samsung/include/plat/gpio-cfg-helpers.h
+++ b/arch/arm/plat-samsung/include/plat/gpio-cfg-helpers.h
@@ -42,6 +42,12 @@ static inline int s3c_gpio_do_setpull(struct s3c_gpio_chip *chip,
         return (chip->config->set_pull)(chip, off, pull);
 }
 
+static inline s3c_gpio_pull_t s3c_gpio_do_getpull(struct s3c_gpio_chip *chip,
+                                                  unsigned int off)
+{
+        return chip->config->get_pull(chip, off);
+}
+
 /**
  * s3c_gpio_setcfg_s3c24xx - S3C24XX style GPIO configuration.
  * @chip: The gpio chip that is being configured.
diff --git a/arch/arm/plat-samsung/include/plat/gpio-cfg.h b/arch/arm/plat-samsung/include/plat/gpio-cfg.h
index 1c6b92947c5d..e4b5cf126fa9 100644
--- a/arch/arm/plat-samsung/include/plat/gpio-cfg.h
+++ b/arch/arm/plat-samsung/include/plat/gpio-cfg.h
@@ -108,6 +108,19 @@ extern int s3c_gpio_cfgpin(unsigned int pin, unsigned int to);
  */
 extern unsigned s3c_gpio_getcfg(unsigned int pin);
 
+/**
+ * s3c_gpio_cfgpin_range() - Change the GPIO function for configuring pin range
+ * @start: The pin number to start at
+ * @nr: The number of pins to configure from @start.
+ * @cfg: The configuration for the pin's function
+ *
+ * Call s3c_gpio_cfgpin() for the @nr pins starting at @start.
+ *
+ * @sa s3c_gpio_cfgpin.
+ */
+extern int s3c_gpio_cfgpin_range(unsigned int start, unsigned int nr,
+                                 unsigned int cfg);
+
 /* Define values for the pull-{up,down} available for each gpio pin.
  *
  * These values control the state of the weak pull-{up,down} resistors
@@ -140,6 +153,31 @@ extern int s3c_gpio_setpull(unsigned int pin, s3c_gpio_pull_t pull);
 */
 extern s3c_gpio_pull_t s3c_gpio_getpull(unsigned int pin);
 
+/* configure `all` aspects of an gpio */
+
+/**
+ * s3c_gpio_cfgall_range() - configure range of gpio functtion and pull.
+ * @start: The gpio number to start at.
+ * @nr: The number of gpio to configure from @start.
+ * @cfg: The configuration to use
+ * @pull: The pull setting to use.
+ *
+ * Run s3c_gpio_cfgpin() and s3c_gpio_setpull() over the gpio range starting
+ * @gpio and running for @size.
+ *
+ * @sa s3c_gpio_cfgpin
+ * @sa s3c_gpio_setpull
+ * @sa s3c_gpio_cfgpin_range
+ */
+extern int s3c_gpio_cfgall_range(unsigned int start, unsigned int nr,
+                                 unsigned int cfg, s3c_gpio_pull_t pull);
+
+static inline int s3c_gpio_cfgrange_nopull(unsigned int pin, unsigned int size,
+                                           unsigned int cfg)
+{
+        return s3c_gpio_cfgall_range(pin, size, cfg, S3C_GPIO_PULL_NONE);
+}
+
 /* Define values for the drvstr available for each gpio pin.
  *
  * These values control the value of the output signal driver strength,
@@ -169,4 +207,22 @@ extern s5p_gpio_drvstr_t s5p_gpio_get_drvstr(unsigned int pin);
 */
 extern int s5p_gpio_set_drvstr(unsigned int pin, s5p_gpio_drvstr_t drvstr);
 
+/**
+ * s5p_register_gpio_interrupt() - register interrupt support for a gpio group
+ * @pin: The pin number from the group to be registered
+ *
+ * This function registers gpio interrupt support for the group that the
+ * specified pin belongs to.
+ *
+ * The total number of gpio pins is quite large ob s5p series. Registering
+ * irq support for all of them would be a resource waste. Because of that the
+ * interrupt support for standard gpio pins is registered dynamically.
+ *
+ * It will return the irq number of the interrupt that has been registered
+ * or -ENOMEM if no more gpio interrupts can be registered. It is allowed
+ * to call this function more than once for the same gpio group (the group
+ * will be registered only once).
+ */
+extern int s5p_register_gpio_interrupt(int pin);
+
 #endif /* __PLAT_GPIO_CFG_H */
diff --git a/arch/arm/plat-samsung/include/plat/gpio-core.h b/arch/arm/plat-samsung/include/plat/gpio-core.h
index e358c7da8480..13a22b8861ef 100644
--- a/arch/arm/plat-samsung/include/plat/gpio-core.h
+++ b/arch/arm/plat-samsung/include/plat/gpio-core.h
@@ -43,6 +43,8 @@ struct s3c_gpio_cfg;
  * struct s3c_gpio_chip - wrapper for specific implementation of gpio
  * @chip: The chip structure to be exported via gpiolib.
  * @base: The base pointer to the gpio configuration registers.
+ * @group: The group register number for gpio interrupt support.
+ * @irq_base: The base irq number.
  * @config: special function and pull-resistor control information.
  * @lock: Lock for exclusive access to this gpio bank.
  * @pm_save: Save information for suspend/resume support.
@@ -63,6 +65,8 @@ struct s3c_gpio_chip {
         struct s3c_gpio_cfg     *config;
         struct s3c_gpio_pm      *pm;
         void __iomem            *base;
+        int                     irq_base;
+        int                     group;
         spinlock_t               lock;
 #ifdef CONFIG_PM
         u32                     pm_save[4];
@@ -118,6 +122,17 @@ extern void samsung_gpiolib_add_4bit2_chips(struct s3c_gpio_chip *chip,
 extern void samsung_gpiolib_add_4bit(struct s3c_gpio_chip *chip);
 extern void samsung_gpiolib_add_4bit2(struct s3c_gpio_chip *chip);
 
+
+/**
+ * samsung_gpiolib_to_irq - convert gpio pin to irq number
+ * @chip: The gpio chip that the pin belongs to.
+ * @offset: The offset of the pin in the chip.
+ *
+ * This helper returns the irq number calculated from the chip->irq_base and
+ * the provided offset.
+ */
+extern int samsung_gpiolib_to_irq(struct gpio_chip *chip, unsigned int offset);
+
 /* exported for core SoC support to change */
 extern struct s3c_gpio_cfg s3c24xx_gpiocfg_default;
 
diff --git a/arch/arm/plat-samsung/include/plat/iic.h b/arch/arm/plat-samsung/include/plat/iic.h
index 133308bf595d..1543da8f85c1 100644
--- a/arch/arm/plat-samsung/include/plat/iic.h
+++ b/arch/arm/plat-samsung/include/plat/iic.h
@@ -55,10 +55,20 @@ struct s3c2410_platform_i2c {
 extern void s3c_i2c0_set_platdata(struct s3c2410_platform_i2c *i2c);
 extern void s3c_i2c1_set_platdata(struct s3c2410_platform_i2c *i2c);
 extern void s3c_i2c2_set_platdata(struct s3c2410_platform_i2c *i2c);
+extern void s3c_i2c3_set_platdata(struct s3c2410_platform_i2c *i2c);
+extern void s3c_i2c4_set_platdata(struct s3c2410_platform_i2c *i2c);
+extern void s3c_i2c5_set_platdata(struct s3c2410_platform_i2c *i2c);
+extern void s3c_i2c6_set_platdata(struct s3c2410_platform_i2c *i2c);
+extern void s3c_i2c7_set_platdata(struct s3c2410_platform_i2c *i2c);
 
 /* defined by architecture to configure gpio */
 extern void s3c_i2c0_cfg_gpio(struct platform_device *dev);
 extern void s3c_i2c1_cfg_gpio(struct platform_device *dev);
 extern void s3c_i2c2_cfg_gpio(struct platform_device *dev);
+extern void s3c_i2c3_cfg_gpio(struct platform_device *dev);
+extern void s3c_i2c4_cfg_gpio(struct platform_device *dev);
+extern void s3c_i2c5_cfg_gpio(struct platform_device *dev);
+extern void s3c_i2c6_cfg_gpio(struct platform_device *dev);
+extern void s3c_i2c7_cfg_gpio(struct platform_device *dev);
 
 #endif /* __ASM_ARCH_IIC_H */
diff --git a/arch/arm/plat-samsung/include/plat/map-base.h b/arch/arm/plat-samsung/include/plat/map-base.h
index 250be311c85b..3ffac4d2e4f0 100644
--- a/arch/arm/plat-samsung/include/plat/map-base.h
+++ b/arch/arm/plat-samsung/include/plat/map-base.h
@@ -14,7 +14,7 @@
 #ifndef __ASM_PLAT_MAP_H
 #define __ASM_PLAT_MAP_H __FILE__
 
-/* Fit all our registers in at 0xF4000000 upwards, trying to use as
+/* Fit all our registers in at 0xF6000000 upwards, trying to use as
  * little of the VA space as possible so vmalloc and friends have a
  * better chance of getting memory.
  *
@@ -22,7 +22,7 @@
  * an single MOVS instruction (ie, only 8 bits of set data)
  */
 
-#define S3C_ADDR_BASE   (0xF4000000)
+#define S3C_ADDR_BASE   0xF6000000
 
 #ifndef __ASSEMBLY__
 #define S3C_ADDR(x)     ((void __iomem __force *)S3C_ADDR_BASE + (x))
diff --git a/arch/arm/plat-samsung/include/plat/nand-core.h b/arch/arm/plat-samsung/include/plat/nand-core.h
new file mode 100644
index 000000000000..6de20789a95e
--- /dev/null
+++ b/arch/arm/plat-samsung/include/plat/nand-core.h
@@ -0,0 +1,28 @@
+/* arch/arm/plat-samsung/include/plat/nand-core.h
+ *
+ * Copyright (c) 2010 Samsung Electronics Co., Ltd.
+ *              http://www.samsung.com/
+ *
+ * S3C -  Nand Controller core functions
+ *
+ * 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.
+*/
+
+#ifndef __ASM_ARCH_NAND_CORE_H
+#define __ASM_ARCH_NAND_CORE_H __FILE__
+
+/* These functions are only for use with the core support code, such as
+ * the cpu specific initialisation code
+ */
+
+/* re-define device name depending on support. */
+static inline void s3c_nand_setname(char *name)
+{
+#ifdef CONFIG_S3C_DEV_NAND
+        s3c_device_nand.name = name;
+#endif
+}
+
+#endif /* __ASM_ARCH_NAND_CORE_H */
diff --git a/arch/arm/plat-samsung/include/plat/sdhci.h b/arch/arm/plat-samsung/include/plat/sdhci.h
index 30844c263d03..85853f8c4c5d 100644
--- a/arch/arm/plat-samsung/include/plat/sdhci.h
+++ b/arch/arm/plat-samsung/include/plat/sdhci.h
@@ -28,11 +28,17 @@ enum cd_types {
         S3C_SDHCI_CD_PERMANENT, /* no CD line, card permanently wired to host */
 };
 
+enum clk_types {
+        S3C_SDHCI_CLK_DIV_INTERNAL,     /* use mmc internal clock divider */
+        S3C_SDHCI_CLK_DIV_EXTERNAL,     /* use external clock divider */
+};
+
 /**
  * struct s3c_sdhci_platdata() - Platform device data for Samsung SDHCI
  * @max_width: The maximum number of data bits supported.
  * @host_caps: Standard MMC host capabilities bit field.
  * @cd_type: Type of Card Detection method (see cd_types enum above)
+ * @clk_type: Type of clock divider method (see clk_types enum above)
  * @ext_cd_init: Initialize external card detect subsystem. Called on
  *               sdhci-s3c driver probe when cd_type == S3C_SDHCI_CD_EXTERNAL.
  *               notify_func argument is a callback to the sdhci-s3c driver
@@ -59,6 +65,7 @@ struct s3c_sdhci_platdata {
         unsigned int    max_width;
         unsigned int    host_caps;
         enum cd_types   cd_type;
+        enum clk_types  clk_type;
 
         char            **clocks;       /* set of clock sources */
 
@@ -110,6 +117,10 @@ extern void s5pv210_setup_sdhci0_cfg_gpio(struct platform_device *, int w);
 extern void s5pv210_setup_sdhci1_cfg_gpio(struct platform_device *, int w);
 extern void s5pv210_setup_sdhci2_cfg_gpio(struct platform_device *, int w);
 extern void s5pv210_setup_sdhci3_cfg_gpio(struct platform_device *, int w);
+extern void s5pv310_setup_sdhci0_cfg_gpio(struct platform_device *, int w);
+extern void s5pv310_setup_sdhci1_cfg_gpio(struct platform_device *, int w);
+extern void s5pv310_setup_sdhci2_cfg_gpio(struct platform_device *, int w);
+extern void s5pv310_setup_sdhci3_cfg_gpio(struct platform_device *, int w);
 
 /* S3C64XX SDHCI setup */
 
@@ -288,4 +299,57 @@ static inline void s5pv210_default_sdhci3(void) { }
 
 #endif /* CONFIG_S5PV210_SETUP_SDHCI */
 
+/* S5PV310 SDHCI setup */
+#ifdef CONFIG_S5PV310_SETUP_SDHCI
+extern char *s5pv310_hsmmc_clksrcs[4];
+
+extern void s5pv310_setup_sdhci_cfg_card(struct platform_device *dev,
+                                           void __iomem *r,
+                                           struct mmc_ios *ios,
+                                           struct mmc_card *card);
+
+static inline void s5pv310_default_sdhci0(void)
+{
+#ifdef CONFIG_S3C_DEV_HSMMC
+        s3c_hsmmc0_def_platdata.clocks = s5pv310_hsmmc_clksrcs;
+        s3c_hsmmc0_def_platdata.cfg_gpio = s5pv310_setup_sdhci0_cfg_gpio;
+        s3c_hsmmc0_def_platdata.cfg_card = s5pv310_setup_sdhci_cfg_card;
+#endif
+}
+
+static inline void s5pv310_default_sdhci1(void)
+{
+#ifdef CONFIG_S3C_DEV_HSMMC1
+        s3c_hsmmc1_def_platdata.clocks = s5pv310_hsmmc_clksrcs;
+        s3c_hsmmc1_def_platdata.cfg_gpio = s5pv310_setup_sdhci1_cfg_gpio;
+        s3c_hsmmc1_def_platdata.cfg_card = s5pv310_setup_sdhci_cfg_card;
+#endif
+}
+
+static inline void s5pv310_default_sdhci2(void)
+{
+#ifdef CONFIG_S3C_DEV_HSMMC2
+        s3c_hsmmc2_def_platdata.clocks = s5pv310_hsmmc_clksrcs;
+        s3c_hsmmc2_def_platdata.cfg_gpio = s5pv310_setup_sdhci2_cfg_gpio;
+        s3c_hsmmc2_def_platdata.cfg_card = s5pv310_setup_sdhci_cfg_card;
+#endif
+}
+
+static inline void s5pv310_default_sdhci3(void)
+{
+#ifdef CONFIG_S3C_DEV_HSMMC3
+        s3c_hsmmc3_def_platdata.clocks = s5pv310_hsmmc_clksrcs;
+        s3c_hsmmc3_def_platdata.cfg_gpio = s5pv310_setup_sdhci3_cfg_gpio;
+        s3c_hsmmc3_def_platdata.cfg_card = s5pv310_setup_sdhci_cfg_card;
+#endif
+}
+
+#else
+static inline void s5pv310_default_sdhci0(void) { }
+static inline void s5pv310_default_sdhci1(void) { }
+static inline void s5pv310_default_sdhci2(void) { }
+static inline void s5pv310_default_sdhci3(void) { }
+
+#endif /* CONFIG_S5PV310_SETUP_SDHCI */
+
 #endif /* __PLAT_S3C_SDHCI_H */
diff --git a/arch/arm/plat-samsung/pm-gpio.c b/arch/arm/plat-samsung/pm-gpio.c
index 7df03f87fbfa..96528200eb79 100644
--- a/arch/arm/plat-samsung/pm-gpio.c
+++ b/arch/arm/plat-samsung/pm-gpio.c
@@ -192,7 +192,7 @@ struct s3c_gpio_pm s3c_gpio_pm_2bit = {
         .resume = s3c_gpio_pm_2bit_resume,
 };
 
-#ifdef CONFIG_ARCH_S3C64XX
+#if defined(CONFIG_ARCH_S3C64XX) || defined(CONFIG_PLAT_S5P)
 static void s3c_gpio_pm_4bit_save(struct s3c_gpio_chip *chip)
 {
         chip->pm_save[1] = __raw_readl(chip->base + OFFS_CON);
@@ -302,7 +302,7 @@ struct s3c_gpio_pm s3c_gpio_pm_4bit = {
         .save   = s3c_gpio_pm_4bit_save,
         .resume = s3c_gpio_pm_4bit_resume,
 };
-#endif /* CONFIG_ARCH_S3C64XX */
+#endif /* CONFIG_ARCH_S3C64XX || CONFIG_PLAT_S5P */
 
 /**
  * s3c_pm_save_gpio() - save gpio chip data for suspend
diff --git a/arch/arm/plat-samsung/s3c-pl330.c b/arch/arm/plat-samsung/s3c-pl330.c
index a91305a60aed..b4ff8d74ac40 100644
--- a/arch/arm/plat-samsung/s3c-pl330.c
+++ b/arch/arm/plat-samsung/s3c-pl330.c
@@ -15,6 +15,8 @@
 #include <linux/io.h>
 #include <linux/slab.h>
 #include <linux/platform_device.h>
+#include <linux/clk.h>
+#include <linux/err.h>
 
 #include <asm/hardware/pl330.h>
 
@@ -27,6 +29,7 @@
  * @node: To attach to the global list of DMACs.
  * @pi: PL330 configuration info for the DMAC.
  * @kmcache: Pool to quickly allocate xfers for all channels in the dmac.
+ * @clk: Pointer of DMAC operation clock.
  */
 struct s3c_pl330_dmac {
         unsigned                busy_chan;
@@ -34,6 +37,7 @@ struct s3c_pl330_dmac {
         struct list_head        node;
         struct pl330_info       *pi;
         struct kmem_cache       *kmcache;
+        struct clk              *clk;
 };
 
 /**
@@ -1072,16 +1076,25 @@ static int pl330_probe(struct platform_device *pdev)
         if (ret)
                 goto probe_err4;
 
-        ret = pl330_add(pl330_info);
-        if (ret)
-                goto probe_err5;
-
         /* Allocate a new DMAC */
         s3c_pl330_dmac = kmalloc(sizeof(*s3c_pl330_dmac), GFP_KERNEL);
         if (!s3c_pl330_dmac) {
                 ret = -ENOMEM;
+                goto probe_err5;
+        }
+
+        /* Get operation clock and enable it */
+        s3c_pl330_dmac->clk = clk_get(&pdev->dev, "pdma");
+        if (IS_ERR(s3c_pl330_dmac->clk)) {
+                dev_err(&pdev->dev, "Cannot get operation clock.\n");
+                ret = -EINVAL;
                 goto probe_err6;
         }
+        clk_enable(s3c_pl330_dmac->clk);
+
+        ret = pl330_add(pl330_info);
+        if (ret)
+                goto probe_err7;
 
         /* Hook the info */
         s3c_pl330_dmac->pi = pl330_info;
@@ -1094,7 +1107,7 @@ static int pl330_probe(struct platform_device *pdev)
 
         if (!s3c_pl330_dmac->kmcache) {
                 ret = -ENOMEM;
-                goto probe_err7;
+                goto probe_err8;
         }
 
         /* Get the list of peripherals */
@@ -1120,10 +1133,13 @@ static int pl330_probe(struct platform_device *pdev)
 
         return 0;
 
+probe_err8:
+        pl330_del(pl330_info);
 probe_err7:
-        kfree(s3c_pl330_dmac);
+        clk_disable(s3c_pl330_dmac->clk);
+        clk_put(s3c_pl330_dmac->clk);
 probe_err6:
-        pl330_del(pl330_info);
+        kfree(s3c_pl330_dmac);
 probe_err5:
         free_irq(irq, pl330_info);
 probe_err4:
@@ -1188,6 +1204,10 @@ static int pl330_remove(struct platform_device *pdev)
                 }
         }
 
+        /* Disable operation clock */
+        clk_disable(dmac->clk);
+        clk_put(dmac->clk);
+
         /* Remove the DMAC */
         list_del(&dmac->node);
         kfree(dmac);
diff --git a/arch/sh/boards/mach-ap325rxa/setup.c b/arch/sh/boards/mach-ap325rxa/setup.c
index 881a3a5f5647..07ea908c510d 100644
--- a/arch/sh/boards/mach-ap325rxa/setup.c
+++ b/arch/sh/boards/mach-ap325rxa/setup.c
@@ -176,6 +176,21 @@ static void ap320_wvga_power_off(void *board_data)
         __raw_writew(0, FPGA_LCDREG);
 }
 
+const static struct fb_videomode ap325rxa_lcdc_modes[] = {
+        {
+                .name = "LB070WV1",
+                .xres = 800,
+                .yres = 480,
+                .left_margin = 32,
+                .right_margin = 160,
+                .hsync_len = 8,
+                .upper_margin = 63,
+                .lower_margin = 80,
+                .vsync_len = 1,
+                .sync = 0, /* hsync and vsync are active low */
+        },
+};
+
 static struct sh_mobile_lcdc_info lcdc_info = {
         .clock_source = LCDC_CLK_EXTERNAL,
         .ch[0] = {
@@ -183,18 +198,8 @@ static struct sh_mobile_lcdc_info lcdc_info = {
                 .bpp = 16,
                 .interface_type = RGB18,
                 .clock_divider = 1,
-                .lcd_cfg = {
-                        .name = "LB070WV1",
-                        .xres = 800,
-                        .yres = 480,
-                        .left_margin = 32,
-                        .right_margin = 160,
-                        .hsync_len = 8,
-                        .upper_margin = 63,
-                        .lower_margin = 80,
-                        .vsync_len = 1,
-                        .sync = 0, /* hsync and vsync are active low */
-                },
+                .lcd_cfg = ap325rxa_lcdc_modes,
+                .num_cfg = ARRAY_SIZE(ap325rxa_lcdc_modes),
                 .lcd_size_cfg = { /* 7.0 inch */
                         .width = 152,
                         .height = 91,
diff --git a/arch/sh/boards/mach-ecovec24/setup.c b/arch/sh/boards/mach-ecovec24/setup.c
index ddc7e4e4d2a0..2eaeb9e59585 100644
--- a/arch/sh/boards/mach-ecovec24/setup.c
+++ b/arch/sh/boards/mach-ecovec24/setup.c
@@ -231,14 +231,41 @@ static struct platform_device usb1_common_device = {
 };
 
 /* LCDC */
+const static struct fb_videomode ecovec_lcd_modes[] = {
+        {
+                .name           = "Panel",
+                .xres           = 800,
+                .yres           = 480,
+                .left_margin    = 220,
+                .right_margin   = 110,
+                .hsync_len      = 70,
+                .upper_margin   = 20,
+                .lower_margin   = 5,
+                .vsync_len      = 5,
+                .sync           = 0, /* hsync and vsync are active low */
+        },
+};
+
+const static struct fb_videomode ecovec_dvi_modes[] = {
+        {
+                .name           = "DVI",
+                .xres           = 1280,
+                .yres           = 720,
+                .left_margin    = 220,
+                .right_margin   = 110,
+                .hsync_len      = 40,
+                .upper_margin   = 20,
+                .lower_margin   = 5,
+                .vsync_len      = 5,
+                .sync = 0, /* hsync and vsync are active low */
+        },
+};
+
 static struct sh_mobile_lcdc_info lcdc_info = {
         .ch[0] = {
                 .interface_type = RGB18,
                 .chan = LCDC_CHAN_MAINLCD,
                 .bpp = 16,
-                .lcd_cfg = {
-                        .sync = 0, /* hsync and vsync are active low */
-                },
                 .lcd_size_cfg = { /* 7.0 inch */
                         .width = 152,
                         .height = 91,
@@ -1075,33 +1102,18 @@ static int __init arch_setup(void)
         if (gpio_get_value(GPIO_PTE6)) {
                 /* DVI */
                 lcdc_info.clock_source                  = LCDC_CLK_EXTERNAL;
-                lcdc_info.ch[0].clock_divider           = 1,
-                lcdc_info.ch[0].lcd_cfg.name            = "DVI";
-                lcdc_info.ch[0].lcd_cfg.xres            = 1280;
-                lcdc_info.ch[0].lcd_cfg.yres            = 720;
-                lcdc_info.ch[0].lcd_cfg.left_margin     = 220;
-                lcdc_info.ch[0].lcd_cfg.right_margin    = 110;
-                lcdc_info.ch[0].lcd_cfg.hsync_len       = 40;
-                lcdc_info.ch[0].lcd_cfg.upper_margin    = 20;
-                lcdc_info.ch[0].lcd_cfg.lower_margin    = 5;
-                lcdc_info.ch[0].lcd_cfg.vsync_len       = 5;
+                lcdc_info.ch[0].clock_divider           = 1;
+                lcdc_info.ch[0].lcd_cfg                 = ecovec_dvi_modes;
+                lcdc_info.ch[0].num_cfg                 = ARRAY_SIZE(ecovec_dvi_modes);
 
                 gpio_set_value(GPIO_PTA2, 1);
                 gpio_set_value(GPIO_PTU1, 1);
         } else {
                 /* Panel */
-
                 lcdc_info.clock_source                  = LCDC_CLK_PERIPHERAL;
-                lcdc_info.ch[0].clock_divider           = 2,
-                lcdc_info.ch[0].lcd_cfg.name            = "Panel";
-                lcdc_info.ch[0].lcd_cfg.xres            = 800;
-                lcdc_info.ch[0].lcd_cfg.yres            = 480;
-                lcdc_info.ch[0].lcd_cfg.left_margin     = 220;
-                lcdc_info.ch[0].lcd_cfg.right_margin    = 110;
-                lcdc_info.ch[0].lcd_cfg.hsync_len       = 70;
-                lcdc_info.ch[0].lcd_cfg.upper_margin    = 20;
-                lcdc_info.ch[0].lcd_cfg.lower_margin    = 5;
-                lcdc_info.ch[0].lcd_cfg.vsync_len       = 5;
+                lcdc_info.ch[0].clock_divider           = 2;
+                lcdc_info.ch[0].lcd_cfg                 = ecovec_lcd_modes;
+                lcdc_info.ch[0].num_cfg                 = ARRAY_SIZE(ecovec_lcd_modes);
 
                 gpio_set_value(GPIO_PTR1, 1);
 
diff --git a/arch/sh/boards/mach-kfr2r09/setup.c b/arch/sh/boards/mach-kfr2r09/setup.c
index 1742849db648..9b60eaabf8f3 100644
--- a/arch/sh/boards/mach-kfr2r09/setup.c
+++ b/arch/sh/boards/mach-kfr2r09/setup.c
@@ -126,6 +126,21 @@ static struct platform_device kfr2r09_sh_keysc_device = {
         },
 };
 
+const static struct fb_videomode kfr2r09_lcdc_modes[] = {
+        {
+                .name = "TX07D34VM0AAA",
+                .xres = 240,
+                .yres = 400,
+                .left_margin = 0,
+                .right_margin = 16,
+                .hsync_len = 8,
+                .upper_margin = 0,
+                .lower_margin = 1,
+                .vsync_len = 1,
+                .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+        },
+};
+
 static struct sh_mobile_lcdc_info kfr2r09_sh_lcdc_info = {
         .clock_source = LCDC_CLK_BUS,
         .ch[0] = {
@@ -134,18 +149,8 @@ static struct sh_mobile_lcdc_info kfr2r09_sh_lcdc_info = {
                 .interface_type = SYS18,
                 .clock_divider = 6,
                 .flags = LCDC_FLAGS_DWPOL,
-                .lcd_cfg = {
-                        .name = "TX07D34VM0AAA",
-                        .xres = 240,
-                        .yres = 400,
-                        .left_margin = 0,
-                        .right_margin = 16,
-                        .hsync_len = 8,
-                        .upper_margin = 0,
-                        .lower_margin = 1,
-                        .vsync_len = 1,
-                        .sync = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
-                },
+                .lcd_cfg = kfr2r09_lcdc_modes,
+                .num_cfg = ARRAY_SIZE(kfr2r09_lcdc_modes),
                 .lcd_size_cfg = {
                         .width = 35,
                         .height = 58,
diff --git a/arch/sh/boards/mach-migor/setup.c b/arch/sh/boards/mach-migor/setup.c
index 03af84842559..c8acfec98695 100644
--- a/arch/sh/boards/mach-migor/setup.c
+++ b/arch/sh/boards/mach-migor/setup.c
@@ -213,51 +213,55 @@ static struct platform_device migor_nand_flash_device = {
         }
 };
 
+const static struct fb_videomode migor_lcd_modes[] = {
+        {
+#if defined(CONFIG_SH_MIGOR_RTA_WVGA)
+                .name = "LB070WV1",
+                .xres = 800,
+                .yres = 480,
+                .left_margin = 64,
+                .right_margin = 16,
+                .hsync_len = 120,
+                .sync = 0,
+#elif defined(CONFIG_SH_MIGOR_QVGA)
+                .name = "PH240320T",
+                .xres = 320,
+                .yres = 240,
+                .left_margin = 0,
+                .right_margin = 16,
+                .hsync_len = 8,
+                .sync = FB_SYNC_HOR_HIGH_ACT,
+#endif
+                .upper_margin = 1,
+                .lower_margin = 17,
+                .vsync_len = 2,
+        },
+};
+
 static struct sh_mobile_lcdc_info sh_mobile_lcdc_info = {
-#ifdef CONFIG_SH_MIGOR_RTA_WVGA
+#if defined(CONFIG_SH_MIGOR_RTA_WVGA)
         .clock_source = LCDC_CLK_BUS,
         .ch[0] = {
                 .chan = LCDC_CHAN_MAINLCD,
                 .bpp = 16,
                 .interface_type = RGB16,
                 .clock_divider = 2,
-                .lcd_cfg = {
-                        .name = "LB070WV1",
-                        .xres = 800,
-                        .yres = 480,
-                        .left_margin = 64,
-                        .right_margin = 16,
-                        .hsync_len = 120,
-                        .upper_margin = 1,
-                        .lower_margin = 17,
-                        .vsync_len = 2,
-                        .sync = 0,
-                },
+                .lcd_cfg = migor_lcd_modes,
+                .num_cfg = ARRAY_SIZE(migor_lcd_modes),
                 .lcd_size_cfg = { /* 7.0 inch */
                         .width = 152,
                         .height = 91,
                 },
         }
-#endif
-#ifdef CONFIG_SH_MIGOR_QVGA
+#elif defined(CONFIG_SH_MIGOR_QVGA)
         .clock_source = LCDC_CLK_PERIPHERAL,
         .ch[0] = {
                 .chan = LCDC_CHAN_MAINLCD,
                 .bpp = 16,
                 .interface_type = SYS16A,
                 .clock_divider = 10,
-                .lcd_cfg = {
-                        .name = "PH240320T",
-                        .xres = 320,
-                        .yres = 240,
-                        .left_margin = 0,
-                        .right_margin = 16,
-                        .hsync_len = 8,
-                        .upper_margin = 1,
-                        .lower_margin = 17,
-                        .vsync_len = 2,
-                        .sync = FB_SYNC_HOR_HIGH_ACT,
-                },
+                .lcd_cfg = migor_lcd_modes,
+                .num_cfg = ARRAY_SIZE(migor_lcd_modes),
                 .lcd_size_cfg = { /* 2.4 inch */
                         .width = 49,
                         .height = 37,
diff --git a/arch/sh/boards/mach-se/7724/setup.c b/arch/sh/boards/mach-se/7724/setup.c
index 8cc1d7295d85..c31d228fdfc6 100644
--- a/arch/sh/boards/mach-se/7724/setup.c
+++ b/arch/sh/boards/mach-se/7724/setup.c
@@ -144,16 +144,42 @@ static struct platform_device nor_flash_device = {
 };
 
 /* LCDC */
+const static struct fb_videomode lcdc_720p_modes[] = {
+        {
+                .name           = "LB070WV1",
+                .sync           = 0, /* hsync and vsync are active low */
+                .xres           = 1280,
+                .yres           = 720,
+                .left_margin    = 220,
+                .right_margin   = 110,
+                .hsync_len      = 40,
+                .upper_margin   = 20,
+                .lower_margin   = 5,
+                .vsync_len      = 5,
+        },
+};
+
+const static struct fb_videomode lcdc_vga_modes[] = {
+        {
+                .name           = "LB070WV1",
+                .sync           = 0, /* hsync and vsync are active low */
+                .xres           = 640,
+                .yres           = 480,
+                .left_margin    = 105,
+                .right_margin   = 50,
+                .hsync_len      = 96,
+                .upper_margin   = 33,
+                .lower_margin   = 10,
+                .vsync_len      = 2,
+        },
+};
+
 static struct sh_mobile_lcdc_info lcdc_info = {
         .clock_source = LCDC_CLK_EXTERNAL,
         .ch[0] = {
                 .chan = LCDC_CHAN_MAINLCD,
                 .bpp = 16,
                 .clock_divider = 1,
-                .lcd_cfg = {
-                        .name = "LB070WV1",
-                        .sync = 0, /* hsync and vsync are active low */
-                },
                 .lcd_size_cfg = { /* 7.0 inch */
                         .width = 152,
                         .height = 91,
@@ -908,24 +934,12 @@ static int __init devices_setup(void)
 
         if (sw & SW41_B) {
                 /* 720p */
-                lcdc_info.ch[0].lcd_cfg.xres         = 1280;
-                lcdc_info.ch[0].lcd_cfg.yres         = 720;
-                lcdc_info.ch[0].lcd_cfg.left_margin  = 220;
-                lcdc_info.ch[0].lcd_cfg.right_margin = 110;
-                lcdc_info.ch[0].lcd_cfg.hsync_len    = 40;
-                lcdc_info.ch[0].lcd_cfg.upper_margin = 20;
-                lcdc_info.ch[0].lcd_cfg.lower_margin = 5;
-                lcdc_info.ch[0].lcd_cfg.vsync_len    = 5;
+                lcdc_info.ch[0].lcd_cfg = lcdc_720p_modes;
+                lcdc_info.ch[0].num_cfg = ARRAY_SIZE(lcdc_720p_modes);
         } else {
                 /* VGA */
-                lcdc_info.ch[0].lcd_cfg.xres         = 640;
-                lcdc_info.ch[0].lcd_cfg.yres         = 480;
-                lcdc_info.ch[0].lcd_cfg.left_margin  = 105;
-                lcdc_info.ch[0].lcd_cfg.right_margin = 50;
-                lcdc_info.ch[0].lcd_cfg.hsync_len    = 96;
-                lcdc_info.ch[0].lcd_cfg.upper_margin = 33;
-                lcdc_info.ch[0].lcd_cfg.lower_margin = 10;
-                lcdc_info.ch[0].lcd_cfg.vsync_len    = 2;
+                lcdc_info.ch[0].lcd_cfg = lcdc_vga_modes;
+                lcdc_info.ch[0].num_cfg = ARRAY_SIZE(lcdc_vga_modes);
         }
 
         if (sw & SW41_A) {
diff --git a/drivers/video/sh_mipi_dsi.c b/drivers/video/sh_mipi_dsi.c
index 5699ce0c1780..3f3d431033ca 100644
--- a/drivers/video/sh_mipi_dsi.c
+++ b/drivers/video/sh_mipi_dsi.c
@@ -123,83 +123,87 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
         u32 linelength;
         bool yuv;
 
-        /* Select data format */
+        /*
+         * Select data format. MIPI DSI is not hot-pluggable, so, we just use
+         * the default videomode. If this ever becomes a problem, We'll have to
+         * move this to mipi_display_on() above and use info->var.xres
+         */
         switch (pdata->data_format) {
         case MIPI_RGB888:
                 pctype = 0;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_24;
                 pixfmt = MIPI_DCS_PIXEL_FMT_24BIT;
-                linelength = ch->lcd_cfg.xres * 3;
+                linelength = ch->lcd_cfg[0].xres * 3;
                 yuv = false;
                 break;
         case MIPI_RGB565:
                 pctype = 1;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_16;
                 pixfmt = MIPI_DCS_PIXEL_FMT_16BIT;
-                linelength = ch->lcd_cfg.xres * 2;
+                linelength = ch->lcd_cfg[0].xres * 2;
                 yuv = false;
                 break;
         case MIPI_RGB666_LP:
                 pctype = 2;
                 datatype = MIPI_DSI_PIXEL_STREAM_3BYTE_18;
                 pixfmt = MIPI_DCS_PIXEL_FMT_24BIT;
-                linelength = ch->lcd_cfg.xres * 3;
+                linelength = ch->lcd_cfg[0].xres * 3;
                 yuv = false;
                 break;
         case MIPI_RGB666:
                 pctype = 3;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_18;
                 pixfmt = MIPI_DCS_PIXEL_FMT_18BIT;
-                linelength = (ch->lcd_cfg.xres * 18 + 7) / 8;
+                linelength = (ch->lcd_cfg[0].xres * 18 + 7) / 8;
                 yuv = false;
                 break;
         case MIPI_BGR888:
                 pctype = 8;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_24;
                 pixfmt = MIPI_DCS_PIXEL_FMT_24BIT;
-                linelength = ch->lcd_cfg.xres * 3;
+                linelength = ch->lcd_cfg[0].xres * 3;
                 yuv = false;
                 break;
         case MIPI_BGR565:
                 pctype = 9;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_16;
                 pixfmt = MIPI_DCS_PIXEL_FMT_16BIT;
-                linelength = ch->lcd_cfg.xres * 2;
+                linelength = ch->lcd_cfg[0].xres * 2;
                 yuv = false;
                 break;
         case MIPI_BGR666_LP:
                 pctype = 0xa;
                 datatype = MIPI_DSI_PIXEL_STREAM_3BYTE_18;
                 pixfmt = MIPI_DCS_PIXEL_FMT_24BIT;
-                linelength = ch->lcd_cfg.xres * 3;
+                linelength = ch->lcd_cfg[0].xres * 3;
                 yuv = false;
                 break;
         case MIPI_BGR666:
                 pctype = 0xb;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_18;
                 pixfmt = MIPI_DCS_PIXEL_FMT_18BIT;
-                linelength = (ch->lcd_cfg.xres * 18 + 7) / 8;
+                linelength = (ch->lcd_cfg[0].xres * 18 + 7) / 8;
                 yuv = false;
                 break;
         case MIPI_YUYV:
                 pctype = 4;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR16;
                 pixfmt = MIPI_DCS_PIXEL_FMT_16BIT;
-                linelength = ch->lcd_cfg.xres * 2;
+                linelength = ch->lcd_cfg[0].xres * 2;
                 yuv = true;
                 break;
         case MIPI_UYVY:
                 pctype = 5;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR16;
                 pixfmt = MIPI_DCS_PIXEL_FMT_16BIT;
-                linelength = ch->lcd_cfg.xres * 2;
+                linelength = ch->lcd_cfg[0].xres * 2;
                 yuv = true;
                 break;
         case MIPI_YUV420_L:
                 pctype = 6;
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR12;
                 pixfmt = MIPI_DCS_PIXEL_FMT_12BIT;
-                linelength = (ch->lcd_cfg.xres * 12 + 7) / 8;
+                linelength = (ch->lcd_cfg[0].xres * 12 + 7) / 8;
                 yuv = true;
                 break;
         case MIPI_YUV420:
@@ -207,7 +211,7 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
                 datatype = MIPI_DSI_PACKED_PIXEL_STREAM_YCBCR12;
                 pixfmt = MIPI_DCS_PIXEL_FMT_12BIT;
                 /* Length of U/V line */
-                linelength = (ch->lcd_cfg.xres + 1) / 2;
+                linelength = (ch->lcd_cfg[0].xres + 1) / 2;
                 yuv = true;
                 break;
         default:
@@ -281,7 +285,7 @@ static int __init sh_mipi_setup(struct sh_mipi *mipi,
         iowrite32(0x00e00000, base + 0x8024); /* VMCTR2 */
         /*
          * 0x660 = 1632 bytes per line (RGB24, 544 pixels: see
-         * sh_mobile_lcdc_info.ch[0].lcd_cfg.xres), HSALEN = 1 - default
+         * sh_mobile_lcdc_info.ch[0].lcd_cfg[0].xres), HSALEN = 1 - default
          * (unused, since VMCTR2[HSABM] = 0)
          */
         iowrite32(1 | (linelength << 16), base + 0x8028); /* VMLEN1 */
diff --git a/drivers/video/sh_mobile_hdmi.c b/drivers/video/sh_mobile_hdmi.c
index ef989d94511c..55b3077ff6ff 100644
--- a/drivers/video/sh_mobile_hdmi.c
+++ b/drivers/video/sh_mobile_hdmi.c
@@ -28,6 +28,8 @@
 #include <video/sh_mobile_hdmi.h>
 #include <video/sh_mobile_lcdc.h>
 
+#include "sh_mobile_lcdcfb.h"
+
 #define HDMI_SYSTEM_CTRL                        0x00 /* System control */
 #define HDMI_L_R_DATA_SWAP_CTRL_RPKT            0x01 /* L/R data swap control,
                                                         bits 19..16 of 20-bit N for Audio Clock Regeneration packet */
@@ -206,12 +208,15 @@ enum hotplug_state {
 
 struct sh_hdmi {
         void __iomem *base;
-        enum hotplug_state hp_state;
+        enum hotplug_state hp_state;    /* hot-plug status */
+        bool preprogrammed_mode;        /* use a pre-programmed VIC or the external mode */
         struct clk *hdmi_clk;
         struct device *dev;
         struct fb_info *info;
+        struct mutex mutex;             /* Protect the info pointer */
         struct delayed_work edid_work;
         struct fb_var_screeninfo var;
+        struct fb_monspecs monspec;
 };
 
 static void hdmi_write(struct sh_hdmi *hdmi, u8 data, u8 reg)
@@ -277,7 +282,7 @@ static struct snd_soc_codec_driver soc_codec_dev_sh_hdmi = {
  */
 
 /* External video parameter settings */
-static void hdmi_external_video_param(struct sh_hdmi *hdmi)
+static void sh_hdmi_external_video_param(struct sh_hdmi *hdmi)
 {
         struct fb_var_screeninfo *var = &hdmi->var;
         u16 htotal, hblank, hdelay, vtotal, vblank, vdelay, voffset;
@@ -309,9 +314,9 @@ static void hdmi_external_video_param(struct sh_hdmi *hdmi)
         if (var->sync & FB_SYNC_VERT_HIGH_ACT)
                 sync |= 8;
 
-        pr_debug("H: %u, %u, %u, %u; V: %u, %u, %u, %u; sync 0x%x\n",
-                 htotal, hblank, hdelay, var->hsync_len,
-                 vtotal, vblank, vdelay, var->vsync_len, sync);
+        dev_dbg(hdmi->dev, "H: %u, %u, %u, %u; V: %u, %u, %u, %u; sync 0x%x\n",
+                htotal, hblank, hdelay, var->hsync_len,
+                vtotal, vblank, vdelay, var->vsync_len, sync);
 
         hdmi_write(hdmi, sync | (voffset << 4), HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
 
@@ -336,7 +341,10 @@ static void hdmi_external_video_param(struct sh_hdmi *hdmi)
 
         hdmi_write(hdmi, var->vsync_len, HDMI_EXTERNAL_V_DURATION);
 
-        /* Set bit 0 of HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS here for manual mode */
+        /* Set bit 0 of HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS here for external mode */
+        if (!hdmi->preprogrammed_mode)
+                hdmi_write(hdmi, sync | 1 | (voffset << 4),
+                           HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
 }
 
 /**
@@ -454,21 +462,61 @@ static void sh_hdmi_audio_config(struct sh_hdmi *hdmi)
 }
 
 /**
- * sh_hdmi_phy_config()
+ * sh_hdmi_phy_config() - configure the HDMI PHY for the used video mode
  */
 static void sh_hdmi_phy_config(struct sh_hdmi *hdmi)
 {
-        /* 720p, 8bit, 74.25MHz. Might need to be adjusted for other formats */
-        hdmi_write(hdmi, 0x19, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
-        hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
-        hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
-        /* PLLA_CONFIG[7:0]: VCO gain, VCO offset, LPF resistance[0] */
-        hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
-        hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
-        hdmi_write(hdmi, 0x4A, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
-        hdmi_write(hdmi, 0x0E, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
-        hdmi_write(hdmi, 0x25, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
-        hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+        if (hdmi->var.yres > 480) {
+                /* 720p, 8bit, 74.25MHz. Might need to be adjusted for other formats */
+                /*
+                 * [1:0]        Speed_A
+                 * [3:2]        Speed_B
+                 * [4]          PLLA_Bypass
+                 * [6]          DRV_TEST_EN
+                 * [7]          DRV_TEST_IN
+                 */
+                hdmi_write(hdmi, 0x0f, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
+                /* PLLB_CONFIG[17], PLLA_CONFIG[17] - not in PHY datasheet */
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
+                /*
+                 * [2:0]        BGR_I_OFFSET
+                 * [6:4]        BGR_V_OFFSET
+                 */
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
+                /* PLLA_CONFIG[7:0]: VCO gain, VCO offset, LPF resistance[0] */
+                hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
+                /*
+                 * PLLA_CONFIG[15:8]: regulator voltage[0], CP current,
+                 * LPF capacitance, LPF resistance[1]
+                 */
+                hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
+                /* PLLB_CONFIG[7:0]: LPF resistance[0], VCO offset, VCO gain */
+                hdmi_write(hdmi, 0x4A, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
+                /*
+                 * PLLB_CONFIG[15:8]: regulator voltage[0], CP current,
+                 * LPF capacitance, LPF resistance[1]
+                 */
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
+                /* DRV_CONFIG, PE_CONFIG */
+                hdmi_write(hdmi, 0x25, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
+                /*
+                 * [2:0]        AMON_SEL (4 == LPF voltage)
+                 * [4]          PLLA_CONFIG[16]
+                 * [5]          PLLB_CONFIG[16]
+                 */
+                hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+        } else {
+                /* for 480p8bit 27MHz */
+                hdmi_write(hdmi, 0x19, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
+                hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
+                hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
+                hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
+                hdmi_write(hdmi, 0x48, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
+                hdmi_write(hdmi, 0x0F, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
+                hdmi_write(hdmi, 0x20, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
+                hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+        }
 }
 
 /**
@@ -476,6 +524,8 @@ static void sh_hdmi_phy_config(struct sh_hdmi *hdmi)
  */
 static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
 {
+        u8 vic;
+
         /* AVI InfoFrame */
         hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_INDEX);
 
@@ -500,9 +550,9 @@ static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
         hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB1);
 
         /*
-         * C = No Data
-         * M = 16:9 Picture Aspect Ratio
-         * R = Same as picture aspect ratio
+         * [7:6] C = Colorimetry: no data
+         * [5:4] M = 2: 16:9, 1: 4:3 Picture Aspect Ratio
+         * [3:0] R = 8: Active Frame Aspect Ratio: same as picture aspect ratio
          */
         hdmi_write(hdmi, 0x28, HDMI_CTRL_PKT_BUF_ACCESS_PB2);
 
@@ -516,9 +566,15 @@ static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
 
         /*
          * VIC = 1280 x 720p: ignored if external config is used
-         * Send 2 for 720 x 480p, 16 for 1080p
+         * Send 2 for 720 x 480p, 16 for 1080p, ignored in external mode
          */
-        hdmi_write(hdmi, 4, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
+        if (hdmi->var.yres == 1080 && hdmi->var.xres == 1920)
+                vic = 16;
+        else if (hdmi->var.yres == 480 && hdmi->var.xres == 720)
+                vic = 2;
+        else
+                vic = 4;
+        hdmi_write(hdmi, vic, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
 
         /* PR = No Repetition */
         hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB5);
@@ -592,100 +648,6 @@ static void sh_hdmi_audio_infoframe_setup(struct sh_hdmi *hdmi)
 }
 
 /**
- * sh_hdmi_gamut_metadata_setup() - Gamut Metadata Packet of CONTROL PACKET
- */
-static void sh_hdmi_gamut_metadata_setup(struct sh_hdmi *hdmi)
-{
-        int i;
-
-        /* Gamut Metadata Packet */
-        hdmi_write(hdmi, 0x04, HDMI_CTRL_PKT_BUF_INDEX);
-
-        /* Packet Type = 0x0A */
-        hdmi_write(hdmi, 0x0A, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-        /* Gamut Packet is not used, so default value */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-        /* Gamut Packet is not used, so default value */
-        hdmi_write(hdmi, 0x10, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-        /* GBD bytes 0 through 27 */
-        for (i = 0; i <= 27; i++)
-                /* HDMI_CTRL_PKT_BUF_ACCESS_PB0_63H - PB27_7EH */
-                hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
-}
-
-/**
- * sh_hdmi_acp_setup() - Audio Content Protection Packet (ACP)
- */
-static void sh_hdmi_acp_setup(struct sh_hdmi *hdmi)
-{
-        int i;
-
-        /* Audio Content Protection Packet (ACP) */
-        hdmi_write(hdmi, 0x01, HDMI_CTRL_PKT_BUF_INDEX);
-
-        /* Packet Type = 0x04 */
-        hdmi_write(hdmi, 0x04, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-        /* ACP_Type */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-        /* Reserved (0) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-        /* GBD bytes 0 through 27 */
-        for (i = 0; i <= 27; i++)
-                /* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
-                hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
-}
-
-/**
- * sh_hdmi_isrc1_setup() - ISRC1 Packet
- */
-static void sh_hdmi_isrc1_setup(struct sh_hdmi *hdmi)
-{
-        int i;
-
-        /* ISRC1 Packet */
-        hdmi_write(hdmi, 0x02, HDMI_CTRL_PKT_BUF_INDEX);
-
-        /* Packet Type = 0x05 */
-        hdmi_write(hdmi, 0x05, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-        /* ISRC_Cont, ISRC_Valid, Reserved (0), ISRC_Status */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-        /* Reserved (0) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-        /* PB0 UPC_EAN_ISRC_0-15 */
-        /* Bytes PB16-PB27 shall be set to a value of 0. */
-        for (i = 0; i <= 27; i++)
-                /* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
-                hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
-}
-
-/**
- * sh_hdmi_isrc2_setup() - ISRC2 Packet
- */
-static void sh_hdmi_isrc2_setup(struct sh_hdmi *hdmi)
-{
-        int i;
-
-        /* ISRC2 Packet */
-        hdmi_write(hdmi, 0x03, HDMI_CTRL_PKT_BUF_INDEX);
-
-        /* HB0 Packet Type = 0x06 */
-        hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-        /* Reserved (0) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-        /* Reserved (0) */
-        hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-        /* PB0 UPC_EAN_ISRC_16-31 */
-        /* Bytes PB16-PB27 shall be set to a value of 0. */
-        for (i = 0; i <= 27; i++)
-                /* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
-                hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
-}
-
-/**
  * sh_hdmi_configure() - Initialise HDMI for output
  */
 static void sh_hdmi_configure(struct sh_hdmi *hdmi)
@@ -705,18 +667,6 @@ static void sh_hdmi_configure(struct sh_hdmi *hdmi)
         /* Audio InfoFrame */
         sh_hdmi_audio_infoframe_setup(hdmi);
 
-        /* Gamut Metadata packet */
-        sh_hdmi_gamut_metadata_setup(hdmi);
-
-        /* Audio Content Protection (ACP) Packet */
-        sh_hdmi_acp_setup(hdmi);
-
-        /* ISRC1 Packet */
-        sh_hdmi_isrc1_setup(hdmi);
-
-        /* ISRC2 Packet */
-        sh_hdmi_isrc2_setup(hdmi);
-
         /*
          * Control packet auto send with VSYNC control: auto send
          * General control, Gamut metadata, ISRC, and ACP packets
@@ -734,17 +684,42 @@ static void sh_hdmi_configure(struct sh_hdmi *hdmi)
         hdmi_write(hdmi, 0x40, HDMI_SYSTEM_CTRL);
 }
 
-static void sh_hdmi_read_edid(struct sh_hdmi *hdmi)
+static unsigned long sh_hdmi_rate_error(struct sh_hdmi *hdmi,
+                                        const struct fb_videomode *mode)
 {
-        struct fb_var_screeninfo *var = &hdmi->var;
-        struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
-        struct fb_videomode *lcd_cfg = &pdata->lcd_chan->lcd_cfg;
-        unsigned long height = var->height, width = var->width;
-        int i;
+        long target = PICOS2KHZ(mode->pixclock) * 1000,
+                rate = clk_round_rate(hdmi->hdmi_clk, target);
+        unsigned long rate_error = rate > 0 ? abs(rate - target) : ULONG_MAX;
+
+        dev_dbg(hdmi->dev, "%u-%u-%u-%u x %u-%u-%u-%u\n",
+                mode->left_margin, mode->xres,
+                mode->right_margin, mode->hsync_len,
+                mode->upper_margin, mode->yres,
+                mode->lower_margin, mode->vsync_len);
+
+        dev_dbg(hdmi->dev, "\t@%lu(+/-%lu)Hz, e=%lu / 1000, r=%uHz\n", target,
+                 rate_error, rate_error ? 10000 / (10 * target / rate_error) : 0,
+                 mode->refresh);
+
+        return rate_error;
+}
+
+static int sh_hdmi_read_edid(struct sh_hdmi *hdmi)
+{
+        struct fb_var_screeninfo tmpvar;
+        struct fb_var_screeninfo *var = &tmpvar;
+        const struct fb_videomode *mode, *found = NULL;
+        struct fb_info *info = hdmi->info;
+        struct fb_modelist *modelist = NULL;
+        unsigned int f_width = 0, f_height = 0, f_refresh = 0;
+        unsigned long found_rate_error = ULONG_MAX; /* silly compiler... */
+        bool exact_match = false;
         u8 edid[128];
+        char *forced;
+        int i;
 
         /* Read EDID */
-        pr_debug("Read back EDID code:");
+        dev_dbg(hdmi->dev, "Read back EDID code:");
         for (i = 0; i < 128; i++) {
                 edid[i] = hdmi_read(hdmi, HDMI_EDID_KSV_FIFO_ACCESS_WINDOW);
 #ifdef DEBUG
@@ -759,29 +734,97 @@ static void sh_hdmi_read_edid(struct sh_hdmi *hdmi)
 #ifdef DEBUG
         printk(KERN_CONT "\n");
 #endif
-        fb_parse_edid(edid, var);
-        pr_debug("%u-%u-%u-%u x %u-%u-%u-%u @ %lu kHz monitor detected\n",
-                 var->left_margin, var->xres, var->right_margin, var->hsync_len,
-                 var->upper_margin, var->yres, var->lower_margin, var->vsync_len,
-                 PICOS2KHZ(var->pixclock));
-
-        /* FIXME: Use user-provided configuration instead of EDID */
-        var->width              = width;
-        var->xres               = lcd_cfg->xres;
-        var->xres_virtual       = lcd_cfg->xres;
-        var->left_margin        = lcd_cfg->left_margin;
-        var->right_margin       = lcd_cfg->right_margin;
-        var->hsync_len          = lcd_cfg->hsync_len;
-        var->height             = height;
-        var->yres               = lcd_cfg->yres;
-        var->yres_virtual       = lcd_cfg->yres * 2;
-        var->upper_margin       = lcd_cfg->upper_margin;
-        var->lower_margin       = lcd_cfg->lower_margin;
-        var->vsync_len          = lcd_cfg->vsync_len;
-        var->sync               = lcd_cfg->sync;
-        var->pixclock           = lcd_cfg->pixclock;
-
-        hdmi_external_video_param(hdmi);
+
+        fb_edid_to_monspecs(edid, &hdmi->monspec);
+
+        fb_get_options("sh_mobile_lcdc", &forced);
+        if (forced && *forced) {
+                /* Only primitive parsing so far */
+                i = sscanf(forced, "%ux%u@%u",
+                           &f_width, &f_height, &f_refresh);
+                if (i < 2) {
+                        f_width = 0;
+                        f_height = 0;
+                }
+                dev_dbg(hdmi->dev, "Forced mode %ux%u@%uHz\n",
+                        f_width, f_height, f_refresh);
+        }
+
+        /* Walk monitor modes to find the best or the exact match */
+        for (i = 0, mode = hdmi->monspec.modedb;
+             f_width && f_height && i < hdmi->monspec.modedb_len && !exact_match;
+             i++, mode++) {
+                unsigned long rate_error = sh_hdmi_rate_error(hdmi, mode);
+
+                /* No interest in unmatching modes */
+                if (f_width != mode->xres || f_height != mode->yres)
+                        continue;
+                if (f_refresh == mode->refresh || (!f_refresh && !rate_error))
+                        /*
+                         * Exact match if either the refresh rate matches or it
+                         * hasn't been specified and we've found a mode, for
+                         * which we can configure the clock precisely
+                         */
+                        exact_match = true;
+                else if (found && found_rate_error <= rate_error)
+                        /*
+                         * We otherwise search for the closest matching clock
+                         * rate - either if no refresh rate has been specified
+                         * or we cannot find an exactly matching one
+                         */
+                        continue;
+
+                /* Check if supported: sufficient fb memory, supported clock-rate */
+                fb_videomode_to_var(var, mode);
+
+                if (info && info->fbops->fb_check_var &&
+                    info->fbops->fb_check_var(var, info)) {
+                        exact_match = false;
+                        continue;
+                }
+
+                found = mode;
+                found_rate_error = rate_error;
+        }
+
+        /*
+         * TODO 1: if no ->info is present, postpone running the config until
+         * after ->info first gets registered.
+         * TODO 2: consider registering the HDMI platform device from the LCDC
+         * driver, and passing ->info with HDMI platform data.
+         */
+        if (info && !found) {
+                modelist = hdmi->info->modelist.next &&
+                        !list_empty(&hdmi->info->modelist) ?
+                        list_entry(hdmi->info->modelist.next,
+                                   struct fb_modelist, list) :
+                        NULL;
+
+                if (modelist) {
+                        found = &modelist->mode;
+                        found_rate_error = sh_hdmi_rate_error(hdmi, found);
+                }
+        }
+
+        /* No cookie today */
+        if (!found)
+                return -ENXIO;
+
+        dev_info(hdmi->dev, "Using %s mode %ux%u@%uHz (%luHz), clock error %luHz\n",
+                 modelist ? "default" : "EDID", found->xres, found->yres,
+                 found->refresh, PICOS2KHZ(found->pixclock) * 1000, found_rate_error);
+
+        if ((found->xres == 720 && found->yres == 480) ||
+            (found->xres == 1280 && found->yres == 720) ||
+            (found->xres == 1920 && found->yres == 1080))
+                hdmi->preprogrammed_mode = true;
+        else
+                hdmi->preprogrammed_mode = false;
+
+        fb_videomode_to_var(&hdmi->var, found);
+        sh_hdmi_external_video_param(hdmi);
+
+        return 0;
 }
 
 static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
@@ -809,8 +852,8 @@ static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
         hdmi_write(hdmi, 0xFF, HDMI_INTERRUPT_STATUS_2);
 
         if (printk_ratelimit())
-                pr_debug("IRQ #%d: Status #1: 0x%x & 0x%x, #2: 0x%x & 0x%x\n",
-                         irq, status1, mask1, status2, mask2);
+                dev_dbg(hdmi->dev, "IRQ #%d: Status #1: 0x%x & 0x%x, #2: 0x%x & 0x%x\n",
+                        irq, status1, mask1, status2, mask2);
 
         if (!((status1 & mask1) | (status2 & mask2))) {
                 return IRQ_NONE;
@@ -821,7 +864,7 @@ static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
                 udelay(500);
 
                 msens = hdmi_read(hdmi, HDMI_HOT_PLUG_MSENS_STATUS);
-                pr_debug("MSENS 0x%x\n", msens);
+                dev_dbg(hdmi->dev, "MSENS 0x%x\n", msens);
                 /* Check, if hot plug & MSENS pin status are both high */
                 if ((msens & 0xC0) == 0xC0) {
                         /* Display plug in */
@@ -857,83 +900,176 @@ static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
         return IRQ_HANDLED;
 }
 
-static void hdmi_display_on(void *arg, struct fb_info *info)
+/* locking:     called with info->lock held, or before register_framebuffer() */
+static void sh_hdmi_display_on(void *arg, struct fb_info *info)
 {
+        /*
+         * info is guaranteed to be valid, when we are called, because our
+         * FB_EVENT_FB_UNBIND notify is also called with info->lock held
+         */
         struct sh_hdmi *hdmi = arg;
         struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+        struct sh_mobile_lcdc_chan *ch = info->par;
 
-        if (info->var.xres != 1280 || info->var.yres != 720) {
-                dev_warn(info->device, "Unsupported framebuffer geometry %ux%u\n",
-                         info->var.xres, info->var.yres);
-                return;
-        }
+        dev_dbg(hdmi->dev, "%s(%p): state %x\n", __func__,
+                pdata->lcd_dev, info->state);
+
+        /* No need to lock */
+        hdmi->info = info;
 
-        pr_debug("%s(%p): state %x\n", __func__, pdata->lcd_dev, info->state);
         /*
-         * FIXME: not a good place to store fb_info. And we cannot nullify it
-         * even on monitor disconnect. What should the lifecycle be?
+         * hp_state can be set to
+         * HDMI_HOTPLUG_DISCONNECTED:   on monitor unplug
+         * HDMI_HOTPLUG_CONNECTED:      on monitor plug-in
+         * HDMI_HOTPLUG_EDID_DONE:      on EDID read completion
          */
-        hdmi->info = info;
         switch (hdmi->hp_state) {
         case HDMI_HOTPLUG_EDID_DONE:
                 /* PS mode d->e. All functions are active */
                 hdmi_write(hdmi, 0x80, HDMI_SYSTEM_CTRL);
-                pr_debug("HDMI running\n");
+                dev_dbg(hdmi->dev, "HDMI running\n");
                 break;
         case HDMI_HOTPLUG_DISCONNECTED:
                 info->state = FBINFO_STATE_SUSPENDED;
         default:
-                hdmi->var = info->var;
+                hdmi->var = ch->display_var;
         }
 }
 
-static void hdmi_display_off(void *arg)
+/* locking: called with info->lock held */
+static void sh_hdmi_display_off(void *arg)
 {
         struct sh_hdmi *hdmi = arg;
         struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
 
-        pr_debug("%s(%p)\n", __func__, pdata->lcd_dev);
+        dev_dbg(hdmi->dev, "%s(%p)\n", __func__, pdata->lcd_dev);
         /* PS mode e->a */
         hdmi_write(hdmi, 0x10, HDMI_SYSTEM_CTRL);
 }
 
+static bool sh_hdmi_must_reconfigure(struct sh_hdmi *hdmi)
+{
+        struct fb_info *info = hdmi->info;
+        struct sh_mobile_lcdc_chan *ch = info->par;
+        struct fb_var_screeninfo *new_var = &hdmi->var, *old_var = &ch->display_var;
+        struct fb_videomode mode1, mode2;
+
+        fb_var_to_videomode(&mode1, old_var);
+        fb_var_to_videomode(&mode2, new_var);
+
+        dev_dbg(info->dev, "Old %ux%u, new %ux%u\n",
+                mode1.xres, mode1.yres, mode2.xres, mode2.yres);
+
+        if (fb_mode_is_equal(&mode1, &mode2))
+                return false;
+
+        dev_dbg(info->dev, "Switching %u -> %u lines\n",
+                mode1.yres, mode2.yres);
+        *old_var = *new_var;
+
+        return true;
+}
+
+/**
+ * sh_hdmi_clk_configure() - set HDMI clock frequency and enable the clock
+ * @hdmi:       driver context
+ * @pixclock:   pixel clock period in picoseconds
+ * return:      configured positive rate if successful
+ *              0 if couldn't set the rate, but managed to enable the clock
+ *              negative error, if couldn't enable the clock
+ */
+static long sh_hdmi_clk_configure(struct sh_hdmi *hdmi, unsigned long pixclock)
+{
+        long rate;
+        int ret;
+
+        rate = PICOS2KHZ(pixclock) * 1000;
+        rate = clk_round_rate(hdmi->hdmi_clk, rate);
+        if (rate > 0) {
+                ret = clk_set_rate(hdmi->hdmi_clk, rate);
+                if (ret < 0) {
+                        dev_warn(hdmi->dev, "Cannot set rate %ld: %d\n", rate, ret);
+                        rate = 0;
+                } else {
+                        dev_dbg(hdmi->dev, "HDMI set frequency %lu\n", rate);
+                }
+        } else {
+                rate = 0;
+                dev_warn(hdmi->dev, "Cannot get suitable rate: %ld\n", rate);
+        }
+
+        ret = clk_enable(hdmi->hdmi_clk);
+        if (ret < 0) {
+                dev_err(hdmi->dev, "Cannot enable clock: %d\n", ret);
+                return ret;
+        }
+
+        return rate;
+}
+
 /* Hotplug interrupt occurred, read EDID */
-static void edid_work_fn(struct work_struct *work)
+static void sh_hdmi_edid_work_fn(struct work_struct *work)
 {
         struct sh_hdmi *hdmi = container_of(work, struct sh_hdmi, edid_work.work);
         struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+        struct sh_mobile_lcdc_chan *ch;
+        int ret;
 
-        pr_debug("%s(%p): begin, hotplug status %d\n", __func__,
-                 pdata->lcd_dev, hdmi->hp_state);
+        dev_dbg(hdmi->dev, "%s(%p): begin, hotplug status %d\n", __func__,
+                pdata->lcd_dev, hdmi->hp_state);
 
         if (!pdata->lcd_dev)
                 return;
 
+        mutex_lock(&hdmi->mutex);
+
         if (hdmi->hp_state == HDMI_HOTPLUG_EDID_DONE) {
-                pm_runtime_get_sync(hdmi->dev);
                 /* A device has been plugged in */
-                sh_hdmi_read_edid(hdmi);
+                pm_runtime_get_sync(hdmi->dev);
+
+                ret = sh_hdmi_read_edid(hdmi);
+                if (ret < 0)
+                        goto out;
+
+                /* Reconfigure the clock */
+                clk_disable(hdmi->hdmi_clk);
+                ret = sh_hdmi_clk_configure(hdmi, hdmi->var.pixclock);
+                if (ret < 0)
+                        goto out;
+
                 msleep(10);
                 sh_hdmi_configure(hdmi);
                 /* Switched to another (d) power-save mode */
                 msleep(10);
 
                 if (!hdmi->info)
-                        return;
+                        goto out;
+
+                ch = hdmi->info->par;
 
                 acquire_console_sem();
 
                 /* HDMI plug in */
-                hdmi->info->var = hdmi->var;
-                if (hdmi->info->state != FBINFO_STATE_RUNNING)
+                if (!sh_hdmi_must_reconfigure(hdmi) &&
+                    hdmi->info->state == FBINFO_STATE_RUNNING) {
+                        /*
+                         * First activation with the default monitor - just turn
+                         * on, if we run a resume here, the logo disappears
+                         */
+                        if (lock_fb_info(hdmi->info)) {
+                                sh_hdmi_display_on(hdmi, hdmi->info);
+                                unlock_fb_info(hdmi->info);
+                        }
+                } else {
+                        /* New monitor or have to wake up */
                         fb_set_suspend(hdmi->info, 0);
-                else
-                        hdmi_display_on(hdmi, hdmi->info);
+                }
 
                 release_console_sem();
         } else {
+                ret = 0;
                 if (!hdmi->info)
-                        return;
+                        goto out;
 
                 acquire_console_sem();
 
@@ -942,15 +1078,67 @@ static void edid_work_fn(struct work_struct *work)
 
                 release_console_sem();
                 pm_runtime_put(hdmi->dev);
+                fb_destroy_modedb(hdmi->monspec.modedb);
         }
 
-        pr_debug("%s(%p): end\n", __func__, pdata->lcd_dev);
+out:
+        if (ret < 0)
+                hdmi->hp_state = HDMI_HOTPLUG_DISCONNECTED;
+        mutex_unlock(&hdmi->mutex);
+
+        dev_dbg(hdmi->dev, "%s(%p): end\n", __func__, pdata->lcd_dev);
+}
+
+static int sh_hdmi_notify(struct notifier_block *nb,
+                          unsigned long action, void *data);
+
+static struct notifier_block sh_hdmi_notifier = {
+        .notifier_call = sh_hdmi_notify,
+};
+
+static int sh_hdmi_notify(struct notifier_block *nb,
+                          unsigned long action, void *data)
+{
+        struct fb_event *event = data;
+        struct fb_info *info = event->info;
+        struct sh_mobile_lcdc_chan *ch = info->par;
+        struct sh_mobile_lcdc_board_cfg *board_cfg = &ch->cfg.board_cfg;
+        struct sh_hdmi *hdmi = board_cfg->board_data;
+
+        if (nb != &sh_hdmi_notifier || !hdmi || hdmi->info != info)
+                return NOTIFY_DONE;
+
+        switch(action) {
+        case FB_EVENT_FB_REGISTERED:
+                /* Unneeded, activation taken care by sh_hdmi_display_on() */
+                break;
+        case FB_EVENT_FB_UNREGISTERED:
+                /*
+                 * We are called from unregister_framebuffer() with the
+                 * info->lock held. This is bad for us, because we can race with
+                 * the scheduled work, which has to call fb_set_suspend(), which
+                 * takes info->lock internally, so, sh_hdmi_edid_work_fn()
+                 * cannot take and hold info->lock for the whole function
+                 * duration. Using an additional lock creates a classical AB-BA
+                 * lock up. Therefore, we have to release the info->lock
+                 * temporarily, synchronise with the work queue and re-acquire
+                 * the info->lock.
+                 */
+                unlock_fb_info(hdmi->info);
+                mutex_lock(&hdmi->mutex);
+                hdmi->info = NULL;
+                mutex_unlock(&hdmi->mutex);
+                lock_fb_info(hdmi->info);
+                return NOTIFY_OK;
+        }
+        return NOTIFY_DONE;
 }
 
 static int __init sh_hdmi_probe(struct platform_device *pdev)
 {
         struct sh_mobile_hdmi_info *pdata = pdev->dev.platform_data;
         struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        struct sh_mobile_lcdc_board_cfg *board_cfg;
         int irq = platform_get_irq(pdev, 0), ret;
         struct sh_hdmi *hdmi;
         long rate;
@@ -964,10 +1152,7 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
                 return -ENOMEM;
         }
 
-        ret =  snd_soc_register_codec(&pdev->dev,
-                        &soc_codec_dev_sh_hdmi, &sh_hdmi_dai, 1);
-        if (ret < 0)
-                goto esndreg;
+        mutex_init(&hdmi->mutex);
 
         hdmi->dev = &pdev->dev;
 
@@ -978,30 +1163,14 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
                 goto egetclk;
         }
 
-        rate = PICOS2KHZ(pdata->lcd_chan->lcd_cfg.pixclock) * 1000;
-
-        rate = clk_round_rate(hdmi->hdmi_clk, rate);
+        /* Some arbitrary relaxed pixclock just to get things started */
+        rate = sh_hdmi_clk_configure(hdmi, 37037);
         if (rate < 0) {
                 ret = rate;
-                dev_err(&pdev->dev, "Cannot get suitable rate: %ld\n", rate);
                 goto erate;
         }
 
-        ret = clk_set_rate(hdmi->hdmi_clk, rate);
-        if (ret < 0) {
-                dev_err(&pdev->dev, "Cannot set rate %ld: %d\n", rate, ret);
-                goto erate;
-        }
-
-        pr_debug("HDMI set frequency %lu\n", rate);
-
-        ret = clk_enable(hdmi->hdmi_clk);
-        if (ret < 0) {
-                dev_err(&pdev->dev, "Cannot enable clock: %d\n", ret);
-                goto eclkenable;
-        }
-
-        dev_info(&pdev->dev, "Enabled HDMI clock at %luHz\n", rate);
+        dev_dbg(&pdev->dev, "Enabled HDMI clock at %luHz\n", rate);
 
         if (!request_mem_region(res->start, resource_size(res), dev_name(&pdev->dev))) {
                 dev_err(&pdev->dev, "HDMI register region already claimed\n");
@@ -1018,18 +1187,18 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 
         platform_set_drvdata(pdev, hdmi);
 
-#if 1
         /* Product and revision IDs are 0 in sh-mobile version */
         dev_info(&pdev->dev, "Detected HDMI controller 0x%x:0x%x\n",
                  hdmi_read(hdmi, HDMI_PRODUCT_ID), hdmi_read(hdmi, HDMI_REVISION_ID));
-#endif
 
         /* Set up LCDC callbacks */
-        pdata->lcd_chan->board_cfg.board_data = hdmi;
-        pdata->lcd_chan->board_cfg.display_on = hdmi_display_on;
-        pdata->lcd_chan->board_cfg.display_off = hdmi_display_off;
+        board_cfg = &pdata->lcd_chan->board_cfg;
+        board_cfg->owner = THIS_MODULE;
+        board_cfg->board_data = hdmi;
+        board_cfg->display_on = sh_hdmi_display_on;
+        board_cfg->display_off = sh_hdmi_display_off;
 
-        INIT_DELAYED_WORK(&hdmi->edid_work, edid_work_fn);
+        INIT_DELAYED_WORK(&hdmi->edid_work, sh_hdmi_edid_work_fn);
 
         pm_runtime_enable(&pdev->dev);
         pm_runtime_resume(&pdev->dev);
@@ -1041,8 +1210,17 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
                 goto ereqirq;
         }
 
+        ret = snd_soc_register_codec(&pdev->dev,
+                        &soc_codec_dev_sh_hdmi, &sh_hdmi_dai, 1);
+        if (ret < 0) {
+                dev_err(&pdev->dev, "codec registration failed\n");
+                goto ecodec;
+        }
+
         return 0;
 
+ecodec:
+        free_irq(irq, hdmi);
 ereqirq:
         pm_runtime_disable(&pdev->dev);
         iounmap(hdmi->base);
@@ -1050,12 +1228,10 @@ emap:
         release_mem_region(res->start, resource_size(res));
 ereqreg:
         clk_disable(hdmi->hdmi_clk);
-eclkenable:
 erate:
         clk_put(hdmi->hdmi_clk);
 egetclk:
-        snd_soc_unregister_codec(&pdev->dev);
-esndreg:
+        mutex_destroy(&hdmi->mutex);
         kfree(hdmi);
 
         return ret;
@@ -1066,21 +1242,26 @@ static int __exit sh_hdmi_remove(struct platform_device *pdev)
         struct sh_mobile_hdmi_info *pdata = pdev->dev.platform_data;
         struct sh_hdmi *hdmi = platform_get_drvdata(pdev);
         struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+        struct sh_mobile_lcdc_board_cfg *board_cfg = &pdata->lcd_chan->board_cfg;
         int irq = platform_get_irq(pdev, 0);
 
         snd_soc_unregister_codec(&pdev->dev);
 
-        pdata->lcd_chan->board_cfg.display_on = NULL;
-        pdata->lcd_chan->board_cfg.display_off = NULL;
-        pdata->lcd_chan->board_cfg.board_data = NULL;
+        board_cfg->display_on = NULL;
+        board_cfg->display_off = NULL;
+        board_cfg->board_data = NULL;
+        board_cfg->owner = NULL;
 
+        /* No new work will be scheduled, wait for running ISR */
         free_irq(irq, hdmi);
-        pm_runtime_disable(&pdev->dev);
+        /* Wait for already scheduled work */
         cancel_delayed_work_sync(&hdmi->edid_work);
+        pm_runtime_disable(&pdev->dev);
         clk_disable(hdmi->hdmi_clk);
         clk_put(hdmi->hdmi_clk);
         iounmap(hdmi->base);
         release_mem_region(res->start, resource_size(res));
+        mutex_destroy(&hdmi->mutex);
         kfree(hdmi);
 
         return 0;
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index 7a1419279c8f..50963739a409 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -12,7 +12,6 @@
 #include <linux/init.h>
 #include <linux/delay.h>
 #include <linux/mm.h>
-#include <linux/fb.h>
 #include <linux/clk.h>
 #include <linux/pm_runtime.h>
 #include <linux/platform_device.h>
@@ -21,10 +20,12 @@
 #include <linux/vmalloc.h>
 #include <linux/ioctl.h>
 #include <linux/slab.h>
+#include <linux/console.h>
 #include <video/sh_mobile_lcdc.h>
 #include <asm/atomic.h>
 
-#define PALETTE_NR 16
+#include "sh_mobile_lcdcfb.h"
+
 #define SIDE_B_OFFSET 0x1000
 #define MIRROR_OFFSET 0x2000
 
@@ -53,11 +54,8 @@ static int lcdc_shared_regs[] = {
 };
 #define NR_SHARED_REGS ARRAY_SIZE(lcdc_shared_regs)
 
-/* per-channel registers */
-enum { LDDCKPAT1R, LDDCKPAT2R, LDMT1R, LDMT2R, LDMT3R, LDDFR, LDSM1R,
-       LDSM2R, LDSA1R, LDMLSR, LDHCNR, LDHSYNR, LDVLNR, LDVSYNR, LDPMR,
-       LDHAJR,
-       NR_CH_REGS };
+#define DEFAULT_XRES 1280
+#define DEFAULT_YRES 1024
 
 static unsigned long lcdc_offs_mainlcd[NR_CH_REGS] = {
         [LDDCKPAT1R] = 0x400,
@@ -112,23 +110,21 @@ static unsigned long lcdc_offs_sublcd[NR_CH_REGS] = {
 #define LDRCNTR_MRC     0x00000001
 #define LDSR_MRS        0x00000100
 
-struct sh_mobile_lcdc_priv;
-struct sh_mobile_lcdc_chan {
-        struct sh_mobile_lcdc_priv *lcdc;
-        unsigned long *reg_offs;
-        unsigned long ldmt1r_value;
-        unsigned long enabled; /* ME and SE in LDCNT2R */
-        struct sh_mobile_lcdc_chan_cfg cfg;
-        u32 pseudo_palette[PALETTE_NR];
-        unsigned long saved_ch_regs[NR_CH_REGS];
-        struct fb_info *info;
-        dma_addr_t dma_handle;
-        struct fb_deferred_io defio;
-        struct scatterlist *sglist;
-        unsigned long frame_end;
-        unsigned long pan_offset;
-        wait_queue_head_t frame_end_wait;
-        struct completion vsync_completion;
+static const struct fb_videomode default_720p = {
+        .name = "HDMI 720p",
+        .xres = 1280,
+        .yres = 720,
+
+        .left_margin = 200,
+        .right_margin = 88,
+        .hsync_len = 48,
+
+        .upper_margin = 20,
+        .lower_margin = 5,
+        .vsync_len = 5,
+
+        .pixclock = 13468,
+        .sync = FB_SYNC_VERT_HIGH_ACT | FB_SYNC_HOR_HIGH_ACT,
 };
 
 struct sh_mobile_lcdc_priv {
@@ -409,8 +405,8 @@ static void sh_mobile_lcdc_start_stop(struct sh_mobile_lcdc_priv *priv,
 
 static void sh_mobile_lcdc_geometry(struct sh_mobile_lcdc_chan *ch)
 {
-        struct fb_var_screeninfo *var = &ch->info->var;
-        unsigned long h_total, hsync_pos;
+        struct fb_var_screeninfo *var = &ch->info->var, *display_var = &ch->display_var;
+        unsigned long h_total, hsync_pos, display_h_total;
         u32 tmp;
 
         tmp = ch->ldmt1r_value;
@@ -428,31 +424,33 @@ static void sh_mobile_lcdc_geometry(struct sh_mobile_lcdc_chan *ch)
         lcdc_write_chan(ch, LDMT3R, ch->cfg.sys_bus_cfg.ldmt3r);
 
         /* horizontal configuration */
-        h_total = var->xres + var->hsync_len +
-                var->left_margin + var->right_margin;
+        h_total = display_var->xres + display_var->hsync_len +
+                display_var->left_margin + display_var->right_margin;
         tmp = h_total / 8; /* HTCN */
-        tmp |= (var->xres / 8) << 16; /* HDCN */
+        tmp |= (min(display_var->xres, var->xres) / 8) << 16; /* HDCN */
         lcdc_write_chan(ch, LDHCNR, tmp);
 
-        hsync_pos = var->xres + var->right_margin;
+        hsync_pos = display_var->xres + display_var->right_margin;
         tmp = hsync_pos / 8; /* HSYNP */
-        tmp |= (var->hsync_len / 8) << 16; /* HSYNW */
+        tmp |= (display_var->hsync_len / 8) << 16; /* HSYNW */
         lcdc_write_chan(ch, LDHSYNR, tmp);
 
         /* vertical configuration */
-        tmp = var->yres + var->vsync_len +
-                var->upper_margin + var->lower_margin; /* VTLN */
-        tmp |= var->yres << 16; /* VDLN */
+        tmp = display_var->yres + display_var->vsync_len +
+                display_var->upper_margin + display_var->lower_margin; /* VTLN */
+        tmp |= min(display_var->yres, var->yres) << 16; /* VDLN */
         lcdc_write_chan(ch, LDVLNR, tmp);
 
-        tmp = var->yres + var->lower_margin; /* VSYNP */
-        tmp |= var->vsync_len << 16; /* VSYNW */
+        tmp = display_var->yres + display_var->lower_margin; /* VSYNP */
+        tmp |= display_var->vsync_len << 16; /* VSYNW */
         lcdc_write_chan(ch, LDVSYNR, tmp);
 
         /* Adjust horizontal synchronisation for HDMI */
-        tmp = ((var->xres & 7) << 24) |
-                ((h_total & 7) << 16) |
-                ((var->hsync_len & 7) << 8) |
+        display_h_total = display_var->xres + display_var->hsync_len +
+                display_var->left_margin + display_var->right_margin;
+        tmp = ((display_var->xres & 7) << 24) |
+                ((display_h_total & 7) << 16) |
+                ((display_var->hsync_len & 7) << 8) |
                 hsync_pos;
         lcdc_write_chan(ch, LDHAJR, tmp);
 }
@@ -460,7 +458,6 @@ static void sh_mobile_lcdc_geometry(struct sh_mobile_lcdc_chan *ch)
 static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 {
         struct sh_mobile_lcdc_chan *ch;
-        struct fb_videomode *lcd_cfg;
         struct sh_mobile_lcdc_board_cfg *board_cfg;
         unsigned long tmp;
         int k, m;
@@ -503,7 +500,8 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
                         m = 1 << 6;
                 tmp |= m << (lcdc_chan_is_sublcd(ch) ? 8 : 0);
 
-                lcdc_write_chan(ch, LDDCKPAT1R, 0x00000000);
+                /* FIXME: sh7724 can only use 42, 48, 54 and 60 for the divider denominator */
+                lcdc_write_chan(ch, LDDCKPAT1R, 0);
                 lcdc_write_chan(ch, LDDCKPAT2R, (1 << (m/2)) - 1);
         }
 
@@ -518,7 +516,6 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 
         for (k = 0; k < ARRAY_SIZE(priv->ch); k++) {
                 ch = &priv->ch[k];
-                lcd_cfg = &ch->cfg.lcd_cfg;
 
                 if (!ch->enabled)
                         continue;
@@ -547,7 +544,7 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
 
                 /* set bpp format in PKF[4:0] */
                 tmp = lcdc_read_chan(ch, LDDFR);
-                tmp &= ~(0x0001001f);
+                tmp &= ~0x0001001f;
                 tmp |= (ch->info->var.bits_per_pixel == 16) ? 3 : 0;
                 lcdc_write_chan(ch, LDDFR, tmp);
 
@@ -591,8 +588,10 @@ static int sh_mobile_lcdc_start(struct sh_mobile_lcdc_priv *priv)
                         continue;
 
                 board_cfg = &ch->cfg.board_cfg;
-                if (board_cfg->display_on)
+                if (try_module_get(board_cfg->owner) && board_cfg->display_on) {
                         board_cfg->display_on(board_cfg->board_data, ch->info);
+                        module_put(board_cfg->owner);
+                }
         }
 
         return 0;
@@ -614,7 +613,7 @@ static void sh_mobile_lcdc_stop(struct sh_mobile_lcdc_priv *priv)
                  * flush frame, and wait for frame end interrupt
                  * clean up deferred io and enable clock
                  */
-                if (ch->info->fbdefio) {
+                if (ch->info && ch->info->fbdefio) {
                         ch->frame_end = 0;
                         schedule_delayed_work(&ch->info->deferred_work, 0);
                         wait_event(ch->frame_end_wait, ch->frame_end);
@@ -624,8 +623,10 @@ static void sh_mobile_lcdc_stop(struct sh_mobile_lcdc_priv *priv)
                 }
 
                 board_cfg = &ch->cfg.board_cfg;
-                if (board_cfg->display_off)
+                if (try_module_get(board_cfg->owner) && board_cfg->display_off) {
                         board_cfg->display_off(board_cfg->board_data);
+                        module_put(board_cfg->owner);
+                }
         }
 
         /* stop the lcdc */
@@ -704,7 +705,6 @@ static int sh_mobile_lcdc_setup_clocks(struct platform_device *pdev,
                         return PTR_ERR(priv->dot_clk);
                 }
         }
-        atomic_set(&priv->hw_usecnt, -1);
 
         /* Runtime PM support involves two step for this driver:
          * 1) Enable Runtime PM
@@ -837,6 +837,102 @@ static int sh_mobile_ioctl(struct fb_info *info, unsigned int cmd,
         return retval;
 }
 
+static void sh_mobile_fb_reconfig(struct fb_info *info)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+        struct fb_videomode mode1, mode2;
+        struct fb_event event;
+        int evnt = FB_EVENT_MODE_CHANGE_ALL;
+
+        if (ch->use_count > 1 || (ch->use_count == 1 && !info->fbcon_par))
+                /* More framebuffer users are active */
+                return;
+
+        fb_var_to_videomode(&mode1, &ch->display_var);
+        fb_var_to_videomode(&mode2, &info->var);
+
+        if (fb_mode_is_equal(&mode1, &mode2))
+                return;
+
+        /* Display has been re-plugged, framebuffer is free now, reconfigure */
+        if (fb_set_var(info, &ch->display_var) < 0)
+                /* Couldn't reconfigure, hopefully, can continue as before */
+                return;
+
+        info->fix.line_length = mode2.xres * (ch->cfg.bpp / 8);
+
+        /*
+         * fb_set_var() calls the notifier change internally, only if
+         * FBINFO_MISC_USEREVENT flag is set. Since we do not want to fake a
+         * user event, we have to call the chain ourselves.
+         */
+        event.info = info;
+        event.data = &mode2;
+        fb_notifier_call_chain(evnt, &event);
+}
+
+/*
+ * Locking: both .fb_release() and .fb_open() are called with info->lock held if
+ * user == 1, or with console sem held, if user == 0.
+ */
+static int sh_mobile_release(struct fb_info *info, int user)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+
+        mutex_lock(&ch->open_lock);
+        dev_dbg(info->dev, "%s(): %d users\n", __func__, ch->use_count);
+
+        ch->use_count--;
+
+        /* Nothing to reconfigure, when called from fbcon */
+        if (user) {
+                acquire_console_sem();
+                sh_mobile_fb_reconfig(info);
+                release_console_sem();
+        }
+
+        mutex_unlock(&ch->open_lock);
+
+        return 0;
+}
+
+static int sh_mobile_open(struct fb_info *info, int user)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+
+        mutex_lock(&ch->open_lock);
+        ch->use_count++;
+
+        dev_dbg(info->dev, "%s(): %d users\n", __func__, ch->use_count);
+        mutex_unlock(&ch->open_lock);
+
+        return 0;
+}
+
+static int sh_mobile_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+        struct sh_mobile_lcdc_chan *ch = info->par;
+
+        if (var->xres < 160 || var->xres > 1920 ||
+            var->yres < 120 || var->yres > 1080 ||
+            var->left_margin < 32 || var->left_margin > 320 ||
+            var->right_margin < 12 || var->right_margin > 240 ||
+            var->upper_margin < 12 || var->upper_margin > 120 ||
+            var->lower_margin < 1 || var->lower_margin > 64 ||
+            var->hsync_len < 32 || var->hsync_len > 240 ||
+            var->vsync_len < 2 || var->vsync_len > 64 ||
+            var->pixclock < 6000 || var->pixclock > 40000 ||
+            var->xres * var->yres * (ch->cfg.bpp / 8) * 2 > info->fix.smem_len) {
+                dev_warn(info->dev, "Invalid info: %u %u %u %u %u %u %u %u %u!\n",
+                         var->xres, var->yres,
+                         var->left_margin, var->right_margin,
+                         var->upper_margin, var->lower_margin,
+                         var->hsync_len, var->vsync_len,
+                         var->pixclock);
+                return -EINVAL;
+        }
+        return 0;
+}
 
 static struct fb_ops sh_mobile_lcdc_ops = {
         .owner          = THIS_MODULE,
@@ -848,6 +944,9 @@ static struct fb_ops sh_mobile_lcdc_ops = {
         .fb_imageblit   = sh_mobile_lcdc_imageblit,
         .fb_pan_display = sh_mobile_fb_pan_display,
         .fb_ioctl       = sh_mobile_ioctl,
+        .fb_open        = sh_mobile_open,
+        .fb_release     = sh_mobile_release,
+        .fb_check_var   = sh_mobile_check_var,
 };
 
 static int sh_mobile_lcdc_set_bpp(struct fb_var_screeninfo *var, int bpp)
@@ -958,6 +1057,7 @@ static const struct dev_pm_ops sh_mobile_lcdc_dev_pm_ops = {
         .runtime_resume = sh_mobile_lcdc_runtime_resume,
 };
 
+/* locking: called with info->lock held */
 static int sh_mobile_lcdc_notify(struct notifier_block *nb,
                                  unsigned long action, void *data)
 {
@@ -965,53 +1065,40 @@ static int sh_mobile_lcdc_notify(struct notifier_block *nb,
         struct fb_info *info = event->info;
         struct sh_mobile_lcdc_chan *ch = info->par;
         struct sh_mobile_lcdc_board_cfg *board_cfg = &ch->cfg.board_cfg;
-        struct fb_var_screeninfo *var;
+        int ret;
 
         if (&ch->lcdc->notifier != nb)
-                return 0;
+                return NOTIFY_DONE;
 
         dev_dbg(info->dev, "%s(): action = %lu, data = %p\n",
                 __func__, action, event->data);
 
         switch(action) {
         case FB_EVENT_SUSPEND:
-                if (board_cfg->display_off)
+                if (try_module_get(board_cfg->owner) && board_cfg->display_off) {
                         board_cfg->display_off(board_cfg->board_data);
+                        module_put(board_cfg->owner);
+                }
                 pm_runtime_put(info->device);
+                sh_mobile_lcdc_stop(ch->lcdc);
                 break;
         case FB_EVENT_RESUME:
-                var = &info->var;
+                mutex_lock(&ch->open_lock);
+                sh_mobile_fb_reconfig(info);
+                mutex_unlock(&ch->open_lock);
 
                 /* HDMI must be enabled before LCDC configuration */
-                if (board_cfg->display_on)
-                        board_cfg->display_on(board_cfg->board_data, ch->info);
-
-                /* Check if the new display is not in our modelist */
-                if (ch->info->modelist.next &&
-                    !fb_match_mode(var, &ch->info->modelist)) {
-                        struct fb_videomode mode;
-                        int ret;
-
-                        /* Can we handle this display? */
-                        if (var->xres > ch->cfg.lcd_cfg.xres ||
-                            var->yres > ch->cfg.lcd_cfg.yres)
-                                return -ENOMEM;
-
-                        /* Add to the modelist */
-                        fb_var_to_videomode(&mode, var);
-                        ret = fb_add_videomode(&mode, &ch->info->modelist);
-                        if (ret < 0)
-                                return ret;
+                if (try_module_get(board_cfg->owner) && board_cfg->display_on) {
+                        board_cfg->display_on(board_cfg->board_data, info);
+                        module_put(board_cfg->owner);
                 }
 
-                pm_runtime_get_sync(info->device);
-
-                sh_mobile_lcdc_geometry(ch);
-
-                break;
+                ret = sh_mobile_lcdc_start(ch->lcdc);
+                if (!ret)
+                        pm_runtime_get_sync(info->device);
         }
 
-        return 0;
+        return NOTIFY_OK;
 }
 
 static int sh_mobile_lcdc_remove(struct platform_device *pdev);
@@ -1020,14 +1107,13 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 {
         struct fb_info *info;
         struct sh_mobile_lcdc_priv *priv;
-        struct sh_mobile_lcdc_info *pdata;
-        struct sh_mobile_lcdc_chan_cfg *cfg;
+        struct sh_mobile_lcdc_info *pdata = pdev->dev.platform_data;
         struct resource *res;
         int error;
         void *buf;
         int i, j;
 
-        if (!pdev->dev.platform_data) {
+        if (!pdata) {
                 dev_err(&pdev->dev, "no platform data defined\n");
                 return -EINVAL;
         }
@@ -1055,31 +1141,33 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
         }
 
         priv->irq = i;
-        pdata = pdev->dev.platform_data;
+        atomic_set(&priv->hw_usecnt, -1);
 
         j = 0;
         for (i = 0; i < ARRAY_SIZE(pdata->ch); i++) {
-                priv->ch[j].lcdc = priv;
-                memcpy(&priv->ch[j].cfg, &pdata->ch[i], sizeof(pdata->ch[i]));
+                struct sh_mobile_lcdc_chan *ch = priv->ch + j;
+
+                ch->lcdc = priv;
+                memcpy(&ch->cfg, &pdata->ch[i], sizeof(pdata->ch[i]));
 
-                error = sh_mobile_lcdc_check_interface(&priv->ch[j]);
+                error = sh_mobile_lcdc_check_interface(ch);
                 if (error) {
                         dev_err(&pdev->dev, "unsupported interface type\n");
                         goto err1;
                 }
-                init_waitqueue_head(&priv->ch[j].frame_end_wait);
-                init_completion(&priv->ch[j].vsync_completion);
-                priv->ch[j].pan_offset = 0;
+                init_waitqueue_head(&ch->frame_end_wait);
+                init_completion(&ch->vsync_completion);
+                ch->pan_offset = 0;
 
                 switch (pdata->ch[i].chan) {
                 case LCDC_CHAN_MAINLCD:
-                        priv->ch[j].enabled = 1 << 1;
-                        priv->ch[j].reg_offs = lcdc_offs_mainlcd;
+                        ch->enabled = 1 << 1;
+                        ch->reg_offs = lcdc_offs_mainlcd;
                         j++;
                         break;
                 case LCDC_CHAN_SUBLCD:
-                        priv->ch[j].enabled = 1 << 2;
-                        priv->ch[j].reg_offs = lcdc_offs_sublcd;
+                        ch->enabled = 1 << 2;
+                        ch->reg_offs = lcdc_offs_sublcd;
                         j++;
                         break;
                 }
@@ -1103,69 +1191,83 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 
         for (i = 0; i < j; i++) {
                 struct fb_var_screeninfo *var;
-                struct fb_videomode *lcd_cfg;
-                cfg = &priv->ch[i].cfg;
+                const struct fb_videomode *lcd_cfg, *max_cfg = NULL;
+                struct sh_mobile_lcdc_chan *ch = priv->ch + i;
+                struct sh_mobile_lcdc_chan_cfg *cfg = &ch->cfg;
+                const struct fb_videomode *mode = cfg->lcd_cfg;
+                unsigned long max_size = 0;
+                int k;
 
-                priv->ch[i].info = framebuffer_alloc(0, &pdev->dev);
-                if (!priv->ch[i].info) {
+                ch->info = framebuffer_alloc(0, &pdev->dev);
+                if (!ch->info) {
                         dev_err(&pdev->dev, "unable to allocate fb_info\n");
                         error = -ENOMEM;
                         break;
                 }
 
-                info = priv->ch[i].info;
+                info = ch->info;
                 var = &info->var;
-                lcd_cfg = &cfg->lcd_cfg;
                 info->fbops = &sh_mobile_lcdc_ops;
-                var->xres = var->xres_virtual = lcd_cfg->xres;
-                var->yres = lcd_cfg->yres;
+                info->par = ch;
+
+                mutex_init(&ch->open_lock);
+
+                for (k = 0, lcd_cfg = mode;
+                     k < cfg->num_cfg && lcd_cfg;
+                     k++, lcd_cfg++) {
+                        unsigned long size = lcd_cfg->yres * lcd_cfg->xres;
+
+                        if (size > max_size) {
+                                max_cfg = lcd_cfg;
+                                max_size = size;
+                        }
+                }
+
+                if (!mode)
+                        max_size = DEFAULT_XRES * DEFAULT_YRES;
+                else if (max_cfg)
+                        dev_dbg(&pdev->dev, "Found largest videomode %ux%u\n",
+                                max_cfg->xres, max_cfg->yres);
+
+                info->fix = sh_mobile_lcdc_fix;
+                info->fix.smem_len = max_size * (cfg->bpp / 8) * 2;
+
+                if (!mode)
+                        mode = &default_720p;
+
+                fb_videomode_to_var(var, mode);
                 /* Default Y virtual resolution is 2x panel size */
                 var->yres_virtual = var->yres * 2;
-                var->width = cfg->lcd_size_cfg.width;
-                var->height = cfg->lcd_size_cfg.height;
                 var->activate = FB_ACTIVATE_NOW;
-                var->left_margin = lcd_cfg->left_margin;
-                var->right_margin = lcd_cfg->right_margin;
-                var->upper_margin = lcd_cfg->upper_margin;
-                var->lower_margin = lcd_cfg->lower_margin;
-                var->hsync_len = lcd_cfg->hsync_len;
-                var->vsync_len = lcd_cfg->vsync_len;
-                var->sync = lcd_cfg->sync;
-                var->pixclock = lcd_cfg->pixclock;
 
                 error = sh_mobile_lcdc_set_bpp(var, cfg->bpp);
                 if (error)
                         break;
 
-                info->fix = sh_mobile_lcdc_fix;
-                info->fix.line_length = lcd_cfg->xres * (cfg->bpp / 8);
-                info->fix.smem_len = info->fix.line_length *
-                        var->yres_virtual;
-
                 buf = dma_alloc_coherent(&pdev->dev, info->fix.smem_len,
-                                         &priv->ch[i].dma_handle, GFP_KERNEL);
+                                         &ch->dma_handle, GFP_KERNEL);
                 if (!buf) {
                         dev_err(&pdev->dev, "unable to allocate buffer\n");
                         error = -ENOMEM;
                         break;
                 }
 
-                info->pseudo_palette = &priv->ch[i].pseudo_palette;
+                info->pseudo_palette = &ch->pseudo_palette;
                 info->flags = FBINFO_FLAG_DEFAULT;
 
                 error = fb_alloc_cmap(&info->cmap, PALETTE_NR, 0);
                 if (error < 0) {
                         dev_err(&pdev->dev, "unable to allocate cmap\n");
                         dma_free_coherent(&pdev->dev, info->fix.smem_len,
-                                          buf, priv->ch[i].dma_handle);
+                                          buf, ch->dma_handle);
                         break;
                 }
 
-                memset(buf, 0, info->fix.smem_len);
-                info->fix.smem_start = priv->ch[i].dma_handle;
+                info->fix.smem_start = ch->dma_handle;
+                info->fix.line_length = var->xres * (cfg->bpp / 8);
                 info->screen_base = buf;
                 info->device = &pdev->dev;
-                info->par = &priv->ch[i];
+                ch->display_var = *var;
         }
 
         if (error)
@@ -1179,6 +1281,10 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
 
         for (i = 0; i < j; i++) {
                 struct sh_mobile_lcdc_chan *ch = priv->ch + i;
+                const struct fb_videomode *mode = ch->cfg.lcd_cfg;
+
+                if (!mode)
+                        mode = &default_720p;
 
                 info = ch->info;
 
@@ -1191,6 +1297,7 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                         }
                 }
 
+                fb_videomode_to_modelist(mode, ch->cfg.num_cfg, &info->modelist);
                 error = register_framebuffer(info);
                 if (error < 0)
                         goto err1;
@@ -1200,8 +1307,7 @@ static int __devinit sh_mobile_lcdc_probe(struct platform_device *pdev)
                          pdev->name,
                          (ch->cfg.chan == LCDC_CHAN_MAINLCD) ?
                          "mainlcd" : "sublcd",
-                         (int) ch->cfg.lcd_cfg.xres,
-                         (int) ch->cfg.lcd_cfg.yres,
+                         info->var.xres, info->var.yres,
                          ch->cfg.bpp);
 
                 /* deferred io mode: disable clock to save power */
diff --git a/drivers/video/sh_mobile_lcdcfb.h b/drivers/video/sh_mobile_lcdcfb.h
new file mode 100644
index 000000000000..9ecee2fba1d7
--- /dev/null
+++ b/drivers/video/sh_mobile_lcdcfb.h
@@ -0,0 +1,41 @@
+#ifndef SH_MOBILE_LCDCFB_H
+#define SH_MOBILE_LCDCFB_H
+
+#include <linux/completion.h>
+#include <linux/fb.h>
+#include <linux/mutex.h>
+#include <linux/wait.h>
+
+/* per-channel registers */
+enum { LDDCKPAT1R, LDDCKPAT2R, LDMT1R, LDMT2R, LDMT3R, LDDFR, LDSM1R,
+       LDSM2R, LDSA1R, LDMLSR, LDHCNR, LDHSYNR, LDVLNR, LDVSYNR, LDPMR,
+       LDHAJR,
+       NR_CH_REGS };
+
+#define PALETTE_NR 16
+
+struct sh_mobile_lcdc_priv;
+struct fb_info;
+
+struct sh_mobile_lcdc_chan {
+        struct sh_mobile_lcdc_priv *lcdc;
+        unsigned long *reg_offs;
+        unsigned long ldmt1r_value;
+        unsigned long enabled; /* ME and SE in LDCNT2R */
+        struct sh_mobile_lcdc_chan_cfg cfg;
+        u32 pseudo_palette[PALETTE_NR];
+        unsigned long saved_ch_regs[NR_CH_REGS];
+        struct fb_info *info;
+        dma_addr_t dma_handle;
+        struct fb_deferred_io defio;
+        struct scatterlist *sglist;
+        unsigned long frame_end;
+        unsigned long pan_offset;
+        wait_queue_head_t frame_end_wait;
+        struct completion vsync_completion;
+        struct fb_var_screeninfo display_var;
+        int use_count;
+        struct mutex open_lock;         /* protects the use counter */
+};
+
+#endif
diff --git a/include/video/sh_mobile_lcdc.h b/include/video/sh_mobile_lcdc.h
index 55d700e8566e..daabae5817c6 100644
--- a/include/video/sh_mobile_lcdc.h
+++ b/include/video/sh_mobile_lcdc.h
@@ -49,7 +49,9 @@ struct sh_mobile_lcdc_sys_bus_ops {
         unsigned long (*read_data)(void *handle);
 };
 
+struct module;
 struct sh_mobile_lcdc_board_cfg {
+        struct module *owner;
         void *board_data;
         int (*setup_sys)(void *board_data, void *sys_ops_handle,
                          struct sh_mobile_lcdc_sys_bus_ops *sys_ops);
@@ -70,7 +72,8 @@ struct sh_mobile_lcdc_chan_cfg {
         int interface_type; /* selects RGBn or SYSn I/F, see above */
         int clock_divider;
         unsigned long flags; /* LCDC_FLAGS_... */
-        struct fb_videomode lcd_cfg;
+        const struct fb_videomode *lcd_cfg;
+        int num_cfg;
         struct sh_mobile_lcdc_lcd_size_cfg lcd_size_cfg;
         struct sh_mobile_lcdc_board_cfg board_cfg;
         struct sh_mobile_lcdc_sys_bus_cfg sys_bus_cfg; /* only for SYSn I/F */