165 files changed, 11716 insertions, 2923 deletions

diff --git a/drivers/char/Kconfig b/drivers/char/Kconfig
index 04f8b2d083c6..ad59b4e0a9b5 100644
--- a/drivers/char/Kconfig
+++ b/drivers/char/Kconfig
@@ -608,5 +608,13 @@ config RAMOOPS
           This enables panic and oops messages to be logged to a circular
           buffer in RAM where it can be read back at some later point.
 
+config MSM_SMD_PKT
+        bool "Enable device interface for some SMD packet ports"
+        default n
+        depends on MSM_SMD
+        help
+          Enables userspace clients to read and write to some packet SMD
+          ports via device interface for MSM chipset.
+
 endmenu
 
diff --git a/drivers/char/Makefile b/drivers/char/Makefile
index 057f65452e7b..7a00672bd85d 100644
--- a/drivers/char/Makefile
+++ b/drivers/char/Makefile
@@ -9,6 +9,7 @@ obj-$(CONFIG_ATARI_DSP56K)	+= dsp56k.o
 obj-$(CONFIG_VIRTIO_CONSOLE)    += virtio_console.o
 obj-$(CONFIG_RAW_DRIVER)        += raw.o
 obj-$(CONFIG_SGI_SNSC)          += snsc.o snsc_event.o
+obj-$(CONFIG_MSM_SMD_PKT)       += msm_smd_pkt.o
 obj-$(CONFIG_MSPEC)             += mspec.o
 obj-$(CONFIG_MMTIMER)           += mmtimer.o
 obj-$(CONFIG_UV_MMTIMER)        += uv_mmtimer.o
diff --git a/drivers/char/ipmi/ipmi_si_intf.c b/drivers/char/ipmi/ipmi_si_intf.c
index c86d43b88e1e..d28b484aee45 100644
--- a/drivers/char/ipmi/ipmi_si_intf.c
+++ b/drivers/char/ipmi/ipmi_si_intf.c
@@ -3521,7 +3521,7 @@ static void cleanup_one_si(struct smi_info *to_clean)
         kfree(to_clean);
 }
 
-static void __exit cleanup_ipmi_si(void)
+static void cleanup_ipmi_si(void)
 {
         struct smi_info *e, *tmp_e;
 
diff --git a/drivers/char/mem.c b/drivers/char/mem.c
index 1256454b2d43..436a99017998 100644
--- a/drivers/char/mem.c
+++ b/drivers/char/mem.c
@@ -47,10 +47,7 @@ static inline unsigned long size_inside_page(unsigned long start,
 #ifndef ARCH_HAS_VALID_PHYS_ADDR_RANGE
 static inline int valid_phys_addr_range(unsigned long addr, size_t count)
 {
-        if (addr + count > __pa(high_memory))
-                return 0;
-
-        return 1;
+        return addr + count <= __pa(high_memory);
 }
 
 static inline int valid_mmap_phys_addr_range(unsigned long pfn, size_t size)
diff --git a/drivers/char/msm_smd_pkt.c b/drivers/char/msm_smd_pkt.c
new file mode 100644
index 000000000000..b6f8a65c9960
--- /dev/null
+++ b/drivers/char/msm_smd_pkt.c
@@ -0,0 +1,466 @@
+/* Copyright (c) 2008-2010, Code Aurora Forum. 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 and
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * 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.
+ *
+ */
+/*
+ * SMD Packet Driver -- Provides userspace interface to SMD packet ports.
+ */
+
+#include <linux/slab.h>
+#include <linux/cdev.h>
+#include <linux/module.h>
+#include <linux/fs.h>
+#include <linux/device.h>
+#include <linux/sched.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/uaccess.h>
+#include <linux/workqueue.h>
+#include <linux/poll.h>
+
+#include <mach/msm_smd.h>
+
+#define NUM_SMD_PKT_PORTS 9
+#define DEVICE_NAME "smdpkt"
+#define MAX_BUF_SIZE 2048
+
+struct smd_pkt_dev {
+        struct cdev cdev;
+        struct device *devicep;
+
+        struct smd_channel *ch;
+        int open_count;
+        struct mutex ch_lock;
+        struct mutex rx_lock;
+        struct mutex tx_lock;
+        wait_queue_head_t ch_read_wait_queue;
+        wait_queue_head_t ch_opened_wait_queue;
+
+        int i;
+
+        unsigned char tx_buf[MAX_BUF_SIZE];
+        unsigned char rx_buf[MAX_BUF_SIZE];
+        int remote_open;
+
+} *smd_pkt_devp[NUM_SMD_PKT_PORTS];
+
+struct class *smd_pkt_classp;
+static dev_t smd_pkt_number;
+
+static int msm_smd_pkt_debug_enable;
+module_param_named(debug_enable, msm_smd_pkt_debug_enable,
+                   int, S_IRUGO | S_IWUSR | S_IWGRP);
+
+#ifdef DEBUG
+#define D_DUMP_BUFFER(prestr, cnt, buf) do {                    \
+                int i;                                          \
+                if (msm_smd_pkt_debug_enable) {                 \
+                        pr_debug("%s", prestr);                 \
+                        for (i = 0; i < cnt; i++)               \
+                                pr_debug("%.2x", buf[i]);       \
+                        pr_debug("\n");                         \
+                }                                               \
+        } while (0)
+#else
+#define D_DUMP_BUFFER(prestr, cnt, buf) do {} while (0)
+#endif
+
+#ifdef DEBUG
+#define DBG(x...) do {                  \
+        if (msm_smd_pkt_debug_enable)   \
+                pr_debug(x);            \
+        } while (0)
+#else
+#define DBG(x...) do {} while (0)
+#endif
+
+static void check_and_wakeup_reader(struct smd_pkt_dev *smd_pkt_devp)
+{
+        int sz;
+
+        if (!smd_pkt_devp || !smd_pkt_devp->ch)
+                return;
+
+        sz = smd_cur_packet_size(smd_pkt_devp->ch);
+        if (sz == 0) {
+                DBG("no packet\n");
+                return;
+        }
+        if (sz > smd_read_avail(smd_pkt_devp->ch)) {
+                DBG("incomplete packet\n");
+                return;
+        }
+
+        DBG("waking up reader\n");
+        wake_up_interruptible(&smd_pkt_devp->ch_read_wait_queue);
+}
+
+static int smd_pkt_read(struct file *file, char __user *buf,
+                        size_t count, loff_t *ppos)
+{
+        int r, bytes_read;
+        struct smd_pkt_dev *smd_pkt_devp;
+        struct smd_channel *chl;
+
+        DBG("read %d bytes\n", count);
+        if (count > MAX_BUF_SIZE)
+                return -EINVAL;
+
+        smd_pkt_devp = file->private_data;
+        if (!smd_pkt_devp || !smd_pkt_devp->ch)
+                return -EINVAL;
+
+        chl = smd_pkt_devp->ch;
+wait_for_packet:
+        r = wait_event_interruptible(smd_pkt_devp->ch_read_wait_queue,
+                                     (smd_cur_packet_size(chl) > 0 &&
+                                      smd_read_avail(chl) >=
+                                      smd_cur_packet_size(chl)));
+
+        if (r < 0) {
+                if (r != -ERESTARTSYS)
+                        pr_err("wait returned %d\n", r);
+                return r;
+        }
+
+        mutex_lock(&smd_pkt_devp->rx_lock);
+
+        bytes_read = smd_cur_packet_size(smd_pkt_devp->ch);
+        if (bytes_read == 0 ||
+            bytes_read < smd_read_avail(smd_pkt_devp->ch)) {
+                mutex_unlock(&smd_pkt_devp->rx_lock);
+                DBG("Nothing to read\n");
+                goto wait_for_packet;
+        }
+
+        if (bytes_read > count) {
+                mutex_unlock(&smd_pkt_devp->rx_lock);
+                pr_info("packet size %d > buffer size %d", bytes_read, count);
+                return -EINVAL;
+        }
+
+        r = smd_read(smd_pkt_devp->ch, smd_pkt_devp->rx_buf, bytes_read);
+        if (r != bytes_read) {
+                mutex_unlock(&smd_pkt_devp->rx_lock);
+                pr_err("smd_read failed to read %d bytes: %d\n", bytes_read, r);
+                return -EIO;
+        }
+
+        D_DUMP_BUFFER("read: ", bytes_read, smd_pkt_devp->rx_buf);
+        r = copy_to_user(buf, smd_pkt_devp->rx_buf, bytes_read);
+        mutex_unlock(&smd_pkt_devp->rx_lock);
+        if (r) {
+                pr_err("copy_to_user failed %d\n", r);
+                return -EFAULT;
+        }
+
+        DBG("read complete %d bytes\n", bytes_read);
+        check_and_wakeup_reader(smd_pkt_devp);
+
+        return bytes_read;
+}
+
+static int smd_pkt_write(struct file *file, const char __user *buf,
+                         size_t count, loff_t *ppos)
+{
+        int r;
+        struct smd_pkt_dev *smd_pkt_devp;
+
+        if (count > MAX_BUF_SIZE)
+                return -EINVAL;
+
+        DBG("writting %d bytes\n", count);
+
+        smd_pkt_devp = file->private_data;
+        if (!smd_pkt_devp || !smd_pkt_devp->ch)
+                return -EINVAL;
+
+        mutex_lock(&smd_pkt_devp->tx_lock);
+        if (smd_write_avail(smd_pkt_devp->ch) < count) {
+                mutex_unlock(&smd_pkt_devp->tx_lock);
+                DBG("Not enough space to write\n");
+                return -ENOMEM;
+        }
+
+        D_DUMP_BUFFER("write: ", count, buf);
+        r = copy_from_user(smd_pkt_devp->tx_buf, buf, count);
+        if (r) {
+                mutex_unlock(&smd_pkt_devp->tx_lock);
+                pr_err("copy_from_user failed %d\n", r);
+                return -EFAULT;
+        }
+
+        r = smd_write(smd_pkt_devp->ch, smd_pkt_devp->tx_buf, count);
+        if (r != count) {
+                mutex_unlock(&smd_pkt_devp->tx_lock);
+                pr_err("smd_write failed to write %d bytes: %d.\n", count, r);
+                return -EIO;
+        }
+        mutex_unlock(&smd_pkt_devp->tx_lock);
+
+        DBG("wrote %d bytes\n", count);
+        return count;
+}
+
+static unsigned int smd_pkt_poll(struct file *file, poll_table *wait)
+{
+        struct smd_pkt_dev *smd_pkt_devp;
+        unsigned int mask = 0;
+
+        smd_pkt_devp = file->private_data;
+        if (!smd_pkt_devp)
+                return POLLERR;
+
+        DBG("poll waiting\n");
+        poll_wait(file, &smd_pkt_devp->ch_read_wait_queue, wait);
+        if (smd_read_avail(smd_pkt_devp->ch))
+                mask |= POLLIN | POLLRDNORM;
+
+        DBG("poll return\n");
+        return mask;
+}
+
+static void smd_pkt_ch_notify(void *priv, unsigned event)
+{
+        struct smd_pkt_dev *smd_pkt_devp = priv;
+
+        if (smd_pkt_devp->ch == 0)
+                return;
+
+        switch (event) {
+        case SMD_EVENT_DATA:
+                DBG("data\n");
+                check_and_wakeup_reader(smd_pkt_devp);
+                break;
+
+        case SMD_EVENT_OPEN:
+                DBG("remote open\n");
+                smd_pkt_devp->remote_open = 1;
+                wake_up_interruptible(&smd_pkt_devp->ch_opened_wait_queue);
+                break;
+
+        case SMD_EVENT_CLOSE:
+                smd_pkt_devp->remote_open = 0;
+                pr_info("remote closed\n");
+                break;
+
+        default:
+                pr_err("unknown event %d\n", event);
+                break;
+        }
+}
+
+static char *smd_pkt_dev_name[] = {
+        "smdcntl0",
+        "smdcntl1",
+        "smdcntl2",
+        "smdcntl3",
+        "smdcntl4",
+        "smdcntl5",
+        "smdcntl6",
+        "smdcntl7",
+        "smd22",
+};
+
+static char *smd_ch_name[] = {
+        "DATA5_CNTL",
+        "DATA6_CNTL",
+        "DATA7_CNTL",
+        "DATA8_CNTL",
+        "DATA9_CNTL",
+        "DATA12_CNTL",
+        "DATA13_CNTL",
+        "DATA14_CNTL",
+        "DATA22",
+};
+
+static int smd_pkt_open(struct inode *inode, struct file *file)
+{
+        int r = 0;
+        struct smd_pkt_dev *smd_pkt_devp;
+
+        smd_pkt_devp = container_of(inode->i_cdev, struct smd_pkt_dev, cdev);
+        if (!smd_pkt_devp)
+                return -EINVAL;
+
+        file->private_data = smd_pkt_devp;
+
+        mutex_lock(&smd_pkt_devp->ch_lock);
+        if (smd_pkt_devp->open_count == 0) {
+                r = smd_open(smd_ch_name[smd_pkt_devp->i],
+                             &smd_pkt_devp->ch, smd_pkt_devp,
+                             smd_pkt_ch_notify);
+                if (r < 0) {
+                        pr_err("smd_open failed for %s, %d\n",
+                               smd_ch_name[smd_pkt_devp->i], r);
+                        goto out;
+                }
+
+                r = wait_event_interruptible_timeout(
+                                smd_pkt_devp->ch_opened_wait_queue,
+                                smd_pkt_devp->remote_open,
+                                msecs_to_jiffies(2 * HZ));
+                if (r == 0)
+                        r = -ETIMEDOUT;
+
+                if (r < 0) {
+                        pr_err("wait returned %d\n", r);
+                        smd_close(smd_pkt_devp->ch);
+                        smd_pkt_devp->ch = 0;
+                } else {
+                        smd_pkt_devp->open_count++;
+                        r = 0;
+                }
+        }
+out:
+        mutex_unlock(&smd_pkt_devp->ch_lock);
+        return r;
+}
+
+static int smd_pkt_release(struct inode *inode, struct file *file)
+{
+        int r = 0;
+        struct smd_pkt_dev *smd_pkt_devp = file->private_data;
+
+        if (!smd_pkt_devp)
+                return -EINVAL;
+
+        mutex_lock(&smd_pkt_devp->ch_lock);
+        if (--smd_pkt_devp->open_count == 0) {
+                r = smd_close(smd_pkt_devp->ch);
+                smd_pkt_devp->ch = 0;
+        }
+        mutex_unlock(&smd_pkt_devp->ch_lock);
+
+        return r;
+}
+
+static const struct file_operations smd_pkt_fops = {
+        .owner = THIS_MODULE,
+        .open = smd_pkt_open,
+        .release = smd_pkt_release,
+        .read = smd_pkt_read,
+        .write = smd_pkt_write,
+        .poll = smd_pkt_poll,
+};
+
+static int __init smd_pkt_init(void)
+{
+        int i;
+        int r;
+
+        r = alloc_chrdev_region(&smd_pkt_number, 0,
+                                NUM_SMD_PKT_PORTS, DEVICE_NAME);
+        if (r) {
+                pr_err("alloc_chrdev_region() failed %d\n", r);
+                return r;
+        }
+
+        smd_pkt_classp = class_create(THIS_MODULE, DEVICE_NAME);
+        if (IS_ERR(smd_pkt_classp)) {
+                r = PTR_ERR(smd_pkt_classp);
+                pr_err("class_create() failed %d\n", r);
+                goto unreg_chardev;
+        }
+
+        for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
+                smd_pkt_devp[i] = kzalloc(sizeof(struct smd_pkt_dev),
+                                          GFP_KERNEL);
+                if (IS_ERR(smd_pkt_devp[i])) {
+                        r = PTR_ERR(smd_pkt_devp[i]);
+                        pr_err("kmalloc() failed %d\n", r);
+                        goto clean_cdevs;
+                }
+
+                smd_pkt_devp[i]->i = i;
+
+                init_waitqueue_head(&smd_pkt_devp[i]->ch_read_wait_queue);
+                smd_pkt_devp[i]->remote_open = 0;
+                init_waitqueue_head(&smd_pkt_devp[i]->ch_opened_wait_queue);
+
+                mutex_init(&smd_pkt_devp[i]->ch_lock);
+                mutex_init(&smd_pkt_devp[i]->rx_lock);
+                mutex_init(&smd_pkt_devp[i]->tx_lock);
+
+                cdev_init(&smd_pkt_devp[i]->cdev, &smd_pkt_fops);
+                smd_pkt_devp[i]->cdev.owner = THIS_MODULE;
+
+                r = cdev_add(&smd_pkt_devp[i]->cdev,
+                             (smd_pkt_number + i), 1);
+                if (r) {
+                        pr_err("cdev_add() failed %d\n", r);
+                        kfree(smd_pkt_devp[i]);
+                        goto clean_cdevs;
+                }
+
+                smd_pkt_devp[i]->devicep =
+                        device_create(smd_pkt_classp, NULL,
+                                      (smd_pkt_number + i), NULL,
+                                      smd_pkt_dev_name[i]);
+                if (IS_ERR(smd_pkt_devp[i]->devicep)) {
+                        r = PTR_ERR(smd_pkt_devp[i]->devicep);
+                        pr_err("device_create() failed %d\n", r);
+                        cdev_del(&smd_pkt_devp[i]->cdev);
+                        kfree(smd_pkt_devp[i]);
+                        goto clean_cdevs;
+                }
+
+        }
+
+        pr_info("SMD Packet Port Driver Initialized.\n");
+        return 0;
+
+clean_cdevs:
+        if (i > 0) {
+                while (--i >= 0) {
+                        mutex_destroy(&smd_pkt_devp[i]->ch_lock);
+                        mutex_destroy(&smd_pkt_devp[i]->rx_lock);
+                        mutex_destroy(&smd_pkt_devp[i]->tx_lock);
+                        cdev_del(&smd_pkt_devp[i]->cdev);
+                        kfree(smd_pkt_devp[i]);
+                        device_destroy(smd_pkt_classp,
+                                       MKDEV(MAJOR(smd_pkt_number), i));
+                }
+        }
+
+        class_destroy(smd_pkt_classp);
+unreg_chardev:
+        unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
+        return r;
+}
+module_init(smd_pkt_init);
+
+static void __exit smd_pkt_cleanup(void)
+{
+        int i;
+
+        for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
+                mutex_destroy(&smd_pkt_devp[i]->ch_lock);
+                mutex_destroy(&smd_pkt_devp[i]->rx_lock);
+                mutex_destroy(&smd_pkt_devp[i]->tx_lock);
+                cdev_del(&smd_pkt_devp[i]->cdev);
+                kfree(smd_pkt_devp[i]);
+                device_destroy(smd_pkt_classp,
+                               MKDEV(MAJOR(smd_pkt_number), i));
+        }
+
+        class_destroy(smd_pkt_classp);
+        unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
+}
+module_exit(smd_pkt_cleanup);
+
+MODULE_DESCRIPTION("MSM Shared Memory Packet Port");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/dma/timb_dma.c b/drivers/dma/timb_dma.c
index f69f90a61873..d2c75feff7df 100644
--- a/drivers/dma/timb_dma.c
+++ b/drivers/dma/timb_dma.c
@@ -27,6 +27,7 @@
 #include <linux/io.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 
 #include <linux/timb_dma.h>
@@ -684,7 +685,7 @@ static irqreturn_t td_irq(int irq, void *devid)
 
 static int __devinit td_probe(struct platform_device *pdev)
 {
-        struct timb_dma_platform_data *pdata = pdev->dev.platform_data;
+        struct timb_dma_platform_data *pdata = mfd_get_data(pdev);
         struct timb_dma *td;
         struct resource *iomem;
         int irq;
diff --git a/drivers/gpio/Kconfig b/drivers/gpio/Kconfig
index b46442d7d66e..d8d0cda2641d 100644
--- a/drivers/gpio/Kconfig
+++ b/drivers/gpio/Kconfig
@@ -100,18 +100,21 @@ config GPIO_VR41XX
           Say yes here to support the NEC VR4100 series General-purpose I/O Uint
 
 config GPIO_SCH
-        tristate "Intel SCH GPIO"
+        tristate "Intel SCH/TunnelCreek GPIO"
         depends on GPIOLIB && PCI && X86
         select MFD_CORE
         select LPC_SCH
         help
-          Say yes here to support GPIO interface on Intel Poulsbo SCH.
+          Say yes here to support GPIO interface on Intel Poulsbo SCH
+          or Intel Tunnel Creek processor.
           The Intel SCH contains a total of 14 GPIO pins. Ten GPIOs are
           powered by the core power rail and are turned off during sleep
           modes (S3 and higher). The remaining four GPIOs are powered by
           the Intel SCH suspend power supply. These GPIOs remain
           active during S3. The suspend powered GPIOs can be used to wake the
           system from the Suspend-to-RAM state.
+          The Intel Tunnel Creek processor has 5 GPIOs powered by the
+          core power rail and 9 from suspend power supply.
 
           This driver can also be built as a module. If so, the module
           will be called sch-gpio.
diff --git a/drivers/gpio/janz-ttl.c b/drivers/gpio/janz-ttl.c
index 813ac077e5d7..2514fb075f4a 100644
--- a/drivers/gpio/janz-ttl.c
+++ b/drivers/gpio/janz-ttl.c
@@ -15,6 +15,7 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/io.h>
 #include <linux/gpio.h>
 #include <linux/slab.h>
@@ -149,7 +150,7 @@ static int __devinit ttl_probe(struct platform_device *pdev)
         struct resource *res;
         int ret;
 
-        pdata = pdev->dev.platform_data;
+        pdata = mfd_get_data(pdev);
         if (!pdata) {
                 dev_err(dev, "no platform data\n");
                 ret = -ENXIO;
diff --git a/drivers/gpio/rdc321x-gpio.c b/drivers/gpio/rdc321x-gpio.c
index 897e0577e65e..a9bda881935a 100644
--- a/drivers/gpio/rdc321x-gpio.c
+++ b/drivers/gpio/rdc321x-gpio.c
@@ -27,6 +27,7 @@
 #include <linux/pci.h>
 #include <linux/gpio.h>
 #include <linux/mfd/rdc321x.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 
 struct rdc321x_gpio {
@@ -135,7 +136,7 @@ static int __devinit rdc321x_gpio_probe(struct platform_device *pdev)
         struct rdc321x_gpio *rdc321x_gpio_dev;
         struct rdc321x_gpio_pdata *pdata;
 
-        pdata = platform_get_drvdata(pdev);
+        pdata = mfd_get_data(pdev);
         if (!pdata) {
                 dev_err(&pdev->dev, "no platform data supplied\n");
                 return -ENODEV;
diff --git a/drivers/gpio/sch_gpio.c b/drivers/gpio/sch_gpio.c
index 583521352c16..56060421cdff 100644
--- a/drivers/gpio/sch_gpio.c
+++ b/drivers/gpio/sch_gpio.c
@@ -25,6 +25,7 @@
 #include <linux/errno.h>
 #include <linux/acpi.h>
 #include <linux/platform_device.h>
+#include <linux/pci_ids.h>
 
 #include <linux/gpio.h>
 
@@ -187,7 +188,11 @@ static struct gpio_chip sch_gpio_resume = {
 static int __devinit sch_gpio_probe(struct platform_device *pdev)
 {
         struct resource *res;
-        int err;
+        int err, id;
+
+        id = pdev->id;
+        if (!id)
+                return -ENODEV;
 
         res = platform_get_resource(pdev, IORESOURCE_IO, 0);
         if (!res)
@@ -198,12 +203,40 @@ static int __devinit sch_gpio_probe(struct platform_device *pdev)
 
         gpio_ba = res->start;
 
-        sch_gpio_core.base = 0;
-        sch_gpio_core.ngpio = 10;
-        sch_gpio_core.dev = &pdev->dev;
+        switch (id) {
+                case PCI_DEVICE_ID_INTEL_SCH_LPC:
+                        sch_gpio_core.base = 0;
+                        sch_gpio_core.ngpio = 10;
+
+                        sch_gpio_resume.base = 10;
+                        sch_gpio_resume.ngpio = 4;
+
+                        /*
+                         * GPIO[6:0] enabled by default
+                         * GPIO7 is configured by the CMC as SLPIOVR
+                         * Enable GPIO[9:8] core powered gpios explicitly
+                         */
+                        outb(0x3, gpio_ba + CGEN + 1);
+                        /*
+                         * SUS_GPIO[2:0] enabled by default
+                         * Enable SUS_GPIO3 resume powered gpio explicitly
+                         */
+                        outb(0x8, gpio_ba + RGEN);
+                        break;
+
+                case PCI_DEVICE_ID_INTEL_ITC_LPC:
+                        sch_gpio_core.base = 0;
+                        sch_gpio_core.ngpio = 5;
+
+                        sch_gpio_resume.base = 5;
+                        sch_gpio_resume.ngpio = 9;
+                        break;
+
+                default:
+                        return -ENODEV;
+        }
 
-        sch_gpio_resume.base = 10;
-        sch_gpio_resume.ngpio = 4;
+        sch_gpio_core.dev = &pdev->dev;
         sch_gpio_resume.dev = &pdev->dev;
 
         err = gpiochip_add(&sch_gpio_core);
@@ -214,18 +247,6 @@ static int __devinit sch_gpio_probe(struct platform_device *pdev)
         if (err < 0)
                 goto err_sch_gpio_resume;
 
-        /*
-         * GPIO[6:0] enabled by default
-         * GPIO7 is configured by the CMC as SLPIOVR
-         * Enable GPIO[9:8] core powered gpios explicitly
-         */
-        outb(0x3, gpio_ba + CGEN + 1);
-        /*
-         * SUS_GPIO[2:0] enabled by default
-         * Enable SUS_GPIO3 resume powered gpio explicitly
-         */
-        outb(0x8, gpio_ba + RGEN);
-
         return 0;
 
 err_sch_gpio_resume:
diff --git a/drivers/gpio/timbgpio.c b/drivers/gpio/timbgpio.c
index 58c8f30352dd..ffcd815b8b8b 100644
--- a/drivers/gpio/timbgpio.c
+++ b/drivers/gpio/timbgpio.c
@@ -23,6 +23,7 @@
 #include <linux/module.h>
 #include <linux/gpio.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/irq.h>
 #include <linux/io.h>
 #include <linux/timb_gpio.h>
@@ -228,7 +229,7 @@ static int __devinit timbgpio_probe(struct platform_device *pdev)
         struct gpio_chip *gc;
         struct timbgpio *tgpio;
         struct resource *iomem;
-        struct timbgpio_platform_data *pdata = pdev->dev.platform_data;
+        struct timbgpio_platform_data *pdata = mfd_get_data(pdev);
         int irq = platform_get_irq(pdev, 0);
 
         if (!pdata || pdata->nr_pins > 32) {
@@ -319,14 +320,13 @@ err_mem:
 static int __devexit timbgpio_remove(struct platform_device *pdev)
 {
         int err;
-        struct timbgpio_platform_data *pdata = pdev->dev.platform_data;
         struct timbgpio *tgpio = platform_get_drvdata(pdev);
         struct resource *iomem = platform_get_resource(pdev, IORESOURCE_MEM, 0);
         int irq = platform_get_irq(pdev, 0);
 
         if (irq >= 0 && tgpio->irq_base > 0) {
                 int i;
-                for (i = 0; i < pdata->nr_pins; i++) {
+                for (i = 0; i < tgpio->gpio.ngpio; i++) {
                         set_irq_chip(tgpio->irq_base + i, NULL);
                         set_irq_chip_data(tgpio->irq_base + i, NULL);
                 }
diff --git a/drivers/gpu/drm/i915/i915_drv.h b/drivers/gpu/drm/i915/i915_drv.h
index 449650545bb4..5004724ea57e 100644
--- a/drivers/gpu/drm/i915/i915_drv.h
+++ b/drivers/gpu/drm/i915/i915_drv.h
@@ -383,6 +383,7 @@ typedef struct drm_i915_private {
         u32 saveDSPACNTR;
         u32 saveDSPBCNTR;
         u32 saveDSPARB;
+        u32 saveHWS;
         u32 savePIPEACONF;
         u32 savePIPEBCONF;
         u32 savePIPEASRC;
diff --git a/drivers/gpu/drm/i915/i915_suspend.c b/drivers/gpu/drm/i915/i915_suspend.c
index 7e992a8e9098..da474153a0a2 100644
--- a/drivers/gpu/drm/i915/i915_suspend.c
+++ b/drivers/gpu/drm/i915/i915_suspend.c
@@ -796,6 +796,9 @@ int i915_save_state(struct drm_device *dev)
 
         pci_read_config_byte(dev->pdev, LBB, &dev_priv->saveLBB);
 
+        /* Hardware status page */
+        dev_priv->saveHWS = I915_READ(HWS_PGA);
+
         i915_save_display(dev);
 
         /* Interrupt state */
@@ -842,6 +845,9 @@ int i915_restore_state(struct drm_device *dev)
 
         pci_write_config_byte(dev->pdev, LBB, dev_priv->saveLBB);
 
+        /* Hardware status page */
+        I915_WRITE(HWS_PGA, dev_priv->saveHWS);
+
         i915_restore_display(dev);
 
         /* Interrupt state */
diff --git a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
index 66c9af1b3d96..41a5d48e657b 100644
--- a/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
+++ b/drivers/gpu/drm/radeon/radeon_legacy_crtc.c
@@ -889,7 +889,7 @@ static void radeon_set_pll(struct drm_crtc *crtc, struct drm_display_mode *mode)
                 }
 
                 if (rdev->flags & RADEON_IS_MOBILITY) {
-                        /* A temporal workaround for the occational blanking on certain laptop panels.
+                        /* A temporal workaround for the occasional blanking on certain laptop panels.
                            This appears to related to the PLL divider registers (fail to lock?).
                            It occurs even when all dividers are the same with their old settings.
                            In this case we really don't need to fiddle with PLL registers.
diff --git a/drivers/hwmon/Kconfig b/drivers/hwmon/Kconfig
index e4bd13b3cd8b..81131eda5544 100644
--- a/drivers/hwmon/Kconfig
+++ b/drivers/hwmon/Kconfig
@@ -1047,6 +1047,16 @@ config SENSORS_TMP421
           This driver can also be built as a module.  If so, the module
           will be called tmp421.
 
+config SENSORS_TWL4030_MADC
+        tristate "Texas Instruments TWL4030 MADC Hwmon"
+        depends on TWL4030_MADC
+        help
+        If you say yes here you get hwmon support for triton
+        TWL4030-MADC.
+
+        This driver can also be built as a module. If so it will be called
+        twl4030-madc-hwmon.
+
 config SENSORS_VIA_CPUTEMP
         tristate "VIA CPU temperature sensor"
         depends on X86
diff --git a/drivers/hwmon/Makefile b/drivers/hwmon/Makefile
index 54ca5939d028..967d0ea9447f 100644
--- a/drivers/hwmon/Makefile
+++ b/drivers/hwmon/Makefile
@@ -104,6 +104,7 @@ obj-$(CONFIG_SENSORS_THMC50)	+= thmc50.o
 obj-$(CONFIG_SENSORS_TMP102)    += tmp102.o
 obj-$(CONFIG_SENSORS_TMP401)    += tmp401.o
 obj-$(CONFIG_SENSORS_TMP421)    += tmp421.o
+obj-$(CONFIG_SENSORS_TWL4030_MADC)+= twl4030-madc-hwmon.o
 obj-$(CONFIG_SENSORS_VIA_CPUTEMP)+= via-cputemp.o
 obj-$(CONFIG_SENSORS_VIA686A)   += via686a.o
 obj-$(CONFIG_SENSORS_VT1211)    += vt1211.o
diff --git a/drivers/hwmon/jz4740-hwmon.c b/drivers/hwmon/jz4740-hwmon.c
index 1c8b3d9e2051..fea292d43407 100644
--- a/drivers/hwmon/jz4740-hwmon.c
+++ b/drivers/hwmon/jz4740-hwmon.c
@@ -32,7 +32,7 @@ struct jz4740_hwmon {
 
         int irq;
 
-        struct mfd_cell *cell;
+        const struct mfd_cell *cell;
         struct device *hwmon;
 
         struct completion read_completion;
@@ -112,7 +112,7 @@ static int __devinit jz4740_hwmon_probe(struct platform_device *pdev)
                 return -ENOMEM;
         }
 
-        hwmon->cell = pdev->dev.platform_data;
+        hwmon->cell = mfd_get_cell(pdev);
 
         hwmon->irq = platform_get_irq(pdev, 0);
         if (hwmon->irq < 0) {
diff --git a/drivers/hwmon/twl4030-madc-hwmon.c b/drivers/hwmon/twl4030-madc-hwmon.c
new file mode 100644
index 000000000000..97e22bef85ab
--- /dev/null
+++ b/drivers/hwmon/twl4030-madc-hwmon.c
@@ -0,0 +1,157 @@
+/*
+ *
+ * TWL4030 MADC Hwmon driver-This driver monitors the real time
+ * conversion of analog signals like battery temperature,
+ * battery type, battery level etc. User can ask for the conversion on a
+ * particular channel using the sysfs nodes.
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ * J Keerthy <j-keerthy@ti.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.
+ *
+ * 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 St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/i2c/twl.h>
+#include <linux/device.h>
+#include <linux/platform_device.h>
+#include <linux/i2c/twl4030-madc.h>
+#include <linux/hwmon.h>
+#include <linux/hwmon-sysfs.h>
+#include <linux/stddef.h>
+#include <linux/sysfs.h>
+#include <linux/err.h>
+#include <linux/types.h>
+
+/*
+ * sysfs hook function
+ */
+static ssize_t madc_read(struct device *dev,
+                         struct device_attribute *devattr, char *buf)
+{
+        struct sensor_device_attribute *attr = to_sensor_dev_attr(devattr);
+        struct twl4030_madc_request req;
+        long val;
+
+        req.channels = (1 << attr->index);
+        req.method = TWL4030_MADC_SW2;
+        req.func_cb = NULL;
+        val = twl4030_madc_conversion(&req);
+        if (val < 0)
+                return val;
+
+        return sprintf(buf, "%d\n", req.rbuf[attr->index]);
+}
+
+/* sysfs nodes to read individual channels from user side */
+static SENSOR_DEVICE_ATTR(in0_input, S_IRUGO, madc_read, NULL, 0);
+static SENSOR_DEVICE_ATTR(temp1_input, S_IRUGO, madc_read, NULL, 1);
+static SENSOR_DEVICE_ATTR(in2_input, S_IRUGO, madc_read, NULL, 2);
+static SENSOR_DEVICE_ATTR(in3_input, S_IRUGO, madc_read, NULL, 3);
+static SENSOR_DEVICE_ATTR(in4_input, S_IRUGO, madc_read, NULL, 4);
+static SENSOR_DEVICE_ATTR(in5_input, S_IRUGO, madc_read, NULL, 5);
+static SENSOR_DEVICE_ATTR(in6_input, S_IRUGO, madc_read, NULL, 6);
+static SENSOR_DEVICE_ATTR(in7_input, S_IRUGO, madc_read, NULL, 7);
+static SENSOR_DEVICE_ATTR(in8_input, S_IRUGO, madc_read, NULL, 8);
+static SENSOR_DEVICE_ATTR(in9_input, S_IRUGO, madc_read, NULL, 9);
+static SENSOR_DEVICE_ATTR(curr10_input, S_IRUGO, madc_read, NULL, 10);
+static SENSOR_DEVICE_ATTR(in11_input, S_IRUGO, madc_read, NULL, 11);
+static SENSOR_DEVICE_ATTR(in12_input, S_IRUGO, madc_read, NULL, 12);
+static SENSOR_DEVICE_ATTR(in15_input, S_IRUGO, madc_read, NULL, 15);
+
+static struct attribute *twl4030_madc_attributes[] = {
+        &sensor_dev_attr_in0_input.dev_attr.attr,
+        &sensor_dev_attr_temp1_input.dev_attr.attr,
+        &sensor_dev_attr_in2_input.dev_attr.attr,
+        &sensor_dev_attr_in3_input.dev_attr.attr,
+        &sensor_dev_attr_in4_input.dev_attr.attr,
+        &sensor_dev_attr_in5_input.dev_attr.attr,
+        &sensor_dev_attr_in6_input.dev_attr.attr,
+        &sensor_dev_attr_in7_input.dev_attr.attr,
+        &sensor_dev_attr_in8_input.dev_attr.attr,
+        &sensor_dev_attr_in9_input.dev_attr.attr,
+        &sensor_dev_attr_curr10_input.dev_attr.attr,
+        &sensor_dev_attr_in11_input.dev_attr.attr,
+        &sensor_dev_attr_in12_input.dev_attr.attr,
+        &sensor_dev_attr_in15_input.dev_attr.attr,
+        NULL
+};
+
+static const struct attribute_group twl4030_madc_group = {
+        .attrs = twl4030_madc_attributes,
+};
+
+static int __devinit twl4030_madc_hwmon_probe(struct platform_device *pdev)
+{
+        int ret;
+        int status;
+        struct device *hwmon;
+
+        ret = sysfs_create_group(&pdev->dev.kobj, &twl4030_madc_group);
+        if (ret)
+                goto err_sysfs;
+        hwmon = hwmon_device_register(&pdev->dev);
+        if (IS_ERR(hwmon)) {
+                dev_err(&pdev->dev, "hwmon_device_register failed.\n");
+                status = PTR_ERR(hwmon);
+                goto err_reg;
+        }
+
+        return 0;
+
+err_reg:
+        sysfs_remove_group(&pdev->dev.kobj, &twl4030_madc_group);
+err_sysfs:
+
+        return ret;
+}
+
+static int __devexit twl4030_madc_hwmon_remove(struct platform_device *pdev)
+{
+        hwmon_device_unregister(&pdev->dev);
+        sysfs_remove_group(&pdev->dev.kobj, &twl4030_madc_group);
+
+        return 0;
+}
+
+static struct platform_driver twl4030_madc_hwmon_driver = {
+        .probe = twl4030_madc_hwmon_probe,
+        .remove = __exit_p(twl4030_madc_hwmon_remove),
+        .driver = {
+                   .name = "twl4030_madc_hwmon",
+                   .owner = THIS_MODULE,
+                   },
+};
+
+static int __init twl4030_madc_hwmon_init(void)
+{
+        return platform_driver_register(&twl4030_madc_hwmon_driver);
+}
+
+module_init(twl4030_madc_hwmon_init);
+
+static void __exit twl4030_madc_hwmon_exit(void)
+{
+        platform_driver_unregister(&twl4030_madc_hwmon_driver);
+}
+
+module_exit(twl4030_madc_hwmon_exit);
+
+MODULE_DESCRIPTION("TWL4030 ADC Hwmon driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("J Keerthy");
+MODULE_ALIAS("twl4030_madc_hwmon");
diff --git a/drivers/i2c/busses/i2c-ocores.c b/drivers/i2c/busses/i2c-ocores.c
index 1b46a9d9f907..fee1a2613861 100644
--- a/drivers/i2c/busses/i2c-ocores.c
+++ b/drivers/i2c/busses/i2c-ocores.c
@@ -49,6 +49,7 @@
 #include <linux/init.h>
 #include <linux/errno.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/wait.h>
@@ -305,7 +306,7 @@ static int __devinit ocores_i2c_probe(struct platform_device *pdev)
                 return -EIO;
         }
 
-        pdata = pdev->dev.platform_data;
+        pdata = mfd_get_data(pdev);
         if (pdata) {
                 i2c->regstep = pdata->regstep;
                 i2c->clock_khz = pdata->clock_khz;
diff --git a/drivers/i2c/busses/i2c-xiic.c b/drivers/i2c/busses/i2c-xiic.c
index a9c419e075a5..9fbd7e6fe32e 100644
--- a/drivers/i2c/busses/i2c-xiic.c
+++ b/drivers/i2c/busses/i2c-xiic.c
@@ -34,6 +34,7 @@
 #include <linux/errno.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/i2c.h>
 #include <linux/interrupt.h>
 #include <linux/wait.h>
@@ -704,7 +705,7 @@ static int __devinit xiic_i2c_probe(struct platform_device *pdev)
         if (irq < 0)
                 goto resource_missing;
 
-        pdata = (struct xiic_i2c_platform_data *) pdev->dev.platform_data;
+        pdata = mfd_get_data(pdev);
         if (!pdata)
                 return -EINVAL;
 
diff --git a/drivers/infiniband/core/addr.c b/drivers/infiniband/core/addr.c
index e0ef5fdc361e..4ffc224faa7f 100644
--- a/drivers/infiniband/core/addr.c
+++ b/drivers/infiniband/core/addr.c
@@ -204,7 +204,7 @@ static int addr4_resolve(struct sockaddr_in *src_in,
 
         /* If the device does ARP internally, return 'done' */
         if (rt->dst.dev->flags & IFF_NOARP) {
-                rdma_copy_addr(addr, rt->dst.dev, NULL);
+                ret = rdma_copy_addr(addr, rt->dst.dev, NULL);
                 goto put;
         }
 
diff --git a/drivers/infiniband/core/agent.c b/drivers/infiniband/core/agent.c
index 91916a8d5de4..2bc7f5af64f4 100644
--- a/drivers/infiniband/core/agent.c
+++ b/drivers/infiniband/core/agent.c
@@ -101,7 +101,8 @@ void agent_send_response(struct ib_mad *mad, struct ib_grh *grh,
         agent = port_priv->agent[qpn];
         ah = ib_create_ah_from_wc(agent->qp->pd, wc, grh, port_num);
         if (IS_ERR(ah)) {
-                printk(KERN_ERR SPFX "ib_create_ah_from_wc error\n");
+                printk(KERN_ERR SPFX "ib_create_ah_from_wc error %ld\n",
+                        PTR_ERR(ah));
                 return;
         }
 
diff --git a/drivers/infiniband/hw/mthca/mthca_main.c b/drivers/infiniband/hw/mthca/mthca_main.c
index 8a40cd539ab1..f24b79b805f2 100644
--- a/drivers/infiniband/hw/mthca/mthca_main.c
+++ b/drivers/infiniband/hw/mthca/mthca_main.c
@@ -1043,6 +1043,9 @@ static int __mthca_init_one(struct pci_dev *pdev, int hca_type)
                 }
         }
 
+        /* We can handle large RDMA requests, so allow larger segments. */
+        dma_set_max_seg_size(&pdev->dev, 1024 * 1024 * 1024);
+
         mdev = (struct mthca_dev *) ib_alloc_device(sizeof *mdev);
         if (!mdev) {
                 dev_err(&pdev->dev, "Device struct alloc failed, "
diff --git a/drivers/infiniband/hw/nes/nes.c b/drivers/infiniband/hw/nes/nes.c
index 3d7f3664b67b..13de1192927c 100644
--- a/drivers/infiniband/hw/nes/nes.c
+++ b/drivers/infiniband/hw/nes/nes.c
@@ -694,7 +694,7 @@ static int __devinit nes_probe(struct pci_dev *pcidev, const struct pci_device_i
         nesdev->netdev_count++;
         nesdev->nesadapter->netdev_count++;
 
-        printk(KERN_ERR PFX "%s: NetEffect RNIC driver successfully loaded.\n",
+        printk(KERN_INFO PFX "%s: NetEffect RNIC driver successfully loaded.\n",
                         pci_name(pcidev));
         return 0;
 
diff --git a/drivers/infiniband/ulp/srp/ib_srp.c b/drivers/infiniband/ulp/srp/ib_srp.c
index 83664ed2804f..376d640487d2 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.c
+++ b/drivers/infiniband/ulp/srp/ib_srp.c
@@ -59,25 +59,31 @@ MODULE_DESCRIPTION("InfiniBand SCSI RDMA Protocol initiator "
                    "v" DRV_VERSION " (" DRV_RELDATE ")");
 MODULE_LICENSE("Dual BSD/GPL");
 
-static int srp_sg_tablesize = SRP_DEF_SG_TABLESIZE;
-static int srp_max_iu_len;
+static unsigned int srp_sg_tablesize;
+static unsigned int cmd_sg_entries;
+static unsigned int indirect_sg_entries;
+static bool allow_ext_sg;
+static int topspin_workarounds = 1;
 
-module_param(srp_sg_tablesize, int, 0444);
-MODULE_PARM_DESC(srp_sg_tablesize,
-                 "Max number of gather/scatter entries per I/O (default is 12, max 255)");
+module_param(srp_sg_tablesize, uint, 0444);
+MODULE_PARM_DESC(srp_sg_tablesize, "Deprecated name for cmd_sg_entries");
 
-static int topspin_workarounds = 1;
+module_param(cmd_sg_entries, uint, 0444);
+MODULE_PARM_DESC(cmd_sg_entries,
+                 "Default number of gather/scatter entries in the SRP command (default is 12, max 255)");
+
+module_param(indirect_sg_entries, uint, 0444);
+MODULE_PARM_DESC(indirect_sg_entries,
+                 "Default max number of gather/scatter entries (default is 12, max is " __stringify(SCSI_MAX_SG_CHAIN_SEGMENTS) ")");
+
+module_param(allow_ext_sg, bool, 0444);
+MODULE_PARM_DESC(allow_ext_sg,
+                  "Default behavior when there are more than cmd_sg_entries S/G entries after mapping; fails the request when false (default false)");
 
 module_param(topspin_workarounds, int, 0444);
 MODULE_PARM_DESC(topspin_workarounds,
                  "Enable workarounds for Topspin/Cisco SRP target bugs if != 0");
 
-static int mellanox_workarounds = 1;
-
-module_param(mellanox_workarounds, int, 0444);
-MODULE_PARM_DESC(mellanox_workarounds,
-                 "Enable workarounds for Mellanox SRP target bugs if != 0");
-
 static void srp_add_one(struct ib_device *device);
 static void srp_remove_one(struct ib_device *device);
 static void srp_recv_completion(struct ib_cq *cq, void *target_ptr);
@@ -114,14 +120,6 @@ static int srp_target_is_topspin(struct srp_target_port *target)
                  !memcmp(&target->ioc_guid, cisco_oui, sizeof cisco_oui));
 }
 
-static int srp_target_is_mellanox(struct srp_target_port *target)
-{
-        static const u8 mellanox_oui[3] = { 0x00, 0x02, 0xc9 };
-
-        return mellanox_workarounds &&
-                !memcmp(&target->ioc_guid, mellanox_oui, sizeof mellanox_oui);
-}
-
 static struct srp_iu *srp_alloc_iu(struct srp_host *host, size_t size,
                                    gfp_t gfp_mask,
                                    enum dma_data_direction direction)
@@ -378,7 +376,7 @@ static int srp_send_req(struct srp_target_port *target)
 
         req->priv.opcode        = SRP_LOGIN_REQ;
         req->priv.tag           = 0;
-        req->priv.req_it_iu_len = cpu_to_be32(srp_max_iu_len);
+        req->priv.req_it_iu_len = cpu_to_be32(target->max_iu_len);
         req->priv.req_buf_fmt   = cpu_to_be16(SRP_BUF_FORMAT_DIRECT |
                                               SRP_BUF_FORMAT_INDIRECT);
         /*
@@ -456,6 +454,24 @@ static bool srp_change_state(struct srp_target_port *target,
         return changed;
 }
 
+static void srp_free_req_data(struct srp_target_port *target)
+{
+        struct ib_device *ibdev = target->srp_host->srp_dev->dev;
+        struct srp_request *req;
+        int i;
+
+        for (i = 0, req = target->req_ring; i < SRP_CMD_SQ_SIZE; ++i, ++req) {
+                kfree(req->fmr_list);
+                kfree(req->map_page);
+                if (req->indirect_dma_addr) {
+                        ib_dma_unmap_single(ibdev, req->indirect_dma_addr,
+                                            target->indirect_size,
+                                            DMA_TO_DEVICE);
+                }
+                kfree(req->indirect_desc);
+        }
+}
+
 static void srp_remove_work(struct work_struct *work)
 {
         struct srp_target_port *target =
@@ -472,6 +488,7 @@ static void srp_remove_work(struct work_struct *work)
         scsi_remove_host(target->scsi_host);
         ib_destroy_cm_id(target->cm_id);
         srp_free_target_ib(target);
+        srp_free_req_data(target);
         scsi_host_put(target->scsi_host);
 }
 
@@ -535,18 +552,20 @@ static void srp_unmap_data(struct scsi_cmnd *scmnd,
                            struct srp_target_port *target,
                            struct srp_request *req)
 {
+        struct ib_device *ibdev = target->srp_host->srp_dev->dev;
+        struct ib_pool_fmr **pfmr;
+
         if (!scsi_sglist(scmnd) ||
             (scmnd->sc_data_direction != DMA_TO_DEVICE &&
              scmnd->sc_data_direction != DMA_FROM_DEVICE))
                 return;
 
-        if (req->fmr) {
-                ib_fmr_pool_unmap(req->fmr);
-                req->fmr = NULL;
-        }
+        pfmr = req->fmr_list;
+        while (req->nfmr--)
+                ib_fmr_pool_unmap(*pfmr++);
 
-        ib_dma_unmap_sg(target->srp_host->srp_dev->dev, scsi_sglist(scmnd),
-                        scsi_sg_count(scmnd), scmnd->sc_data_direction);
+        ib_dma_unmap_sg(ibdev, scsi_sglist(scmnd), scsi_sg_count(scmnd),
+                        scmnd->sc_data_direction);
 }
 
 static void srp_remove_req(struct srp_target_port *target,
@@ -645,96 +664,151 @@ err:
         return ret;
 }
 
-static int srp_map_fmr(struct srp_target_port *target, struct scatterlist *scat,
-                       int sg_cnt, struct srp_request *req,
-                       struct srp_direct_buf *buf)
+static void srp_map_desc(struct srp_map_state *state, dma_addr_t dma_addr,
+                         unsigned int dma_len, u32 rkey)
 {
-        u64 io_addr = 0;
-        u64 *dma_pages;
-        u32 len;
-        int page_cnt;
-        int i, j;
-        int ret;
-        struct srp_device *dev = target->srp_host->srp_dev;
-        struct ib_device *ibdev = dev->dev;
-        struct scatterlist *sg;
+        struct srp_direct_buf *desc = state->desc;
 
-        if (!dev->fmr_pool)
-                return -ENODEV;
+        desc->va = cpu_to_be64(dma_addr);
+        desc->key = cpu_to_be32(rkey);
+        desc->len = cpu_to_be32(dma_len);
 
-        if (srp_target_is_mellanox(target) &&
-            (ib_sg_dma_address(ibdev, &scat[0]) & ~dev->fmr_page_mask))
-                return -EINVAL;
+        state->total_len += dma_len;
+        state->desc++;
+        state->ndesc++;
+}
 
-        len = page_cnt = 0;
-        scsi_for_each_sg(req->scmnd, sg, sg_cnt, i) {
-                unsigned int dma_len = ib_sg_dma_len(ibdev, sg);
+static int srp_map_finish_fmr(struct srp_map_state *state,
+                              struct srp_target_port *target)
+{
+        struct srp_device *dev = target->srp_host->srp_dev;
+        struct ib_pool_fmr *fmr;
+        u64 io_addr = 0;
 
-                if (ib_sg_dma_address(ibdev, sg) & ~dev->fmr_page_mask) {
-                        if (i > 0)
-                                return -EINVAL;
-                        else
-                                ++page_cnt;
-                }
-                if ((ib_sg_dma_address(ibdev, sg) + dma_len) &
-                    ~dev->fmr_page_mask) {
-                        if (i < sg_cnt - 1)
-                                return -EINVAL;
-                        else
-                                ++page_cnt;
-                }
+        if (!state->npages)
+                return 0;
 
-                len += dma_len;
+        if (state->npages == 1) {
+                srp_map_desc(state, state->base_dma_addr, state->fmr_len,
+                             target->rkey);
+                state->npages = state->fmr_len = 0;
+                return 0;
         }
 
-        page_cnt += len >> dev->fmr_page_shift;
-        if (page_cnt > SRP_FMR_SIZE)
-                return -ENOMEM;
+        fmr = ib_fmr_pool_map_phys(dev->fmr_pool, state->pages,
+                                   state->npages, io_addr);
+        if (IS_ERR(fmr))
+                return PTR_ERR(fmr);
 
-        dma_pages = kmalloc(sizeof (u64) * page_cnt, GFP_ATOMIC);
-        if (!dma_pages)
-                return -ENOMEM;
+        *state->next_fmr++ = fmr;
+        state->nfmr++;
 
-        page_cnt = 0;
-        scsi_for_each_sg(req->scmnd, sg, sg_cnt, i) {
-                unsigned int dma_len = ib_sg_dma_len(ibdev, sg);
+        srp_map_desc(state, 0, state->fmr_len, fmr->fmr->rkey);
+        state->npages = state->fmr_len = 0;
+        return 0;
+}
 
-                for (j = 0; j < dma_len; j += dev->fmr_page_size)
-                        dma_pages[page_cnt++] =
-                                (ib_sg_dma_address(ibdev, sg) &
-                                 dev->fmr_page_mask) + j;
+static void srp_map_update_start(struct srp_map_state *state,
+                                 struct scatterlist *sg, int sg_index,
+                                 dma_addr_t dma_addr)
+{
+        state->unmapped_sg = sg;
+        state->unmapped_index = sg_index;
+        state->unmapped_addr = dma_addr;
+}
+
+static int srp_map_sg_entry(struct srp_map_state *state,
+                            struct srp_target_port *target,
+                            struct scatterlist *sg, int sg_index,
+                            int use_fmr)
+{
+        struct srp_device *dev = target->srp_host->srp_dev;
+        struct ib_device *ibdev = dev->dev;
+        dma_addr_t dma_addr = ib_sg_dma_address(ibdev, sg);
+        unsigned int dma_len = ib_sg_dma_len(ibdev, sg);
+        unsigned int len;
+        int ret;
+
+        if (!dma_len)
+                return 0;
+
+        if (use_fmr == SRP_MAP_NO_FMR) {
+                /* Once we're in direct map mode for a request, we don't
+                 * go back to FMR mode, so no need to update anything
+                 * other than the descriptor.
+                 */
+                srp_map_desc(state, dma_addr, dma_len, target->rkey);
+                return 0;
         }
 
-        req->fmr = ib_fmr_pool_map_phys(dev->fmr_pool,
-                                        dma_pages, page_cnt, io_addr);
-        if (IS_ERR(req->fmr)) {
-                ret = PTR_ERR(req->fmr);
-                req->fmr = NULL;
-                goto out;
+        /* If we start at an offset into the FMR page, don't merge into
+         * the current FMR. Finish it out, and use the kernel's MR for this
+         * sg entry. This is to avoid potential bugs on some SRP targets
+         * that were never quite defined, but went away when the initiator
+         * avoided using FMR on such page fragments.
+         */
+        if (dma_addr & ~dev->fmr_page_mask || dma_len > dev->fmr_max_size) {
+                ret = srp_map_finish_fmr(state, target);
+                if (ret)
+                        return ret;
+
+                srp_map_desc(state, dma_addr, dma_len, target->rkey);
+                srp_map_update_start(state, NULL, 0, 0);
+                return 0;
         }
 
-        buf->va  = cpu_to_be64(ib_sg_dma_address(ibdev, &scat[0]) &
-                               ~dev->fmr_page_mask);
-        buf->key = cpu_to_be32(req->fmr->fmr->rkey);
-        buf->len = cpu_to_be32(len);
+        /* If this is the first sg to go into the FMR, save our position.
+         * We need to know the first unmapped entry, its index, and the
+         * first unmapped address within that entry to be able to restart
+         * mapping after an error.
+         */
+        if (!state->unmapped_sg)
+                srp_map_update_start(state, sg, sg_index, dma_addr);
 
-        ret = 0;
+        while (dma_len) {
+                if (state->npages == SRP_FMR_SIZE) {
+                        ret = srp_map_finish_fmr(state, target);
+                        if (ret)
+                                return ret;
 
-out:
-        kfree(dma_pages);
+                        srp_map_update_start(state, sg, sg_index, dma_addr);
+                }
+
+                len = min_t(unsigned int, dma_len, dev->fmr_page_size);
 
+                if (!state->npages)
+                        state->base_dma_addr = dma_addr;
+                state->pages[state->npages++] = dma_addr;
+                state->fmr_len += len;
+                dma_addr += len;
+                dma_len -= len;
+        }
+
+        /* If the last entry of the FMR wasn't a full page, then we need to
+         * close it out and start a new one -- we can only merge at page
+         * boundries.
+         */
+        ret = 0;
+        if (len != dev->fmr_page_size) {
+                ret = srp_map_finish_fmr(state, target);
+                if (!ret)
+                        srp_map_update_start(state, NULL, 0, 0);
+        }
         return ret;
 }
 
 static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_target_port *target,
                         struct srp_request *req)
 {
-        struct scatterlist *scat;
+        struct scatterlist *scat, *sg;
         struct srp_cmd *cmd = req->cmd->buf;
-        int len, nents, count;
-        u8 fmt = SRP_DATA_DESC_DIRECT;
+        int i, len, nents, count, use_fmr;
         struct srp_device *dev;
         struct ib_device *ibdev;
+        struct srp_map_state state;
+        struct srp_indirect_buf *indirect_hdr;
+        u32 table_len;
+        u8 fmt;
 
         if (!scsi_sglist(scmnd) || scmnd->sc_data_direction == DMA_NONE)
                 return sizeof (struct srp_cmd);
@@ -754,6 +828,8 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_target_port *target,
         ibdev = dev->dev;
 
         count = ib_dma_map_sg(ibdev, scat, nents, scmnd->sc_data_direction);
+        if (unlikely(count == 0))
+                return -EIO;
 
         fmt = SRP_DATA_DESC_DIRECT;
         len = sizeof (struct srp_cmd) + sizeof (struct srp_direct_buf);
@@ -770,49 +846,99 @@ static int srp_map_data(struct scsi_cmnd *scmnd, struct srp_target_port *target,
                 buf->va  = cpu_to_be64(ib_sg_dma_address(ibdev, scat));
                 buf->key = cpu_to_be32(target->rkey);
                 buf->len = cpu_to_be32(ib_sg_dma_len(ibdev, scat));
-        } else if (srp_map_fmr(target, scat, count, req,
-                               (void *) cmd->add_data)) {
-                /*
-                 * FMR mapping failed, and the scatterlist has more
-                 * than one entry.  Generate an indirect memory
-                 * descriptor.
-                 */
-                struct srp_indirect_buf *buf = (void *) cmd->add_data;
-                struct scatterlist *sg;
-                u32 datalen = 0;
-                int i;
-
-                fmt = SRP_DATA_DESC_INDIRECT;
-                len = sizeof (struct srp_cmd) +
-                        sizeof (struct srp_indirect_buf) +
-                        count * sizeof (struct srp_direct_buf);
-
-                scsi_for_each_sg(scmnd, sg, count, i) {
-                        unsigned int dma_len = ib_sg_dma_len(ibdev, sg);
-
-                        buf->desc_list[i].va  =
-                                cpu_to_be64(ib_sg_dma_address(ibdev, sg));
-                        buf->desc_list[i].key =
-                                cpu_to_be32(target->rkey);
-                        buf->desc_list[i].len = cpu_to_be32(dma_len);
-                        datalen += dma_len;
+
+                req->nfmr = 0;
+                goto map_complete;
+        }
+
+        /* We have more than one scatter/gather entry, so build our indirect
+         * descriptor table, trying to merge as many entries with FMR as we
+         * can.
+         */
+        indirect_hdr = (void *) cmd->add_data;
+
+        ib_dma_sync_single_for_cpu(ibdev, req->indirect_dma_addr,
+                                   target->indirect_size, DMA_TO_DEVICE);
+
+        memset(&state, 0, sizeof(state));
+        state.desc      = req->indirect_desc;
+        state.pages     = req->map_page;
+        state.next_fmr  = req->fmr_list;
+
+        use_fmr = dev->fmr_pool ? SRP_MAP_ALLOW_FMR : SRP_MAP_NO_FMR;
+
+        for_each_sg(scat, sg, count, i) {
+                if (srp_map_sg_entry(&state, target, sg, i, use_fmr)) {
+                        /* FMR mapping failed, so backtrack to the first
+                         * unmapped entry and continue on without using FMR.
+                         */
+                        dma_addr_t dma_addr;
+                        unsigned int dma_len;
+
+backtrack:
+                        sg = state.unmapped_sg;
+                        i = state.unmapped_index;
+
+                        dma_addr = ib_sg_dma_address(ibdev, sg);
+                        dma_len = ib_sg_dma_len(ibdev, sg);
+                        dma_len -= (state.unmapped_addr - dma_addr);
+                        dma_addr = state.unmapped_addr;
+                        use_fmr = SRP_MAP_NO_FMR;
+                        srp_map_desc(&state, dma_addr, dma_len, target->rkey);
                 }
+        }
 
-                if (scmnd->sc_data_direction == DMA_TO_DEVICE)
-                        cmd->data_out_desc_cnt = count;
-                else
-                        cmd->data_in_desc_cnt = count;
+        if (use_fmr == SRP_MAP_ALLOW_FMR && srp_map_finish_fmr(&state, target))
+                goto backtrack;
 
-                buf->table_desc.va  =
-                        cpu_to_be64(req->cmd->dma + sizeof *cmd + sizeof *buf);
-                buf->table_desc.key =
-                        cpu_to_be32(target->rkey);
-                buf->table_desc.len =
-                        cpu_to_be32(count * sizeof (struct srp_direct_buf));
+        /* We've mapped the request, now pull as much of the indirect
+         * descriptor table as we can into the command buffer. If this
+         * target is not using an external indirect table, we are
+         * guaranteed to fit into the command, as the SCSI layer won't
+         * give us more S/G entries than we allow.
+         */
+        req->nfmr = state.nfmr;
+        if (state.ndesc == 1) {
+                /* FMR mapping was able to collapse this to one entry,
+                 * so use a direct descriptor.
+                 */
+                struct srp_direct_buf *buf = (void *) cmd->add_data;
 
-                buf->len = cpu_to_be32(datalen);
+                *buf = req->indirect_desc[0];
+                goto map_complete;
+        }
+
+        if (unlikely(target->cmd_sg_cnt < state.ndesc &&
+                                                !target->allow_ext_sg)) {
+                shost_printk(KERN_ERR, target->scsi_host,
+                             "Could not fit S/G list into SRP_CMD\n");
+                return -EIO;
         }
 
+        count = min(state.ndesc, target->cmd_sg_cnt);
+        table_len = state.ndesc * sizeof (struct srp_direct_buf);
+
+        fmt = SRP_DATA_DESC_INDIRECT;
+        len = sizeof(struct srp_cmd) + sizeof (struct srp_indirect_buf);
+        len += count * sizeof (struct srp_direct_buf);
+
+        memcpy(indirect_hdr->desc_list, req->indirect_desc,
+               count * sizeof (struct srp_direct_buf));
+
+        indirect_hdr->table_desc.va = cpu_to_be64(req->indirect_dma_addr);
+        indirect_hdr->table_desc.key = cpu_to_be32(target->rkey);
+        indirect_hdr->table_desc.len = cpu_to_be32(table_len);
+        indirect_hdr->len = cpu_to_be32(state.total_len);
+
+        if (scmnd->sc_data_direction == DMA_TO_DEVICE)
+                cmd->data_out_desc_cnt = count;
+        else
+                cmd->data_in_desc_cnt = count;
+
+        ib_dma_sync_single_for_device(ibdev, req->indirect_dma_addr, table_len,
+                                      DMA_TO_DEVICE);
+
+map_complete:
         if (scmnd->sc_data_direction == DMA_TO_DEVICE)
                 cmd->buf_fmt = fmt << 4;
         else
@@ -1140,7 +1266,7 @@ static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
         spin_unlock_irqrestore(&target->lock, flags);
 
         dev = target->srp_host->srp_dev->dev;
-        ib_dma_sync_single_for_cpu(dev, iu->dma, srp_max_iu_len,
+        ib_dma_sync_single_for_cpu(dev, iu->dma, target->max_iu_len,
                                    DMA_TO_DEVICE);
 
         scmnd->result        = 0;
@@ -1164,7 +1290,7 @@ static int srp_queuecommand(struct Scsi_Host *shost, struct scsi_cmnd *scmnd)
                 goto err_iu;
         }
 
-        ib_dma_sync_single_for_device(dev, iu->dma, srp_max_iu_len,
+        ib_dma_sync_single_for_device(dev, iu->dma, target->max_iu_len,
                                       DMA_TO_DEVICE);
 
         if (srp_post_send(target, iu, len)) {
@@ -1204,7 +1330,7 @@ static int srp_alloc_iu_bufs(struct srp_target_port *target)
 
         for (i = 0; i < SRP_SQ_SIZE; ++i) {
                 target->tx_ring[i] = srp_alloc_iu(target->srp_host,
-                                                  srp_max_iu_len,
+                                                  target->max_iu_len,
                                                   GFP_KERNEL, DMA_TO_DEVICE);
                 if (!target->tx_ring[i])
                         goto err;
@@ -1228,6 +1354,78 @@ err:
         return -ENOMEM;
 }
 
+static void srp_cm_rep_handler(struct ib_cm_id *cm_id,
+                               struct srp_login_rsp *lrsp,
+                               struct srp_target_port *target)
+{
+        struct ib_qp_attr *qp_attr = NULL;
+        int attr_mask = 0;
+        int ret;
+        int i;
+
+        if (lrsp->opcode == SRP_LOGIN_RSP) {
+                target->max_ti_iu_len = be32_to_cpu(lrsp->max_ti_iu_len);
+                target->req_lim       = be32_to_cpu(lrsp->req_lim_delta);
+
+                /*
+                 * Reserve credits for task management so we don't
+                 * bounce requests back to the SCSI mid-layer.
+                 */
+                target->scsi_host->can_queue
+                        = min(target->req_lim - SRP_TSK_MGMT_SQ_SIZE,
+                              target->scsi_host->can_queue);
+        } else {
+                shost_printk(KERN_WARNING, target->scsi_host,
+                             PFX "Unhandled RSP opcode %#x\n", lrsp->opcode);
+                ret = -ECONNRESET;
+                goto error;
+        }
+
+        if (!target->rx_ring[0]) {
+                ret = srp_alloc_iu_bufs(target);
+                if (ret)
+                        goto error;
+        }
+
+        ret = -ENOMEM;
+        qp_attr = kmalloc(sizeof *qp_attr, GFP_KERNEL);
+        if (!qp_attr)
+                goto error;
+
+        qp_attr->qp_state = IB_QPS_RTR;
+        ret = ib_cm_init_qp_attr(cm_id, qp_attr, &attr_mask);
+        if (ret)
+                goto error_free;
+
+        ret = ib_modify_qp(target->qp, qp_attr, attr_mask);
+        if (ret)
+                goto error_free;
+
+        for (i = 0; i < SRP_RQ_SIZE; i++) {
+                struct srp_iu *iu = target->rx_ring[i];
+                ret = srp_post_recv(target, iu);
+                if (ret)
+                        goto error_free;
+        }
+
+        qp_attr->qp_state = IB_QPS_RTS;
+        ret = ib_cm_init_qp_attr(cm_id, qp_attr, &attr_mask);
+        if (ret)
+                goto error_free;
+
+        ret = ib_modify_qp(target->qp, qp_attr, attr_mask);
+        if (ret)
+                goto error_free;
+
+        ret = ib_send_cm_rtu(cm_id, NULL, 0);
+
+error_free:
+        kfree(qp_attr);
+
+error:
+        target->status = ret;
+}
+
 static void srp_cm_rej_handler(struct ib_cm_id *cm_id,
                                struct ib_cm_event *event,
                                struct srp_target_port *target)
@@ -1311,11 +1509,7 @@ static void srp_cm_rej_handler(struct ib_cm_id *cm_id,
 static int srp_cm_handler(struct ib_cm_id *cm_id, struct ib_cm_event *event)
 {
         struct srp_target_port *target = cm_id->context;
-        struct ib_qp_attr *qp_attr = NULL;
-        int attr_mask = 0;
         int comp = 0;
-        int opcode = 0;
-        int i;
 
         switch (event->event) {
         case IB_CM_REQ_ERROR:
@@ -1327,71 +1521,7 @@ static int srp_cm_handler(struct ib_cm_id *cm_id, struct ib_cm_event *event)
 
         case IB_CM_REP_RECEIVED:
                 comp = 1;
-                opcode = *(u8 *) event->private_data;
-
-                if (opcode == SRP_LOGIN_RSP) {
-                        struct srp_login_rsp *rsp = event->private_data;
-
-                        target->max_ti_iu_len = be32_to_cpu(rsp->max_ti_iu_len);
-                        target->req_lim       = be32_to_cpu(rsp->req_lim_delta);
-
-                        /*
-                         * Reserve credits for task management so we don't
-                         * bounce requests back to the SCSI mid-layer.
-                         */
-                        target->scsi_host->can_queue
-                                = min(target->req_lim - SRP_TSK_MGMT_SQ_SIZE,
-                                      target->scsi_host->can_queue);
-                } else {
-                        shost_printk(KERN_WARNING, target->scsi_host,
-                                    PFX "Unhandled RSP opcode %#x\n", opcode);
-                        target->status = -ECONNRESET;
-                        break;
-                }
-
-                if (!target->rx_ring[0]) {
-                        target->status = srp_alloc_iu_bufs(target);
-                        if (target->status)
-                                break;
-                }
-
-                qp_attr = kmalloc(sizeof *qp_attr, GFP_KERNEL);
-                if (!qp_attr) {
-                        target->status = -ENOMEM;
-                        break;
-                }
-
-                qp_attr->qp_state = IB_QPS_RTR;
-                target->status = ib_cm_init_qp_attr(cm_id, qp_attr, &attr_mask);
-                if (target->status)
-                        break;
-
-                target->status = ib_modify_qp(target->qp, qp_attr, attr_mask);
-                if (target->status)
-                        break;
-
-                for (i = 0; i < SRP_RQ_SIZE; i++) {
-                        struct srp_iu *iu = target->rx_ring[i];
-                        target->status = srp_post_recv(target, iu);
-                        if (target->status)
-                                break;
-                }
-                if (target->status)
-                        break;
-
-                qp_attr->qp_state = IB_QPS_RTS;
-                target->status = ib_cm_init_qp_attr(cm_id, qp_attr, &attr_mask);
-                if (target->status)
-                        break;
-
-                target->status = ib_modify_qp(target->qp, qp_attr, attr_mask);
-                if (target->status)
-                        break;
-
-                target->status = ib_send_cm_rtu(cm_id, NULL, 0);
-                if (target->status)
-                        break;
-
+                srp_cm_rep_handler(cm_id, event->private_data, target);
                 break;
 
         case IB_CM_REJ_RECEIVED:
@@ -1431,8 +1561,6 @@ static int srp_cm_handler(struct ib_cm_id *cm_id, struct ib_cm_event *event)
         if (comp)
                 complete(&target->done);
 
-        kfree(qp_attr);
-
         return 0;
 }
 
@@ -1658,6 +1786,22 @@ static ssize_t show_local_ib_device(struct device *dev,
         return sprintf(buf, "%s\n", target->srp_host->srp_dev->dev->name);
 }
 
+static ssize_t show_cmd_sg_entries(struct device *dev,
+                                   struct device_attribute *attr, char *buf)
+{
+        struct srp_target_port *target = host_to_target(class_to_shost(dev));
+
+        return sprintf(buf, "%u\n", target->cmd_sg_cnt);
+}
+
+static ssize_t show_allow_ext_sg(struct device *dev,
+                                 struct device_attribute *attr, char *buf)
+{
+        struct srp_target_port *target = host_to_target(class_to_shost(dev));
+
+        return sprintf(buf, "%s\n", target->allow_ext_sg ? "true" : "false");
+}
+
 static DEVICE_ATTR(id_ext,          S_IRUGO, show_id_ext,          NULL);
 static DEVICE_ATTR(ioc_guid,        S_IRUGO, show_ioc_guid,        NULL);
 static DEVICE_ATTR(service_id,      S_IRUGO, show_service_id,      NULL);
@@ -1668,6 +1812,8 @@ static DEVICE_ATTR(req_lim,         S_IRUGO, show_req_lim,         NULL);
 static DEVICE_ATTR(zero_req_lim,    S_IRUGO, show_zero_req_lim,    NULL);
 static DEVICE_ATTR(local_ib_port,   S_IRUGO, show_local_ib_port,   NULL);
 static DEVICE_ATTR(local_ib_device, S_IRUGO, show_local_ib_device, NULL);
+static DEVICE_ATTR(cmd_sg_entries,  S_IRUGO, show_cmd_sg_entries,  NULL);
+static DEVICE_ATTR(allow_ext_sg,    S_IRUGO, show_allow_ext_sg,    NULL);
 
 static struct device_attribute *srp_host_attrs[] = {
         &dev_attr_id_ext,
@@ -1680,6 +1826,8 @@ static struct device_attribute *srp_host_attrs[] = {
         &dev_attr_zero_req_lim,
         &dev_attr_local_ib_port,
         &dev_attr_local_ib_device,
+        &dev_attr_cmd_sg_entries,
+        &dev_attr_allow_ext_sg,
         NULL
 };
 
@@ -1692,6 +1840,7 @@ static struct scsi_host_template srp_template = {
         .eh_abort_handler               = srp_abort,
         .eh_device_reset_handler        = srp_reset_device,
         .eh_host_reset_handler          = srp_reset_host,
+        .sg_tablesize                   = SRP_DEF_SG_TABLESIZE,
         .can_queue                      = SRP_CMD_SQ_SIZE,
         .this_id                        = -1,
         .cmd_per_lun                    = SRP_CMD_SQ_SIZE,
@@ -1763,6 +1912,9 @@ enum {
         SRP_OPT_MAX_CMD_PER_LUN = 1 << 6,
         SRP_OPT_IO_CLASS        = 1 << 7,
         SRP_OPT_INITIATOR_EXT   = 1 << 8,
+        SRP_OPT_CMD_SG_ENTRIES  = 1 << 9,
+        SRP_OPT_ALLOW_EXT_SG    = 1 << 10,
+        SRP_OPT_SG_TABLESIZE    = 1 << 11,
         SRP_OPT_ALL             = (SRP_OPT_ID_EXT       |
                                    SRP_OPT_IOC_GUID     |
                                    SRP_OPT_DGID         |
@@ -1780,6 +1932,9 @@ static const match_table_t srp_opt_tokens = {
         { SRP_OPT_MAX_CMD_PER_LUN,      "max_cmd_per_lun=%d"    },
         { SRP_OPT_IO_CLASS,             "io_class=%x"           },
         { SRP_OPT_INITIATOR_EXT,        "initiator_ext=%s"      },
+        { SRP_OPT_CMD_SG_ENTRIES,       "cmd_sg_entries=%u"     },
+        { SRP_OPT_ALLOW_EXT_SG,         "allow_ext_sg=%u"       },
+        { SRP_OPT_SG_TABLESIZE,         "sg_tablesize=%u"       },
         { SRP_OPT_ERR,                  NULL                    }
 };
 
@@ -1907,6 +2062,31 @@ static int srp_parse_options(const char *buf, struct srp_target_port *target)
                         kfree(p);
                         break;
 
+                case SRP_OPT_CMD_SG_ENTRIES:
+                        if (match_int(args, &token) || token < 1 || token > 255) {
+                                printk(KERN_WARNING PFX "bad max cmd_sg_entries parameter '%s'\n", p);
+                                goto out;
+                        }
+                        target->cmd_sg_cnt = token;
+                        break;
+
+                case SRP_OPT_ALLOW_EXT_SG:
+                        if (match_int(args, &token)) {
+                                printk(KERN_WARNING PFX "bad allow_ext_sg parameter '%s'\n", p);
+                                goto out;
+                        }
+                        target->allow_ext_sg = !!token;
+                        break;
+
+                case SRP_OPT_SG_TABLESIZE:
+                        if (match_int(args, &token) || token < 1 ||
+                                        token > SCSI_MAX_SG_CHAIN_SEGMENTS) {
+                                printk(KERN_WARNING PFX "bad max sg_tablesize parameter '%s'\n", p);
+                                goto out;
+                        }
+                        target->sg_tablesize = token;
+                        break;
+
                 default:
                         printk(KERN_WARNING PFX "unknown parameter or missing value "
                                "'%s' in target creation request\n", p);
@@ -1937,39 +2117,73 @@ static ssize_t srp_create_target(struct device *dev,
                 container_of(dev, struct srp_host, dev);
         struct Scsi_Host *target_host;
         struct srp_target_port *target;
-        int ret;
-        int i;
+        struct ib_device *ibdev = host->srp_dev->dev;
+        dma_addr_t dma_addr;
+        int i, ret;
 
         target_host = scsi_host_alloc(&srp_template,
                                       sizeof (struct srp_target_port));
         if (!target_host)
                 return -ENOMEM;
 
-        target_host->transportt = ib_srp_transport_template;
+        target_host->transportt  = ib_srp_transport_template;
         target_host->max_lun     = SRP_MAX_LUN;
         target_host->max_cmd_len = sizeof ((struct srp_cmd *) (void *) 0L)->cdb;
 
         target = host_to_target(target_host);
 
-        target->io_class   = SRP_REV16A_IB_IO_CLASS;
-        target->scsi_host  = target_host;
-        target->srp_host   = host;
-        target->lkey       = host->srp_dev->mr->lkey;
-        target->rkey       = host->srp_dev->mr->rkey;
+        target->io_class        = SRP_REV16A_IB_IO_CLASS;
+        target->scsi_host       = target_host;
+        target->srp_host        = host;
+        target->lkey            = host->srp_dev->mr->lkey;
+        target->rkey            = host->srp_dev->mr->rkey;
+        target->cmd_sg_cnt      = cmd_sg_entries;
+        target->sg_tablesize    = indirect_sg_entries ? : cmd_sg_entries;
+        target->allow_ext_sg    = allow_ext_sg;
+
+        ret = srp_parse_options(buf, target);
+        if (ret)
+                goto err;
+
+        if (!host->srp_dev->fmr_pool && !target->allow_ext_sg &&
+                                target->cmd_sg_cnt < target->sg_tablesize) {
+                printk(KERN_WARNING PFX "No FMR pool and no external indirect descriptors, limiting sg_tablesize to cmd_sg_cnt\n");
+                target->sg_tablesize = target->cmd_sg_cnt;
+        }
+
+        target_host->sg_tablesize = target->sg_tablesize;
+        target->indirect_size = target->sg_tablesize *
+                                sizeof (struct srp_direct_buf);
+        target->max_iu_len = sizeof (struct srp_cmd) +
+                             sizeof (struct srp_indirect_buf) +
+                             target->cmd_sg_cnt * sizeof (struct srp_direct_buf);
 
         spin_lock_init(&target->lock);
         INIT_LIST_HEAD(&target->free_tx);
         INIT_LIST_HEAD(&target->free_reqs);
         for (i = 0; i < SRP_CMD_SQ_SIZE; ++i) {
-                target->req_ring[i].index = i;
-                list_add_tail(&target->req_ring[i].list, &target->free_reqs);
-        }
+                struct srp_request *req = &target->req_ring[i];
 
-        ret = srp_parse_options(buf, target);
-        if (ret)
-                goto err;
+                req->fmr_list = kmalloc(target->cmd_sg_cnt * sizeof (void *),
+                                        GFP_KERNEL);
+                req->map_page = kmalloc(SRP_FMR_SIZE * sizeof (void *),
+                                        GFP_KERNEL);
+                req->indirect_desc = kmalloc(target->indirect_size, GFP_KERNEL);
+                if (!req->fmr_list || !req->map_page || !req->indirect_desc)
+                        goto err_free_mem;
+
+                dma_addr = ib_dma_map_single(ibdev, req->indirect_desc,
+                                             target->indirect_size,
+                                             DMA_TO_DEVICE);
+                if (ib_dma_mapping_error(ibdev, dma_addr))
+                        goto err_free_mem;
+
+                req->indirect_dma_addr = dma_addr;
+                req->index = i;
+                list_add_tail(&req->list, &target->free_reqs);
+        }
 
-        ib_query_gid(host->srp_dev->dev, host->port, 0, &target->path.sgid);
+        ib_query_gid(ibdev, host->port, 0, &target->path.sgid);
 
         shost_printk(KERN_DEBUG, target->scsi_host, PFX
                      "new target: id_ext %016llx ioc_guid %016llx pkey %04x "
@@ -1982,11 +2196,11 @@ static ssize_t srp_create_target(struct device *dev,
 
         ret = srp_create_target_ib(target);
         if (ret)
-                goto err;
+                goto err_free_mem;
 
         ret = srp_new_cm_id(target);
         if (ret)
-                goto err_free;
+                goto err_free_ib;
 
         target->qp_in_error = 0;
         ret = srp_connect_target(target);
@@ -2008,9 +2222,12 @@ err_disconnect:
 err_cm_id:
         ib_destroy_cm_id(target->cm_id);
 
-err_free:
+err_free_ib:
         srp_free_target_ib(target);
 
+err_free_mem:
+        srp_free_req_data(target);
+
 err:
         scsi_host_put(target_host);
 
@@ -2083,7 +2300,7 @@ static void srp_add_one(struct ib_device *device)
         struct ib_device_attr *dev_attr;
         struct ib_fmr_pool_param fmr_param;
         struct srp_host *host;
-        int s, e, p;
+        int max_pages_per_fmr, fmr_page_shift, s, e, p;
 
         dev_attr = kmalloc(sizeof *dev_attr, GFP_KERNEL);
         if (!dev_attr)
@@ -2101,12 +2318,13 @@ static void srp_add_one(struct ib_device *device)
 
         /*
          * Use the smallest page size supported by the HCA, down to a
-         * minimum of 512 bytes (which is the smallest sector that a
-         * SCSI command will ever carry).
+         * minimum of 4096 bytes. We're unlikely to build large sglists
+         * out of smaller entries.
          */
-        srp_dev->fmr_page_shift = max(9, ffs(dev_attr->page_size_cap) - 1);
-        srp_dev->fmr_page_size  = 1 << srp_dev->fmr_page_shift;
-        srp_dev->fmr_page_mask  = ~((u64) srp_dev->fmr_page_size - 1);
+        fmr_page_shift          = max(12, ffs(dev_attr->page_size_cap) - 1);
+        srp_dev->fmr_page_size  = 1 << fmr_page_shift;
+        srp_dev->fmr_page_mask  = ~((u64) srp_dev->fmr_page_size - 1);
+        srp_dev->fmr_max_size   = srp_dev->fmr_page_size * SRP_FMR_SIZE;
 
         INIT_LIST_HEAD(&srp_dev->dev_list);
 
@@ -2122,17 +2340,24 @@ static void srp_add_one(struct ib_device *device)
         if (IS_ERR(srp_dev->mr))
                 goto err_pd;
 
-        memset(&fmr_param, 0, sizeof fmr_param);
-        fmr_param.pool_size         = SRP_FMR_POOL_SIZE;
-        fmr_param.dirty_watermark   = SRP_FMR_DIRTY_SIZE;
-        fmr_param.cache             = 1;
-        fmr_param.max_pages_per_fmr = SRP_FMR_SIZE;
-        fmr_param.page_shift        = srp_dev->fmr_page_shift;
-        fmr_param.access            = (IB_ACCESS_LOCAL_WRITE |
-                                       IB_ACCESS_REMOTE_WRITE |
-                                       IB_ACCESS_REMOTE_READ);
-
-        srp_dev->fmr_pool = ib_create_fmr_pool(srp_dev->pd, &fmr_param);
+        for (max_pages_per_fmr = SRP_FMR_SIZE;
+                        max_pages_per_fmr >= SRP_FMR_MIN_SIZE;
+                        max_pages_per_fmr /= 2, srp_dev->fmr_max_size /= 2) {
+                memset(&fmr_param, 0, sizeof fmr_param);
+                fmr_param.pool_size         = SRP_FMR_POOL_SIZE;
+                fmr_param.dirty_watermark   = SRP_FMR_DIRTY_SIZE;
+                fmr_param.cache             = 1;
+                fmr_param.max_pages_per_fmr = max_pages_per_fmr;
+                fmr_param.page_shift        = fmr_page_shift;
+                fmr_param.access            = (IB_ACCESS_LOCAL_WRITE |
+                                               IB_ACCESS_REMOTE_WRITE |
+                                               IB_ACCESS_REMOTE_READ);
+
+                srp_dev->fmr_pool = ib_create_fmr_pool(srp_dev->pd, &fmr_param);
+                if (!IS_ERR(srp_dev->fmr_pool))
+                        break;
+        }
+
         if (IS_ERR(srp_dev->fmr_pool))
                 srp_dev->fmr_pool = NULL;
 
@@ -2207,6 +2432,7 @@ static void srp_remove_one(struct ib_device *device)
                         srp_disconnect_target(target);
                         ib_destroy_cm_id(target->cm_id);
                         srp_free_target_ib(target);
+                        srp_free_req_data(target);
                         scsi_host_put(target->scsi_host);
                 }
 
@@ -2230,9 +2456,25 @@ static int __init srp_init_module(void)
 
         BUILD_BUG_ON(FIELD_SIZEOF(struct ib_wc, wr_id) < sizeof(void *));
 
-        if (srp_sg_tablesize > 255) {
-                printk(KERN_WARNING PFX "Clamping srp_sg_tablesize to 255\n");
-                srp_sg_tablesize = 255;
+        if (srp_sg_tablesize) {
+                printk(KERN_WARNING PFX "srp_sg_tablesize is deprecated, please use cmd_sg_entries\n");
+                if (!cmd_sg_entries)
+                        cmd_sg_entries = srp_sg_tablesize;
+        }
+
+        if (!cmd_sg_entries)
+                cmd_sg_entries = SRP_DEF_SG_TABLESIZE;
+
+        if (cmd_sg_entries > 255) {
+                printk(KERN_WARNING PFX "Clamping cmd_sg_entries to 255\n");
+                cmd_sg_entries = 255;
+        }
+
+        if (!indirect_sg_entries)
+                indirect_sg_entries = cmd_sg_entries;
+        else if (indirect_sg_entries < cmd_sg_entries) {
+                printk(KERN_WARNING PFX "Bumping up indirect_sg_entries to match cmd_sg_entries (%u)\n", cmd_sg_entries);
+                indirect_sg_entries = cmd_sg_entries;
         }
 
         ib_srp_transport_template =
@@ -2240,11 +2482,6 @@ static int __init srp_init_module(void)
         if (!ib_srp_transport_template)
                 return -ENOMEM;
 
-        srp_template.sg_tablesize = srp_sg_tablesize;
-        srp_max_iu_len = (sizeof (struct srp_cmd) +
-                          sizeof (struct srp_indirect_buf) +
-                          srp_sg_tablesize * 16);
-
         ret = class_register(&srp_class);
         if (ret) {
                 printk(KERN_ERR PFX "couldn't register class infiniband_srp\n");
diff --git a/drivers/infiniband/ulp/srp/ib_srp.h b/drivers/infiniband/ulp/srp/ib_srp.h
index 9dc6fc3fd894..020caf0c3789 100644
--- a/drivers/infiniband/ulp/srp/ib_srp.h
+++ b/drivers/infiniband/ulp/srp/ib_srp.h
@@ -69,9 +69,13 @@ enum {
         SRP_TAG_NO_REQ          = ~0U,
         SRP_TAG_TSK_MGMT        = 1U << 31,
 
-        SRP_FMR_SIZE            = 256,
+        SRP_FMR_SIZE            = 512,
+        SRP_FMR_MIN_SIZE        = 128,
         SRP_FMR_POOL_SIZE       = 1024,
-        SRP_FMR_DIRTY_SIZE      = SRP_FMR_POOL_SIZE / 4
+        SRP_FMR_DIRTY_SIZE      = SRP_FMR_POOL_SIZE / 4,
+
+        SRP_MAP_ALLOW_FMR       = 0,
+        SRP_MAP_NO_FMR          = 1,
 };
 
 enum srp_target_state {
@@ -93,9 +97,9 @@ struct srp_device {
         struct ib_pd           *pd;
         struct ib_mr           *mr;
         struct ib_fmr_pool     *fmr_pool;
-        int                     fmr_page_shift;
-        int                     fmr_page_size;
         u64                     fmr_page_mask;
+        int                     fmr_page_size;
+        int                     fmr_max_size;
 };
 
 struct srp_host {
@@ -112,7 +116,11 @@ struct srp_request {
         struct list_head        list;
         struct scsi_cmnd       *scmnd;
         struct srp_iu          *cmd;
-        struct ib_pool_fmr     *fmr;
+        struct ib_pool_fmr    **fmr_list;
+        u64                    *map_page;
+        struct srp_direct_buf  *indirect_desc;
+        dma_addr_t              indirect_dma_addr;
+        short                   nfmr;
         short                   index;
 };
 
@@ -130,6 +138,10 @@ struct srp_target_port {
         u32                     lkey;
         u32                     rkey;
         enum srp_target_state   state;
+        unsigned int            max_iu_len;
+        unsigned int            cmd_sg_cnt;
+        unsigned int            indirect_size;
+        bool                    allow_ext_sg;
 
         /* Everything above this point is used in the hot path of
          * command processing. Try to keep them packed into cachelines.
@@ -144,6 +156,7 @@ struct srp_target_port {
         struct Scsi_Host       *scsi_host;
         char                    target_name[32];
         unsigned int            scsi_id;
+        unsigned int            sg_tablesize;
 
         struct ib_sa_path_rec   path;
         __be16                  orig_dgid[8];
@@ -179,4 +192,19 @@ struct srp_iu {
         enum dma_data_direction direction;
 };
 
+struct srp_map_state {
+        struct ib_pool_fmr    **next_fmr;
+        struct srp_direct_buf  *desc;
+        u64                    *pages;
+        dma_addr_t              base_dma_addr;
+        u32                     fmr_len;
+        u32                     total_len;
+        unsigned int            npages;
+        unsigned int            nfmr;
+        unsigned int            ndesc;
+        struct scatterlist     *unmapped_sg;
+        int                     unmapped_index;
+        dma_addr_t              unmapped_addr;
+};
+
 #endif /* IB_SRP_H */
diff --git a/drivers/input/misc/88pm860x_onkey.c b/drivers/input/misc/88pm860x_onkey.c
index 4cc82826ea6b..3dca3c14510e 100644
--- a/drivers/input/misc/88pm860x_onkey.c
+++ b/drivers/input/misc/88pm860x_onkey.c
@@ -74,7 +74,7 @@ static int __devinit pm860x_onkey_probe(struct platform_device *pdev)
         info->chip = chip;
         info->i2c = (chip->id == CHIP_PM8607) ? chip->client : chip->companion;
         info->dev = &pdev->dev;
-        info->irq = irq + chip->irq_base;
+        info->irq = irq;
 
         info->idev = input_allocate_device();
         if (!info->idev) {
diff --git a/drivers/input/misc/twl4030-vibra.c b/drivers/input/misc/twl4030-vibra.c
index 014dd4ad0d4f..6a11694e3fc7 100644
--- a/drivers/input/misc/twl4030-vibra.c
+++ b/drivers/input/misc/twl4030-vibra.c
@@ -29,6 +29,7 @@
 #include <linux/workqueue.h>
 #include <linux/i2c/twl.h>
 #include <linux/mfd/twl4030-codec.h>
+#include <linux/mfd/core.h>
 #include <linux/input.h>
 #include <linux/slab.h>
 
@@ -196,7 +197,7 @@ static SIMPLE_DEV_PM_OPS(twl4030_vibra_pm_ops,
 
 static int __devinit twl4030_vibra_probe(struct platform_device *pdev)
 {
-        struct twl4030_codec_vibra_data *pdata = pdev->dev.platform_data;
+        struct twl4030_codec_vibra_data *pdata = mfd_get_data(pdev);
         struct vibra_info *info;
         int ret;
 
diff --git a/drivers/leds/leds-88pm860x.c b/drivers/leds/leds-88pm860x.c
index e672b44ee172..416def84d045 100644
--- a/drivers/leds/leds-88pm860x.c
+++ b/drivers/leds/leds-88pm860x.c
@@ -17,6 +17,7 @@
 #include <linux/leds.h>
 #include <linux/slab.h>
 #include <linux/workqueue.h>
+#include <linux/mfd/core.h>
 #include <linux/mfd/88pm860x.h>
 
 #define LED_PWM_SHIFT           (3)
@@ -118,7 +119,8 @@ static void pm860x_led_work(struct work_struct *work)
 
         struct pm860x_led *led;
         struct pm860x_chip *chip;
-        int mask;
+        unsigned char buf[3];
+        int mask, ret;
 
         led = container_of(work, struct pm860x_led, work);
         chip = led->chip;
@@ -128,16 +130,27 @@ static void pm860x_led_work(struct work_struct *work)
                         pm860x_set_bits(led->i2c, __led_off(led->port),
                                         LED_CURRENT_MASK, led->iset);
                 }
+                pm860x_set_bits(led->i2c, __blink_off(led->port),
+                                LED_BLINK_MASK, LED_ON_CONTINUOUS);
                 mask = __blink_ctl_mask(led->port);
                 pm860x_set_bits(led->i2c, PM8606_WLED3B, mask, mask);
-        } else if (led->brightness == 0) {
-                pm860x_set_bits(led->i2c, __led_off(led->port),
-                                LED_CURRENT_MASK, 0);
-                mask = __blink_ctl_mask(led->port);
-                pm860x_set_bits(led->i2c, PM8606_WLED3B, mask, 0);
         }
         pm860x_set_bits(led->i2c, __led_off(led->port), LED_PWM_MASK,
                         led->brightness);
+
+        if (led->brightness == 0) {
+                pm860x_bulk_read(led->i2c, __led_off(led->port), 3, buf);
+                ret = buf[0] & LED_PWM_MASK;
+                ret |= buf[1] & LED_PWM_MASK;
+                ret |= buf[2] & LED_PWM_MASK;
+                if (ret == 0) {
+                        /* unset current since no led is lighting */
+                        pm860x_set_bits(led->i2c, __led_off(led->port),
+                                        LED_CURRENT_MASK, 0);
+                        mask = __blink_ctl_mask(led->port);
+                        pm860x_set_bits(led->i2c, PM8606_WLED3B, mask, 0);
+                }
+        }
         led->current_brightness = led->brightness;
         dev_dbg(chip->dev, "Update LED. (reg:%d, brightness:%d)\n",
                 __led_off(led->port), led->brightness);
@@ -153,31 +166,12 @@ static void pm860x_led_set(struct led_classdev *cdev,
         schedule_work(&data->work);
 }
 
-static int __check_device(struct pm860x_led_pdata *pdata, char *name)
-{
-        struct pm860x_led_pdata *p = pdata;
-        int ret = -EINVAL;
-
-        while (p && p->id) {
-                if ((p->id != PM8606_ID_LED) || (p->flags < 0))
-                        break;
-
-                if (!strncmp(name, pm860x_led_name[p->flags],
-                        MFD_NAME_SIZE)) {
-                        ret = (int)p->flags;
-                        break;
-                }
-                p++;
-        }
-        return ret;
-}
-
 static int pm860x_led_probe(struct platform_device *pdev)
 {
         struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
-        struct pm860x_platform_data *pm860x_pdata;
         struct pm860x_led_pdata *pdata;
         struct pm860x_led *data;
+        struct mfd_cell *cell;
         struct resource *res;
         int ret;
 
@@ -187,10 +181,11 @@ static int pm860x_led_probe(struct platform_device *pdev)
                 return -EINVAL;
         }
 
-        if (pdev->dev.parent->platform_data) {
-                pm860x_pdata = pdev->dev.parent->platform_data;
-                pdata = pm860x_pdata->led;
-        } else {
+        cell = pdev->dev.platform_data;
+        if (cell == NULL)
+                return -ENODEV;
+        pdata = cell->mfd_data;
+        if (pdata == NULL) {
                 dev_err(&pdev->dev, "No platform data!\n");
                 return -EINVAL;
         }
@@ -198,12 +193,12 @@ static int pm860x_led_probe(struct platform_device *pdev)
         data = kzalloc(sizeof(struct pm860x_led), GFP_KERNEL);
         if (data == NULL)
                 return -ENOMEM;
-        strncpy(data->name, res->name, MFD_NAME_SIZE);
+        strncpy(data->name, res->name, MFD_NAME_SIZE - 1);
         dev_set_drvdata(&pdev->dev, data);
         data->chip = chip;
         data->i2c = (chip->id == CHIP_PM8606) ? chip->client : chip->companion;
         data->iset = pdata->iset;
-        data->port = __check_device(pdata, data->name);
+        data->port = pdata->flags;
         if (data->port < 0) {
                 dev_err(&pdev->dev, "check device failed\n");
                 kfree(data);
@@ -221,6 +216,7 @@ static int pm860x_led_probe(struct platform_device *pdev)
                 dev_err(&pdev->dev, "Failed to register LED: %d\n", ret);
                 goto out;
         }
+        pm860x_led_set(&data->cdev, 0);
         return 0;
 out:
         kfree(data);
diff --git a/drivers/leds/leds-mc13783.c b/drivers/leds/leds-mc13783.c
index f05bb08d0f09..06a5bb484707 100644
--- a/drivers/leds/leds-mc13783.c
+++ b/drivers/leds/leds-mc13783.c
@@ -22,6 +22,7 @@
 #include <linux/leds.h>
 #include <linux/workqueue.h>
 #include <linux/mfd/mc13783.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 
 struct mc13783_led {
@@ -183,7 +184,7 @@ static int __devinit mc13783_led_setup(struct mc13783_led *led, int max_current)
 
 static int __devinit mc13783_leds_prepare(struct platform_device *pdev)
 {
-        struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+        struct mc13783_leds_platform_data *pdata = mfd_get_data(pdev);
         struct mc13783 *dev = dev_get_drvdata(pdev->dev.parent);
         int ret = 0;
         int reg = 0;
@@ -264,7 +265,7 @@ out:
 
 static int __devinit mc13783_led_probe(struct platform_device *pdev)
 {
-        struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+        struct mc13783_leds_platform_data *pdata = mfd_get_data(pdev);
         struct mc13783_led_platform_data *led_cur;
         struct mc13783_led *led, *led_dat;
         int ret, i;
@@ -351,7 +352,7 @@ err_free:
 
 static int __devexit mc13783_led_remove(struct platform_device *pdev)
 {
-        struct mc13783_leds_platform_data *pdata = dev_get_platdata(&pdev->dev);
+        struct mc13783_leds_platform_data *pdata = mfd_get_data(pdev);
         struct mc13783_led *led = platform_get_drvdata(pdev);
         struct mc13783 *dev = dev_get_drvdata(pdev->dev.parent);
         int i;
diff --git a/drivers/md/bitmap.c b/drivers/md/bitmap.c
index 9a35320fb59f..a2ce0b2da281 100644
--- a/drivers/md/bitmap.c
+++ b/drivers/md/bitmap.c
@@ -854,7 +854,7 @@ static void bitmap_file_set_bit(struct bitmap *bitmap, sector_t block)
                 if (bitmap->flags & BITMAP_HOSTENDIAN)
                         set_bit(bit, kaddr);
                 else
-                        ext2_set_bit(bit, kaddr);
+                        __test_and_set_bit_le(bit, kaddr);
                 kunmap_atomic(kaddr, KM_USER0);
                 PRINTK("set file bit %lu page %lu\n", bit, page->index);
         }
@@ -1050,7 +1050,7 @@ static int bitmap_init_from_disk(struct bitmap *bitmap, sector_t start)
                 if (bitmap->flags & BITMAP_HOSTENDIAN)
                         b = test_bit(bit, paddr);
                 else
-                        b = ext2_test_bit(bit, paddr);
+                        b = test_bit_le(bit, paddr);
                 kunmap_atomic(paddr, KM_USER0);
                 if (b) {
                         /* if the disk bit is set, set the memory bit */
@@ -1226,7 +1226,7 @@ void bitmap_daemon_work(mddev_t *mddev)
                                                 clear_bit(file_page_offset(bitmap, j),
                                                           paddr);
                                         else
-                                                ext2_clear_bit(file_page_offset(bitmap, j),
+                                                __test_and_clear_bit_le(file_page_offset(bitmap, j),
                                                                paddr);
                                         kunmap_atomic(paddr, KM_USER0);
                                 } else
diff --git a/drivers/md/dm-log.c b/drivers/md/dm-log.c
index 6951536ea29c..57968eb382c1 100644
--- a/drivers/md/dm-log.c
+++ b/drivers/md/dm-log.c
@@ -251,20 +251,20 @@ struct log_c {
  */
 static inline int log_test_bit(uint32_t *bs, unsigned bit)
 {
-        return ext2_test_bit(bit, (unsigned long *) bs) ? 1 : 0;
+        return test_bit_le(bit, (unsigned long *) bs) ? 1 : 0;
 }
 
 static inline void log_set_bit(struct log_c *l,
                                uint32_t *bs, unsigned bit)
 {
-        ext2_set_bit(bit, (unsigned long *) bs);
+        __test_and_set_bit_le(bit, (unsigned long *) bs);
         l->touched_cleaned = 1;
 }
 
 static inline void log_clear_bit(struct log_c *l,
                                  uint32_t *bs, unsigned bit)
 {
-        ext2_clear_bit(bit, (unsigned long *) bs);
+        __test_and_clear_bit_le(bit, (unsigned long *) bs);
         l->touched_dirtied = 1;
 }
 
@@ -740,7 +740,7 @@ static int core_get_resync_work(struct dm_dirty_log *log, region_t *region)
                 return 0;
 
         do {
-                *region = ext2_find_next_zero_bit(
+                *region = find_next_zero_bit_le(
                                              (unsigned long *) lc->sync_bits,
                                              lc->region_count,
                                              lc->sync_search);
diff --git a/drivers/media/radio/radio-timb.c b/drivers/media/radio/radio-timb.c
index a185610b376b..1e3a8dd820a4 100644
--- a/drivers/media/radio/radio-timb.c
+++ b/drivers/media/radio/radio-timb.c
@@ -21,6 +21,7 @@
 #include <media/v4l2-ioctl.h>
 #include <media/v4l2-device.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/interrupt.h>
 #include <linux/slab.h>
 #include <linux/i2c.h>
@@ -148,7 +149,7 @@ static const struct v4l2_file_operations timbradio_fops = {
 
 static int __devinit timbradio_probe(struct platform_device *pdev)
 {
-        struct timb_radio_platform_data *pdata = pdev->dev.platform_data;
+        struct timb_radio_platform_data *pdata = mfd_get_data(pdev);
         struct timbradio *tr;
         int err;
 
diff --git a/drivers/media/radio/radio-wl1273.c b/drivers/media/radio/radio-wl1273.c
index 7ecc8e657663..4698eb00b59f 100644
--- a/drivers/media/radio/radio-wl1273.c
+++ b/drivers/media/radio/radio-wl1273.c
@@ -2138,7 +2138,7 @@ static int wl1273_fm_radio_remove(struct platform_device *pdev)
 
 static int __devinit wl1273_fm_radio_probe(struct platform_device *pdev)
 {
-        struct wl1273_core **core = pdev->dev.platform_data;
+        struct wl1273_core **core = mfd_get_data(pdev);
         struct wl1273_device *radio;
         struct v4l2_ctrl *ctrl;
         int r = 0;
diff --git a/drivers/media/video/timblogiw.c b/drivers/media/video/timblogiw.c
index fc611ebeb82c..84d4c7c83435 100644
--- a/drivers/media/video/timblogiw.c
+++ b/drivers/media/video/timblogiw.c
@@ -24,6 +24,7 @@
 #include <linux/platform_device.h>
 #include <linux/slab.h>
 #include <linux/dmaengine.h>
+#include <linux/mfd/core.h>
 #include <linux/scatterlist.h>
 #include <linux/interrupt.h>
 #include <linux/list.h>
@@ -790,7 +791,7 @@ static int __devinit timblogiw_probe(struct platform_device *pdev)
 {
         int err;
         struct timblogiw *lw = NULL;
-        struct timb_video_platform_data *pdata = pdev->dev.platform_data;
+        struct timb_video_platform_data *pdata = mfd_get_data(pdev);
 
         if (!pdata) {
                 dev_err(&pdev->dev, "No platform data\n");
diff --git a/drivers/mfd/88pm860x-core.c b/drivers/mfd/88pm860x-core.c
index 793300c554b4..9c511c1604a5 100644
--- a/drivers/mfd/88pm860x-core.c
+++ b/drivers/mfd/88pm860x-core.c
@@ -17,230 +17,138 @@
 #include <linux/platform_device.h>
 #include <linux/mfd/core.h>
 #include <linux/mfd/88pm860x.h>
+#include <linux/regulator/machine.h>
 
 #define INT_STATUS_NUM                  3
 
-char pm860x_backlight_name[][MFD_NAME_SIZE] = {
-        "backlight-0",
-        "backlight-1",
-        "backlight-2",
+static struct resource bk_resources[] __initdata = {
+        {PM8606_BACKLIGHT1, PM8606_BACKLIGHT1, "backlight-0", IORESOURCE_IO,},
+        {PM8606_BACKLIGHT2, PM8606_BACKLIGHT2, "backlight-1", IORESOURCE_IO,},
+        {PM8606_BACKLIGHT3, PM8606_BACKLIGHT3, "backlight-2", IORESOURCE_IO,},
 };
-EXPORT_SYMBOL(pm860x_backlight_name);
-
-char pm860x_led_name[][MFD_NAME_SIZE] = {
-        "led0-red",
-        "led0-green",
-        "led0-blue",
-        "led1-red",
-        "led1-green",
-        "led1-blue",
-};
-EXPORT_SYMBOL(pm860x_led_name);
-
-#define PM8606_BACKLIGHT_RESOURCE(_i, _x)               \
-{                                                       \
-        .name   = pm860x_backlight_name[_i],            \
-        .start  = PM8606_##_x,                          \
-        .end    = PM8606_##_x,                          \
-        .flags  = IORESOURCE_IO,                        \
-}
 
-static struct resource backlight_resources[] = {
-        PM8606_BACKLIGHT_RESOURCE(PM8606_BACKLIGHT1, WLED1A),
-        PM8606_BACKLIGHT_RESOURCE(PM8606_BACKLIGHT2, WLED2A),
-        PM8606_BACKLIGHT_RESOURCE(PM8606_BACKLIGHT3, WLED3A),
+static struct resource led_resources[] __initdata = {
+        {PM8606_LED1_RED,   PM8606_LED1_RED,   "led0-red",   IORESOURCE_IO,},
+        {PM8606_LED1_GREEN, PM8606_LED1_GREEN, "led0-green", IORESOURCE_IO,},
+        {PM8606_LED1_BLUE,  PM8606_LED1_BLUE,  "led0-blue",  IORESOURCE_IO,},
+        {PM8606_LED2_RED,   PM8606_LED2_RED,   "led1-red",   IORESOURCE_IO,},
+        {PM8606_LED2_GREEN, PM8606_LED2_GREEN, "led1-green", IORESOURCE_IO,},
+        {PM8606_LED2_BLUE,  PM8606_LED2_BLUE,  "led1-blue",  IORESOURCE_IO,},
 };
 
-#define PM8606_BACKLIGHT_DEVS(_i)                       \
-{                                                       \
-        .name           = "88pm860x-backlight",         \
-        .num_resources  = 1,                            \
-        .resources      = &backlight_resources[_i],     \
-        .id             = _i,                           \
-}
-
-static struct mfd_cell backlight_devs[] = {
-        PM8606_BACKLIGHT_DEVS(PM8606_BACKLIGHT1),
-        PM8606_BACKLIGHT_DEVS(PM8606_BACKLIGHT2),
-        PM8606_BACKLIGHT_DEVS(PM8606_BACKLIGHT3),
+static struct resource regulator_resources[] __initdata = {
+        {PM8607_ID_BUCK1, PM8607_ID_BUCK1, "buck-1", IORESOURCE_IO,},
+        {PM8607_ID_BUCK2, PM8607_ID_BUCK2, "buck-2", IORESOURCE_IO,},
+        {PM8607_ID_BUCK3, PM8607_ID_BUCK3, "buck-3", IORESOURCE_IO,},
+        {PM8607_ID_LDO1,  PM8607_ID_LDO1,  "ldo-01", IORESOURCE_IO,},
+        {PM8607_ID_LDO2,  PM8607_ID_LDO2,  "ldo-02", IORESOURCE_IO,},
+        {PM8607_ID_LDO3,  PM8607_ID_LDO3,  "ldo-03", IORESOURCE_IO,},
+        {PM8607_ID_LDO4,  PM8607_ID_LDO4,  "ldo-04", IORESOURCE_IO,},
+        {PM8607_ID_LDO5,  PM8607_ID_LDO5,  "ldo-05", IORESOURCE_IO,},
+        {PM8607_ID_LDO6,  PM8607_ID_LDO6,  "ldo-06", IORESOURCE_IO,},
+        {PM8607_ID_LDO7,  PM8607_ID_LDO7,  "ldo-07", IORESOURCE_IO,},
+        {PM8607_ID_LDO8,  PM8607_ID_LDO8,  "ldo-08", IORESOURCE_IO,},
+        {PM8607_ID_LDO9,  PM8607_ID_LDO9,  "ldo-09", IORESOURCE_IO,},
+        {PM8607_ID_LDO10, PM8607_ID_LDO10, "ldo-10", IORESOURCE_IO,},
+        {PM8607_ID_LDO11, PM8607_ID_LDO11, "ldo-11", IORESOURCE_IO,},
+        {PM8607_ID_LDO12, PM8607_ID_LDO12, "ldo-12", IORESOURCE_IO,},
+        {PM8607_ID_LDO13, PM8607_ID_LDO13, "ldo-13", IORESOURCE_IO,},
+        {PM8607_ID_LDO14, PM8607_ID_LDO14, "ldo-14", IORESOURCE_IO,},
+        {PM8607_ID_LDO15, PM8607_ID_LDO15, "ldo-15", IORESOURCE_IO,},
 };
 
-#define PM8606_LED_RESOURCE(_i, _x)                     \
-{                                                       \
-        .name   = pm860x_led_name[_i],                  \
-        .start  = PM8606_##_x,                          \
-        .end    = PM8606_##_x,                          \
-        .flags  = IORESOURCE_IO,                        \
-}
-
-static struct resource led_resources[] = {
-        PM8606_LED_RESOURCE(PM8606_LED1_RED, RGB1B),
-        PM8606_LED_RESOURCE(PM8606_LED1_GREEN, RGB1C),
-        PM8606_LED_RESOURCE(PM8606_LED1_BLUE, RGB1D),
-        PM8606_LED_RESOURCE(PM8606_LED2_RED, RGB2B),
-        PM8606_LED_RESOURCE(PM8606_LED2_GREEN, RGB2C),
-        PM8606_LED_RESOURCE(PM8606_LED2_BLUE, RGB2D),
+static struct resource touch_resources[] __initdata = {
+        {PM8607_IRQ_PEN, PM8607_IRQ_PEN, "touch", IORESOURCE_IRQ,},
 };
 
-#define PM8606_LED_DEVS(_i)                             \
-{                                                       \
-        .name           = "88pm860x-led",               \
-        .num_resources  = 1,                            \
-        .resources      = &led_resources[_i],           \
-        .id             = _i,                           \
-}
-
-static struct mfd_cell led_devs[] = {
-        PM8606_LED_DEVS(PM8606_LED1_RED),
-        PM8606_LED_DEVS(PM8606_LED1_GREEN),
-        PM8606_LED_DEVS(PM8606_LED1_BLUE),
-        PM8606_LED_DEVS(PM8606_LED2_RED),
-        PM8606_LED_DEVS(PM8606_LED2_GREEN),
-        PM8606_LED_DEVS(PM8606_LED2_BLUE),
+static struct resource onkey_resources[] __initdata = {
+        {PM8607_IRQ_ONKEY, PM8607_IRQ_ONKEY, "onkey", IORESOURCE_IRQ,},
 };
 
-static struct resource touch_resources[] = {
-        {
-                .start  = PM8607_IRQ_PEN,
-                .end    = PM8607_IRQ_PEN,
-                .flags  = IORESOURCE_IRQ,
-        },
+static struct resource codec_resources[] __initdata = {
+        /* Headset microphone insertion or removal */
+        {PM8607_IRQ_MICIN,   PM8607_IRQ_MICIN,   "micin",   IORESOURCE_IRQ,},
+        /* Hook-switch press or release */
+        {PM8607_IRQ_HOOK,    PM8607_IRQ_HOOK,    "hook",    IORESOURCE_IRQ,},
+        /* Headset insertion or removal */
+        {PM8607_IRQ_HEADSET, PM8607_IRQ_HEADSET, "headset", IORESOURCE_IRQ,},
+        /* Audio short */
+        {PM8607_IRQ_AUDIO_SHORT, PM8607_IRQ_AUDIO_SHORT, "audio-short", IORESOURCE_IRQ,},
 };
 
-static struct mfd_cell touch_devs[] = {
-        {
-                .name           = "88pm860x-touch",
-                .num_resources  = 1,
-                .resources      = &touch_resources[0],
-        },
+static struct resource battery_resources[] __initdata = {
+        {PM8607_IRQ_CC,  PM8607_IRQ_CC,  "columb counter", IORESOURCE_IRQ,},
+        {PM8607_IRQ_BAT, PM8607_IRQ_BAT, "battery",        IORESOURCE_IRQ,},
 };
 
-#define PM8607_REG_RESOURCE(_start, _end)               \
-{                                                       \
-        .start  = PM8607_##_start,                      \
-        .end    = PM8607_##_end,                        \
-        .flags  = IORESOURCE_IO,                        \
-}
-
-static struct resource power_supply_resources[] = {
-        {
-                .name           = "88pm860x-power",
-                .start          = PM8607_IRQ_CHG,
-                .end            = PM8607_IRQ_CHG,
-                .flags          = IORESOURCE_IRQ,
-        },
+static struct resource charger_resources[] __initdata = {
+        {PM8607_IRQ_CHG,  PM8607_IRQ_CHG,  "charger detect",  IORESOURCE_IRQ,},
+        {PM8607_IRQ_CHG_DONE,  PM8607_IRQ_CHG_DONE,  "charging done",       IORESOURCE_IRQ,},
+        {PM8607_IRQ_CHG_FAULT, PM8607_IRQ_CHG_FAULT, "charging timeout",    IORESOURCE_IRQ,},
+        {PM8607_IRQ_GPADC1,    PM8607_IRQ_GPADC1,    "battery temperature", IORESOURCE_IRQ,},
+        {PM8607_IRQ_VBAT, PM8607_IRQ_VBAT, "battery voltage", IORESOURCE_IRQ,},
+        {PM8607_IRQ_VCHG, PM8607_IRQ_VCHG, "vchg voltage",    IORESOURCE_IRQ,},
 };
 
-static struct mfd_cell power_devs[] = {
-        {
-                .name           = "88pm860x-power",
-                .num_resources  = 1,
-                .resources      = &power_supply_resources[0],
-                .id             = -1,
-        },
+static struct mfd_cell bk_devs[] __initdata = {
+        {"88pm860x-backlight", 0,},
+        {"88pm860x-backlight", 1,},
+        {"88pm860x-backlight", 2,},
 };
 
-static struct resource onkey_resources[] = {
-        {
-                .name           = "88pm860x-onkey",
-                .start          = PM8607_IRQ_ONKEY,
-                .end            = PM8607_IRQ_ONKEY,
-                .flags          = IORESOURCE_IRQ,
-        },
+static struct mfd_cell led_devs[] __initdata = {
+        {"88pm860x-led", 0,},
+        {"88pm860x-led", 1,},
+        {"88pm860x-led", 2,},
+        {"88pm860x-led", 3,},
+        {"88pm860x-led", 4,},
+        {"88pm860x-led", 5,},
 };
 
-static struct mfd_cell onkey_devs[] = {
-        {
-                .name           = "88pm860x-onkey",
-                .num_resources  = 1,
-                .resources      = &onkey_resources[0],
-                .id             = -1,
-        },
+static struct mfd_cell regulator_devs[] __initdata = {
+        {"88pm860x-regulator", 0,},
+        {"88pm860x-regulator", 1,},
+        {"88pm860x-regulator", 2,},
+        {"88pm860x-regulator", 3,},
+        {"88pm860x-regulator", 4,},
+        {"88pm860x-regulator", 5,},
+        {"88pm860x-regulator", 6,},
+        {"88pm860x-regulator", 7,},
+        {"88pm860x-regulator", 8,},
+        {"88pm860x-regulator", 9,},
+        {"88pm860x-regulator", 10,},
+        {"88pm860x-regulator", 11,},
+        {"88pm860x-regulator", 12,},
+        {"88pm860x-regulator", 13,},
+        {"88pm860x-regulator", 14,},
+        {"88pm860x-regulator", 15,},
+        {"88pm860x-regulator", 16,},
+        {"88pm860x-regulator", 17,},
 };
 
-static struct resource codec_resources[] = {
-        {
-                /* Headset microphone insertion or removal */
-                .name           = "micin",
-                .start          = PM8607_IRQ_MICIN,
-                .end            = PM8607_IRQ_MICIN,
-                .flags          = IORESOURCE_IRQ,
-        }, {
-                /* Hook-switch press or release */
-                .name           = "hook",
-                .start          = PM8607_IRQ_HOOK,
-                .end            = PM8607_IRQ_HOOK,
-                .flags          = IORESOURCE_IRQ,
-        }, {
-                /* Headset insertion or removal */
-                .name           = "headset",
-                .start          = PM8607_IRQ_HEADSET,
-                .end            = PM8607_IRQ_HEADSET,
-                .flags          = IORESOURCE_IRQ,
-        }, {
-                /* Audio short */
-                .name           = "audio-short",
-                .start          = PM8607_IRQ_AUDIO_SHORT,
-                .end            = PM8607_IRQ_AUDIO_SHORT,
-                .flags          = IORESOURCE_IRQ,
-        },
+static struct mfd_cell touch_devs[] __initdata = {
+        {"88pm860x-touch", -1,},
 };
 
-static struct mfd_cell codec_devs[] = {
-        {
-                .name           = "88pm860x-codec",
-                .num_resources  = ARRAY_SIZE(codec_resources),
-                .resources      = &codec_resources[0],
-                .id             = -1,
-        },
+static struct mfd_cell onkey_devs[] __initdata = {
+        {"88pm860x-onkey", -1,},
 };
 
-static struct resource regulator_resources[] = {
-        PM8607_REG_RESOURCE(BUCK1, BUCK1),
-        PM8607_REG_RESOURCE(BUCK2, BUCK2),
-        PM8607_REG_RESOURCE(BUCK3, BUCK3),
-        PM8607_REG_RESOURCE(LDO1,  LDO1),
-        PM8607_REG_RESOURCE(LDO2,  LDO2),
-        PM8607_REG_RESOURCE(LDO3,  LDO3),
-        PM8607_REG_RESOURCE(LDO4,  LDO4),
-        PM8607_REG_RESOURCE(LDO5,  LDO5),
-        PM8607_REG_RESOURCE(LDO6,  LDO6),
-        PM8607_REG_RESOURCE(LDO7,  LDO7),
-        PM8607_REG_RESOURCE(LDO8,  LDO8),
-        PM8607_REG_RESOURCE(LDO9,  LDO9),
-        PM8607_REG_RESOURCE(LDO10, LDO10),
-        PM8607_REG_RESOURCE(LDO12, LDO12),
-        PM8607_REG_RESOURCE(VIBRATOR_SET, VIBRATOR_SET),
-        PM8607_REG_RESOURCE(LDO14, LDO14),
+static struct mfd_cell codec_devs[] __initdata = {
+        {"88pm860x-codec", -1,},
 };
 
-#define PM8607_REG_DEVS(_id)                                            \
-{                                                                       \
-        .name           = "88pm860x-regulator",                         \
-        .num_resources  = 1,                                            \
-        .resources      = &regulator_resources[PM8607_ID_##_id],        \
-        .id             = PM8607_ID_##_id,                              \
-}
-
-static struct mfd_cell regulator_devs[] = {
-        PM8607_REG_DEVS(BUCK1),
-        PM8607_REG_DEVS(BUCK2),
-        PM8607_REG_DEVS(BUCK3),
-        PM8607_REG_DEVS(LDO1),
-        PM8607_REG_DEVS(LDO2),
-        PM8607_REG_DEVS(LDO3),
-        PM8607_REG_DEVS(LDO4),
-        PM8607_REG_DEVS(LDO5),
-        PM8607_REG_DEVS(LDO6),
-        PM8607_REG_DEVS(LDO7),
-        PM8607_REG_DEVS(LDO8),
-        PM8607_REG_DEVS(LDO9),
-        PM8607_REG_DEVS(LDO10),
-        PM8607_REG_DEVS(LDO12),
-        PM8607_REG_DEVS(LDO13),
-        PM8607_REG_DEVS(LDO14),
+static struct mfd_cell power_devs[] = {
+        {"88pm860x-battery", -1,},
+        {"88pm860x-charger", -1,},
 };
 
+static struct pm860x_backlight_pdata bk_pdata[ARRAY_SIZE(bk_devs)];
+static struct pm860x_led_pdata led_pdata[ARRAY_SIZE(led_devs)];
+static struct regulator_init_data regulator_pdata[ARRAY_SIZE(regulator_devs)];
+static struct pm860x_touch_pdata touch_pdata;
+static struct pm860x_power_pdata power_pdata;
+
 struct pm860x_irq_data {
         int     reg;
         int     mask_reg;
@@ -595,37 +503,212 @@ static void device_irq_exit(struct pm860x_chip *chip)
                 free_irq(chip->core_irq, chip);
 }
 
-static void __devinit device_8606_init(struct pm860x_chip *chip,
-                                       struct i2c_client *i2c,
-                                       struct pm860x_platform_data *pdata)
+static void __devinit device_bk_init(struct pm860x_chip *chip,
+                                     struct i2c_client *i2c,
+                                     struct pm860x_platform_data *pdata)
 {
         int ret;
+        int i, j, id;
+
+        if ((pdata == NULL) || (pdata->backlight == NULL))
+                return;
+
+        if (pdata->num_backlights > ARRAY_SIZE(bk_devs))
+                pdata->num_backlights = ARRAY_SIZE(bk_devs);
+
+        for (i = 0; i < pdata->num_backlights; i++) {
+                memcpy(&bk_pdata[i], &pdata->backlight[i],
+                        sizeof(struct pm860x_backlight_pdata));
+                bk_devs[i].mfd_data = &bk_pdata[i];
+
+                for (j = 0; j < ARRAY_SIZE(bk_devs); j++) {
+                        id = bk_resources[j].start;
+                        if (bk_pdata[i].flags != id)
+                                continue;
+
+                        bk_devs[i].num_resources = 1;
+                        bk_devs[i].resources = &bk_resources[j];
+                        ret = mfd_add_devices(chip->dev, 0,
+                                              &bk_devs[i], 1,
+                                              &bk_resources[j], 0);
+                        if (ret < 0) {
+                                dev_err(chip->dev, "Failed to add "
+                                        "backlight subdev\n");
+                                return;
+                        }
+                }
+        }
+}
 
-        if (pdata && pdata->backlight) {
-                ret = mfd_add_devices(chip->dev, 0, &backlight_devs[0],
-                                      ARRAY_SIZE(backlight_devs),
-                                      &backlight_resources[0], 0);
-                if (ret < 0) {
-                        dev_err(chip->dev, "Failed to add backlight "
-                                "subdev\n");
-                        goto out_dev;
+static void __devinit device_led_init(struct pm860x_chip *chip,
+                                      struct i2c_client *i2c,
+                                      struct pm860x_platform_data *pdata)
+{
+        int ret;
+        int i, j, id;
+
+        if ((pdata == NULL) || (pdata->led == NULL))
+                return;
+
+        if (pdata->num_leds > ARRAY_SIZE(led_devs))
+                pdata->num_leds = ARRAY_SIZE(led_devs);
+
+        for (i = 0; i < pdata->num_leds; i++) {
+                memcpy(&led_pdata[i], &pdata->led[i],
+                        sizeof(struct pm860x_led_pdata));
+                led_devs[i].mfd_data = &led_pdata[i];
+
+                for (j = 0; j < ARRAY_SIZE(led_devs); j++) {
+                        id = led_resources[j].start;
+                        if (led_pdata[i].flags != id)
+                                continue;
+
+                        led_devs[i].num_resources = 1;
+                        led_devs[i].resources = &led_resources[j],
+                        ret = mfd_add_devices(chip->dev, 0,
+                                              &led_devs[i], 1,
+                                              &led_resources[j], 0);
+                        if (ret < 0) {
+                                dev_err(chip->dev, "Failed to add "
+                                        "led subdev\n");
+                                return;
+                        }
                 }
         }
+}
 
-        if (pdata && pdata->led) {
-                ret = mfd_add_devices(chip->dev, 0, &led_devs[0],
-                                      ARRAY_SIZE(led_devs),
-                                      &led_resources[0], 0);
+static void __devinit device_regulator_init(struct pm860x_chip *chip,
+                                            struct i2c_client *i2c,
+                                            struct pm860x_platform_data *pdata)
+{
+        struct regulator_init_data *initdata;
+        int ret;
+        int i, j;
+
+        if ((pdata == NULL) || (pdata->regulator == NULL))
+                return;
+
+        if (pdata->num_regulators > ARRAY_SIZE(regulator_devs))
+                pdata->num_regulators = ARRAY_SIZE(regulator_devs);
+
+        for (i = 0, j = -1; i < pdata->num_regulators; i++) {
+                initdata = &pdata->regulator[i];
+                if (strstr(initdata->constraints.name, "BUCK")) {
+                        sscanf(initdata->constraints.name, "BUCK%d", &j);
+                        /* BUCK1 ~ BUCK3 */
+                        if ((j < 1) || (j > 3)) {
+                                dev_err(chip->dev, "Failed to add constraint "
+                                        "(%s)\n", initdata->constraints.name);
+                                goto out;
+                        }
+                        j = (j - 1) + PM8607_ID_BUCK1;
+                }
+                if (strstr(initdata->constraints.name, "LDO")) {
+                        sscanf(initdata->constraints.name, "LDO%d", &j);
+                        /* LDO1 ~ LDO15 */
+                        if ((j < 1) || (j > 15)) {
+                                dev_err(chip->dev, "Failed to add constraint "
+                                        "(%s)\n", initdata->constraints.name);
+                                goto out;
+                        }
+                        j = (j - 1) + PM8607_ID_LDO1;
+                }
+                if (j == -1) {
+                        dev_err(chip->dev, "Failed to add constraint (%s)\n",
+                                initdata->constraints.name);
+                        goto out;
+                }
+                memcpy(&regulator_pdata[i], &pdata->regulator[i],
+                        sizeof(struct regulator_init_data));
+                regulator_devs[i].mfd_data = &regulator_pdata[i];
+                regulator_devs[i].num_resources = 1;
+                regulator_devs[i].resources = &regulator_resources[j];
+
+                ret = mfd_add_devices(chip->dev, 0, &regulator_devs[i], 1,
+                                      &regulator_resources[j], 0);
                 if (ret < 0) {
-                        dev_err(chip->dev, "Failed to add led "
-                                "subdev\n");
-                        goto out_dev;
+                        dev_err(chip->dev, "Failed to add regulator subdev\n");
+                        goto out;
                 }
         }
+out:
         return;
-out_dev:
-        mfd_remove_devices(chip->dev);
-        device_irq_exit(chip);
+}
+
+static void __devinit device_touch_init(struct pm860x_chip *chip,
+                                        struct i2c_client *i2c,
+                                        struct pm860x_platform_data *pdata)
+{
+        int ret;
+
+        if ((pdata == NULL) || (pdata->touch == NULL))
+                return;
+
+        memcpy(&touch_pdata, pdata->touch, sizeof(struct pm860x_touch_pdata));
+        touch_devs[0].mfd_data = &touch_pdata;
+        touch_devs[0].num_resources = ARRAY_SIZE(touch_resources);
+        touch_devs[0].resources = &touch_resources[0];
+        ret = mfd_add_devices(chip->dev, 0, &touch_devs[0],
+                              ARRAY_SIZE(touch_devs), &touch_resources[0],
+                              chip->irq_base);
+        if (ret < 0)
+                dev_err(chip->dev, "Failed to add touch subdev\n");
+}
+
+static void __devinit device_power_init(struct pm860x_chip *chip,
+                                        struct i2c_client *i2c,
+                                        struct pm860x_platform_data *pdata)
+{
+        int ret;
+
+        if ((pdata == NULL) || (pdata->power == NULL))
+                return;
+
+        memcpy(&power_pdata, pdata->power, sizeof(struct pm860x_power_pdata));
+        power_devs[0].mfd_data = &power_pdata;
+        power_devs[0].num_resources = ARRAY_SIZE(battery_resources);
+        power_devs[0].resources = &battery_resources[0],
+        ret = mfd_add_devices(chip->dev, 0, &power_devs[0], 1,
+                              &battery_resources[0], chip->irq_base);
+        if (ret < 0)
+                dev_err(chip->dev, "Failed to add battery subdev\n");
+
+        power_devs[1].mfd_data = &power_pdata;
+        power_devs[1].num_resources = ARRAY_SIZE(charger_resources);
+        power_devs[1].resources = &charger_resources[0],
+        ret = mfd_add_devices(chip->dev, 0, &power_devs[1], 1,
+                              &charger_resources[0], chip->irq_base);
+        if (ret < 0)
+                dev_err(chip->dev, "Failed to add charger subdev\n");
+}
+
+static void __devinit device_onkey_init(struct pm860x_chip *chip,
+                                        struct i2c_client *i2c,
+                                        struct pm860x_platform_data *pdata)
+{
+        int ret;
+
+        onkey_devs[0].num_resources = ARRAY_SIZE(onkey_resources);
+        onkey_devs[0].resources = &onkey_resources[0],
+        ret = mfd_add_devices(chip->dev, 0, &onkey_devs[0],
+                              ARRAY_SIZE(onkey_devs), &onkey_resources[0],
+                              chip->irq_base);
+        if (ret < 0)
+                dev_err(chip->dev, "Failed to add onkey subdev\n");
+}
+
+static void __devinit device_codec_init(struct pm860x_chip *chip,
+                                        struct i2c_client *i2c,
+                                        struct pm860x_platform_data *pdata)
+{
+        int ret;
+
+        codec_devs[0].num_resources = ARRAY_SIZE(codec_resources);
+        codec_devs[0].resources = &codec_resources[0],
+        ret = mfd_add_devices(chip->dev, 0, &codec_devs[0],
+                              ARRAY_SIZE(codec_devs), &codec_resources[0], 0);
+        if (ret < 0)
+                dev_err(chip->dev, "Failed to add codec subdev\n");
 }
 
 static void __devinit device_8607_init(struct pm860x_chip *chip,
@@ -683,55 +766,11 @@ static void __devinit device_8607_init(struct pm860x_chip *chip,
         if (ret < 0)
                 goto out;
 
-        ret = mfd_add_devices(chip->dev, 0, &regulator_devs[0],
-                              ARRAY_SIZE(regulator_devs),
-                              &regulator_resources[0], 0);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to add regulator subdev\n");
-                goto out_dev;
-        }
-
-        if (pdata && pdata->touch) {
-                ret = mfd_add_devices(chip->dev, 0, &touch_devs[0],
-                                      ARRAY_SIZE(touch_devs),
-                                      &touch_resources[0], 0);
-                if (ret < 0) {
-                        dev_err(chip->dev, "Failed to add touch "
-                                "subdev\n");
-                        goto out_dev;
-                }
-        }
-
-        if (pdata && pdata->power) {
-                ret = mfd_add_devices(chip->dev, 0, &power_devs[0],
-                                      ARRAY_SIZE(power_devs),
-                                      &power_supply_resources[0], 0);
-                if (ret < 0) {
-                        dev_err(chip->dev, "Failed to add power supply "
-                                "subdev\n");
-                        goto out_dev;
-                }
-        }
-
-        ret = mfd_add_devices(chip->dev, 0, &onkey_devs[0],
-                              ARRAY_SIZE(onkey_devs),
-                              &onkey_resources[0], 0);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to add onkey subdev\n");
-                goto out_dev;
-        }
-
-        ret = mfd_add_devices(chip->dev, 0, &codec_devs[0],
-                              ARRAY_SIZE(codec_devs),
-                              &codec_resources[0], 0);
-        if (ret < 0) {
-                dev_err(chip->dev, "Failed to add codec subdev\n");
-                goto out_dev;
-        }
-        return;
-out_dev:
-        mfd_remove_devices(chip->dev);
-        device_irq_exit(chip);
+        device_regulator_init(chip, i2c, pdata);
+        device_onkey_init(chip, i2c, pdata);
+        device_touch_init(chip, i2c, pdata);
+        device_power_init(chip, i2c, pdata);
+        device_codec_init(chip, i2c, pdata);
 out:
         return;
 }
@@ -743,7 +782,8 @@ int __devinit pm860x_device_init(struct pm860x_chip *chip,
 
         switch (chip->id) {
         case CHIP_PM8606:
-                device_8606_init(chip, chip->client, pdata);
+                device_bk_init(chip, chip->client, pdata);
+                device_led_init(chip, chip->client, pdata);
                 break;
         case CHIP_PM8607:
                 device_8607_init(chip, chip->client, pdata);
@@ -753,7 +793,8 @@ int __devinit pm860x_device_init(struct pm860x_chip *chip,
         if (chip->companion) {
                 switch (chip->id) {
                 case CHIP_PM8607:
-                        device_8606_init(chip, chip->companion, pdata);
+                        device_bk_init(chip, chip->companion, pdata);
+                        device_led_init(chip, chip->companion, pdata);
                         break;
                 case CHIP_PM8606:
                         device_8607_init(chip, chip->companion, pdata);
diff --git a/drivers/mfd/88pm860x-i2c.c b/drivers/mfd/88pm860x-i2c.c
index bc02e6b21608..e017dc88622a 100644
--- a/drivers/mfd/88pm860x-i2c.c
+++ b/drivers/mfd/88pm860x-i2c.c
@@ -126,6 +126,109 @@ out:
 }
 EXPORT_SYMBOL(pm860x_set_bits);
 
+int pm860x_page_reg_read(struct i2c_client *i2c, int reg)
+{
+        struct pm860x_chip *chip = i2c_get_clientdata(i2c);
+        unsigned char zero = 0;
+        unsigned char data;
+        int ret;
+
+        mutex_lock(&chip->io_lock);
+        pm860x_write_device(i2c, 0xFA, 0, &zero);
+        pm860x_write_device(i2c, 0xFB, 0, &zero);
+        pm860x_write_device(i2c, 0xFF, 0, &zero);
+        ret = pm860x_read_device(i2c, reg, 1, &data);
+        if (ret >= 0)
+                ret = (int)data;
+        pm860x_write_device(i2c, 0xFE, 0, &zero);
+        pm860x_write_device(i2c, 0xFC, 0, &zero);
+        mutex_unlock(&chip->io_lock);
+        return ret;
+}
+EXPORT_SYMBOL(pm860x_page_reg_read);
+
+int pm860x_page_reg_write(struct i2c_client *i2c, int reg,
+                          unsigned char data)
+{
+        struct pm860x_chip *chip = i2c_get_clientdata(i2c);
+        unsigned char zero;
+        int ret;
+
+        mutex_lock(&chip->io_lock);
+        pm860x_write_device(i2c, 0xFA, 0, &zero);
+        pm860x_write_device(i2c, 0xFB, 0, &zero);
+        pm860x_write_device(i2c, 0xFF, 0, &zero);
+        ret = pm860x_write_device(i2c, reg, 1, &data);
+        pm860x_write_device(i2c, 0xFE, 0, &zero);
+        pm860x_write_device(i2c, 0xFC, 0, &zero);
+        mutex_unlock(&chip->io_lock);
+        return ret;
+}
+EXPORT_SYMBOL(pm860x_page_reg_write);
+
+int pm860x_page_bulk_read(struct i2c_client *i2c, int reg,
+                          int count, unsigned char *buf)
+{
+        struct pm860x_chip *chip = i2c_get_clientdata(i2c);
+        unsigned char zero = 0;
+        int ret;
+
+        mutex_lock(&chip->io_lock);
+        pm860x_write_device(i2c, 0xFA, 0, &zero);
+        pm860x_write_device(i2c, 0xFB, 0, &zero);
+        pm860x_write_device(i2c, 0xFF, 0, &zero);
+        ret = pm860x_read_device(i2c, reg, count, buf);
+        pm860x_write_device(i2c, 0xFE, 0, &zero);
+        pm860x_write_device(i2c, 0xFC, 0, &zero);
+        mutex_unlock(&chip->io_lock);
+        return ret;
+}
+EXPORT_SYMBOL(pm860x_page_bulk_read);
+
+int pm860x_page_bulk_write(struct i2c_client *i2c, int reg,
+                           int count, unsigned char *buf)
+{
+        struct pm860x_chip *chip = i2c_get_clientdata(i2c);
+        unsigned char zero = 0;
+        int ret;
+
+        mutex_lock(&chip->io_lock);
+        pm860x_write_device(i2c, 0xFA, 0, &zero);
+        pm860x_write_device(i2c, 0xFB, 0, &zero);
+        pm860x_write_device(i2c, 0xFF, 0, &zero);
+        ret = pm860x_write_device(i2c, reg, count, buf);
+        pm860x_write_device(i2c, 0xFE, 0, &zero);
+        pm860x_write_device(i2c, 0xFC, 0, &zero);
+        mutex_unlock(&chip->io_lock);
+        return ret;
+}
+EXPORT_SYMBOL(pm860x_page_bulk_write);
+
+int pm860x_page_set_bits(struct i2c_client *i2c, int reg,
+                         unsigned char mask, unsigned char data)
+{
+        struct pm860x_chip *chip = i2c_get_clientdata(i2c);
+        unsigned char zero;
+        unsigned char value;
+        int ret;
+
+        mutex_lock(&chip->io_lock);
+        pm860x_write_device(i2c, 0xFA, 0, &zero);
+        pm860x_write_device(i2c, 0xFB, 0, &zero);
+        pm860x_write_device(i2c, 0xFF, 0, &zero);
+        ret = pm860x_read_device(i2c, reg, 1, &value);
+        if (ret < 0)
+                goto out;
+        value &= ~mask;
+        value |= data;
+        ret = pm860x_write_device(i2c, reg, 1, &value);
+out:
+        pm860x_write_device(i2c, 0xFE, 0, &zero);
+        pm860x_write_device(i2c, 0xFC, 0, &zero);
+        mutex_unlock(&chip->io_lock);
+        return ret;
+}
+EXPORT_SYMBOL(pm860x_page_set_bits);
 
 static const struct i2c_device_id pm860x_id_table[] = {
         { "88PM860x", 0 },
diff --git a/drivers/mfd/Kconfig b/drivers/mfd/Kconfig
index fdca643249e1..8d7d098f7a03 100644
--- a/drivers/mfd/Kconfig
+++ b/drivers/mfd/Kconfig
@@ -129,6 +129,17 @@ config UCB1400_CORE
           To compile this driver as a module, choose M here: the
           module will be called ucb1400_core.
 
+config TPS6105X
+        tristate "TPS61050/61052 Boost Converters"
+        depends on I2C
+        select REGULATOR
+        select REGULATOR_FIXED_VOLTAGE
+        help
+          This option enables a driver for the TP61050/TPS61052
+          high-power "white LED driver". This boost converter is
+          sometimes used for other things than white LEDs, and
+          also contains a GPIO pin.
+
 config TPS65010
         tristate "TPS6501x Power Management chips"
         depends on I2C && GPIOLIB
@@ -178,6 +189,16 @@ config TWL4030_CORE
           high speed USB OTG transceiver, an audio codec (on most
           versions) and many other features.
 
+config TWL4030_MADC
+        tristate "Texas Instruments TWL4030 MADC"
+        depends on TWL4030_CORE
+        help
+        This driver provides support for triton TWL4030-MADC. The
+        driver supports both RT and SW conversion methods.
+
+        This driver can be built as a module. If so it will be
+        named twl4030-madc
+
 config TWL4030_POWER
         bool "Support power resources on TWL4030 family chips"
         depends on TWL4030_CORE && ARM
@@ -304,6 +325,18 @@ config MFD_MAX8925
           accessing the device, additional drivers must be enabled in order
           to use the functionality of the device.
 
+config MFD_MAX8997
+        bool "Maxim Semiconductor MAX8997/8966 PMIC Support"
+        depends on I2C=y && GENERIC_HARDIRQS
+        select MFD_CORE
+        help
+          Say yes here to support for Maxim Semiconductor MAX8998/8966.
+          This is a Power Management IC with RTC, Flash, Fuel Gauge, Haptic,
+          MUIC controls on chip.
+          This driver provides common support for accessing the device;
+          additional drivers must be enabled in order to use the functionality
+          of the device.
+
 config MFD_MAX8998
         bool "Maxim Semiconductor MAX8998/National LP3974 PMIC Support"
         depends on I2C=y && GENERIC_HARDIRQS
@@ -534,6 +567,13 @@ config AB8500_DEBUG
          Select this option if you want debug information using the debug
          filesystem, debugfs.
 
+config AB8500_GPADC
+        bool "AB8500 GPADC driver"
+        depends on AB8500_CORE && REGULATOR_AB8500
+        default y
+        help
+          AB8500 GPADC driver used to convert Acc and battery/ac/usb voltage
+
 config AB3550_CORE
         bool "ST-Ericsson AB3550 Mixed Signal Circuit core functions"
         select MFD_CORE
diff --git a/drivers/mfd/Makefile b/drivers/mfd/Makefile
index f0e25cad762e..47f5709f3828 100644
--- a/drivers/mfd/Makefile
+++ b/drivers/mfd/Makefile
@@ -33,11 +33,13 @@ obj-$(CONFIG_MFD_WM8350)	+= wm8350.o
 obj-$(CONFIG_MFD_WM8350_I2C)    += wm8350-i2c.o
 obj-$(CONFIG_MFD_WM8994)        += wm8994-core.o wm8994-irq.o
 
+obj-$(CONFIG_TPS6105X)          += tps6105x.o
 obj-$(CONFIG_TPS65010)          += tps65010.o
 obj-$(CONFIG_TPS6507X)          += tps6507x.o
 obj-$(CONFIG_MENELAUS)          += menelaus.o
 
 obj-$(CONFIG_TWL4030_CORE)      += twl-core.o twl4030-irq.o twl6030-irq.o
+obj-$(CONFIG_TWL4030_MADC)      += twl4030-madc.o
 obj-$(CONFIG_TWL4030_POWER)    += twl4030-power.o
 obj-$(CONFIG_TWL4030_CODEC)     += twl4030-codec.o
 obj-$(CONFIG_TWL6030_PWM)       += twl6030-pwm.o
@@ -61,6 +63,7 @@ obj-$(CONFIG_UCB1400_CORE)	+= ucb1400_core.o
 obj-$(CONFIG_PMIC_DA903X)       += da903x.o
 max8925-objs                    := max8925-core.o max8925-i2c.o
 obj-$(CONFIG_MFD_MAX8925)       += max8925.o
+obj-$(CONFIG_MFD_MAX8997)       += max8997.o
 obj-$(CONFIG_MFD_MAX8998)       += max8998.o max8998-irq.o
 
 pcf50633-objs                   := pcf50633-core.o pcf50633-irq.o
@@ -71,9 +74,10 @@ obj-$(CONFIG_ABX500_CORE)	+= abx500-core.o
 obj-$(CONFIG_AB3100_CORE)       += ab3100-core.o
 obj-$(CONFIG_AB3100_OTP)        += ab3100-otp.o
 obj-$(CONFIG_AB3550_CORE)       += ab3550-core.o
-obj-$(CONFIG_AB8500_CORE)       += ab8500-core.o
+obj-$(CONFIG_AB8500_CORE)       += ab8500-core.o ab8500-sysctrl.o
 obj-$(CONFIG_AB8500_I2C_CORE)   += ab8500-i2c.o
 obj-$(CONFIG_AB8500_DEBUG)      += ab8500-debugfs.o
+obj-$(CONFIG_AB8500_GPADC)      += ab8500-gpadc.o
 obj-$(CONFIG_MFD_TIMBERDALE)    += timberdale.o
 obj-$(CONFIG_PMIC_ADP5520)      += adp5520.o
 obj-$(CONFIG_LPC_SCH)           += lpc_sch.o
diff --git a/drivers/mfd/ab3100-core.c b/drivers/mfd/ab3100-core.c
index 4193af5f2743..a751927047ac 100644
--- a/drivers/mfd/ab3100-core.c
+++ b/drivers/mfd/ab3100-core.c
@@ -613,7 +613,7 @@ static void ab3100_setup_debugfs(struct ab3100 *ab3100)
         ab3100_get_priv.ab3100 = ab3100;
         ab3100_get_priv.mode = false;
         ab3100_get_reg_file = debugfs_create_file("get_reg",
-                                S_IWUGO, ab3100_dir, &ab3100_get_priv,
+                                S_IWUSR, ab3100_dir, &ab3100_get_priv,
                                 &ab3100_get_set_reg_fops);
         if (!ab3100_get_reg_file) {
                 err = -ENOMEM;
@@ -623,7 +623,7 @@ static void ab3100_setup_debugfs(struct ab3100 *ab3100)
         ab3100_set_priv.ab3100 = ab3100;
         ab3100_set_priv.mode = true;
         ab3100_set_reg_file = debugfs_create_file("set_reg",
-                                S_IWUGO, ab3100_dir, &ab3100_set_priv,
+                                S_IWUSR, ab3100_dir, &ab3100_set_priv,
                                 &ab3100_get_set_reg_fops);
         if (!ab3100_set_reg_file) {
                 err = -ENOMEM;
@@ -949,10 +949,8 @@ static int __devinit ab3100_probe(struct i2c_client *client,
                 goto exit_no_ops;
 
         /* Set up and register the platform devices. */
-        for (i = 0; i < ARRAY_SIZE(ab3100_devs); i++) {
-                ab3100_devs[i].platform_data = ab3100_plf_data;
-                ab3100_devs[i].data_size = sizeof(struct ab3100_platform_data);
-        }
+        for (i = 0; i < ARRAY_SIZE(ab3100_devs); i++)
+                ab3100_devs[i].mfd_data = ab3100_plf_data;
 
         err = mfd_add_devices(&client->dev, 0, ab3100_devs,
                 ARRAY_SIZE(ab3100_devs), NULL, 0);
diff --git a/drivers/mfd/ab3550-core.c b/drivers/mfd/ab3550-core.c
index 5fbca346b998..c12d04285226 100644
--- a/drivers/mfd/ab3550-core.c
+++ b/drivers/mfd/ab3550-core.c
@@ -1053,17 +1053,17 @@ static inline void ab3550_setup_debugfs(struct ab3550 *ab)
                 goto exit_destroy_dir;
 
         ab3550_bank_file = debugfs_create_file("register-bank",
-                (S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_bank_fops);
+                (S_IRUGO | S_IWUSR), ab3550_dir, ab, &ab3550_bank_fops);
         if (!ab3550_bank_file)
                 goto exit_destroy_reg;
 
         ab3550_address_file = debugfs_create_file("register-address",
-                (S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_address_fops);
+                (S_IRUGO | S_IWUSR), ab3550_dir, ab, &ab3550_address_fops);
         if (!ab3550_address_file)
                 goto exit_destroy_bank;
 
         ab3550_val_file = debugfs_create_file("register-value",
-                (S_IRUGO | S_IWUGO), ab3550_dir, ab, &ab3550_val_fops);
+                (S_IRUGO | S_IWUSR), ab3550_dir, ab, &ab3550_val_fops);
         if (!ab3550_val_file)
                 goto exit_destroy_address;
 
@@ -1320,10 +1320,8 @@ static int __init ab3550_probe(struct i2c_client *client,
                 goto exit_no_ops;
 
         /* Set up and register the platform devices. */
-        for (i = 0; i < AB3550_NUM_DEVICES; i++) {
-                ab3550_devs[i].platform_data = ab3550_plf_data->dev_data[i];
-                ab3550_devs[i].data_size = ab3550_plf_data->dev_data_sz[i];
-        }
+        for (i = 0; i < AB3550_NUM_DEVICES; i++)
+                ab3550_devs[i].mfd_data = ab3550_plf_data->dev_data[i];
 
         err = mfd_add_devices(&client->dev, 0, ab3550_devs,
                 ARRAY_SIZE(ab3550_devs), NULL,
diff --git a/drivers/mfd/ab8500-core.c b/drivers/mfd/ab8500-core.c
index b6887014d687..6e185b272d00 100644
--- a/drivers/mfd/ab8500-core.c
+++ b/drivers/mfd/ab8500-core.c
@@ -4,7 +4,7 @@
  * License Terms: GNU General Public License v2
  * Author: Srinidhi Kasagar <srinidhi.kasagar@stericsson.com>
  * Author: Rabin Vincent <rabin.vincent@stericsson.com>
- * Changes: Mattias Wallin <mattias.wallin@stericsson.com>
+ * Author: Mattias Wallin <mattias.wallin@stericsson.com>
  */
 
 #include <linux/kernel.h>
@@ -90,6 +90,7 @@
 #define AB8500_IT_MASK24_REG            0x57
 
 #define AB8500_REV_REG                  0x80
+#define AB8500_SWITCH_OFF_STATUS        0x00
 
 /*
  * Map interrupt numbers to the LATCH and MASK register offsets, Interrupt
@@ -652,10 +653,38 @@ static ssize_t show_chip_id(struct device *dev,
         return sprintf(buf, "%#x\n", ab8500 ? ab8500->chip_id : -EINVAL);
 }
 
+/*
+ * ab8500 has switched off due to (SWITCH_OFF_STATUS):
+ * 0x01 Swoff bit programming
+ * 0x02 Thermal protection activation
+ * 0x04 Vbat lower then BattOk falling threshold
+ * 0x08 Watchdog expired
+ * 0x10 Non presence of 32kHz clock
+ * 0x20 Battery level lower than power on reset threshold
+ * 0x40 Power on key 1 pressed longer than 10 seconds
+ * 0x80 DB8500 thermal shutdown
+ */
+static ssize_t show_switch_off_status(struct device *dev,
+                                struct device_attribute *attr, char *buf)
+{
+        int ret;
+        u8 value;
+        struct ab8500 *ab8500;
+
+        ab8500 = dev_get_drvdata(dev);
+        ret = get_register_interruptible(ab8500, AB8500_RTC,
+                AB8500_SWITCH_OFF_STATUS, &value);
+        if (ret < 0)
+                return ret;
+        return sprintf(buf, "%#x\n", value);
+}
+
 static DEVICE_ATTR(chip_id, S_IRUGO, show_chip_id, NULL);
+static DEVICE_ATTR(switch_off_status, S_IRUGO, show_switch_off_status, NULL);
 
 static struct attribute *ab8500_sysfs_entries[] = {
         &dev_attr_chip_id.attr,
+        &dev_attr_switch_off_status.attr,
         NULL,
 };
 
@@ -686,9 +715,10 @@ int __devinit ab8500_init(struct ab8500 *ab8500)
          * 0x10 - Cut 1.0
          * 0x11 - Cut 1.1
          * 0x20 - Cut 2.0
+         * 0x30 - Cut 3.0
          */
-        if (value == 0x0 || value == 0x10 || value == 0x11 || value == 0x20) {
-                ab8500->revision = value;
+        if (value == 0x0 || value == 0x10 || value == 0x11 || value == 0x20 ||
+                value == 0x30) {
                 dev_info(ab8500->dev, "detected chip, revision: %#x\n", value);
         } else {
                 dev_err(ab8500->dev, "unknown chip, revision: %#x\n", value);
@@ -696,6 +726,24 @@ int __devinit ab8500_init(struct ab8500 *ab8500)
         }
         ab8500->chip_id = value;
 
+        /*
+         * ab8500 has switched off due to (SWITCH_OFF_STATUS):
+         * 0x01 Swoff bit programming
+         * 0x02 Thermal protection activation
+         * 0x04 Vbat lower then BattOk falling threshold
+         * 0x08 Watchdog expired
+         * 0x10 Non presence of 32kHz clock
+         * 0x20 Battery level lower than power on reset threshold
+         * 0x40 Power on key 1 pressed longer than 10 seconds
+         * 0x80 DB8500 thermal shutdown
+         */
+
+        ret = get_register_interruptible(ab8500, AB8500_RTC,
+                AB8500_SWITCH_OFF_STATUS, &value);
+        if (ret < 0)
+                return ret;
+        dev_info(ab8500->dev, "switch off status: %#x", value);
+
         if (plat && plat->init)
                 plat->init(ab8500);
 
@@ -764,6 +812,6 @@ int __devexit ab8500_exit(struct ab8500 *ab8500)
         return 0;
 }
 
-MODULE_AUTHOR("Srinidhi Kasagar, Rabin Vincent");
+MODULE_AUTHOR("Mattias Wallin, Srinidhi Kasagar, Rabin Vincent");
 MODULE_DESCRIPTION("AB8500 MFD core");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/ab8500-debugfs.c b/drivers/mfd/ab8500-debugfs.c
index 3c1541ae7223..64748e42ac03 100644
--- a/drivers/mfd/ab8500-debugfs.c
+++ b/drivers/mfd/ab8500-debugfs.c
@@ -585,18 +585,18 @@ static int __devinit ab8500_debug_probe(struct platform_device *plf)
                 goto exit_destroy_dir;
 
         ab8500_bank_file = debugfs_create_file("register-bank",
-                (S_IRUGO | S_IWUGO), ab8500_dir, &plf->dev, &ab8500_bank_fops);
+                (S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev, &ab8500_bank_fops);
         if (!ab8500_bank_file)
                 goto exit_destroy_reg;
 
         ab8500_address_file = debugfs_create_file("register-address",
-                (S_IRUGO | S_IWUGO), ab8500_dir, &plf->dev,
+                (S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev,
                 &ab8500_address_fops);
         if (!ab8500_address_file)
                 goto exit_destroy_bank;
 
         ab8500_val_file = debugfs_create_file("register-value",
-                (S_IRUGO | S_IWUGO), ab8500_dir, &plf->dev, &ab8500_val_fops);
+                (S_IRUGO | S_IWUSR), ab8500_dir, &plf->dev, &ab8500_val_fops);
         if (!ab8500_val_file)
                 goto exit_destroy_address;
 
diff --git a/drivers/mfd/ab8500-gpadc.c b/drivers/mfd/ab8500-gpadc.c
new file mode 100644
index 000000000000..bc93b2e8230c
--- /dev/null
+++ b/drivers/mfd/ab8500-gpadc.c
@@ -0,0 +1,614 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ *
+ * License Terms: GNU General Public License v2
+ * Author: Arun R Murthy <arun.murthy@stericsson.com>
+ * Author: Daniel Willerud <daniel.willerud@stericsson.com>
+ * Author: Johan Palsson <johan.palsson@stericsson.com>
+ */
+#include <linux/init.h>
+#include <linux/module.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/spinlock.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/completion.h>
+#include <linux/regulator/consumer.h>
+#include <linux/err.h>
+#include <linux/slab.h>
+#include <linux/list.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500/gpadc.h>
+
+/*
+ * GPADC register offsets
+ * Bank : 0x0A
+ */
+#define AB8500_GPADC_CTRL1_REG          0x00
+#define AB8500_GPADC_CTRL2_REG          0x01
+#define AB8500_GPADC_CTRL3_REG          0x02
+#define AB8500_GPADC_AUTO_TIMER_REG     0x03
+#define AB8500_GPADC_STAT_REG           0x04
+#define AB8500_GPADC_MANDATAL_REG       0x05
+#define AB8500_GPADC_MANDATAH_REG       0x06
+#define AB8500_GPADC_AUTODATAL_REG      0x07
+#define AB8500_GPADC_AUTODATAH_REG      0x08
+#define AB8500_GPADC_MUX_CTRL_REG       0x09
+
+/*
+ * OTP register offsets
+ * Bank : 0x15
+ */
+#define AB8500_GPADC_CAL_1              0x0F
+#define AB8500_GPADC_CAL_2              0x10
+#define AB8500_GPADC_CAL_3              0x11
+#define AB8500_GPADC_CAL_4              0x12
+#define AB8500_GPADC_CAL_5              0x13
+#define AB8500_GPADC_CAL_6              0x14
+#define AB8500_GPADC_CAL_7              0x15
+
+/* gpadc constants */
+#define EN_VINTCORE12                   0x04
+#define EN_VTVOUT                       0x02
+#define EN_GPADC                        0x01
+#define DIS_GPADC                       0x00
+#define SW_AVG_16                       0x60
+#define ADC_SW_CONV                     0x04
+#define EN_ICHAR                        0x80
+#define EN_BUF                          0x40
+#define DIS_ZERO                        0x00
+#define GPADC_BUSY                      0x01
+
+/* GPADC constants from AB8500 spec, UM0836 */
+#define ADC_RESOLUTION                  1024
+#define ADC_CH_BTEMP_MIN                0
+#define ADC_CH_BTEMP_MAX                1350
+#define ADC_CH_DIETEMP_MIN              0
+#define ADC_CH_DIETEMP_MAX              1350
+#define ADC_CH_CHG_V_MIN                0
+#define ADC_CH_CHG_V_MAX                20030
+#define ADC_CH_ACCDET2_MIN              0
+#define ADC_CH_ACCDET2_MAX              2500
+#define ADC_CH_VBAT_MIN                 2300
+#define ADC_CH_VBAT_MAX                 4800
+#define ADC_CH_CHG_I_MIN                0
+#define ADC_CH_CHG_I_MAX                1500
+#define ADC_CH_BKBAT_MIN                0
+#define ADC_CH_BKBAT_MAX                3200
+
+/* This is used to not lose precision when dividing to get gain and offset */
+#define CALIB_SCALE                     1000
+
+enum cal_channels {
+        ADC_INPUT_VMAIN = 0,
+        ADC_INPUT_BTEMP,
+        ADC_INPUT_VBAT,
+        NBR_CAL_INPUTS,
+};
+
+/**
+ * struct adc_cal_data - Table for storing gain and offset for the calibrated
+ * ADC channels
+ * @gain:               Gain of the ADC channel
+ * @offset:             Offset of the ADC channel
+ */
+struct adc_cal_data {
+        u64 gain;
+        u64 offset;
+};
+
+/**
+ * struct ab8500_gpadc - AB8500 GPADC device information
+ * @dev:                        pointer to the struct device
+ * @node:                       a list of AB8500 GPADCs, hence prepared for
+                                reentrance
+ * @ab8500_gpadc_complete:      pointer to the struct completion, to indicate
+ *                              the completion of gpadc conversion
+ * @ab8500_gpadc_lock:          structure of type mutex
+ * @regu:                       pointer to the struct regulator
+ * @irq:                        interrupt number that is used by gpadc
+ * @cal_data                    array of ADC calibration data structs
+ */
+struct ab8500_gpadc {
+        struct device *dev;
+        struct list_head node;
+        struct completion ab8500_gpadc_complete;
+        struct mutex ab8500_gpadc_lock;
+        struct regulator *regu;
+        int irq;
+        struct adc_cal_data cal_data[NBR_CAL_INPUTS];
+};
+
+static LIST_HEAD(ab8500_gpadc_list);
+
+/**
+ * ab8500_gpadc_get() - returns a reference to the primary AB8500 GPADC
+ * (i.e. the first GPADC in the instance list)
+ */
+struct ab8500_gpadc *ab8500_gpadc_get(char *name)
+{
+        struct ab8500_gpadc *gpadc;
+
+        list_for_each_entry(gpadc, &ab8500_gpadc_list, node) {
+                if (!strcmp(name, dev_name(gpadc->dev)))
+                    return gpadc;
+        }
+
+        return ERR_PTR(-ENOENT);
+}
+EXPORT_SYMBOL(ab8500_gpadc_get);
+
+static int ab8500_gpadc_ad_to_voltage(struct ab8500_gpadc *gpadc, u8 input,
+        int ad_value)
+{
+        int res;
+
+        switch (input) {
+        case MAIN_CHARGER_V:
+                /* For some reason we don't have calibrated data */
+                if (!gpadc->cal_data[ADC_INPUT_VMAIN].gain) {
+                        res = ADC_CH_CHG_V_MIN + (ADC_CH_CHG_V_MAX -
+                                ADC_CH_CHG_V_MIN) * ad_value /
+                                ADC_RESOLUTION;
+                        break;
+                }
+                /* Here we can use the calibrated data */
+                res = (int) (ad_value * gpadc->cal_data[ADC_INPUT_VMAIN].gain +
+                        gpadc->cal_data[ADC_INPUT_VMAIN].offset) / CALIB_SCALE;
+                break;
+
+        case BAT_CTRL:
+        case BTEMP_BALL:
+        case ACC_DETECT1:
+        case ADC_AUX1:
+        case ADC_AUX2:
+                /* For some reason we don't have calibrated data */
+                if (!gpadc->cal_data[ADC_INPUT_BTEMP].gain) {
+                        res = ADC_CH_BTEMP_MIN + (ADC_CH_BTEMP_MAX -
+                                ADC_CH_BTEMP_MIN) * ad_value /
+                                ADC_RESOLUTION;
+                        break;
+                }
+                /* Here we can use the calibrated data */
+                res = (int) (ad_value * gpadc->cal_data[ADC_INPUT_BTEMP].gain +
+                        gpadc->cal_data[ADC_INPUT_BTEMP].offset) / CALIB_SCALE;
+                break;
+
+        case MAIN_BAT_V:
+                /* For some reason we don't have calibrated data */
+                if (!gpadc->cal_data[ADC_INPUT_VBAT].gain) {
+                        res = ADC_CH_VBAT_MIN + (ADC_CH_VBAT_MAX -
+                                ADC_CH_VBAT_MIN) * ad_value /
+                                ADC_RESOLUTION;
+                        break;
+                }
+                /* Here we can use the calibrated data */
+                res = (int) (ad_value * gpadc->cal_data[ADC_INPUT_VBAT].gain +
+                        gpadc->cal_data[ADC_INPUT_VBAT].offset) / CALIB_SCALE;
+                break;
+
+        case DIE_TEMP:
+                res = ADC_CH_DIETEMP_MIN +
+                        (ADC_CH_DIETEMP_MAX - ADC_CH_DIETEMP_MIN) * ad_value /
+                        ADC_RESOLUTION;
+                break;
+
+        case ACC_DETECT2:
+                res = ADC_CH_ACCDET2_MIN +
+                        (ADC_CH_ACCDET2_MAX - ADC_CH_ACCDET2_MIN) * ad_value /
+                        ADC_RESOLUTION;
+                break;
+
+        case VBUS_V:
+                res = ADC_CH_CHG_V_MIN +
+                        (ADC_CH_CHG_V_MAX - ADC_CH_CHG_V_MIN) * ad_value /
+                        ADC_RESOLUTION;
+                break;
+
+        case MAIN_CHARGER_C:
+        case USB_CHARGER_C:
+                res = ADC_CH_CHG_I_MIN +
+                        (ADC_CH_CHG_I_MAX - ADC_CH_CHG_I_MIN) * ad_value /
+                        ADC_RESOLUTION;
+                break;
+
+        case BK_BAT_V:
+                res = ADC_CH_BKBAT_MIN +
+                        (ADC_CH_BKBAT_MAX - ADC_CH_BKBAT_MIN) * ad_value /
+                        ADC_RESOLUTION;
+                break;
+
+        default:
+                dev_err(gpadc->dev,
+                        "unknown channel, not possible to convert\n");
+                res = -EINVAL;
+                break;
+
+        }
+        return res;
+}
+
+/**
+ * ab8500_gpadc_convert() - gpadc conversion
+ * @input:      analog input to be converted to digital data
+ *
+ * This function converts the selected analog i/p to digital
+ * data.
+ */
+int ab8500_gpadc_convert(struct ab8500_gpadc *gpadc, u8 input)
+{
+        int ret;
+        u16 data = 0;
+        int looplimit = 0;
+        u8 val, low_data, high_data;
+
+        if (!gpadc)
+                return -ENODEV;
+
+        mutex_lock(&gpadc->ab8500_gpadc_lock);
+        /* Enable VTVout LDO this is required for GPADC */
+        regulator_enable(gpadc->regu);
+
+        /* Check if ADC is not busy, lock and proceed */
+        do {
+                ret = abx500_get_register_interruptible(gpadc->dev,
+                        AB8500_GPADC, AB8500_GPADC_STAT_REG, &val);
+                if (ret < 0)
+                        goto out;
+                if (!(val & GPADC_BUSY))
+                        break;
+                msleep(10);
+        } while (++looplimit < 10);
+        if (looplimit >= 10 && (val & GPADC_BUSY)) {
+                dev_err(gpadc->dev, "gpadc_conversion: GPADC busy");
+                ret = -EINVAL;
+                goto out;
+        }
+
+        /* Enable GPADC */
+        ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+                AB8500_GPADC, AB8500_GPADC_CTRL1_REG, EN_GPADC, EN_GPADC);
+        if (ret < 0) {
+                dev_err(gpadc->dev, "gpadc_conversion: enable gpadc failed\n");
+                goto out;
+        }
+        /* Select the input source and set average samples to 16 */
+        ret = abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
+                AB8500_GPADC_CTRL2_REG, (input | SW_AVG_16));
+        if (ret < 0) {
+                dev_err(gpadc->dev,
+                        "gpadc_conversion: set avg samples failed\n");
+                goto out;
+        }
+        /*
+         * Enable ADC, buffering, select rising edge and enable ADC path
+         * charging current sense if it needed
+         */
+        switch (input) {
+        case MAIN_CHARGER_C:
+        case USB_CHARGER_C:
+                ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+                        AB8500_GPADC, AB8500_GPADC_CTRL1_REG,
+                        EN_BUF | EN_ICHAR,
+                        EN_BUF | EN_ICHAR);
+                break;
+        default:
+                ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+                        AB8500_GPADC, AB8500_GPADC_CTRL1_REG, EN_BUF, EN_BUF);
+                break;
+        }
+        if (ret < 0) {
+                dev_err(gpadc->dev,
+                        "gpadc_conversion: select falling edge failed\n");
+                goto out;
+        }
+        ret = abx500_mask_and_set_register_interruptible(gpadc->dev,
+                AB8500_GPADC, AB8500_GPADC_CTRL1_REG, ADC_SW_CONV, ADC_SW_CONV);
+        if (ret < 0) {
+                dev_err(gpadc->dev,
+                        "gpadc_conversion: start s/w conversion failed\n");
+                goto out;
+        }
+        /* wait for completion of conversion */
+        if (!wait_for_completion_timeout(&gpadc->ab8500_gpadc_complete, 2*HZ)) {
+                dev_err(gpadc->dev,
+                        "timeout: didnt recieve GPADC conversion interrupt\n");
+                ret = -EINVAL;
+                goto out;
+        }
+
+        /* Read the converted RAW data */
+        ret = abx500_get_register_interruptible(gpadc->dev, AB8500_GPADC,
+                AB8500_GPADC_MANDATAL_REG, &low_data);
+        if (ret < 0) {
+                dev_err(gpadc->dev, "gpadc_conversion: read low data failed\n");
+                goto out;
+        }
+
+        ret = abx500_get_register_interruptible(gpadc->dev, AB8500_GPADC,
+                AB8500_GPADC_MANDATAH_REG, &high_data);
+        if (ret < 0) {
+                dev_err(gpadc->dev,
+                        "gpadc_conversion: read high data failed\n");
+                goto out;
+        }
+
+        data = (high_data << 8) | low_data;
+        /* Disable GPADC */
+        ret = abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
+                AB8500_GPADC_CTRL1_REG, DIS_GPADC);
+        if (ret < 0) {
+                dev_err(gpadc->dev, "gpadc_conversion: disable gpadc failed\n");
+                goto out;
+        }
+        /* Disable VTVout LDO this is required for GPADC */
+        regulator_disable(gpadc->regu);
+        mutex_unlock(&gpadc->ab8500_gpadc_lock);
+        ret = ab8500_gpadc_ad_to_voltage(gpadc, input, data);
+        return ret;
+
+out:
+        /*
+         * It has shown to be needed to turn off the GPADC if an error occurs,
+         * otherwise we might have problem when waiting for the busy bit in the
+         * GPADC status register to go low. In V1.1 there wait_for_completion
+         * seems to timeout when waiting for an interrupt.. Not seen in V2.0
+         */
+        (void) abx500_set_register_interruptible(gpadc->dev, AB8500_GPADC,
+                AB8500_GPADC_CTRL1_REG, DIS_GPADC);
+        regulator_disable(gpadc->regu);
+        mutex_unlock(&gpadc->ab8500_gpadc_lock);
+        dev_err(gpadc->dev,
+                "gpadc_conversion: Failed to AD convert channel %d\n", input);
+        return ret;
+}
+EXPORT_SYMBOL(ab8500_gpadc_convert);
+
+/**
+ * ab8500_bm_gpswadcconvend_handler() - isr for s/w gpadc conversion completion
+ * @irq:        irq number
+ * @data:       pointer to the data passed during request irq
+ *
+ * This is a interrupt service routine for s/w gpadc conversion completion.
+ * Notifies the gpadc completion is completed and the converted raw value
+ * can be read from the registers.
+ * Returns IRQ status(IRQ_HANDLED)
+ */
+static irqreturn_t ab8500_bm_gpswadcconvend_handler(int irq, void *_gpadc)
+{
+        struct ab8500_gpadc *gpadc = _gpadc;
+
+        complete(&gpadc->ab8500_gpadc_complete);
+
+        return IRQ_HANDLED;
+}
+
+static int otp_cal_regs[] = {
+        AB8500_GPADC_CAL_1,
+        AB8500_GPADC_CAL_2,
+        AB8500_GPADC_CAL_3,
+        AB8500_GPADC_CAL_4,
+        AB8500_GPADC_CAL_5,
+        AB8500_GPADC_CAL_6,
+        AB8500_GPADC_CAL_7,
+};
+
+static void ab8500_gpadc_read_calibration_data(struct ab8500_gpadc *gpadc)
+{
+        int i;
+        int ret[ARRAY_SIZE(otp_cal_regs)];
+        u8 gpadc_cal[ARRAY_SIZE(otp_cal_regs)];
+
+        int vmain_high, vmain_low;
+        int btemp_high, btemp_low;
+        int vbat_high, vbat_low;
+
+        /* First we read all OTP registers and store the error code */
+        for (i = 0; i < ARRAY_SIZE(otp_cal_regs); i++) {
+                ret[i] = abx500_get_register_interruptible(gpadc->dev,
+                        AB8500_OTP_EMUL, otp_cal_regs[i],  &gpadc_cal[i]);
+                if (ret[i] < 0)
+                        dev_err(gpadc->dev, "%s: read otp reg 0x%02x failed\n",
+                                __func__, otp_cal_regs[i]);
+        }
+
+        /*
+         * The ADC calibration data is stored in OTP registers.
+         * The layout of the calibration data is outlined below and a more
+         * detailed description can be found in UM0836
+         *
+         * vm_h/l = vmain_high/low
+         * bt_h/l = btemp_high/low
+         * vb_h/l = vbat_high/low
+         *
+         * Data bits:
+         * | 7     | 6     | 5     | 4     | 3     | 2     | 1     | 0
+         * |.......|.......|.......|.......|.......|.......|.......|.......
+         * |                                               | vm_h9 | vm_h8
+         * |.......|.......|.......|.......|.......|.......|.......|.......
+         * |               | vm_h7 | vm_h6 | vm_h5 | vm_h4 | vm_h3 | vm_h2
+         * |.......|.......|.......|.......|.......|.......|.......|.......
+         * | vm_h1 | vm_h0 | vm_l4 | vm_l3 | vm_l2 | vm_l1 | vm_l0 | bt_h9
+         * |.......|.......|.......|.......|.......|.......|.......|.......
+         * | bt_h8 | bt_h7 | bt_h6 | bt_h5 | bt_h4 | bt_h3 | bt_h2 | bt_h1
+         * |.......|.......|.......|.......|.......|.......|.......|.......
+         * | bt_h0 | bt_l4 | bt_l3 | bt_l2 | bt_l1 | bt_l0 | vb_h9 | vb_h8
+         * |.......|.......|.......|.......|.......|.......|.......|.......
+         * | vb_h7 | vb_h6 | vb_h5 | vb_h4 | vb_h3 | vb_h2 | vb_h1 | vb_h0
+         * |.......|.......|.......|.......|.......|.......|.......|.......
+         * | vb_l5 | vb_l4 | vb_l3 | vb_l2 | vb_l1 | vb_l0 |
+         * |.......|.......|.......|.......|.......|.......|.......|.......
+         *
+         *
+         * Ideal output ADC codes corresponding to injected input voltages
+         * during manufacturing is:
+         *
+         * vmain_high: Vin = 19500mV / ADC ideal code = 997
+         * vmain_low:  Vin = 315mV   / ADC ideal code = 16
+         * btemp_high: Vin = 1300mV  / ADC ideal code = 985
+         * btemp_low:  Vin = 21mV    / ADC ideal code = 16
+         * vbat_high:  Vin = 4700mV  / ADC ideal code = 982
+         * vbat_low:   Vin = 2380mV  / ADC ideal code = 33
+         */
+
+        /* Calculate gain and offset for VMAIN if all reads succeeded */
+        if (!(ret[0] < 0 || ret[1] < 0 || ret[2] < 0)) {
+                vmain_high = (((gpadc_cal[0] & 0x03) << 8) |
+                        ((gpadc_cal[1] & 0x3F) << 2) |
+                        ((gpadc_cal[2] & 0xC0) >> 6));
+
+                vmain_low = ((gpadc_cal[2] & 0x3E) >> 1);
+
+                gpadc->cal_data[ADC_INPUT_VMAIN].gain = CALIB_SCALE *
+                        (19500 - 315) / (vmain_high - vmain_low);
+
+                gpadc->cal_data[ADC_INPUT_VMAIN].offset = CALIB_SCALE * 19500 -
+                        (CALIB_SCALE * (19500 - 315) /
+                         (vmain_high - vmain_low)) * vmain_high;
+        } else {
+                gpadc->cal_data[ADC_INPUT_VMAIN].gain = 0;
+        }
+
+        /* Calculate gain and offset for BTEMP if all reads succeeded */
+        if (!(ret[2] < 0 || ret[3] < 0 || ret[4] < 0)) {
+                btemp_high = (((gpadc_cal[2] & 0x01) << 9) |
+                        (gpadc_cal[3] << 1) |
+                        ((gpadc_cal[4] & 0x80) >> 7));
+
+                btemp_low = ((gpadc_cal[4] & 0x7C) >> 2);
+
+                gpadc->cal_data[ADC_INPUT_BTEMP].gain =
+                        CALIB_SCALE * (1300 - 21) / (btemp_high - btemp_low);
+
+                gpadc->cal_data[ADC_INPUT_BTEMP].offset = CALIB_SCALE * 1300 -
+                        (CALIB_SCALE * (1300 - 21) /
+                        (btemp_high - btemp_low)) * btemp_high;
+        } else {
+                gpadc->cal_data[ADC_INPUT_BTEMP].gain = 0;
+        }
+
+        /* Calculate gain and offset for VBAT if all reads succeeded */
+        if (!(ret[4] < 0 || ret[5] < 0 || ret[6] < 0)) {
+                vbat_high = (((gpadc_cal[4] & 0x03) << 8) | gpadc_cal[5]);
+                vbat_low = ((gpadc_cal[6] & 0xFC) >> 2);
+
+                gpadc->cal_data[ADC_INPUT_VBAT].gain = CALIB_SCALE *
+                        (4700 - 2380) / (vbat_high - vbat_low);
+
+                gpadc->cal_data[ADC_INPUT_VBAT].offset = CALIB_SCALE * 4700 -
+                        (CALIB_SCALE * (4700 - 2380) /
+                        (vbat_high - vbat_low)) * vbat_high;
+        } else {
+                gpadc->cal_data[ADC_INPUT_VBAT].gain = 0;
+        }
+
+        dev_dbg(gpadc->dev, "VMAIN gain %llu offset %llu\n",
+                gpadc->cal_data[ADC_INPUT_VMAIN].gain,
+                gpadc->cal_data[ADC_INPUT_VMAIN].offset);
+
+        dev_dbg(gpadc->dev, "BTEMP gain %llu offset %llu\n",
+                gpadc->cal_data[ADC_INPUT_BTEMP].gain,
+                gpadc->cal_data[ADC_INPUT_BTEMP].offset);
+
+        dev_dbg(gpadc->dev, "VBAT gain %llu offset %llu\n",
+                gpadc->cal_data[ADC_INPUT_VBAT].gain,
+                gpadc->cal_data[ADC_INPUT_VBAT].offset);
+}
+
+static int __devinit ab8500_gpadc_probe(struct platform_device *pdev)
+{
+        int ret = 0;
+        struct ab8500_gpadc *gpadc;
+
+        gpadc = kzalloc(sizeof(struct ab8500_gpadc), GFP_KERNEL);
+        if (!gpadc) {
+                dev_err(&pdev->dev, "Error: No memory\n");
+                return -ENOMEM;
+        }
+
+        gpadc->irq = platform_get_irq_byname(pdev, "SW_CONV_END");
+        if (gpadc->irq < 0) {
+                dev_err(gpadc->dev, "failed to get platform irq-%d\n",
+                        gpadc->irq);
+                ret = gpadc->irq;
+                goto fail;
+        }
+
+        gpadc->dev = &pdev->dev;
+        mutex_init(&gpadc->ab8500_gpadc_lock);
+
+        /* Initialize completion used to notify completion of conversion */
+        init_completion(&gpadc->ab8500_gpadc_complete);
+
+        /* Register interrupt  - SwAdcComplete */
+        ret = request_threaded_irq(gpadc->irq, NULL,
+                ab8500_bm_gpswadcconvend_handler,
+                IRQF_NO_SUSPEND | IRQF_SHARED, "ab8500-gpadc", gpadc);
+        if (ret < 0) {
+                dev_err(gpadc->dev, "Failed to register interrupt, irq: %d\n",
+                        gpadc->irq);
+                goto fail;
+        }
+
+        /* VTVout LDO used to power up ab8500-GPADC */
+        gpadc->regu = regulator_get(&pdev->dev, "vddadc");
+        if (IS_ERR(gpadc->regu)) {
+                ret = PTR_ERR(gpadc->regu);
+                dev_err(gpadc->dev, "failed to get vtvout LDO\n");
+                goto fail_irq;
+        }
+        ab8500_gpadc_read_calibration_data(gpadc);
+        list_add_tail(&gpadc->node, &ab8500_gpadc_list);
+        dev_dbg(gpadc->dev, "probe success\n");
+        return 0;
+fail_irq:
+        free_irq(gpadc->irq, gpadc);
+fail:
+        kfree(gpadc);
+        gpadc = NULL;
+        return ret;
+}
+
+static int __devexit ab8500_gpadc_remove(struct platform_device *pdev)
+{
+        struct ab8500_gpadc *gpadc = platform_get_drvdata(pdev);
+
+        /* remove this gpadc entry from the list */
+        list_del(&gpadc->node);
+        /* remove interrupt  - completion of Sw ADC conversion */
+        free_irq(gpadc->irq, gpadc);
+        /* disable VTVout LDO that is being used by GPADC */
+        regulator_put(gpadc->regu);
+        kfree(gpadc);
+        gpadc = NULL;
+        return 0;
+}
+
+static struct platform_driver ab8500_gpadc_driver = {
+        .probe = ab8500_gpadc_probe,
+        .remove = __devexit_p(ab8500_gpadc_remove),
+        .driver = {
+                .name = "ab8500-gpadc",
+                .owner = THIS_MODULE,
+        },
+};
+
+static int __init ab8500_gpadc_init(void)
+{
+        return platform_driver_register(&ab8500_gpadc_driver);
+}
+
+static void __exit ab8500_gpadc_exit(void)
+{
+        platform_driver_unregister(&ab8500_gpadc_driver);
+}
+
+subsys_initcall_sync(ab8500_gpadc_init);
+module_exit(ab8500_gpadc_exit);
+
+MODULE_LICENSE("GPL v2");
+MODULE_AUTHOR("Arun R Murthy, Daniel Willerud, Johan Palsson");
+MODULE_ALIAS("platform:ab8500_gpadc");
+MODULE_DESCRIPTION("AB8500 GPADC driver");
diff --git a/drivers/mfd/ab8500-sysctrl.c b/drivers/mfd/ab8500-sysctrl.c
new file mode 100644
index 000000000000..392185965b39
--- /dev/null
+++ b/drivers/mfd/ab8500-sysctrl.c
@@ -0,0 +1,80 @@
+/*
+ * Copyright (C) ST-Ericsson SA 2010
+ * Author: Mattias Nilsson <mattias.i.nilsson@stericsson.com> for ST Ericsson.
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/err.h>
+#include <linux/platform_device.h>
+#include <linux/mfd/ab8500.h>
+#include <linux/mfd/abx500.h>
+#include <linux/mfd/ab8500/sysctrl.h>
+
+static struct device *sysctrl_dev;
+
+static inline bool valid_bank(u8 bank)
+{
+        return ((bank == AB8500_SYS_CTRL1_BLOCK) ||
+                (bank == AB8500_SYS_CTRL2_BLOCK));
+}
+
+int ab8500_sysctrl_read(u16 reg, u8 *value)
+{
+        u8 bank;
+
+        if (sysctrl_dev == NULL)
+                return -EAGAIN;
+
+        bank = (reg >> 8);
+        if (!valid_bank(bank))
+                return -EINVAL;
+
+        return abx500_get_register_interruptible(sysctrl_dev, bank,
+                (u8)(reg & 0xFF), value);
+}
+
+int ab8500_sysctrl_write(u16 reg, u8 mask, u8 value)
+{
+        u8 bank;
+
+        if (sysctrl_dev == NULL)
+                return -EAGAIN;
+
+        bank = (reg >> 8);
+        if (!valid_bank(bank))
+                return -EINVAL;
+
+        return abx500_mask_and_set_register_interruptible(sysctrl_dev, bank,
+                (u8)(reg & 0xFF), mask, value);
+}
+
+static int __devinit ab8500_sysctrl_probe(struct platform_device *pdev)
+{
+        sysctrl_dev = &pdev->dev;
+        return 0;
+}
+
+static int __devexit ab8500_sysctrl_remove(struct platform_device *pdev)
+{
+        sysctrl_dev = NULL;
+        return 0;
+}
+
+static struct platform_driver ab8500_sysctrl_driver = {
+        .driver = {
+                .name = "ab8500-sysctrl",
+                .owner = THIS_MODULE,
+        },
+        .probe = ab8500_sysctrl_probe,
+        .remove = __devexit_p(ab8500_sysctrl_remove),
+};
+
+static int __init ab8500_sysctrl_init(void)
+{
+        return platform_driver_register(&ab8500_sysctrl_driver);
+}
+subsys_initcall(ab8500_sysctrl_init);
+
+MODULE_AUTHOR("Mattias Nilsson <mattias.i.nilsson@stericsson.com");
+MODULE_DESCRIPTION("AB8500 system control driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/adp5520.c b/drivers/mfd/adp5520.c
index 3122139b4300..f1d88483112c 100644
--- a/drivers/mfd/adp5520.c
+++ b/drivers/mfd/adp5520.c
@@ -321,27 +321,27 @@ static int __devexit adp5520_remove(struct i2c_client *client)
 }
 
 #ifdef CONFIG_PM
-static int adp5520_suspend(struct i2c_client *client,
-                                 pm_message_t state)
+static int adp5520_suspend(struct device *dev)
 {
+        struct i2c_client *client = to_i2c_client(dev);
         struct adp5520_chip *chip = dev_get_drvdata(&client->dev);
 
         adp5520_clr_bits(chip->dev, ADP5520_MODE_STATUS, ADP5520_nSTNBY);
         return 0;
 }
 
-static int adp5520_resume(struct i2c_client *client)
+static int adp5520_resume(struct device *dev)
 {
+        struct i2c_client *client = to_i2c_client(dev);
         struct adp5520_chip *chip = dev_get_drvdata(&client->dev);
 
         adp5520_set_bits(chip->dev, ADP5520_MODE_STATUS, ADP5520_nSTNBY);
         return 0;
 }
-#else
-#define adp5520_suspend NULL
-#define adp5520_resume  NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(adp5520_pm, adp5520_suspend, adp5520_resume);
+
 static const struct i2c_device_id adp5520_id[] = {
         { "pmic-adp5520", ID_ADP5520 },
         { "pmic-adp5501", ID_ADP5501 },
@@ -353,11 +353,10 @@ static struct i2c_driver adp5520_driver = {
         .driver = {
                 .name   = "adp5520",
                 .owner  = THIS_MODULE,
+                .pm     = &adp5520_pm,
         },
         .probe          = adp5520_probe,
         .remove         = __devexit_p(adp5520_remove),
-        .suspend        = adp5520_suspend,
-        .resume         = adp5520_resume,
         .id_table       = adp5520_id,
 };
 
diff --git a/drivers/mfd/asic3.c b/drivers/mfd/asic3.c
index c45e6305b26f..0241f08fc00d 100644
--- a/drivers/mfd/asic3.c
+++ b/drivers/mfd/asic3.c
@@ -682,7 +682,7 @@ static struct mfd_cell asic3_cell_ds1wm = {
         .name          = "ds1wm",
         .enable        = ds1wm_enable,
         .disable       = ds1wm_disable,
-        .driver_data   = &ds1wm_pdata,
+        .mfd_data      = &ds1wm_pdata,
         .num_resources = ARRAY_SIZE(ds1wm_resources),
         .resources     = ds1wm_resources,
 };
@@ -783,7 +783,7 @@ static struct mfd_cell asic3_cell_mmc = {
         .name          = "tmio-mmc",
         .enable        = asic3_mmc_enable,
         .disable       = asic3_mmc_disable,
-        .driver_data   = &asic3_mmc_data,
+        .mfd_data      = &asic3_mmc_data,
         .num_resources = ARRAY_SIZE(asic3_mmc_resources),
         .resources     = asic3_mmc_resources,
 };
@@ -810,9 +810,6 @@ static int __init asic3_mfd_probe(struct platform_device *pdev,
         ds1wm_resources[0].start >>= asic->bus_shift;
         ds1wm_resources[0].end   >>= asic->bus_shift;
 
-        asic3_cell_ds1wm.platform_data = &asic3_cell_ds1wm;
-        asic3_cell_ds1wm.data_size = sizeof(asic3_cell_ds1wm);
-
         /* MMC */
         asic->tmio_cnf = ioremap((ASIC3_SD_CONFIG_BASE >> asic->bus_shift) +
                                  mem_sdio->start, 0x400 >> asic->bus_shift);
@@ -824,9 +821,6 @@ static int __init asic3_mfd_probe(struct platform_device *pdev,
         asic3_mmc_resources[0].start >>= asic->bus_shift;
         asic3_mmc_resources[0].end   >>= asic->bus_shift;
 
-        asic3_cell_mmc.platform_data = &asic3_cell_mmc;
-        asic3_cell_mmc.data_size = sizeof(asic3_cell_mmc);
-
         ret = mfd_add_devices(&pdev->dev, pdev->id,
                         &asic3_cell_ds1wm, 1, mem, asic->irq_base);
         if (ret < 0)
diff --git a/drivers/mfd/cs5535-mfd.c b/drivers/mfd/cs5535-mfd.c
index 59ca6f151e78..886a06871065 100644
--- a/drivers/mfd/cs5535-mfd.c
+++ b/drivers/mfd/cs5535-mfd.c
@@ -39,6 +39,37 @@ enum cs5535_mfd_bars {
         NR_BARS,
 };
 
+static int cs5535_mfd_res_enable(struct platform_device *pdev)
+{
+        struct resource *res;
+
+        res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+        if (!res) {
+                dev_err(&pdev->dev, "can't fetch device resource info\n");
+                return -EIO;
+        }
+
+        if (!request_region(res->start, resource_size(res), DRV_NAME)) {
+                dev_err(&pdev->dev, "can't request region\n");
+                return -EIO;
+        }
+
+        return 0;
+}
+
+static int cs5535_mfd_res_disable(struct platform_device *pdev)
+{
+        struct resource *res;
+        res = platform_get_resource(pdev, IORESOURCE_IO, 0);
+        if (!res) {
+                dev_err(&pdev->dev, "can't fetch device resource info\n");
+                return -EIO;
+        }
+
+        release_region(res->start, resource_size(res));
+        return 0;
+}
+
 static __devinitdata struct resource cs5535_mfd_resources[NR_BARS];
 
 static __devinitdata struct mfd_cell cs5535_mfd_cells[] = {
@@ -65,12 +96,18 @@ static __devinitdata struct mfd_cell cs5535_mfd_cells[] = {
                 .name = "cs5535-pms",
                 .num_resources = 1,
                 .resources = &cs5535_mfd_resources[PMS_BAR],
+
+                .enable = cs5535_mfd_res_enable,
+                .disable = cs5535_mfd_res_disable,
         },
         {
                 .id = ACPI_BAR,
                 .name = "cs5535-acpi",
                 .num_resources = 1,
                 .resources = &cs5535_mfd_resources[ACPI_BAR],
+
+                .enable = cs5535_mfd_res_enable,
+                .disable = cs5535_mfd_res_disable,
         },
 };
 
diff --git a/drivers/mfd/davinci_voicecodec.c b/drivers/mfd/davinci_voicecodec.c
index fdd8a1b8bc67..414783b04849 100644
--- a/drivers/mfd/davinci_voicecodec.c
+++ b/drivers/mfd/davinci_voicecodec.c
@@ -119,12 +119,12 @@ static int __init davinci_vc_probe(struct platform_device *pdev)
         /* Voice codec interface client */
         cell = &davinci_vc->cells[DAVINCI_VC_VCIF_CELL];
         cell->name = "davinci-vcif";
-        cell->driver_data = davinci_vc;
+        cell->mfd_data = davinci_vc;
 
         /* Voice codec CQ93VC client */
         cell = &davinci_vc->cells[DAVINCI_VC_CQ93VC_CELL];
         cell->name = "cq93vc-codec";
-        cell->driver_data = davinci_vc;
+        cell->mfd_data = davinci_vc;
 
         ret = mfd_add_devices(&pdev->dev, pdev->id, davinci_vc->cells,
                               DAVINCI_VC_CELLS, NULL, 0);
diff --git a/drivers/mfd/htc-pasic3.c b/drivers/mfd/htc-pasic3.c
index 7bc752272dc1..fb9770b39a32 100644
--- a/drivers/mfd/htc-pasic3.c
+++ b/drivers/mfd/htc-pasic3.c
@@ -117,7 +117,7 @@ static struct mfd_cell ds1wm_cell __initdata = {
         .name          = "ds1wm",
         .enable        = ds1wm_enable,
         .disable       = ds1wm_disable,
-        .driver_data   = &ds1wm_pdata,
+        .mfd_data      = &ds1wm_pdata,
         .num_resources = 2,
         .resources     = ds1wm_resources,
 };
@@ -165,8 +165,6 @@ static int __init pasic3_probe(struct platform_device *pdev)
                 ds1wm_pdata.clock_rate = pdata->clock_rate;
                 /* the first 5 PASIC3 registers control the DS1WM */
                 ds1wm_resources[0].end = (5 << asic->bus_shift) - 1;
-                ds1wm_cell.platform_data = &ds1wm_cell;
-                ds1wm_cell.data_size = sizeof(ds1wm_cell);
                 ret = mfd_add_devices(&pdev->dev, pdev->id,
                                 &ds1wm_cell, 1, r, irq);
                 if (ret < 0)
@@ -174,9 +172,6 @@ static int __init pasic3_probe(struct platform_device *pdev)
         }
 
         if (pdata && pdata->led_pdata) {
-                led_cell.driver_data = pdata->led_pdata;
-                led_cell.platform_data = &led_cell;
-                led_cell.data_size = sizeof(ds1wm_cell);
                 ret = mfd_add_devices(&pdev->dev, pdev->id, &led_cell, 1, r, 0);
                 if (ret < 0)
                         dev_warn(dev, "failed to register LED device\n");
diff --git a/drivers/mfd/janz-cmodio.c b/drivers/mfd/janz-cmodio.c
index 36a166bcdb08..fc4191137e90 100644
--- a/drivers/mfd/janz-cmodio.c
+++ b/drivers/mfd/janz-cmodio.c
@@ -86,8 +86,7 @@ static int __devinit cmodio_setup_subdevice(struct cmodio_device *priv,
 
         /* Add platform data */
         pdata->modno = modno;
-        cell->platform_data = pdata;
-        cell->data_size = sizeof(*pdata);
+        cell->mfd_data = pdata;
 
         /* MODULbus registers -- PCI BAR3 is big-endian MODULbus access */
         res->flags = IORESOURCE_MEM;
diff --git a/drivers/mfd/jz4740-adc.c b/drivers/mfd/jz4740-adc.c
index 0cc59795f600..aa518b9beaf5 100644
--- a/drivers/mfd/jz4740-adc.c
+++ b/drivers/mfd/jz4740-adc.c
@@ -232,8 +232,6 @@ const struct mfd_cell jz4740_adc_cells[] = {
                 .name = "jz4740-hwmon",
                 .num_resources = ARRAY_SIZE(jz4740_hwmon_resources),
                 .resources = jz4740_hwmon_resources,
-                .platform_data = (void *)&jz4740_adc_cells[0],
-                .data_size = sizeof(struct mfd_cell),
 
                 .enable = jz4740_adc_cell_enable,
                 .disable = jz4740_adc_cell_disable,
@@ -243,8 +241,6 @@ const struct mfd_cell jz4740_adc_cells[] = {
                 .name = "jz4740-battery",
                 .num_resources = ARRAY_SIZE(jz4740_battery_resources),
                 .resources = jz4740_battery_resources,
-                .platform_data = (void *)&jz4740_adc_cells[1],
-                .data_size = sizeof(struct mfd_cell),
 
                 .enable = jz4740_adc_cell_enable,
                 .disable = jz4740_adc_cell_disable,
diff --git a/drivers/mfd/lpc_sch.c b/drivers/mfd/lpc_sch.c
index 51b2f6065a0b..ea3f52c07ef7 100644
--- a/drivers/mfd/lpc_sch.c
+++ b/drivers/mfd/lpc_sch.c
@@ -61,6 +61,7 @@ static struct mfd_cell lpc_sch_cells[] = {
 
 static struct pci_device_id lpc_sch_ids[] = {
         { PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_SCH_LPC) },
+        { PCI_DEVICE(PCI_VENDOR_ID_INTEL, PCI_DEVICE_ID_INTEL_ITC_LPC) },
         { 0, }
 };
 MODULE_DEVICE_TABLE(pci, lpc_sch_ids);
@@ -70,6 +71,7 @@ static int __devinit lpc_sch_probe(struct pci_dev *dev,
 {
         unsigned int base_addr_cfg;
         unsigned short base_addr;
+        int i;
 
         pci_read_config_dword(dev, SMBASE, &base_addr_cfg);
         if (!(base_addr_cfg & (1 << 31))) {
@@ -99,7 +101,10 @@ static int __devinit lpc_sch_probe(struct pci_dev *dev,
         gpio_sch_resource.start = base_addr;
         gpio_sch_resource.end = base_addr + GPIO_IO_SIZE - 1;
 
-        return mfd_add_devices(&dev->dev, -1,
+        for (i=0; i < ARRAY_SIZE(lpc_sch_cells); i++)
+                lpc_sch_cells[i].id = id->device;
+
+        return mfd_add_devices(&dev->dev, 0,
                         lpc_sch_cells, ARRAY_SIZE(lpc_sch_cells), NULL, 0);
 }
 
diff --git a/drivers/mfd/max8997.c b/drivers/mfd/max8997.c
new file mode 100644
index 000000000000..5d1fca0277ef
--- /dev/null
+++ b/drivers/mfd/max8997.c
@@ -0,0 +1,427 @@
+/*
+ * max8997.c - mfd core driver for the Maxim 8966 and 8997
+ *
+ * Copyright (C) 2011 Samsung Electronics
+ * MyungJoo Ham <myungjoo.ham@smasung.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.
+ *
+ * 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
+ *
+ * This driver is based on max8998.c
+ */
+
+#include <linux/slab.h>
+#include <linux/i2c.h>
+#include <linux/pm_runtime.h>
+#include <linux/mutex.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/max8997.h>
+#include <linux/mfd/max8997-private.h>
+
+#define I2C_ADDR_PMIC   (0xCC >> 1)
+#define I2C_ADDR_MUIC   (0x4A >> 1)
+#define I2C_ADDR_BATTERY        (0x6C >> 1)
+#define I2C_ADDR_RTC    (0x0C >> 1)
+#define I2C_ADDR_HAPTIC (0x90 >> 1)
+
+static struct mfd_cell max8997_devs[] = {
+        { .name = "max8997-pmic", },
+        { .name = "max8997-rtc", },
+        { .name = "max8997-battery", },
+        { .name = "max8997-haptic", },
+        { .name = "max8997-muic", },
+        { .name = "max8997-flash", },
+};
+
+int max8997_read_reg(struct i2c_client *i2c, u8 reg, u8 *dest)
+{
+        struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
+        int ret;
+
+        mutex_lock(&max8997->iolock);
+        ret = i2c_smbus_read_byte_data(i2c, reg);
+        mutex_unlock(&max8997->iolock);
+        if (ret < 0)
+                return ret;
+
+        ret &= 0xff;
+        *dest = ret;
+        return 0;
+}
+EXPORT_SYMBOL_GPL(max8997_read_reg);
+
+int max8997_bulk_read(struct i2c_client *i2c, u8 reg, int count, u8 *buf)
+{
+        struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
+        int ret;
+
+        mutex_lock(&max8997->iolock);
+        ret = i2c_smbus_read_i2c_block_data(i2c, reg, count, buf);
+        mutex_unlock(&max8997->iolock);
+        if (ret < 0)
+                return ret;
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(max8997_bulk_read);
+
+int max8997_write_reg(struct i2c_client *i2c, u8 reg, u8 value)
+{
+        struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
+        int ret;
+
+        mutex_lock(&max8997->iolock);
+        ret = i2c_smbus_write_byte_data(i2c, reg, value);
+        mutex_unlock(&max8997->iolock);
+        return ret;
+}
+EXPORT_SYMBOL_GPL(max8997_write_reg);
+
+int max8997_bulk_write(struct i2c_client *i2c, u8 reg, int count, u8 *buf)
+{
+        struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
+        int ret;
+
+        mutex_lock(&max8997->iolock);
+        ret = i2c_smbus_write_i2c_block_data(i2c, reg, count, buf);
+        mutex_unlock(&max8997->iolock);
+        if (ret < 0)
+                return ret;
+
+        return 0;
+}
+EXPORT_SYMBOL_GPL(max8997_bulk_write);
+
+int max8997_update_reg(struct i2c_client *i2c, u8 reg, u8 val, u8 mask)
+{
+        struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
+        int ret;
+
+        mutex_lock(&max8997->iolock);
+        ret = i2c_smbus_read_byte_data(i2c, reg);
+        if (ret >= 0) {
+                u8 old_val = ret & 0xff;
+                u8 new_val = (val & mask) | (old_val & (~mask));
+                ret = i2c_smbus_write_byte_data(i2c, reg, new_val);
+        }
+        mutex_unlock(&max8997->iolock);
+        return ret;
+}
+EXPORT_SYMBOL_GPL(max8997_update_reg);
+
+static int max8997_i2c_probe(struct i2c_client *i2c,
+                            const struct i2c_device_id *id)
+{
+        struct max8997_dev *max8997;
+        struct max8997_platform_data *pdata = i2c->dev.platform_data;
+        int ret = 0;
+
+        max8997 = kzalloc(sizeof(struct max8997_dev), GFP_KERNEL);
+        if (max8997 == NULL)
+                return -ENOMEM;
+
+        i2c_set_clientdata(i2c, max8997);
+        max8997->dev = &i2c->dev;
+        max8997->i2c = i2c;
+        max8997->type = id->driver_data;
+
+        if (!pdata)
+                goto err;
+
+        max8997->wakeup = pdata->wakeup;
+
+        mutex_init(&max8997->iolock);
+
+        max8997->rtc = i2c_new_dummy(i2c->adapter, I2C_ADDR_RTC);
+        i2c_set_clientdata(max8997->rtc, max8997);
+        max8997->haptic = i2c_new_dummy(i2c->adapter, I2C_ADDR_HAPTIC);
+        i2c_set_clientdata(max8997->haptic, max8997);
+        max8997->muic = i2c_new_dummy(i2c->adapter, I2C_ADDR_MUIC);
+        i2c_set_clientdata(max8997->muic, max8997);
+
+        pm_runtime_set_active(max8997->dev);
+
+        mfd_add_devices(max8997->dev, -1, max8997_devs,
+                        ARRAY_SIZE(max8997_devs),
+                        NULL, 0);
+
+        /*
+         * TODO: enable others (flash, muic, rtc, battery, ...) and
+         * check the return value
+         */
+
+        if (ret < 0)
+                goto err_mfd;
+
+        return ret;
+
+err_mfd:
+        mfd_remove_devices(max8997->dev);
+        i2c_unregister_device(max8997->muic);
+        i2c_unregister_device(max8997->haptic);
+        i2c_unregister_device(max8997->rtc);
+err:
+        kfree(max8997);
+        return ret;
+}
+
+static int max8997_i2c_remove(struct i2c_client *i2c)
+{
+        struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
+
+        mfd_remove_devices(max8997->dev);
+        i2c_unregister_device(max8997->muic);
+        i2c_unregister_device(max8997->haptic);
+        i2c_unregister_device(max8997->rtc);
+        kfree(max8997);
+
+        return 0;
+}
+
+static const struct i2c_device_id max8997_i2c_id[] = {
+        { "max8997", TYPE_MAX8997 },
+        { "max8966", TYPE_MAX8966 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, max8998_i2c_id);
+
+u8 max8997_dumpaddr_pmic[] = {
+        MAX8997_REG_INT1MSK,
+        MAX8997_REG_INT2MSK,
+        MAX8997_REG_INT3MSK,
+        MAX8997_REG_INT4MSK,
+        MAX8997_REG_MAINCON1,
+        MAX8997_REG_MAINCON2,
+        MAX8997_REG_BUCKRAMP,
+        MAX8997_REG_BUCK1CTRL,
+        MAX8997_REG_BUCK1DVS1,
+        MAX8997_REG_BUCK1DVS2,
+        MAX8997_REG_BUCK1DVS3,
+        MAX8997_REG_BUCK1DVS4,
+        MAX8997_REG_BUCK1DVS5,
+        MAX8997_REG_BUCK1DVS6,
+        MAX8997_REG_BUCK1DVS7,
+        MAX8997_REG_BUCK1DVS8,
+        MAX8997_REG_BUCK2CTRL,
+        MAX8997_REG_BUCK2DVS1,
+        MAX8997_REG_BUCK2DVS2,
+        MAX8997_REG_BUCK2DVS3,
+        MAX8997_REG_BUCK2DVS4,
+        MAX8997_REG_BUCK2DVS5,
+        MAX8997_REG_BUCK2DVS6,
+        MAX8997_REG_BUCK2DVS7,
+        MAX8997_REG_BUCK2DVS8,
+        MAX8997_REG_BUCK3CTRL,
+        MAX8997_REG_BUCK3DVS,
+        MAX8997_REG_BUCK4CTRL,
+        MAX8997_REG_BUCK4DVS,
+        MAX8997_REG_BUCK5CTRL,
+        MAX8997_REG_BUCK5DVS1,
+        MAX8997_REG_BUCK5DVS2,
+        MAX8997_REG_BUCK5DVS3,
+        MAX8997_REG_BUCK5DVS4,
+        MAX8997_REG_BUCK5DVS5,
+        MAX8997_REG_BUCK5DVS6,
+        MAX8997_REG_BUCK5DVS7,
+        MAX8997_REG_BUCK5DVS8,
+        MAX8997_REG_BUCK6CTRL,
+        MAX8997_REG_BUCK6BPSKIPCTRL,
+        MAX8997_REG_BUCK7CTRL,
+        MAX8997_REG_BUCK7DVS,
+        MAX8997_REG_LDO1CTRL,
+        MAX8997_REG_LDO2CTRL,
+        MAX8997_REG_LDO3CTRL,
+        MAX8997_REG_LDO4CTRL,
+        MAX8997_REG_LDO5CTRL,
+        MAX8997_REG_LDO6CTRL,
+        MAX8997_REG_LDO7CTRL,
+        MAX8997_REG_LDO8CTRL,
+        MAX8997_REG_LDO9CTRL,
+        MAX8997_REG_LDO10CTRL,
+        MAX8997_REG_LDO11CTRL,
+        MAX8997_REG_LDO12CTRL,
+        MAX8997_REG_LDO13CTRL,
+        MAX8997_REG_LDO14CTRL,
+        MAX8997_REG_LDO15CTRL,
+        MAX8997_REG_LDO16CTRL,
+        MAX8997_REG_LDO17CTRL,
+        MAX8997_REG_LDO18CTRL,
+        MAX8997_REG_LDO21CTRL,
+        MAX8997_REG_MBCCTRL1,
+        MAX8997_REG_MBCCTRL2,
+        MAX8997_REG_MBCCTRL3,
+        MAX8997_REG_MBCCTRL4,
+        MAX8997_REG_MBCCTRL5,
+        MAX8997_REG_MBCCTRL6,
+        MAX8997_REG_OTPCGHCVS,
+        MAX8997_REG_SAFEOUTCTRL,
+        MAX8997_REG_LBCNFG1,
+        MAX8997_REG_LBCNFG2,
+        MAX8997_REG_BBCCTRL,
+
+        MAX8997_REG_FLASH1_CUR,
+        MAX8997_REG_FLASH2_CUR,
+        MAX8997_REG_MOVIE_CUR,
+        MAX8997_REG_GSMB_CUR,
+        MAX8997_REG_BOOST_CNTL,
+        MAX8997_REG_LEN_CNTL,
+        MAX8997_REG_FLASH_CNTL,
+        MAX8997_REG_WDT_CNTL,
+        MAX8997_REG_MAXFLASH1,
+        MAX8997_REG_MAXFLASH2,
+        MAX8997_REG_FLASHSTATUSMASK,
+
+        MAX8997_REG_GPIOCNTL1,
+        MAX8997_REG_GPIOCNTL2,
+        MAX8997_REG_GPIOCNTL3,
+        MAX8997_REG_GPIOCNTL4,
+        MAX8997_REG_GPIOCNTL5,
+        MAX8997_REG_GPIOCNTL6,
+        MAX8997_REG_GPIOCNTL7,
+        MAX8997_REG_GPIOCNTL8,
+        MAX8997_REG_GPIOCNTL9,
+        MAX8997_REG_GPIOCNTL10,
+        MAX8997_REG_GPIOCNTL11,
+        MAX8997_REG_GPIOCNTL12,
+
+        MAX8997_REG_LDO1CONFIG,
+        MAX8997_REG_LDO2CONFIG,
+        MAX8997_REG_LDO3CONFIG,
+        MAX8997_REG_LDO4CONFIG,
+        MAX8997_REG_LDO5CONFIG,
+        MAX8997_REG_LDO6CONFIG,
+        MAX8997_REG_LDO7CONFIG,
+        MAX8997_REG_LDO8CONFIG,
+        MAX8997_REG_LDO9CONFIG,
+        MAX8997_REG_LDO10CONFIG,
+        MAX8997_REG_LDO11CONFIG,
+        MAX8997_REG_LDO12CONFIG,
+        MAX8997_REG_LDO13CONFIG,
+        MAX8997_REG_LDO14CONFIG,
+        MAX8997_REG_LDO15CONFIG,
+        MAX8997_REG_LDO16CONFIG,
+        MAX8997_REG_LDO17CONFIG,
+        MAX8997_REG_LDO18CONFIG,
+        MAX8997_REG_LDO21CONFIG,
+
+        MAX8997_REG_DVSOKTIMER1,
+        MAX8997_REG_DVSOKTIMER2,
+        MAX8997_REG_DVSOKTIMER4,
+        MAX8997_REG_DVSOKTIMER5,
+};
+
+u8 max8997_dumpaddr_muic[] = {
+        MAX8997_MUIC_REG_INTMASK1,
+        MAX8997_MUIC_REG_INTMASK2,
+        MAX8997_MUIC_REG_INTMASK3,
+        MAX8997_MUIC_REG_CDETCTRL,
+        MAX8997_MUIC_REG_CONTROL1,
+        MAX8997_MUIC_REG_CONTROL2,
+        MAX8997_MUIC_REG_CONTROL3,
+};
+
+u8 max8997_dumpaddr_haptic[] = {
+        MAX8997_HAPTIC_REG_CONF1,
+        MAX8997_HAPTIC_REG_CONF2,
+        MAX8997_HAPTIC_REG_DRVCONF,
+        MAX8997_HAPTIC_REG_CYCLECONF1,
+        MAX8997_HAPTIC_REG_CYCLECONF2,
+        MAX8997_HAPTIC_REG_SIGCONF1,
+        MAX8997_HAPTIC_REG_SIGCONF2,
+        MAX8997_HAPTIC_REG_SIGCONF3,
+        MAX8997_HAPTIC_REG_SIGCONF4,
+        MAX8997_HAPTIC_REG_SIGDC1,
+        MAX8997_HAPTIC_REG_SIGDC2,
+        MAX8997_HAPTIC_REG_SIGPWMDC1,
+        MAX8997_HAPTIC_REG_SIGPWMDC2,
+        MAX8997_HAPTIC_REG_SIGPWMDC3,
+        MAX8997_HAPTIC_REG_SIGPWMDC4,
+};
+
+static int max8997_freeze(struct device *dev)
+{
+        struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+        struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(max8997_dumpaddr_pmic); i++)
+                max8997_read_reg(i2c, max8997_dumpaddr_pmic[i],
+                                &max8997->reg_dump[i]);
+
+        for (i = 0; i < ARRAY_SIZE(max8997_dumpaddr_muic); i++)
+                max8997_read_reg(i2c, max8997_dumpaddr_muic[i],
+                                &max8997->reg_dump[i + MAX8997_REG_PMIC_END]);
+
+        for (i = 0; i < ARRAY_SIZE(max8997_dumpaddr_haptic); i++)
+                max8997_read_reg(i2c, max8997_dumpaddr_haptic[i],
+                                &max8997->reg_dump[i + MAX8997_REG_PMIC_END +
+                                MAX8997_MUIC_REG_END]);
+
+        return 0;
+}
+
+static int max8997_restore(struct device *dev)
+{
+        struct i2c_client *i2c = container_of(dev, struct i2c_client, dev);
+        struct max8997_dev *max8997 = i2c_get_clientdata(i2c);
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(max8997_dumpaddr_pmic); i++)
+                max8997_write_reg(i2c, max8997_dumpaddr_pmic[i],
+                                max8997->reg_dump[i]);
+
+        for (i = 0; i < ARRAY_SIZE(max8997_dumpaddr_muic); i++)
+                max8997_write_reg(i2c, max8997_dumpaddr_muic[i],
+                                max8997->reg_dump[i + MAX8997_REG_PMIC_END]);
+
+        for (i = 0; i < ARRAY_SIZE(max8997_dumpaddr_haptic); i++)
+                max8997_write_reg(i2c, max8997_dumpaddr_haptic[i],
+                                max8997->reg_dump[i + MAX8997_REG_PMIC_END +
+                                MAX8997_MUIC_REG_END]);
+
+        return 0;
+}
+
+const struct dev_pm_ops max8997_pm = {
+        .freeze = max8997_freeze,
+        .restore = max8997_restore,
+};
+
+static struct i2c_driver max8997_i2c_driver = {
+        .driver = {
+                   .name = "max8997",
+                   .owner = THIS_MODULE,
+                   .pm = &max8997_pm,
+        },
+        .probe = max8997_i2c_probe,
+        .remove = max8997_i2c_remove,
+        .id_table = max8997_i2c_id,
+};
+
+static int __init max8997_i2c_init(void)
+{
+        return i2c_add_driver(&max8997_i2c_driver);
+}
+/* init early so consumer devices can complete system boot */
+subsys_initcall(max8997_i2c_init);
+
+static void __exit max8997_i2c_exit(void)
+{
+        i2c_del_driver(&max8997_i2c_driver);
+}
+module_exit(max8997_i2c_exit);
+
+MODULE_DESCRIPTION("MAXIM 8997 multi-function core driver");
+MODULE_AUTHOR("MyungJoo Ham <myungjoo.ham@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/mfd/max8998.c b/drivers/mfd/max8998.c
index bbfe86732602..c00214257da2 100644
--- a/drivers/mfd/max8998.c
+++ b/drivers/mfd/max8998.c
@@ -233,7 +233,7 @@ struct max8998_reg_dump {
         u8      val;
 };
 #define SAVE_ITEM(x)    { .addr = (x), .val = 0x0, }
-struct max8998_reg_dump max8998_dump[] = {
+static struct max8998_reg_dump max8998_dump[] = {
         SAVE_ITEM(MAX8998_REG_IRQM1),
         SAVE_ITEM(MAX8998_REG_IRQM2),
         SAVE_ITEM(MAX8998_REG_IRQM3),
@@ -298,7 +298,7 @@ static int max8998_restore(struct device *dev)
         return 0;
 }
 
-const struct dev_pm_ops max8998_pm = {
+static const struct dev_pm_ops max8998_pm = {
         .suspend = max8998_suspend,
         .resume = max8998_resume,
         .freeze = max8998_freeze,
diff --git a/drivers/mfd/mc13xxx-core.c b/drivers/mfd/mc13xxx-core.c
index b9fcaf0004da..668634e89e81 100644
--- a/drivers/mfd/mc13xxx-core.c
+++ b/drivers/mfd/mc13xxx-core.c
@@ -683,14 +683,13 @@ out:
 EXPORT_SYMBOL_GPL(mc13783_adc_do_conversion);
 
 static int mc13xxx_add_subdevice_pdata(struct mc13xxx *mc13xxx,
-                const char *format, void *pdata, size_t pdata_size)
+                const char *format, void *pdata)
 {
         char buf[30];
         const char *name = mc13xxx_get_chipname(mc13xxx);
 
         struct mfd_cell cell = {
-                .platform_data = pdata,
-                .data_size = pdata_size,
+                .mfd_data = pdata,
         };
 
         /* there is no asnprintf in the kernel :-( */
@@ -706,7 +705,7 @@ static int mc13xxx_add_subdevice_pdata(struct mc13xxx *mc13xxx,
 
 static int mc13xxx_add_subdevice(struct mc13xxx *mc13xxx, const char *format)
 {
-        return mc13xxx_add_subdevice_pdata(mc13xxx, format, NULL, 0);
+        return mc13xxx_add_subdevice_pdata(mc13xxx, format, NULL);
 }
 
 static int mc13xxx_probe(struct spi_device *spi)
@@ -764,13 +763,8 @@ err_revision:
                 mc13xxx_add_subdevice(mc13xxx, "%s-codec");
 
         if (pdata->flags & MC13XXX_USE_REGULATOR) {
-                struct mc13xxx_regulator_platform_data regulator_pdata = {
-                        .num_regulators = pdata->num_regulators,
-                        .regulators = pdata->regulators,
-                };
-
                 mc13xxx_add_subdevice_pdata(mc13xxx, "%s-regulator",
-                                &regulator_pdata, sizeof(regulator_pdata));
+                                &pdata->regulators);
         }
 
         if (pdata->flags & MC13XXX_USE_RTC)
@@ -779,10 +773,8 @@ err_revision:
         if (pdata->flags & MC13XXX_USE_TOUCHSCREEN)
                 mc13xxx_add_subdevice(mc13xxx, "%s-ts");
 
-        if (pdata->flags & MC13XXX_USE_LED) {
-                mc13xxx_add_subdevice_pdata(mc13xxx, "%s-led",
-                                        pdata->leds, sizeof(*pdata->leds));
-        }
+        if (pdata->flags & MC13XXX_USE_LED)
+                mc13xxx_add_subdevice_pdata(mc13xxx, "%s-led", pdata->leds);
 
         return 0;
 }
@@ -811,6 +803,7 @@ static const struct spi_device_id mc13xxx_device_id[] = {
                 /* sentinel */
         }
 };
+MODULE_DEVICE_TABLE(spi, mc13xxx_device_id);
 
 static struct spi_driver mc13xxx_driver = {
         .id_table = mc13xxx_device_id,
diff --git a/drivers/mfd/mfd-core.c b/drivers/mfd/mfd-core.c
index d83ad0f141af..79eda0264fb2 100644
--- a/drivers/mfd/mfd-core.c
+++ b/drivers/mfd/mfd-core.c
@@ -18,6 +18,43 @@
 #include <linux/pm_runtime.h>
 #include <linux/slab.h>
 
+int mfd_cell_enable(struct platform_device *pdev)
+{
+        const struct mfd_cell *cell = mfd_get_cell(pdev);
+        int err = 0;
+
+        /* only call enable hook if the cell wasn't previously enabled */
+        if (atomic_inc_return(cell->usage_count) == 1)
+                err = cell->enable(pdev);
+
+        /* if the enable hook failed, decrement counter to allow retries */
+        if (err)
+                atomic_dec(cell->usage_count);
+
+        return err;
+}
+EXPORT_SYMBOL(mfd_cell_enable);
+
+int mfd_cell_disable(struct platform_device *pdev)
+{
+        const struct mfd_cell *cell = mfd_get_cell(pdev);
+        int err = 0;
+
+        /* only disable if no other clients are using it */
+        if (atomic_dec_return(cell->usage_count) == 0)
+                err = cell->disable(pdev);
+
+        /* if the disable hook failed, increment to allow retries */
+        if (err)
+                atomic_inc(cell->usage_count);
+
+        /* sanity check; did someone call disable too many times? */
+        WARN_ON(atomic_read(cell->usage_count) < 0);
+
+        return err;
+}
+EXPORT_SYMBOL(mfd_cell_disable);
+
 static int mfd_add_device(struct device *parent, int id,
                           const struct mfd_cell *cell,
                           struct resource *mem_base,
@@ -37,14 +74,10 @@ static int mfd_add_device(struct device *parent, int id,
                 goto fail_device;
 
         pdev->dev.parent = parent;
-        platform_set_drvdata(pdev, cell->driver_data);
 
-        if (cell->data_size) {
-                ret = platform_device_add_data(pdev,
-                                        cell->platform_data, cell->data_size);
-                if (ret)
-                        goto fail_res;
-        }
+        ret = platform_device_add_data(pdev, cell, sizeof(*cell));
+        if (ret)
+                goto fail_res;
 
         for (r = 0; r < cell->num_resources; r++) {
                 res[r].name = cell->resources[r].name;
@@ -100,14 +133,22 @@ fail_alloc:
 }
 
 int mfd_add_devices(struct device *parent, int id,
-                    const struct mfd_cell *cells, int n_devs,
+                    struct mfd_cell *cells, int n_devs,
                     struct resource *mem_base,
                     int irq_base)
 {
         int i;
         int ret = 0;
+        atomic_t *cnts;
+
+        /* initialize reference counting for all cells */
+        cnts = kcalloc(sizeof(*cnts), n_devs, GFP_KERNEL);
+        if (!cnts)
+                return -ENOMEM;
 
         for (i = 0; i < n_devs; i++) {
+                atomic_set(&cnts[i], 0);
+                cells[i].usage_count = &cnts[i];
                 ret = mfd_add_device(parent, id, cells + i, mem_base, irq_base);
                 if (ret)
                         break;
@@ -120,17 +161,89 @@ int mfd_add_devices(struct device *parent, int id,
 }
 EXPORT_SYMBOL(mfd_add_devices);
 
-static int mfd_remove_devices_fn(struct device *dev, void *unused)
+static int mfd_remove_devices_fn(struct device *dev, void *c)
 {
-        platform_device_unregister(to_platform_device(dev));
+        struct platform_device *pdev = to_platform_device(dev);
+        const struct mfd_cell *cell = mfd_get_cell(pdev);
+        atomic_t **usage_count = c;
+
+        /* find the base address of usage_count pointers (for freeing) */
+        if (!*usage_count || (cell->usage_count < *usage_count))
+                *usage_count = cell->usage_count;
+
+        platform_device_unregister(pdev);
         return 0;
 }
 
 void mfd_remove_devices(struct device *parent)
 {
-        device_for_each_child(parent, NULL, mfd_remove_devices_fn);
+        atomic_t *cnts = NULL;
+
+        device_for_each_child(parent, &cnts, mfd_remove_devices_fn);
+        kfree(cnts);
 }
 EXPORT_SYMBOL(mfd_remove_devices);
 
+static int add_shared_platform_device(const char *cell, const char *name)
+{
+        struct mfd_cell cell_entry;
+        struct device *dev;
+        struct platform_device *pdev;
+        int err;
+
+        /* check if we've already registered a device (don't fail if we have) */
+        if (bus_find_device_by_name(&platform_bus_type, NULL, name))
+                return 0;
+
+        /* fetch the parent cell's device (should already be registered!) */
+        dev = bus_find_device_by_name(&platform_bus_type, NULL, cell);
+        if (!dev) {
+                printk(KERN_ERR "failed to find device for cell %s\n", cell);
+                return -ENODEV;
+        }
+        pdev = to_platform_device(dev);
+        memcpy(&cell_entry, mfd_get_cell(pdev), sizeof(cell_entry));
+
+        WARN_ON(!cell_entry.enable);
+
+        cell_entry.name = name;
+        err = mfd_add_device(pdev->dev.parent, -1, &cell_entry, NULL, 0);
+        if (err)
+                dev_err(dev, "MFD add devices failed: %d\n", err);
+        return err;
+}
+
+int mfd_shared_platform_driver_register(struct platform_driver *drv,
+                const char *cellname)
+{
+        int err;
+
+        err = add_shared_platform_device(cellname, drv->driver.name);
+        if (err)
+                printk(KERN_ERR "failed to add platform device %s\n",
+                                drv->driver.name);
+
+        err = platform_driver_register(drv);
+        if (err)
+                printk(KERN_ERR "failed to add platform driver %s\n",
+                                drv->driver.name);
+
+        return err;
+}
+EXPORT_SYMBOL(mfd_shared_platform_driver_register);
+
+void mfd_shared_platform_driver_unregister(struct platform_driver *drv)
+{
+        struct device *dev;
+
+        dev = bus_find_device_by_name(&platform_bus_type, NULL,
+                        drv->driver.name);
+        if (dev)
+                platform_device_unregister(to_platform_device(dev));
+
+        platform_driver_unregister(drv);
+}
+EXPORT_SYMBOL(mfd_shared_platform_driver_unregister);
+
 MODULE_LICENSE("GPL");
 MODULE_AUTHOR("Ian Molton, Dmitry Baryshkov");
diff --git a/drivers/mfd/pcf50633-core.c b/drivers/mfd/pcf50633-core.c
index 501ce13b693e..c1306ed43e3c 100644
--- a/drivers/mfd/pcf50633-core.c
+++ b/drivers/mfd/pcf50633-core.c
@@ -21,6 +21,7 @@
 #include <linux/workqueue.h>
 #include <linux/platform_device.h>
 #include <linux/i2c.h>
+#include <linux/pm.h>
 #include <linux/slab.h>
 
 #include <linux/mfd/pcf50633/core.h>
@@ -230,27 +231,26 @@ pcf50633_client_dev_register(struct pcf50633 *pcf, const char *name,
         }
 }
 
-#ifdef CONFIG_PM
-static int pcf50633_suspend(struct i2c_client *client, pm_message_t state)
+#ifdef CONFIG_PM_SLEEP
+static int pcf50633_suspend(struct device *dev)
 {
-        struct pcf50633 *pcf;
-        pcf = i2c_get_clientdata(client);
+        struct i2c_client *client = to_i2c_client(dev);
+        struct pcf50633 *pcf = i2c_get_clientdata(client);
 
         return pcf50633_irq_suspend(pcf);
 }
 
-static int pcf50633_resume(struct i2c_client *client)
+static int pcf50633_resume(struct device *dev)
 {
-        struct pcf50633 *pcf;
-        pcf = i2c_get_clientdata(client);
+        struct i2c_client *client = to_i2c_client(dev);
+        struct pcf50633 *pcf = i2c_get_clientdata(client);
 
         return pcf50633_irq_resume(pcf);
 }
-#else
-#define pcf50633_suspend NULL
-#define pcf50633_resume NULL
 #endif
 
+static SIMPLE_DEV_PM_OPS(pcf50633_pm, pcf50633_suspend, pcf50633_resume);
+
 static int __devinit pcf50633_probe(struct i2c_client *client,
                                 const struct i2c_device_id *ids)
 {
@@ -360,16 +360,16 @@ static struct i2c_device_id pcf50633_id_table[] = {
         {"pcf50633", 0x73},
         {/* end of list */}
 };
+MODULE_DEVICE_TABLE(i2c, pcf50633_id_table);
 
 static struct i2c_driver pcf50633_driver = {
         .driver = {
                 .name   = "pcf50633",
+                .pm     = &pcf50633_pm,
         },
         .id_table = pcf50633_id_table,
         .probe = pcf50633_probe,
         .remove = __devexit_p(pcf50633_remove),
-        .suspend = pcf50633_suspend,
-        .resume = pcf50633_resume,
 };
 
 static int __init pcf50633_init(void)
diff --git a/drivers/mfd/rdc321x-southbridge.c b/drivers/mfd/rdc321x-southbridge.c
index 50922975bda3..193c940225b5 100644
--- a/drivers/mfd/rdc321x-southbridge.c
+++ b/drivers/mfd/rdc321x-southbridge.c
@@ -61,12 +61,12 @@ static struct mfd_cell rdc321x_sb_cells[] = {
                 .name           = "rdc321x-wdt",
                 .resources      = rdc321x_wdt_resource,
                 .num_resources  = ARRAY_SIZE(rdc321x_wdt_resource),
-                .driver_data    = &rdc321x_wdt_pdata,
+                .mfd_data       = &rdc321x_wdt_pdata,
         }, {
                 .name           = "rdc321x-gpio",
                 .resources      = rdc321x_gpio_resources,
                 .num_resources  = ARRAY_SIZE(rdc321x_gpio_resources),
-                .driver_data    = &rdc321x_gpio_pdata,
+                .mfd_data       = &rdc321x_gpio_pdata,
         },
 };
 
diff --git a/drivers/mfd/sh_mobile_sdhi.c b/drivers/mfd/sh_mobile_sdhi.c
index 0a7df44a93c0..53a63024bf11 100644
--- a/drivers/mfd/sh_mobile_sdhi.c
+++ b/drivers/mfd/sh_mobile_sdhi.c
@@ -146,9 +146,7 @@ static int __devinit sh_mobile_sdhi_probe(struct platform_device *pdev)
         }
 
         memcpy(&priv->cell_mmc, &sh_mobile_sdhi_cell, sizeof(priv->cell_mmc));
-        priv->cell_mmc.driver_data = mmc_data;
-        priv->cell_mmc.platform_data = &priv->cell_mmc;
-        priv->cell_mmc.data_size = sizeof(priv->cell_mmc);
+        priv->cell_mmc.mfd_data = mmc_data;
 
         platform_set_drvdata(pdev, priv);
 
diff --git a/drivers/mfd/sm501.c b/drivers/mfd/sm501.c
index 5de3a760ea1e..df3702c1756d 100644
--- a/drivers/mfd/sm501.c
+++ b/drivers/mfd/sm501.c
@@ -133,10 +133,10 @@ static unsigned long decode_div(unsigned long pll2, unsigned long val,
 
 static void sm501_dump_clk(struct sm501_devdata *sm)
 {
-        unsigned long misct = readl(sm->regs + SM501_MISC_TIMING);
-        unsigned long pm0 = readl(sm->regs + SM501_POWER_MODE_0_CLOCK);
-        unsigned long pm1 = readl(sm->regs + SM501_POWER_MODE_1_CLOCK);
-        unsigned long pmc = readl(sm->regs + SM501_POWER_MODE_CONTROL);
+        unsigned long misct = smc501_readl(sm->regs + SM501_MISC_TIMING);
+        unsigned long pm0 = smc501_readl(sm->regs + SM501_POWER_MODE_0_CLOCK);
+        unsigned long pm1 = smc501_readl(sm->regs + SM501_POWER_MODE_1_CLOCK);
+        unsigned long pmc = smc501_readl(sm->regs + SM501_POWER_MODE_CONTROL);
         unsigned long sdclk0, sdclk1;
         unsigned long pll2 = 0;
 
@@ -193,29 +193,29 @@ static void sm501_dump_regs(struct sm501_devdata *sm)
         void __iomem *regs = sm->regs;
 
         dev_info(sm->dev, "System Control   %08x\n",
-                        readl(regs + SM501_SYSTEM_CONTROL));
+                        smc501_readl(regs + SM501_SYSTEM_CONTROL));
         dev_info(sm->dev, "Misc Control     %08x\n",
-                        readl(regs + SM501_MISC_CONTROL));
+                        smc501_readl(regs + SM501_MISC_CONTROL));
         dev_info(sm->dev, "GPIO Control Low %08x\n",
-                        readl(regs + SM501_GPIO31_0_CONTROL));
+                        smc501_readl(regs + SM501_GPIO31_0_CONTROL));
         dev_info(sm->dev, "GPIO Control Hi  %08x\n",
-                        readl(regs + SM501_GPIO63_32_CONTROL));
+                        smc501_readl(regs + SM501_GPIO63_32_CONTROL));
         dev_info(sm->dev, "DRAM Control     %08x\n",
-                        readl(regs + SM501_DRAM_CONTROL));
+                        smc501_readl(regs + SM501_DRAM_CONTROL));
         dev_info(sm->dev, "Arbitration Ctrl %08x\n",
-                        readl(regs + SM501_ARBTRTN_CONTROL));
+                        smc501_readl(regs + SM501_ARBTRTN_CONTROL));
         dev_info(sm->dev, "Misc Timing      %08x\n",
-                        readl(regs + SM501_MISC_TIMING));
+                        smc501_readl(regs + SM501_MISC_TIMING));
 }
 
 static void sm501_dump_gate(struct sm501_devdata *sm)
 {
         dev_info(sm->dev, "CurrentGate      %08x\n",
-                        readl(sm->regs + SM501_CURRENT_GATE));
+                        smc501_readl(sm->regs + SM501_CURRENT_GATE));
         dev_info(sm->dev, "CurrentClock     %08x\n",
-                        readl(sm->regs + SM501_CURRENT_CLOCK));
+                        smc501_readl(sm->regs + SM501_CURRENT_CLOCK));
         dev_info(sm->dev, "PowerModeControl %08x\n",
-                        readl(sm->regs + SM501_POWER_MODE_CONTROL));
+                        smc501_readl(sm->regs + SM501_POWER_MODE_CONTROL));
 }
 
 #else
@@ -231,7 +231,7 @@ static inline void sm501_dump_clk(struct sm501_devdata *sm) { }
 
 static void sm501_sync_regs(struct sm501_devdata *sm)
 {
-        readl(sm->regs);
+        smc501_readl(sm->regs);
 }
 
 static inline void sm501_mdelay(struct sm501_devdata *sm, unsigned int delay)
@@ -261,11 +261,11 @@ int sm501_misc_control(struct device *dev,
 
         spin_lock_irqsave(&sm->reg_lock, save);
 
-        misc = readl(sm->regs + SM501_MISC_CONTROL);
+        misc = smc501_readl(sm->regs + SM501_MISC_CONTROL);
         to = (misc & ~clear) | set;
 
         if (to != misc) {
-                writel(to, sm->regs + SM501_MISC_CONTROL);
+                smc501_writel(to, sm->regs + SM501_MISC_CONTROL);
                 sm501_sync_regs(sm);
 
                 dev_dbg(sm->dev, "MISC_CONTROL %08lx\n", misc);
@@ -294,11 +294,11 @@ unsigned long sm501_modify_reg(struct device *dev,
 
         spin_lock_irqsave(&sm->reg_lock, save);
 
-        data = readl(sm->regs + reg);
+        data = smc501_readl(sm->regs + reg);
         data |= set;
         data &= ~clear;
 
-        writel(data, sm->regs + reg);
+        smc501_writel(data, sm->regs + reg);
         sm501_sync_regs(sm);
 
         spin_unlock_irqrestore(&sm->reg_lock, save);
@@ -322,9 +322,9 @@ int sm501_unit_power(struct device *dev, unsigned int unit, unsigned int to)
 
         mutex_lock(&sm->clock_lock);
 
-        mode = readl(sm->regs + SM501_POWER_MODE_CONTROL);
-        gate = readl(sm->regs + SM501_CURRENT_GATE);
-        clock = readl(sm->regs + SM501_CURRENT_CLOCK);
+        mode = smc501_readl(sm->regs + SM501_POWER_MODE_CONTROL);
+        gate = smc501_readl(sm->regs + SM501_CURRENT_GATE);
+        clock = smc501_readl(sm->regs + SM501_CURRENT_CLOCK);
 
         mode &= 3;              /* get current power mode */
 
@@ -356,14 +356,14 @@ int sm501_unit_power(struct device *dev, unsigned int unit, unsigned int to)
 
         switch (mode) {
         case 1:
-                writel(gate, sm->regs + SM501_POWER_MODE_0_GATE);
-                writel(clock, sm->regs + SM501_POWER_MODE_0_CLOCK);
+                smc501_writel(gate, sm->regs + SM501_POWER_MODE_0_GATE);
+                smc501_writel(clock, sm->regs + SM501_POWER_MODE_0_CLOCK);
                 mode = 0;
                 break;
         case 2:
         case 0:
-                writel(gate, sm->regs + SM501_POWER_MODE_1_GATE);
-                writel(clock, sm->regs + SM501_POWER_MODE_1_CLOCK);
+                smc501_writel(gate, sm->regs + SM501_POWER_MODE_1_GATE);
+                smc501_writel(clock, sm->regs + SM501_POWER_MODE_1_CLOCK);
                 mode = 1;
                 break;
 
@@ -372,7 +372,7 @@ int sm501_unit_power(struct device *dev, unsigned int unit, unsigned int to)
                 goto already;
         }
 
-        writel(mode, sm->regs + SM501_POWER_MODE_CONTROL);
+        smc501_writel(mode, sm->regs + SM501_POWER_MODE_CONTROL);
         sm501_sync_regs(sm);
 
         dev_dbg(sm->dev, "gate %08lx, clock %08lx, mode %08lx\n",
@@ -519,9 +519,9 @@ unsigned long sm501_set_clock(struct device *dev,
                               unsigned long req_freq)
 {
         struct sm501_devdata *sm = dev_get_drvdata(dev);
-        unsigned long mode = readl(sm->regs + SM501_POWER_MODE_CONTROL);
-        unsigned long gate = readl(sm->regs + SM501_CURRENT_GATE);
-        unsigned long clock = readl(sm->regs + SM501_CURRENT_CLOCK);
+        unsigned long mode = smc501_readl(sm->regs + SM501_POWER_MODE_CONTROL);
+        unsigned long gate = smc501_readl(sm->regs + SM501_CURRENT_GATE);
+        unsigned long clock = smc501_readl(sm->regs + SM501_CURRENT_CLOCK);
         unsigned char reg;
         unsigned int pll_reg = 0;
         unsigned long sm501_freq; /* the actual frequency achieved */
@@ -592,9 +592,9 @@ unsigned long sm501_set_clock(struct device *dev,
 
         mutex_lock(&sm->clock_lock);
 
-        mode = readl(sm->regs + SM501_POWER_MODE_CONTROL);
-        gate = readl(sm->regs + SM501_CURRENT_GATE);
-        clock = readl(sm->regs + SM501_CURRENT_CLOCK);
+        mode = smc501_readl(sm->regs + SM501_POWER_MODE_CONTROL);
+        gate = smc501_readl(sm->regs + SM501_CURRENT_GATE);
+        clock = smc501_readl(sm->regs + SM501_CURRENT_CLOCK);
 
         clock = clock & ~(0xFF << clksrc);
         clock |= reg<<clksrc;
@@ -603,14 +603,14 @@ unsigned long sm501_set_clock(struct device *dev,
 
         switch (mode) {
         case 1:
-                writel(gate, sm->regs + SM501_POWER_MODE_0_GATE);
-                writel(clock, sm->regs + SM501_POWER_MODE_0_CLOCK);
+                smc501_writel(gate, sm->regs + SM501_POWER_MODE_0_GATE);
+                smc501_writel(clock, sm->regs + SM501_POWER_MODE_0_CLOCK);
                 mode = 0;
                 break;
         case 2:
         case 0:
-                writel(gate, sm->regs + SM501_POWER_MODE_1_GATE);
-                writel(clock, sm->regs + SM501_POWER_MODE_1_CLOCK);
+                smc501_writel(gate, sm->regs + SM501_POWER_MODE_1_GATE);
+                smc501_writel(clock, sm->regs + SM501_POWER_MODE_1_CLOCK);
                 mode = 1;
                 break;
 
@@ -619,10 +619,11 @@ unsigned long sm501_set_clock(struct device *dev,
                 return -1;
         }
 
-        writel(mode, sm->regs + SM501_POWER_MODE_CONTROL);
+        smc501_writel(mode, sm->regs + SM501_POWER_MODE_CONTROL);
 
         if (pll_reg)
-                writel(pll_reg, sm->regs + SM501_PROGRAMMABLE_PLL_CONTROL);
+                smc501_writel(pll_reg,
+                                sm->regs + SM501_PROGRAMMABLE_PLL_CONTROL);
 
         sm501_sync_regs(sm);
 
@@ -902,7 +903,7 @@ static int sm501_gpio_get(struct gpio_chip *chip, unsigned offset)
         struct sm501_gpio_chip *smgpio = to_sm501_gpio(chip);
         unsigned long result;
 
-        result = readl(smgpio->regbase + SM501_GPIO_DATA_LOW);
+        result = smc501_readl(smgpio->regbase + SM501_GPIO_DATA_LOW);
         result >>= offset;
 
         return result & 1UL;
@@ -915,13 +916,13 @@ static void sm501_gpio_ensure_gpio(struct sm501_gpio_chip *smchip,
 
         /* check and modify if this pin is not set as gpio. */
 
-        if (readl(smchip->control) & bit) {
+        if (smc501_readl(smchip->control) & bit) {
                 dev_info(sm501_gpio_to_dev(smchip->ourgpio)->dev,
                          "changing mode of gpio, bit %08lx\n", bit);
 
-                ctrl = readl(smchip->control);
+                ctrl = smc501_readl(smchip->control);
                 ctrl &= ~bit;
-                writel(ctrl, smchip->control);
+                smc501_writel(ctrl, smchip->control);
 
                 sm501_sync_regs(sm501_gpio_to_dev(smchip->ourgpio));
         }
@@ -942,10 +943,10 @@ static void sm501_gpio_set(struct gpio_chip *chip, unsigned offset, int value)
 
         spin_lock_irqsave(&smgpio->lock, save);
 
-        val = readl(regs + SM501_GPIO_DATA_LOW) & ~bit;
+        val = smc501_readl(regs + SM501_GPIO_DATA_LOW) & ~bit;
         if (value)
                 val |= bit;
-        writel(val, regs);
+        smc501_writel(val, regs);
 
         sm501_sync_regs(sm501_gpio_to_dev(smgpio));
         sm501_gpio_ensure_gpio(smchip, bit);
@@ -967,8 +968,8 @@ static int sm501_gpio_input(struct gpio_chip *chip, unsigned offset)
 
         spin_lock_irqsave(&smgpio->lock, save);
 
-        ddr = readl(regs + SM501_GPIO_DDR_LOW);
-        writel(ddr & ~bit, regs + SM501_GPIO_DDR_LOW);
+        ddr = smc501_readl(regs + SM501_GPIO_DDR_LOW);
+        smc501_writel(ddr & ~bit, regs + SM501_GPIO_DDR_LOW);
 
         sm501_sync_regs(sm501_gpio_to_dev(smgpio));
         sm501_gpio_ensure_gpio(smchip, bit);
@@ -994,18 +995,18 @@ static int sm501_gpio_output(struct gpio_chip *chip,
 
         spin_lock_irqsave(&smgpio->lock, save);
 
-        val = readl(regs + SM501_GPIO_DATA_LOW);
+        val = smc501_readl(regs + SM501_GPIO_DATA_LOW);
         if (value)
                 val |= bit;
         else
                 val &= ~bit;
-        writel(val, regs);
+        smc501_writel(val, regs);
 
-        ddr = readl(regs + SM501_GPIO_DDR_LOW);
-        writel(ddr | bit, regs + SM501_GPIO_DDR_LOW);
+        ddr = smc501_readl(regs + SM501_GPIO_DDR_LOW);
+        smc501_writel(ddr | bit, regs + SM501_GPIO_DDR_LOW);
 
         sm501_sync_regs(sm501_gpio_to_dev(smgpio));
-        writel(val, regs + SM501_GPIO_DATA_LOW);
+        smc501_writel(val, regs + SM501_GPIO_DATA_LOW);
 
         sm501_sync_regs(sm501_gpio_to_dev(smgpio));
         spin_unlock_irqrestore(&smgpio->lock, save);
@@ -1231,7 +1232,7 @@ static ssize_t sm501_dbg_regs(struct device *dev,
 
         for (reg = 0x00; reg < 0x70; reg += 4) {
                 ret = sprintf(ptr, "%08x = %08x\n",
-                              reg, readl(sm->regs + reg));
+                              reg, smc501_readl(sm->regs + reg));
                 ptr += ret;
         }
 
@@ -1255,10 +1256,10 @@ static inline void sm501_init_reg(struct sm501_devdata *sm,
 {
         unsigned long tmp;
 
-        tmp = readl(sm->regs + reg);
+        tmp = smc501_readl(sm->regs + reg);
         tmp &= ~r->mask;
         tmp |= r->set;
-        writel(tmp, sm->regs + reg);
+        smc501_writel(tmp, sm->regs + reg);
 }
 
 /* sm501_init_regs
@@ -1299,7 +1300,7 @@ static void sm501_init_regs(struct sm501_devdata *sm,
 
 static int sm501_check_clocks(struct sm501_devdata *sm)
 {
-        unsigned long pwrmode = readl(sm->regs + SM501_CURRENT_CLOCK);
+        unsigned long pwrmode = smc501_readl(sm->regs + SM501_CURRENT_CLOCK);
         unsigned long msrc = (pwrmode & SM501_POWERMODE_M_SRC);
         unsigned long m1src = (pwrmode & SM501_POWERMODE_M1_SRC);
 
@@ -1334,7 +1335,7 @@ static int __devinit sm501_init_dev(struct sm501_devdata *sm)
 
         INIT_LIST_HEAD(&sm->devices);
 
-        devid = readl(sm->regs + SM501_DEVICEID);
+        devid = smc501_readl(sm->regs + SM501_DEVICEID);
 
         if ((devid & SM501_DEVICEID_IDMASK) != SM501_DEVICEID_SM501) {
                 dev_err(sm->dev, "incorrect device id %08lx\n", devid);
@@ -1342,9 +1343,9 @@ static int __devinit sm501_init_dev(struct sm501_devdata *sm)
         }
 
         /* disable irqs */
-        writel(0, sm->regs + SM501_IRQ_MASK);
+        smc501_writel(0, sm->regs + SM501_IRQ_MASK);
 
-        dramctrl = readl(sm->regs + SM501_DRAM_CONTROL);
+        dramctrl = smc501_readl(sm->regs + SM501_DRAM_CONTROL);
         mem_avail = sm501_mem_local[(dramctrl >> 13) & 0x7];
 
         dev_info(sm->dev, "SM501 At %p: Version %08lx, %ld Mb, IRQ %d\n",
@@ -1376,7 +1377,7 @@ static int __devinit sm501_init_dev(struct sm501_devdata *sm)
                         sm501_register_gpio(sm);
         }
 
-        if (pdata->gpio_i2c != NULL && pdata->gpio_i2c_nr > 0) {
+        if (pdata && pdata->gpio_i2c != NULL && pdata->gpio_i2c_nr > 0) {
                 if (!sm501_gpio_isregistered(sm))
                         dev_err(sm->dev, "no gpio available for i2c gpio.\n");
                 else
@@ -1421,6 +1422,7 @@ static int __devinit sm501_plat_probe(struct platform_device *dev)
 
         sm->io_res = platform_get_resource(dev, IORESOURCE_MEM, 1);
         sm->mem_res = platform_get_resource(dev, IORESOURCE_MEM, 0);
+
         if (sm->io_res == NULL || sm->mem_res == NULL) {
                 dev_err(&dev->dev, "failed to get IO resource\n");
                 ret = -ENOENT;
@@ -1489,7 +1491,7 @@ static int sm501_plat_suspend(struct platform_device *pdev, pm_message_t state)
         struct sm501_devdata *sm = platform_get_drvdata(pdev);
 
         sm->in_suspend = 1;
-        sm->pm_misc = readl(sm->regs + SM501_MISC_CONTROL);
+        sm->pm_misc = smc501_readl(sm->regs + SM501_MISC_CONTROL);
 
         sm501_dump_regs(sm);
 
@@ -1513,9 +1515,9 @@ static int sm501_plat_resume(struct platform_device *pdev)
 
         /* check to see if we are in the same state as when suspended */
 
-        if (readl(sm->regs + SM501_MISC_CONTROL) != sm->pm_misc) {
+        if (smc501_readl(sm->regs + SM501_MISC_CONTROL) != sm->pm_misc) {
                 dev_info(sm->dev, "SM501_MISC_CONTROL changed over sleep\n");
-                writel(sm->pm_misc, sm->regs + SM501_MISC_CONTROL);
+                smc501_writel(sm->pm_misc, sm->regs + SM501_MISC_CONTROL);
 
                 /* our suspend causes the controller state to change,
                  * either by something attempting setup, power loss,
@@ -1734,10 +1736,16 @@ static struct pci_driver sm501_pci_driver = {
 
 MODULE_ALIAS("platform:sm501");
 
+static struct of_device_id __devinitdata of_sm501_match_tbl[] = {
+        { .compatible = "smi,sm501", },
+        { /* end */ }
+};
+
 static struct platform_driver sm501_plat_driver = {
         .driver         = {
                 .name   = "sm501",
                 .owner  = THIS_MODULE,
+                .of_match_table = of_sm501_match_tbl,
         },
         .probe          = sm501_plat_probe,
         .remove         = sm501_plat_remove,
diff --git a/drivers/mfd/t7l66xb.c b/drivers/mfd/t7l66xb.c
index 9caeb4ac6ea6..af57fc706a4c 100644
--- a/drivers/mfd/t7l66xb.c
+++ b/drivers/mfd/t7l66xb.c
@@ -170,7 +170,7 @@ static struct mfd_cell t7l66xb_cells[] = {
                 .name = "tmio-mmc",
                 .enable = t7l66xb_mmc_enable,
                 .disable = t7l66xb_mmc_disable,
-                .driver_data = &t7166xb_mmc_data,
+                .mfd_data = &t7166xb_mmc_data,
                 .num_resources = ARRAY_SIZE(t7l66xb_mmc_resources),
                 .resources = t7l66xb_mmc_resources,
         },
@@ -383,16 +383,7 @@ static int t7l66xb_probe(struct platform_device *dev)
 
         t7l66xb_attach_irq(dev);
 
-        t7l66xb_cells[T7L66XB_CELL_NAND].driver_data = pdata->nand_data;
-        t7l66xb_cells[T7L66XB_CELL_NAND].platform_data =
-                &t7l66xb_cells[T7L66XB_CELL_NAND];
-        t7l66xb_cells[T7L66XB_CELL_NAND].data_size =
-                sizeof(t7l66xb_cells[T7L66XB_CELL_NAND]);
-
-        t7l66xb_cells[T7L66XB_CELL_MMC].platform_data =
-                &t7l66xb_cells[T7L66XB_CELL_MMC];
-        t7l66xb_cells[T7L66XB_CELL_MMC].data_size =
-                sizeof(t7l66xb_cells[T7L66XB_CELL_MMC]);
+        t7l66xb_cells[T7L66XB_CELL_NAND].mfd_data = pdata->nand_data;
 
         ret = mfd_add_devices(&dev->dev, dev->id,
                               t7l66xb_cells, ARRAY_SIZE(t7l66xb_cells),
diff --git a/drivers/mfd/tc6387xb.c b/drivers/mfd/tc6387xb.c
index 6315f63f017d..b006f7cee952 100644
--- a/drivers/mfd/tc6387xb.c
+++ b/drivers/mfd/tc6387xb.c
@@ -131,7 +131,7 @@ static struct mfd_cell tc6387xb_cells[] = {
                 .name = "tmio-mmc",
                 .enable = tc6387xb_mmc_enable,
                 .disable = tc6387xb_mmc_disable,
-                .driver_data = &tc6387xb_mmc_data,
+                .mfd_data = &tc6387xb_mmc_data,
                 .num_resources = ARRAY_SIZE(tc6387xb_mmc_resources),
                 .resources = tc6387xb_mmc_resources,
         },
@@ -190,11 +190,6 @@ static int __devinit tc6387xb_probe(struct platform_device *dev)
 
         printk(KERN_INFO "Toshiba tc6387xb initialised\n");
 
-        tc6387xb_cells[TC6387XB_CELL_MMC].platform_data =
-                &tc6387xb_cells[TC6387XB_CELL_MMC];
-        tc6387xb_cells[TC6387XB_CELL_MMC].data_size =
-                sizeof(tc6387xb_cells[TC6387XB_CELL_MMC]);
-
         ret = mfd_add_devices(&dev->dev, dev->id, tc6387xb_cells,
                               ARRAY_SIZE(tc6387xb_cells), iomem, irq);
 
diff --git a/drivers/mfd/tc6393xb.c b/drivers/mfd/tc6393xb.c
index 9a238633a54d..3d62ded86a8f 100644
--- a/drivers/mfd/tc6393xb.c
+++ b/drivers/mfd/tc6393xb.c
@@ -393,7 +393,7 @@ static struct mfd_cell __devinitdata tc6393xb_cells[] = {
                 .name = "tmio-mmc",
                 .enable = tc6393xb_mmc_enable,
                 .resume = tc6393xb_mmc_resume,
-                .driver_data = &tc6393xb_mmc_data,
+                .mfd_data = &tc6393xb_mmc_data,
                 .num_resources = ARRAY_SIZE(tc6393xb_mmc_resources),
                 .resources = tc6393xb_mmc_resources,
         },
@@ -693,27 +693,8 @@ static int __devinit tc6393xb_probe(struct platform_device *dev)
                         goto err_setup;
         }
 
-        tc6393xb_cells[TC6393XB_CELL_NAND].driver_data = tcpd->nand_data;
-        tc6393xb_cells[TC6393XB_CELL_NAND].platform_data =
-                &tc6393xb_cells[TC6393XB_CELL_NAND];
-        tc6393xb_cells[TC6393XB_CELL_NAND].data_size =
-                sizeof(tc6393xb_cells[TC6393XB_CELL_NAND]);
-
-        tc6393xb_cells[TC6393XB_CELL_MMC].platform_data =
-                &tc6393xb_cells[TC6393XB_CELL_MMC];
-        tc6393xb_cells[TC6393XB_CELL_MMC].data_size =
-                sizeof(tc6393xb_cells[TC6393XB_CELL_MMC]);
-
-        tc6393xb_cells[TC6393XB_CELL_OHCI].platform_data =
-                &tc6393xb_cells[TC6393XB_CELL_OHCI];
-        tc6393xb_cells[TC6393XB_CELL_OHCI].data_size =
-                sizeof(tc6393xb_cells[TC6393XB_CELL_OHCI]);
-
-        tc6393xb_cells[TC6393XB_CELL_FB].driver_data = tcpd->fb_data;
-        tc6393xb_cells[TC6393XB_CELL_FB].platform_data =
-                &tc6393xb_cells[TC6393XB_CELL_FB];
-        tc6393xb_cells[TC6393XB_CELL_FB].data_size =
-                sizeof(tc6393xb_cells[TC6393XB_CELL_FB]);
+        tc6393xb_cells[TC6393XB_CELL_NAND].mfd_data = tcpd->nand_data;
+        tc6393xb_cells[TC6393XB_CELL_FB].mfd_data = tcpd->fb_data;
 
         ret = mfd_add_devices(&dev->dev, dev->id,
                         tc6393xb_cells, ARRAY_SIZE(tc6393xb_cells),
diff --git a/drivers/mfd/timberdale.c b/drivers/mfd/timberdale.c
index 6ad8a7f8d390..94c6c8afad12 100644
--- a/drivers/mfd/timberdale.c
+++ b/drivers/mfd/timberdale.c
@@ -384,8 +384,7 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg0[] = {
                 .name = "timb-dma",
                 .num_resources = ARRAY_SIZE(timberdale_dma_resources),
                 .resources = timberdale_dma_resources,
-                .platform_data = &timb_dma_platform_data,
-                .data_size = sizeof(timb_dma_platform_data),
+                .mfd_data = &timb_dma_platform_data,
         },
         {
                 .name = "timb-uart",
@@ -396,43 +395,37 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg0[] = {
                 .name = "xiic-i2c",
                 .num_resources = ARRAY_SIZE(timberdale_xiic_resources),
                 .resources = timberdale_xiic_resources,
-                .platform_data = &timberdale_xiic_platform_data,
-                .data_size = sizeof(timberdale_xiic_platform_data),
+                .mfd_data = &timberdale_xiic_platform_data,
         },
         {
                 .name = "timb-gpio",
                 .num_resources = ARRAY_SIZE(timberdale_gpio_resources),
                 .resources = timberdale_gpio_resources,
-                .platform_data = &timberdale_gpio_platform_data,
-                .data_size = sizeof(timberdale_gpio_platform_data),
+                .mfd_data = &timberdale_gpio_platform_data,
         },
         {
                 .name = "timb-video",
                 .num_resources = ARRAY_SIZE(timberdale_video_resources),
                 .resources = timberdale_video_resources,
-                .platform_data = &timberdale_video_platform_data,
-                .data_size = sizeof(timberdale_video_platform_data),
+                .mfd_data = &timberdale_video_platform_data,
         },
         {
                 .name = "timb-radio",
                 .num_resources = ARRAY_SIZE(timberdale_radio_resources),
                 .resources = timberdale_radio_resources,
-                .platform_data = &timberdale_radio_platform_data,
-                .data_size = sizeof(timberdale_radio_platform_data),
+                .mfd_data = &timberdale_radio_platform_data,
         },
         {
                 .name = "xilinx_spi",
                 .num_resources = ARRAY_SIZE(timberdale_spi_resources),
                 .resources = timberdale_spi_resources,
-                .platform_data = &timberdale_xspi_platform_data,
-                .data_size = sizeof(timberdale_xspi_platform_data),
+                .mfd_data = &timberdale_xspi_platform_data,
         },
         {
                 .name = "ks8842",
                 .num_resources = ARRAY_SIZE(timberdale_eth_resources),
                 .resources = timberdale_eth_resources,
-                .platform_data = &timberdale_ks8842_platform_data,
-                .data_size = sizeof(timberdale_ks8842_platform_data)
+                .mfd_data = &timberdale_ks8842_platform_data,
         },
 };
 
@@ -441,8 +434,7 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
                 .name = "timb-dma",
                 .num_resources = ARRAY_SIZE(timberdale_dma_resources),
                 .resources = timberdale_dma_resources,
-                .platform_data = &timb_dma_platform_data,
-                .data_size = sizeof(timb_dma_platform_data),
+                .mfd_data = &timb_dma_platform_data,
         },
         {
                 .name = "timb-uart",
@@ -458,15 +450,13 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
                 .name = "xiic-i2c",
                 .num_resources = ARRAY_SIZE(timberdale_xiic_resources),
                 .resources = timberdale_xiic_resources,
-                .platform_data = &timberdale_xiic_platform_data,
-                .data_size = sizeof(timberdale_xiic_platform_data),
+                .mfd_data = &timberdale_xiic_platform_data,
         },
         {
                 .name = "timb-gpio",
                 .num_resources = ARRAY_SIZE(timberdale_gpio_resources),
                 .resources = timberdale_gpio_resources,
-                .platform_data = &timberdale_gpio_platform_data,
-                .data_size = sizeof(timberdale_gpio_platform_data),
+                .mfd_data = &timberdale_gpio_platform_data,
         },
         {
                 .name = "timb-mlogicore",
@@ -477,29 +467,25 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg1[] = {
                 .name = "timb-video",
                 .num_resources = ARRAY_SIZE(timberdale_video_resources),
                 .resources = timberdale_video_resources,
-                .platform_data = &timberdale_video_platform_data,
-                .data_size = sizeof(timberdale_video_platform_data),
+                .mfd_data = &timberdale_video_platform_data,
         },
         {
                 .name = "timb-radio",
                 .num_resources = ARRAY_SIZE(timberdale_radio_resources),
                 .resources = timberdale_radio_resources,
-                .platform_data = &timberdale_radio_platform_data,
-                .data_size = sizeof(timberdale_radio_platform_data),
+                .mfd_data = &timberdale_radio_platform_data,
         },
         {
                 .name = "xilinx_spi",
                 .num_resources = ARRAY_SIZE(timberdale_spi_resources),
                 .resources = timberdale_spi_resources,
-                .platform_data = &timberdale_xspi_platform_data,
-                .data_size = sizeof(timberdale_xspi_platform_data),
+                .mfd_data = &timberdale_xspi_platform_data,
         },
         {
                 .name = "ks8842",
                 .num_resources = ARRAY_SIZE(timberdale_eth_resources),
                 .resources = timberdale_eth_resources,
-                .platform_data = &timberdale_ks8842_platform_data,
-                .data_size = sizeof(timberdale_ks8842_platform_data)
+                .mfd_data = &timberdale_ks8842_platform_data,
         },
 };
 
@@ -508,8 +494,7 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg2[] = {
                 .name = "timb-dma",
                 .num_resources = ARRAY_SIZE(timberdale_dma_resources),
                 .resources = timberdale_dma_resources,
-                .platform_data = &timb_dma_platform_data,
-                .data_size = sizeof(timb_dma_platform_data),
+                .mfd_data = &timb_dma_platform_data,
         },
         {
                 .name = "timb-uart",
@@ -520,36 +505,31 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg2[] = {
                 .name = "xiic-i2c",
                 .num_resources = ARRAY_SIZE(timberdale_xiic_resources),
                 .resources = timberdale_xiic_resources,
-                .platform_data = &timberdale_xiic_platform_data,
-                .data_size = sizeof(timberdale_xiic_platform_data),
+                .mfd_data = &timberdale_xiic_platform_data,
         },
         {
                 .name = "timb-gpio",
                 .num_resources = ARRAY_SIZE(timberdale_gpio_resources),
                 .resources = timberdale_gpio_resources,
-                .platform_data = &timberdale_gpio_platform_data,
-                .data_size = sizeof(timberdale_gpio_platform_data),
+                .mfd_data = &timberdale_gpio_platform_data,
         },
         {
                 .name = "timb-video",
                 .num_resources = ARRAY_SIZE(timberdale_video_resources),
                 .resources = timberdale_video_resources,
-                .platform_data = &timberdale_video_platform_data,
-                .data_size = sizeof(timberdale_video_platform_data),
+                .mfd_data = &timberdale_video_platform_data,
         },
         {
                 .name = "timb-radio",
                 .num_resources = ARRAY_SIZE(timberdale_radio_resources),
                 .resources = timberdale_radio_resources,
-                .platform_data = &timberdale_radio_platform_data,
-                .data_size = sizeof(timberdale_radio_platform_data),
+                .mfd_data = &timberdale_radio_platform_data,
         },
         {
                 .name = "xilinx_spi",
                 .num_resources = ARRAY_SIZE(timberdale_spi_resources),
                 .resources = timberdale_spi_resources,
-                .platform_data = &timberdale_xspi_platform_data,
-                .data_size = sizeof(timberdale_xspi_platform_data),
+                .mfd_data = &timberdale_xspi_platform_data,
         },
 };
 
@@ -558,8 +538,7 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg3[] = {
                 .name = "timb-dma",
                 .num_resources = ARRAY_SIZE(timberdale_dma_resources),
                 .resources = timberdale_dma_resources,
-                .platform_data = &timb_dma_platform_data,
-                .data_size = sizeof(timb_dma_platform_data),
+                .mfd_data = &timb_dma_platform_data,
         },
         {
                 .name = "timb-uart",
@@ -570,43 +549,37 @@ static __devinitdata struct mfd_cell timberdale_cells_bar0_cfg3[] = {
                 .name = "ocores-i2c",
                 .num_resources = ARRAY_SIZE(timberdale_ocores_resources),
                 .resources = timberdale_ocores_resources,
-                .platform_data = &timberdale_ocores_platform_data,
-                .data_size = sizeof(timberdale_ocores_platform_data),
+                .mfd_data = &timberdale_ocores_platform_data,
         },
         {
                 .name = "timb-gpio",
                 .num_resources = ARRAY_SIZE(timberdale_gpio_resources),
                 .resources = timberdale_gpio_resources,
-                .platform_data = &timberdale_gpio_platform_data,
-                .data_size = sizeof(timberdale_gpio_platform_data),
+                .mfd_data = &timberdale_gpio_platform_data,
         },
         {
                 .name = "timb-video",
                 .num_resources = ARRAY_SIZE(timberdale_video_resources),
                 .resources = timberdale_video_resources,
-                .platform_data = &timberdale_video_platform_data,
-                .data_size = sizeof(timberdale_video_platform_data),
+                .mfd_data = &timberdale_video_platform_data,
         },
         {
                 .name = "timb-radio",
                 .num_resources = ARRAY_SIZE(timberdale_radio_resources),
                 .resources = timberdale_radio_resources,
-                .platform_data = &timberdale_radio_platform_data,
-                .data_size = sizeof(timberdale_radio_platform_data),
+                .mfd_data = &timberdale_radio_platform_data,
         },
         {
                 .name = "xilinx_spi",
                 .num_resources = ARRAY_SIZE(timberdale_spi_resources),
                 .resources = timberdale_spi_resources,
-                .platform_data = &timberdale_xspi_platform_data,
-                .data_size = sizeof(timberdale_xspi_platform_data),
+                .mfd_data = &timberdale_xspi_platform_data,
         },
         {
                 .name = "ks8842",
                 .num_resources = ARRAY_SIZE(timberdale_eth_resources),
                 .resources = timberdale_eth_resources,
-                .platform_data = &timberdale_ks8842_platform_data,
-                .data_size = sizeof(timberdale_ks8842_platform_data)
+                .mfd_data = &timberdale_ks8842_platform_data,
         },
 };
 
diff --git a/drivers/mfd/tps6105x.c b/drivers/mfd/tps6105x.c
new file mode 100644
index 000000000000..46d8205646b6
--- /dev/null
+++ b/drivers/mfd/tps6105x.c
@@ -0,0 +1,246 @@
+/*
+ * Core driver for TPS61050/61052 boost converters, used for while LED
+ * driving, audio power amplification, white LED flash, and generic
+ * boost conversion. Additionally it provides a 1-bit GPIO pin (out or in)
+ * and a flash synchronization pin to synchronize flash events when used as
+ * flashgun.
+ *
+ * Copyright (C) 2011 ST-Ericsson SA
+ * Written on behalf of Linaro for ST-Ericsson
+ *
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/module.h>
+#include <linux/init.h>
+#include <linux/i2c.h>
+#include <linux/mutex.h>
+#include <linux/gpio.h>
+#include <linux/spinlock.h>
+#include <linux/slab.h>
+#include <linux/err.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/tps6105x.h>
+
+int tps6105x_set(struct tps6105x *tps6105x, u8 reg, u8 value)
+{
+        int ret;
+
+        ret = mutex_lock_interruptible(&tps6105x->lock);
+        if (ret)
+                return ret;
+        ret = i2c_smbus_write_byte_data(tps6105x->client, reg, value);
+        mutex_unlock(&tps6105x->lock);
+        if (ret < 0)
+                return ret;
+
+        return 0;
+}
+EXPORT_SYMBOL(tps6105x_set);
+
+int tps6105x_get(struct tps6105x *tps6105x, u8 reg, u8 *buf)
+{
+        int ret;
+
+        ret = mutex_lock_interruptible(&tps6105x->lock);
+        if (ret)
+                return ret;
+        ret = i2c_smbus_read_byte_data(tps6105x->client, reg);
+        mutex_unlock(&tps6105x->lock);
+        if (ret < 0)
+                return ret;
+
+        *buf = ret;
+        return 0;
+}
+EXPORT_SYMBOL(tps6105x_get);
+
+/*
+ * Masks off the bits in the mask and sets the bits in the bitvalues
+ * parameter in one atomic operation
+ */
+int tps6105x_mask_and_set(struct tps6105x *tps6105x, u8 reg,
+                          u8 bitmask, u8 bitvalues)
+{
+        int ret;
+        u8 regval;
+
+        ret = mutex_lock_interruptible(&tps6105x->lock);
+        if (ret)
+                return ret;
+        ret = i2c_smbus_read_byte_data(tps6105x->client, reg);
+        if (ret < 0)
+                goto fail;
+        regval = ret;
+        regval = (~bitmask & regval) | (bitmask & bitvalues);
+        ret = i2c_smbus_write_byte_data(tps6105x->client, reg, regval);
+fail:
+        mutex_unlock(&tps6105x->lock);
+        if (ret < 0)
+                return ret;
+
+        return 0;
+}
+EXPORT_SYMBOL(tps6105x_mask_and_set);
+
+static int __devinit tps6105x_startup(struct tps6105x *tps6105x)
+{
+        int ret;
+        u8 regval;
+
+        ret = tps6105x_get(tps6105x, TPS6105X_REG_0, &regval);
+        if (ret)
+                return ret;
+        switch (regval >> TPS6105X_REG0_MODE_SHIFT) {
+        case TPS6105X_REG0_MODE_SHUTDOWN:
+                dev_info(&tps6105x->client->dev,
+                         "TPS6105x found in SHUTDOWN mode\n");
+                break;
+        case TPS6105X_REG0_MODE_TORCH:
+                dev_info(&tps6105x->client->dev,
+                         "TPS6105x found in TORCH mode\n");
+                break;
+        case TPS6105X_REG0_MODE_TORCH_FLASH:
+                dev_info(&tps6105x->client->dev,
+                         "TPS6105x found in FLASH mode\n");
+                break;
+        case TPS6105X_REG0_MODE_VOLTAGE:
+                dev_info(&tps6105x->client->dev,
+                         "TPS6105x found in VOLTAGE mode\n");
+                break;
+        default:
+                break;
+        }
+
+        return ret;
+}
+
+/*
+ * MFD cells - we have one cell which is selected operation
+ * mode, and we always have a GPIO cell.
+ */
+static struct mfd_cell tps6105x_cells[] = {
+        {
+                /* name will be runtime assigned */
+                .id = -1,
+        },
+        {
+                .name = "tps6105x-gpio",
+                .id = -1,
+        },
+};
+
+static int __devinit tps6105x_probe(struct i2c_client *client,
+                        const struct i2c_device_id *id)
+{
+        struct tps6105x                 *tps6105x;
+        struct tps6105x_platform_data   *pdata;
+        int ret;
+        int i;
+
+        tps6105x = kmalloc(sizeof(*tps6105x), GFP_KERNEL);
+        if (!tps6105x)
+                return -ENOMEM;
+
+        i2c_set_clientdata(client, tps6105x);
+        tps6105x->client = client;
+        pdata = client->dev.platform_data;
+        tps6105x->pdata = pdata;
+        mutex_init(&tps6105x->lock);
+
+        ret = tps6105x_startup(tps6105x);
+        if (ret) {
+                dev_err(&client->dev, "chip initialization failed\n");
+                goto fail;
+        }
+
+        /* Remove warning texts when you implement new cell drivers */
+        switch (pdata->mode) {
+        case TPS6105X_MODE_SHUTDOWN:
+                dev_info(&client->dev,
+                         "present, not used for anything, only GPIO\n");
+                break;
+        case TPS6105X_MODE_TORCH:
+                tps6105x_cells[0].name = "tps6105x-leds";
+                dev_warn(&client->dev,
+                         "torch mode is unsupported\n");
+                break;
+        case TPS6105X_MODE_TORCH_FLASH:
+                tps6105x_cells[0].name = "tps6105x-flash";
+                dev_warn(&client->dev,
+                         "flash mode is unsupported\n");
+                break;
+        case TPS6105X_MODE_VOLTAGE:
+                tps6105x_cells[0].name ="tps6105x-regulator";
+                break;
+        default:
+                break;
+        }
+
+        /* Set up and register the platform devices. */
+        for (i = 0; i < ARRAY_SIZE(tps6105x_cells); i++) {
+                /* One state holder for all drivers, this is simple */
+                tps6105x_cells[i].mfd_data = tps6105x;
+        }
+
+        ret = mfd_add_devices(&client->dev, 0, tps6105x_cells,
+                ARRAY_SIZE(tps6105x_cells), NULL, 0);
+        if (ret)
+                goto fail;
+
+        return 0;
+
+fail:
+        kfree(tps6105x);
+        return ret;
+}
+
+static int __devexit tps6105x_remove(struct i2c_client *client)
+{
+        struct tps6105x *tps6105x = i2c_get_clientdata(client);
+
+        mfd_remove_devices(&client->dev);
+
+        /* Put chip in shutdown mode */
+        tps6105x_mask_and_set(tps6105x, TPS6105X_REG_0,
+                TPS6105X_REG0_MODE_MASK,
+                TPS6105X_MODE_SHUTDOWN << TPS6105X_REG0_MODE_SHIFT);
+
+        kfree(tps6105x);
+        return 0;
+}
+
+static const struct i2c_device_id tps6105x_id[] = {
+        { "tps61050", 0 },
+        { "tps61052", 0 },
+        { }
+};
+MODULE_DEVICE_TABLE(i2c, tps6105x_id);
+
+static struct i2c_driver tps6105x_driver = {
+        .driver = {
+                .name   = "tps6105x",
+        },
+        .probe          = tps6105x_probe,
+        .remove         = __devexit_p(tps6105x_remove),
+        .id_table       = tps6105x_id,
+};
+
+static int __init tps6105x_init(void)
+{
+        return i2c_add_driver(&tps6105x_driver);
+}
+subsys_initcall(tps6105x_init);
+
+static void __exit tps6105x_exit(void)
+{
+        i2c_del_driver(&tps6105x_driver);
+}
+module_exit(tps6105x_exit);
+
+MODULE_AUTHOR("Linus Walleij");
+MODULE_DESCRIPTION("TPS6105x White LED Boost Converter Driver");
+MODULE_LICENSE("GPL v2");
diff --git a/drivers/mfd/tps6586x.c b/drivers/mfd/tps6586x.c
index e9018d1394ee..0aa9186aec19 100644
--- a/drivers/mfd/tps6586x.c
+++ b/drivers/mfd/tps6586x.c
@@ -288,12 +288,10 @@ static int tps6586x_gpio_output(struct gpio_chip *gc, unsigned offset,
         return tps6586x_update(tps6586x->dev, TPS6586X_GPIOSET1, val, mask);
 }
 
-static void tps6586x_gpio_init(struct tps6586x *tps6586x, int gpio_base)
+static int tps6586x_gpio_init(struct tps6586x *tps6586x, int gpio_base)
 {
-        int ret;
-
         if (!gpio_base)
-                return;
+                return 0;
 
         tps6586x->gpio.owner            = THIS_MODULE;
         tps6586x->gpio.label            = tps6586x->client->name;
@@ -307,9 +305,7 @@ static void tps6586x_gpio_init(struct tps6586x *tps6586x, int gpio_base)
         tps6586x->gpio.set              = tps6586x_gpio_set;
         tps6586x->gpio.get              = tps6586x_gpio_get;
 
-        ret = gpiochip_add(&tps6586x->gpio);
-        if (ret)
-                dev_warn(tps6586x->dev, "GPIO registration failed: %d\n", ret);
+        return gpiochip_add(&tps6586x->gpio);
 }
 
 static int __remove_subdev(struct device *dev, void *unused)
@@ -517,17 +513,28 @@ static int __devinit tps6586x_i2c_probe(struct i2c_client *client,
                 }
         }
 
+        ret = tps6586x_gpio_init(tps6586x, pdata->gpio_base);
+        if (ret) {
+                dev_err(&client->dev, "GPIO registration failed: %d\n", ret);
+                goto err_gpio_init;
+        }
+
         ret = tps6586x_add_subdevs(tps6586x, pdata);
         if (ret) {
                 dev_err(&client->dev, "add devices failed: %d\n", ret);
                 goto err_add_devs;
         }
 
-        tps6586x_gpio_init(tps6586x, pdata->gpio_base);
-
         return 0;
 
 err_add_devs:
+        if (pdata->gpio_base) {
+                ret = gpiochip_remove(&tps6586x->gpio);
+                if (ret)
+                        dev_err(&client->dev, "Can't remove gpio chip: %d\n",
+                                ret);
+        }
+err_gpio_init:
         if (client->irq)
                 free_irq(client->irq, tps6586x);
 err_irq_init:
@@ -587,4 +594,3 @@ module_exit(tps6586x_exit);
 MODULE_DESCRIPTION("TPS6586X core driver");
 MODULE_AUTHOR("Mike Rapoport <mike@compulab.co.il>");
 MODULE_LICENSE("GPL");
-
diff --git a/drivers/mfd/twl-core.c b/drivers/mfd/twl-core.c
index a35fa7dcbf53..960b5bed7f52 100644
--- a/drivers/mfd/twl-core.c
+++ b/drivers/mfd/twl-core.c
@@ -721,13 +721,13 @@ add_children(struct twl4030_platform_data *pdata, unsigned long features)
 
         }
 
-        if (twl_has_watchdog()) {
+        if (twl_has_watchdog() && twl_class_is_4030()) {
                 child = add_child(0, "twl4030_wdt", NULL, 0, false, 0, 0);
                 if (IS_ERR(child))
                         return PTR_ERR(child);
         }
 
-        if (twl_has_pwrbutton()) {
+        if (twl_has_pwrbutton() && twl_class_is_4030()) {
                 child = add_child(1, "twl4030_pwrbutton",
                                 NULL, 0, true, pdata->irq_base + 8 + 0, 0);
                 if (IS_ERR(child))
@@ -864,6 +864,10 @@ add_children(struct twl4030_platform_data *pdata, unsigned long features)
                 child = add_regulator(TWL6030_REG_VAUX3_6030, pdata->vaux3);
                 if (IS_ERR(child))
                         return PTR_ERR(child);
+
+                child = add_regulator(TWL6030_REG_CLK32KG, pdata->clk32kg);
+                if (IS_ERR(child))
+                        return PTR_ERR(child);
         }
 
         if (twl_has_bci() && pdata->bci &&
diff --git a/drivers/mfd/twl4030-codec.c b/drivers/mfd/twl4030-codec.c
index 9a4b196d6deb..c02fded316c9 100644
--- a/drivers/mfd/twl4030-codec.c
+++ b/drivers/mfd/twl4030-codec.c
@@ -208,15 +208,13 @@ static int __devinit twl4030_codec_probe(struct platform_device *pdev)
         if (pdata->audio) {
                 cell = &codec->cells[childs];
                 cell->name = "twl4030-codec";
-                cell->platform_data = pdata->audio;
-                cell->data_size = sizeof(*pdata->audio);
+                cell->mfd_data = pdata->audio;
                 childs++;
         }
         if (pdata->vibra) {
                 cell = &codec->cells[childs];
                 cell->name = "twl4030-vibra";
-                cell->platform_data = pdata->vibra;
-                cell->data_size = sizeof(*pdata->vibra);
+                cell->mfd_data = pdata->vibra;
                 childs++;
         }
 
diff --git a/drivers/mfd/twl4030-madc.c b/drivers/mfd/twl4030-madc.c
new file mode 100644
index 000000000000..3941ddcf15fe
--- /dev/null
+++ b/drivers/mfd/twl4030-madc.c
@@ -0,0 +1,802 @@
+/*
+ *
+ * TWL4030 MADC module driver-This driver monitors the real time
+ * conversion of analog signals like battery temperature,
+ * battery type, battery level etc.
+ *
+ * Copyright (C) 2011 Texas Instruments Incorporated - http://www.ti.com/
+ * J Keerthy <j-keerthy@ti.com>
+ *
+ * Based on twl4030-madc.c
+ * Copyright (C) 2008 Nokia Corporation
+ * Mikko Ylinen <mikko.k.ylinen@nokia.com>
+ *
+ * Amit Kucheria <amit.kucheria@canonical.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.
+ *
+ * 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 St, Fifth Floor, Boston, MA
+ * 02110-1301 USA
+ *
+ */
+
+#include <linux/init.h>
+#include <linux/device.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h>
+#include <linux/delay.h>
+#include <linux/platform_device.h>
+#include <linux/slab.h>
+#include <linux/i2c/twl.h>
+#include <linux/i2c/twl4030-madc.h>
+#include <linux/module.h>
+#include <linux/stddef.h>
+#include <linux/mutex.h>
+#include <linux/bitops.h>
+#include <linux/jiffies.h>
+#include <linux/types.h>
+#include <linux/gfp.h>
+#include <linux/err.h>
+
+/*
+ * struct twl4030_madc_data - a container for madc info
+ * @dev - pointer to device structure for madc
+ * @lock - mutex protecting this data structure
+ * @requests - Array of request struct corresponding to SW1, SW2 and RT
+ * @imr - Interrupt mask register of MADC
+ * @isr - Interrupt status register of MADC
+ */
+struct twl4030_madc_data {
+        struct device *dev;
+        struct mutex lock;      /* mutex protecting this data structure */
+        struct twl4030_madc_request requests[TWL4030_MADC_NUM_METHODS];
+        int imr;
+        int isr;
+};
+
+static struct twl4030_madc_data *twl4030_madc;
+
+struct twl4030_prescale_divider_ratios {
+        s16 numerator;
+        s16 denominator;
+};
+
+static const struct twl4030_prescale_divider_ratios
+twl4030_divider_ratios[16] = {
+        {1, 1},         /* CHANNEL 0 No Prescaler */
+        {1, 1},         /* CHANNEL 1 No Prescaler */
+        {6, 10},        /* CHANNEL 2 */
+        {6, 10},        /* CHANNEL 3 */
+        {6, 10},        /* CHANNEL 4 */
+        {6, 10},        /* CHANNEL 5 */
+        {6, 10},        /* CHANNEL 6 */
+        {6, 10},        /* CHANNEL 7 */
+        {3, 14},        /* CHANNEL 8 */
+        {1, 3},         /* CHANNEL 9 */
+        {1, 1},         /* CHANNEL 10 No Prescaler */
+        {15, 100},      /* CHANNEL 11 */
+        {1, 4},         /* CHANNEL 12 */
+        {1, 1},         /* CHANNEL 13 Reserved channels */
+        {1, 1},         /* CHANNEL 14 Reseved channels */
+        {5, 11},        /* CHANNEL 15 */
+};
+
+
+/*
+ * Conversion table from -3 to 55 degree Celcius
+ */
+static int therm_tbl[] = {
+30800,  29500,  28300,  27100,
+26000,  24900,  23900,  22900,  22000,  21100,  20300,  19400,  18700,  17900,
+17200,  16500,  15900,  15300,  14700,  14100,  13600,  13100,  12600,  12100,
+11600,  11200,  10800,  10400,  10000,  9630,   9280,   8950,   8620,   8310,
+8020,   7730,   7460,   7200,   6950,   6710,   6470,   6250,   6040,   5830,
+5640,   5450,   5260,   5090,   4920,   4760,   4600,   4450,   4310,   4170,
+4040,   3910,   3790,   3670,   3550
+};
+
+/*
+ * Structure containing the registers
+ * of different conversion methods supported by MADC.
+ * Hardware or RT real time conversion request initiated by external host
+ * processor for RT Signal conversions.
+ * External host processors can also request for non RT conversions
+ * SW1 and SW2 software conversions also called asynchronous or GPC request.
+ */
+static
+const struct twl4030_madc_conversion_method twl4030_conversion_methods[] = {
+        [TWL4030_MADC_RT] = {
+                             .sel = TWL4030_MADC_RTSELECT_LSB,
+                             .avg = TWL4030_MADC_RTAVERAGE_LSB,
+                             .rbase = TWL4030_MADC_RTCH0_LSB,
+                             },
+        [TWL4030_MADC_SW1] = {
+                              .sel = TWL4030_MADC_SW1SELECT_LSB,
+                              .avg = TWL4030_MADC_SW1AVERAGE_LSB,
+                              .rbase = TWL4030_MADC_GPCH0_LSB,
+                              .ctrl = TWL4030_MADC_CTRL_SW1,
+                              },
+        [TWL4030_MADC_SW2] = {
+                              .sel = TWL4030_MADC_SW2SELECT_LSB,
+                              .avg = TWL4030_MADC_SW2AVERAGE_LSB,
+                              .rbase = TWL4030_MADC_GPCH0_LSB,
+                              .ctrl = TWL4030_MADC_CTRL_SW2,
+                              },
+};
+
+/*
+ * Function to read a particular channel value.
+ * @madc - pointer to struct twl4030_madc_data
+ * @reg - lsb of ADC Channel
+ * If the i2c read fails it returns an error else returns 0.
+ */
+static int twl4030_madc_channel_raw_read(struct twl4030_madc_data *madc, u8 reg)
+{
+        u8 msb, lsb;
+        int ret;
+        /*
+         * For each ADC channel, we have MSB and LSB register pair. MSB address
+         * is always LSB address+1. reg parameter is the address of LSB register
+         */
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &msb, reg + 1);
+        if (ret) {
+                dev_err(madc->dev, "unable to read MSB register 0x%X\n",
+                        reg + 1);
+                return ret;
+        }
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &lsb, reg);
+        if (ret) {
+                dev_err(madc->dev, "unable to read LSB register 0x%X\n", reg);
+                return ret;
+        }
+
+        return (int)(((msb << 8) | lsb) >> 6);
+}
+
+/*
+ * Return battery temperature
+ * Or < 0 on failure.
+ */
+static int twl4030battery_temperature(int raw_volt)
+{
+        u8 val;
+        int temp, curr, volt, res, ret;
+
+        volt = (raw_volt * TEMP_STEP_SIZE) / TEMP_PSR_R;
+        /* Getting and calculating the supply current in micro ampers */
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE, &val,
+                REG_BCICTL2);
+        if (ret < 0)
+                return ret;
+        curr = ((val & TWL4030_BCI_ITHEN) + 1) * 10;
+        /* Getting and calculating the thermistor resistance in ohms */
+        res = volt * 1000 / curr;
+        /* calculating temperature */
+        for (temp = 58; temp >= 0; temp--) {
+                int actual = therm_tbl[temp];
+
+                if ((actual - res) >= 0)
+                        break;
+        }
+
+        return temp + 1;
+}
+
+static int twl4030battery_current(int raw_volt)
+{
+        int ret;
+        u8 val;
+
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE, &val,
+                TWL4030_BCI_BCICTL1);
+        if (ret)
+                return ret;
+        if (val & TWL4030_BCI_CGAIN) /* slope of 0.44 mV/mA */
+                return (raw_volt * CURR_STEP_SIZE) / CURR_PSR_R1;
+        else /* slope of 0.88 mV/mA */
+                return (raw_volt * CURR_STEP_SIZE) / CURR_PSR_R2;
+}
+/*
+ * Function to read channel values
+ * @madc - pointer to twl4030_madc_data struct
+ * @reg_base - Base address of the first channel
+ * @Channels - 16 bit bitmap. If the bit is set, channel value is read
+ * @buf - The channel values are stored here. if read fails error
+ * value is stored
+ * Returns the number of successfully read channels.
+ */
+static int twl4030_madc_read_channels(struct twl4030_madc_data *madc,
+                                      u8 reg_base, unsigned
+                                                long channels, int *buf)
+{
+        int count = 0, count_req = 0, i;
+        u8 reg;
+
+        for_each_set_bit(i, &channels, TWL4030_MADC_MAX_CHANNELS) {
+                reg = reg_base + 2 * i;
+                buf[i] = twl4030_madc_channel_raw_read(madc, reg);
+                if (buf[i] < 0) {
+                        dev_err(madc->dev,
+                                "Unable to read register 0x%X\n", reg);
+                        count_req++;
+                        continue;
+                }
+                switch (i) {
+                case 10:
+                        buf[i] = twl4030battery_current(buf[i]);
+                        if (buf[i] < 0) {
+                                dev_err(madc->dev, "err reading current\n");
+                                count_req++;
+                        } else {
+                                count++;
+                                buf[i] = buf[i] - 750;
+                        }
+                        break;
+                case 1:
+                        buf[i] = twl4030battery_temperature(buf[i]);
+                        if (buf[i] < 0) {
+                                dev_err(madc->dev, "err reading temperature\n");
+                                count_req++;
+                        } else {
+                                buf[i] -= 3;
+                                count++;
+                        }
+                        break;
+                default:
+                        count++;
+                        /* Analog Input (V) = conv_result * step_size / R
+                         * conv_result = decimal value of 10-bit conversion
+                         *               result
+                         * step size = 1.5 / (2 ^ 10 -1)
+                         * R = Prescaler ratio for input channels.
+                         * Result given in mV hence multiplied by 1000.
+                         */
+                        buf[i] = (buf[i] * 3 * 1000 *
+                                 twl4030_divider_ratios[i].denominator)
+                                / (2 * 1023 *
+                                twl4030_divider_ratios[i].numerator);
+                }
+        }
+        if (count_req)
+                dev_err(madc->dev, "%d channel conversion failed\n", count_req);
+
+        return count;
+}
+
+/*
+ * Enables irq.
+ * @madc - pointer to twl4030_madc_data struct
+ * @id - irq number to be enabled
+ * can take one of TWL4030_MADC_RT, TWL4030_MADC_SW1, TWL4030_MADC_SW2
+ * corresponding to RT, SW1, SW2 conversion requests.
+ * If the i2c read fails it returns an error else returns 0.
+ */
+static int twl4030_madc_enable_irq(struct twl4030_madc_data *madc, u8 id)
+{
+        u8 val;
+        int ret;
+
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &val, madc->imr);
+        if (ret) {
+                dev_err(madc->dev, "unable to read imr register 0x%X\n",
+                        madc->imr);
+                return ret;
+        }
+        val &= ~(1 << id);
+        ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, val, madc->imr);
+        if (ret) {
+                dev_err(madc->dev,
+                        "unable to write imr register 0x%X\n", madc->imr);
+                return ret;
+
+        }
+
+        return 0;
+}
+
+/*
+ * Disables irq.
+ * @madc - pointer to twl4030_madc_data struct
+ * @id - irq number to be disabled
+ * can take one of TWL4030_MADC_RT, TWL4030_MADC_SW1, TWL4030_MADC_SW2
+ * corresponding to RT, SW1, SW2 conversion requests.
+ * Returns error if i2c read/write fails.
+ */
+static int twl4030_madc_disable_irq(struct twl4030_madc_data *madc, u8 id)
+{
+        u8 val;
+        int ret;
+
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &val, madc->imr);
+        if (ret) {
+                dev_err(madc->dev, "unable to read imr register 0x%X\n",
+                        madc->imr);
+                return ret;
+        }
+        val |= (1 << id);
+        ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, val, madc->imr);
+        if (ret) {
+                dev_err(madc->dev,
+                        "unable to write imr register 0x%X\n", madc->imr);
+                return ret;
+        }
+
+        return 0;
+}
+
+static irqreturn_t twl4030_madc_threaded_irq_handler(int irq, void *_madc)
+{
+        struct twl4030_madc_data *madc = _madc;
+        const struct twl4030_madc_conversion_method *method;
+        u8 isr_val, imr_val;
+        int i, len, ret;
+        struct twl4030_madc_request *r;
+
+        mutex_lock(&madc->lock);
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &isr_val, madc->isr);
+        if (ret) {
+                dev_err(madc->dev, "unable to read isr register 0x%X\n",
+                        madc->isr);
+                goto err_i2c;
+        }
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &imr_val, madc->imr);
+        if (ret) {
+                dev_err(madc->dev, "unable to read imr register 0x%X\n",
+                        madc->imr);
+                goto err_i2c;
+        }
+        isr_val &= ~imr_val;
+        for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
+                if (!(isr_val & (1 << i)))
+                        continue;
+                ret = twl4030_madc_disable_irq(madc, i);
+                if (ret < 0)
+                        dev_dbg(madc->dev, "Disable interrupt failed%d\n", i);
+                madc->requests[i].result_pending = 1;
+        }
+        for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
+                r = &madc->requests[i];
+                /* No pending results for this method, move to next one */
+                if (!r->result_pending)
+                        continue;
+                method = &twl4030_conversion_methods[r->method];
+                /* Read results */
+                len = twl4030_madc_read_channels(madc, method->rbase,
+                                                 r->channels, r->rbuf);
+                /* Return results to caller */
+                if (r->func_cb != NULL) {
+                        r->func_cb(len, r->channels, r->rbuf);
+                        r->func_cb = NULL;
+                }
+                /* Free request */
+                r->result_pending = 0;
+                r->active = 0;
+        }
+        mutex_unlock(&madc->lock);
+
+        return IRQ_HANDLED;
+
+err_i2c:
+        /*
+         * In case of error check whichever request is active
+         * and service the same.
+         */
+        for (i = 0; i < TWL4030_MADC_NUM_METHODS; i++) {
+                r = &madc->requests[i];
+                if (r->active == 0)
+                        continue;
+                method = &twl4030_conversion_methods[r->method];
+                /* Read results */
+                len = twl4030_madc_read_channels(madc, method->rbase,
+                                                 r->channels, r->rbuf);
+                /* Return results to caller */
+                if (r->func_cb != NULL) {
+                        r->func_cb(len, r->channels, r->rbuf);
+                        r->func_cb = NULL;
+                }
+                /* Free request */
+                r->result_pending = 0;
+                r->active = 0;
+        }
+        mutex_unlock(&madc->lock);
+
+        return IRQ_HANDLED;
+}
+
+static int twl4030_madc_set_irq(struct twl4030_madc_data *madc,
+                                struct twl4030_madc_request *req)
+{
+        struct twl4030_madc_request *p;
+        int ret;
+
+        p = &madc->requests[req->method];
+        memcpy(p, req, sizeof(*req));
+        ret = twl4030_madc_enable_irq(madc, req->method);
+        if (ret < 0) {
+                dev_err(madc->dev, "enable irq failed!!\n");
+                return ret;
+        }
+
+        return 0;
+}
+
+/*
+ * Function which enables the madc conversion
+ * by writing to the control register.
+ * @madc - pointer to twl4030_madc_data struct
+ * @conv_method - can be TWL4030_MADC_RT, TWL4030_MADC_SW2, TWL4030_MADC_SW1
+ * corresponding to RT SW1 or SW2 conversion methods.
+ * Returns 0 if succeeds else a negative error value
+ */
+static int twl4030_madc_start_conversion(struct twl4030_madc_data *madc,
+                                         int conv_method)
+{
+        const struct twl4030_madc_conversion_method *method;
+        int ret = 0;
+        method = &twl4030_conversion_methods[conv_method];
+        switch (conv_method) {
+        case TWL4030_MADC_SW1:
+        case TWL4030_MADC_SW2:
+                ret = twl_i2c_write_u8(TWL4030_MODULE_MADC,
+                                       TWL4030_MADC_SW_START, method->ctrl);
+                if (ret) {
+                        dev_err(madc->dev,
+                                "unable to write ctrl register 0x%X\n",
+                                method->ctrl);
+                        return ret;
+                }
+                break;
+        default:
+                break;
+        }
+
+        return 0;
+}
+
+/*
+ * Function that waits for conversion to be ready
+ * @madc - pointer to twl4030_madc_data struct
+ * @timeout_ms - timeout value in milliseconds
+ * @status_reg - ctrl register
+ * returns 0 if succeeds else a negative error value
+ */
+static int twl4030_madc_wait_conversion_ready(struct twl4030_madc_data *madc,
+                                              unsigned int timeout_ms,
+                                              u8 status_reg)
+{
+        unsigned long timeout;
+        int ret;
+
+        timeout = jiffies + msecs_to_jiffies(timeout_ms);
+        do {
+                u8 reg;
+
+                ret = twl_i2c_read_u8(TWL4030_MODULE_MADC, &reg, status_reg);
+                if (ret) {
+                        dev_err(madc->dev,
+                                "unable to read status register 0x%X\n",
+                                status_reg);
+                        return ret;
+                }
+                if (!(reg & TWL4030_MADC_BUSY) && (reg & TWL4030_MADC_EOC_SW))
+                        return 0;
+                usleep_range(500, 2000);
+        } while (!time_after(jiffies, timeout));
+        dev_err(madc->dev, "conversion timeout!\n");
+
+        return -EAGAIN;
+}
+
+/*
+ * An exported function which can be called from other kernel drivers.
+ * @req twl4030_madc_request structure
+ * req->rbuf will be filled with read values of channels based on the
+ * channel index. If a particular channel reading fails there will
+ * be a negative error value in the corresponding array element.
+ * returns 0 if succeeds else error value
+ */
+int twl4030_madc_conversion(struct twl4030_madc_request *req)
+{
+        const struct twl4030_madc_conversion_method *method;
+        u8 ch_msb, ch_lsb;
+        int ret;
+
+        if (!req)
+                return -EINVAL;
+        mutex_lock(&twl4030_madc->lock);
+        if (req->method < TWL4030_MADC_RT || req->method > TWL4030_MADC_SW2) {
+                ret = -EINVAL;
+                goto out;
+        }
+        /* Do we have a conversion request ongoing */
+        if (twl4030_madc->requests[req->method].active) {
+                ret = -EBUSY;
+                goto out;
+        }
+        ch_msb = (req->channels >> 8) & 0xff;
+        ch_lsb = req->channels & 0xff;
+        method = &twl4030_conversion_methods[req->method];
+        /* Select channels to be converted */
+        ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, ch_msb, method->sel + 1);
+        if (ret) {
+                dev_err(twl4030_madc->dev,
+                        "unable to write sel register 0x%X\n", method->sel + 1);
+                return ret;
+        }
+        ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, ch_lsb, method->sel);
+        if (ret) {
+                dev_err(twl4030_madc->dev,
+                        "unable to write sel register 0x%X\n", method->sel + 1);
+                return ret;
+        }
+        /* Select averaging for all channels if do_avg is set */
+        if (req->do_avg) {
+                ret = twl_i2c_write_u8(TWL4030_MODULE_MADC,
+                                       ch_msb, method->avg + 1);
+                if (ret) {
+                        dev_err(twl4030_madc->dev,
+                                "unable to write avg register 0x%X\n",
+                                method->avg + 1);
+                        return ret;
+                }
+                ret = twl_i2c_write_u8(TWL4030_MODULE_MADC,
+                                       ch_lsb, method->avg);
+                if (ret) {
+                        dev_err(twl4030_madc->dev,
+                                "unable to write sel reg 0x%X\n",
+                                method->sel + 1);
+                        return ret;
+                }
+        }
+        if (req->type == TWL4030_MADC_IRQ_ONESHOT && req->func_cb != NULL) {
+                ret = twl4030_madc_set_irq(twl4030_madc, req);
+                if (ret < 0)
+                        goto out;
+                ret = twl4030_madc_start_conversion(twl4030_madc, req->method);
+                if (ret < 0)
+                        goto out;
+                twl4030_madc->requests[req->method].active = 1;
+                ret = 0;
+                goto out;
+        }
+        /* With RT method we should not be here anymore */
+        if (req->method == TWL4030_MADC_RT) {
+                ret = -EINVAL;
+                goto out;
+        }
+        ret = twl4030_madc_start_conversion(twl4030_madc, req->method);
+        if (ret < 0)
+                goto out;
+        twl4030_madc->requests[req->method].active = 1;
+        /* Wait until conversion is ready (ctrl register returns EOC) */
+        ret = twl4030_madc_wait_conversion_ready(twl4030_madc, 5, method->ctrl);
+        if (ret) {
+                twl4030_madc->requests[req->method].active = 0;
+                goto out;
+        }
+        ret = twl4030_madc_read_channels(twl4030_madc, method->rbase,
+                                         req->channels, req->rbuf);
+        twl4030_madc->requests[req->method].active = 0;
+
+out:
+        mutex_unlock(&twl4030_madc->lock);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(twl4030_madc_conversion);
+
+/*
+ * Return channel value
+ * Or < 0 on failure.
+ */
+int twl4030_get_madc_conversion(int channel_no)
+{
+        struct twl4030_madc_request req;
+        int temp = 0;
+        int ret;
+
+        req.channels = (1 << channel_no);
+        req.method = TWL4030_MADC_SW2;
+        req.active = 0;
+        req.func_cb = NULL;
+        ret = twl4030_madc_conversion(&req);
+        if (ret < 0)
+                return ret;
+        if (req.rbuf[channel_no] > 0)
+                temp = req.rbuf[channel_no];
+
+        return temp;
+}
+EXPORT_SYMBOL_GPL(twl4030_get_madc_conversion);
+
+/*
+ * Function to enable or disable bias current for
+ * main battery type reading or temperature sensing
+ * @madc - pointer to twl4030_madc_data struct
+ * @chan - can be one of the two values
+ * TWL4030_BCI_ITHEN - Enables bias current for main battery type reading
+ * TWL4030_BCI_TYPEN - Enables bias current for main battery temperature
+ * sensing
+ * @on - enable or disable chan.
+ */
+static int twl4030_madc_set_current_generator(struct twl4030_madc_data *madc,
+                                              int chan, int on)
+{
+        int ret;
+        u8 regval;
+
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
+                              &regval, TWL4030_BCI_BCICTL1);
+        if (ret) {
+                dev_err(madc->dev, "unable to read BCICTL1 reg 0x%X",
+                        TWL4030_BCI_BCICTL1);
+                return ret;
+        }
+        if (on)
+                regval |= chan ? TWL4030_BCI_ITHEN : TWL4030_BCI_TYPEN;
+        else
+                regval &= chan ? ~TWL4030_BCI_ITHEN : ~TWL4030_BCI_TYPEN;
+        ret = twl_i2c_write_u8(TWL4030_MODULE_MAIN_CHARGE,
+                               regval, TWL4030_BCI_BCICTL1);
+        if (ret) {
+                dev_err(madc->dev, "unable to write BCICTL1 reg 0x%X\n",
+                        TWL4030_BCI_BCICTL1);
+                return ret;
+        }
+
+        return 0;
+}
+
+/*
+ * Function that sets MADC software power on bit to enable MADC
+ * @madc - pointer to twl4030_madc_data struct
+ * @on - Enable or disable MADC software powen on bit.
+ * returns error if i2c read/write fails else 0
+ */
+static int twl4030_madc_set_power(struct twl4030_madc_data *madc, int on)
+{
+        u8 regval;
+        int ret;
+
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
+                              &regval, TWL4030_MADC_CTRL1);
+        if (ret) {
+                dev_err(madc->dev, "unable to read madc ctrl1 reg 0x%X\n",
+                        TWL4030_MADC_CTRL1);
+                return ret;
+        }
+        if (on)
+                regval |= TWL4030_MADC_MADCON;
+        else
+                regval &= ~TWL4030_MADC_MADCON;
+        ret = twl_i2c_write_u8(TWL4030_MODULE_MADC, regval, TWL4030_MADC_CTRL1);
+        if (ret) {
+                dev_err(madc->dev, "unable to write madc ctrl1 reg 0x%X\n",
+                        TWL4030_MADC_CTRL1);
+                return ret;
+        }
+
+        return 0;
+}
+
+/*
+ * Initialize MADC and request for threaded irq
+ */
+static int __devinit twl4030_madc_probe(struct platform_device *pdev)
+{
+        struct twl4030_madc_data *madc;
+        struct twl4030_madc_platform_data *pdata = pdev->dev.platform_data;
+        int ret;
+        u8 regval;
+
+        if (!pdata) {
+                dev_err(&pdev->dev, "platform_data not available\n");
+                return -EINVAL;
+        }
+        madc = kzalloc(sizeof(*madc), GFP_KERNEL);
+        if (!madc)
+                return -ENOMEM;
+
+        /*
+         * Phoenix provides 2 interrupt lines. The first one is connected to
+         * the OMAP. The other one can be connected to the other processor such
+         * as modem. Hence two separate ISR and IMR registers.
+         */
+        madc->imr = (pdata->irq_line == 1) ?
+            TWL4030_MADC_IMR1 : TWL4030_MADC_IMR2;
+        madc->isr = (pdata->irq_line == 1) ?
+            TWL4030_MADC_ISR1 : TWL4030_MADC_ISR2;
+        ret = twl4030_madc_set_power(madc, 1);
+        if (ret < 0)
+                goto err_power;
+        ret = twl4030_madc_set_current_generator(madc, 0, 1);
+        if (ret < 0)
+                goto err_current_generator;
+
+        ret = twl_i2c_read_u8(TWL4030_MODULE_MAIN_CHARGE,
+                              &regval, TWL4030_BCI_BCICTL1);
+        if (ret) {
+                dev_err(&pdev->dev, "unable to read reg BCI CTL1 0x%X\n",
+                        TWL4030_BCI_BCICTL1);
+                goto err_i2c;
+        }
+        regval |= TWL4030_BCI_MESBAT;
+        ret = twl_i2c_write_u8(TWL4030_MODULE_MAIN_CHARGE,
+                               regval, TWL4030_BCI_BCICTL1);
+        if (ret) {
+                dev_err(&pdev->dev, "unable to write reg BCI Ctl1 0x%X\n",
+                        TWL4030_BCI_BCICTL1);
+                goto err_i2c;
+        }
+        platform_set_drvdata(pdev, madc);
+        mutex_init(&madc->lock);
+        ret = request_threaded_irq(platform_get_irq(pdev, 0), NULL,
+                                   twl4030_madc_threaded_irq_handler,
+                                   IRQF_TRIGGER_RISING, "twl4030_madc", madc);
+        if (ret) {
+                dev_dbg(&pdev->dev, "could not request irq\n");
+                goto err_irq;
+        }
+        twl4030_madc = madc;
+        return 0;
+err_irq:
+        platform_set_drvdata(pdev, NULL);
+err_i2c:
+        twl4030_madc_set_current_generator(madc, 0, 0);
+err_current_generator:
+        twl4030_madc_set_power(madc, 0);
+err_power:
+        kfree(madc);
+
+        return ret;
+}
+
+static int __devexit twl4030_madc_remove(struct platform_device *pdev)
+{
+        struct twl4030_madc_data *madc = platform_get_drvdata(pdev);
+
+        free_irq(platform_get_irq(pdev, 0), madc);
+        platform_set_drvdata(pdev, NULL);
+        twl4030_madc_set_current_generator(madc, 0, 0);
+        twl4030_madc_set_power(madc, 0);
+        kfree(madc);
+
+        return 0;
+}
+
+static struct platform_driver twl4030_madc_driver = {
+        .probe = twl4030_madc_probe,
+        .remove = __exit_p(twl4030_madc_remove),
+        .driver = {
+                   .name = "twl4030_madc",
+                   .owner = THIS_MODULE,
+                   },
+};
+
+static int __init twl4030_madc_init(void)
+{
+        return platform_driver_register(&twl4030_madc_driver);
+}
+
+module_init(twl4030_madc_init);
+
+static void __exit twl4030_madc_exit(void)
+{
+        platform_driver_unregister(&twl4030_madc_driver);
+}
+
+module_exit(twl4030_madc_exit);
+
+MODULE_DESCRIPTION("TWL4030 ADC driver");
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("J Keerthy");
+MODULE_ALIAS("platform:twl4030_madc");
diff --git a/drivers/mfd/ucb1x00-ts.c b/drivers/mfd/ucb1x00-ts.c
index 92b85e28a15e..38ffbd50a0d2 100644
--- a/drivers/mfd/ucb1x00-ts.c
+++ b/drivers/mfd/ucb1x00-ts.c
@@ -60,6 +60,7 @@ static inline void ucb1x00_ts_evt_add(struct ucb1x00_ts *ts, u16 pressure, u16 x
         input_report_abs(idev, ABS_X, x);
         input_report_abs(idev, ABS_Y, y);
         input_report_abs(idev, ABS_PRESSURE, pressure);
+        input_report_key(idev, BTN_TOUCH, 1);
         input_sync(idev);
 }
 
@@ -68,6 +69,7 @@ static inline void ucb1x00_ts_event_release(struct ucb1x00_ts *ts)
         struct input_dev *idev = ts->idev;
 
         input_report_abs(idev, ABS_PRESSURE, 0);
+        input_report_key(idev, BTN_TOUCH, 0);
         input_sync(idev);
 }
 
@@ -384,7 +386,8 @@ static int ucb1x00_ts_add(struct ucb1x00_dev *dev)
         idev->open       = ucb1x00_ts_open;
         idev->close      = ucb1x00_ts_close;
 
-        __set_bit(EV_ABS, idev->evbit);
+        idev->evbit[0]   = BIT_MASK(EV_ABS) | BIT_MASK(EV_KEY);
+        idev->keybit[BIT_WORD(BTN_TOUCH)] = BIT_MASK(BTN_TOUCH);
 
         input_set_drvdata(idev, ts);
 
diff --git a/drivers/mfd/vx855.c b/drivers/mfd/vx855.c
index 348052aa5dbf..d698703dbd46 100644
--- a/drivers/mfd/vx855.c
+++ b/drivers/mfd/vx855.c
@@ -122,6 +122,7 @@ static struct pci_device_id vx855_pci_tbl[] = {
         { PCI_DEVICE(PCI_VENDOR_ID_VIA, PCI_DEVICE_ID_VIA_VX855) },
         { 0, }
 };
+MODULE_DEVICE_TABLE(pci, vx855_pci_tbl);
 
 static struct pci_driver vx855_pci_driver = {
         .name           = "vx855",
diff --git a/drivers/mfd/wl1273-core.c b/drivers/mfd/wl1273-core.c
index d2ecc2435736..529d65ba5353 100644
--- a/drivers/mfd/wl1273-core.c
+++ b/drivers/mfd/wl1273-core.c
@@ -38,7 +38,6 @@ static int wl1273_core_remove(struct i2c_client *client)
         dev_dbg(&client->dev, "%s\n", __func__);
 
         mfd_remove_devices(&client->dev);
-        i2c_set_clientdata(client, NULL);
         kfree(core);
 
         return 0;
@@ -79,8 +78,7 @@ static int __devinit wl1273_core_probe(struct i2c_client *client,
 
         cell = &core->cells[children];
         cell->name = "wl1273_fm_radio";
-        cell->platform_data = &core;
-        cell->data_size = sizeof(core);
+        cell->mfd_data = &core;
         children++;
 
         if (pdata->children & WL1273_CODEC_CHILD) {
@@ -88,8 +86,7 @@ static int __devinit wl1273_core_probe(struct i2c_client *client,
 
                 dev_dbg(&client->dev, "%s: Have codec.\n", __func__);
                 cell->name = "wl1273-codec";
-                cell->platform_data = &core;
-                cell->data_size = sizeof(core);
+                cell->mfd_data = &core;
                 children++;
         }
 
@@ -104,7 +101,6 @@ static int __devinit wl1273_core_probe(struct i2c_client *client,
         return 0;
 
 err:
-        i2c_set_clientdata(client, NULL);
         pdata->free_resources();
         kfree(core);
 
diff --git a/drivers/mfd/wm831x-i2c.c b/drivers/mfd/wm831x-i2c.c
index 3853fa8e7cc2..a06cbc739716 100644
--- a/drivers/mfd/wm831x-i2c.c
+++ b/drivers/mfd/wm831x-i2c.c
@@ -51,17 +51,25 @@ static int wm831x_i2c_write_device(struct wm831x *wm831x, unsigned short reg,
                                    int bytes, void *src)
 {
         struct i2c_client *i2c = wm831x->control_data;
-        unsigned char msg[bytes + 2];
+        struct i2c_msg xfer[2];
         int ret;
 
         reg = cpu_to_be16(reg);
-        memcpy(&msg[0], &reg, 2);
-        memcpy(&msg[2], src, bytes);
 
-        ret = i2c_master_send(i2c, msg, bytes + 2);
+        xfer[0].addr = i2c->addr;
+        xfer[0].flags = 0;
+        xfer[0].len = 2;
+        xfer[0].buf = (char *)&reg;
+
+        xfer[1].addr = i2c->addr;
+        xfer[1].flags = I2C_M_NOSTART;
+        xfer[1].len = bytes;
+        xfer[1].buf = (char *)src;
+
+        ret = i2c_transfer(i2c->adapter, xfer, 2);
         if (ret < 0)
                 return ret;
-        if (ret < bytes + 2)
+        if (ret != 2)
                 return -EIO;
 
         return 0;
diff --git a/drivers/mfd/wm831x-irq.c b/drivers/mfd/wm831x-irq.c
index f7192d438aab..a5cd17e18d09 100644
--- a/drivers/mfd/wm831x-irq.c
+++ b/drivers/mfd/wm831x-irq.c
@@ -26,15 +26,6 @@
 
 #include <linux/delay.h>
 
-/*
- * Since generic IRQs don't currently support interrupt controllers on
- * interrupt driven buses we don't use genirq but instead provide an
- * interface that looks very much like the standard ones.  This leads
- * to some bodges, including storing interrupt handler information in
- * the static irq_data table we use to look up the data for individual
- * interrupts, but hopefully won't last too long.
- */
-
 struct wm831x_irq_data {
         int primary;
         int reg;
@@ -361,6 +352,10 @@ static void wm831x_irq_sync_unlock(struct irq_data *data)
                 /* If there's been a change in the mask write it back
                  * to the hardware. */
                 if (wm831x->irq_masks_cur[i] != wm831x->irq_masks_cache[i]) {
+                        dev_dbg(wm831x->dev, "IRQ mask sync: %x = %x\n",
+                                WM831X_INTERRUPT_STATUS_1_MASK + i,
+                                wm831x->irq_masks_cur[i]);
+
                         wm831x->irq_masks_cache[i] = wm831x->irq_masks_cur[i];
                         wm831x_reg_write(wm831x,
                                          WM831X_INTERRUPT_STATUS_1_MASK + i,
@@ -371,7 +366,7 @@ static void wm831x_irq_sync_unlock(struct irq_data *data)
         mutex_unlock(&wm831x->irq_lock);
 }
 
-static void wm831x_irq_unmask(struct irq_data *data)
+static void wm831x_irq_enable(struct irq_data *data)
 {
         struct wm831x *wm831x = irq_data_get_irq_chip_data(data);
         struct wm831x_irq_data *irq_data = irq_to_wm831x_irq(wm831x,
@@ -380,7 +375,7 @@ static void wm831x_irq_unmask(struct irq_data *data)
         wm831x->irq_masks_cur[irq_data->reg - 1] &= ~irq_data->mask;
 }
 
-static void wm831x_irq_mask(struct irq_data *data)
+static void wm831x_irq_disable(struct irq_data *data)
 {
         struct wm831x *wm831x = irq_data_get_irq_chip_data(data);
         struct wm831x_irq_data *irq_data = irq_to_wm831x_irq(wm831x,
@@ -426,8 +421,8 @@ static struct irq_chip wm831x_irq_chip = {
         .name                   = "wm831x",
         .irq_bus_lock           = wm831x_irq_lock,
         .irq_bus_sync_unlock    = wm831x_irq_sync_unlock,
-        .irq_mask               = wm831x_irq_mask,
-        .irq_unmask             = wm831x_irq_unmask,
+        .irq_disable            = wm831x_irq_disable,
+        .irq_enable             = wm831x_irq_enable,
         .irq_set_type           = wm831x_irq_set_type,
 };
 
@@ -449,6 +444,18 @@ static irqreturn_t wm831x_irq_thread(int irq, void *data)
                 goto out;
         }
 
+        /* The touch interrupts are visible in the primary register as
+         * an optimisation; open code this to avoid complicating the
+         * main handling loop and so we can also skip iterating the
+         * descriptors.
+         */
+        if (primary & WM831X_TCHPD_INT)
+                handle_nested_irq(wm831x->irq_base + WM831X_IRQ_TCHPD);
+        if (primary & WM831X_TCHDATA_INT)
+                handle_nested_irq(wm831x->irq_base + WM831X_IRQ_TCHDATA);
+        if (primary & (WM831X_TCHDATA_EINT | WM831X_TCHPD_EINT))
+                goto out;
+
         for (i = 0; i < ARRAY_SIZE(wm831x_irqs); i++) {
                 int offset = wm831x_irqs[i].reg - 1;
 
@@ -481,6 +488,9 @@ static irqreturn_t wm831x_irq_thread(int irq, void *data)
         }
 
 out:
+        /* Touchscreen interrupts are handled specially in the driver */
+        status_regs[0] &= ~(WM831X_TCHDATA_EINT | WM831X_TCHPD_EINT);
+
         for (i = 0; i < ARRAY_SIZE(status_regs); i++) {
                 if (status_regs[i])
                         wm831x_reg_write(wm831x, WM831X_INTERRUPT_STATUS_1 + i,
@@ -517,6 +527,14 @@ int wm831x_irq_init(struct wm831x *wm831x, int irq)
                 return 0;
         }
 
+        if (pdata->irq_cmos)
+                i = 0;
+        else
+                i = WM831X_IRQ_OD;
+
+        wm831x_set_bits(wm831x, WM831X_IRQ_CONFIG,
+                        WM831X_IRQ_OD, i);
+
         /* Try to flag /IRQ as a wake source; there are a number of
          * unconditional wake sources in the PMIC so this isn't
          * conditional but we don't actually care *too* much if it
diff --git a/drivers/mfd/wm831x-spi.c b/drivers/mfd/wm831x-spi.c
index 0a8f772be88c..eed8e4f7a5a1 100644
--- a/drivers/mfd/wm831x-spi.c
+++ b/drivers/mfd/wm831x-spi.c
@@ -14,6 +14,7 @@
 
 #include <linux/kernel.h>
 #include <linux/module.h>
+#include <linux/pm.h>
 #include <linux/spi/spi.h>
 
 #include <linux/mfd/wm831x/core.h>
@@ -113,22 +114,27 @@ static int __devexit wm831x_spi_remove(struct spi_device *spi)
         return 0;
 }
 
-static int wm831x_spi_suspend(struct spi_device *spi, pm_message_t m)
+static int wm831x_spi_suspend(struct device *dev)
 {
-        struct wm831x *wm831x = dev_get_drvdata(&spi->dev);
+        struct wm831x *wm831x = dev_get_drvdata(dev);
 
         return wm831x_device_suspend(wm831x);
 }
 
+static const struct dev_pm_ops wm831x_spi_pm = {
+        .freeze = wm831x_spi_suspend,
+        .suspend = wm831x_spi_suspend,
+};
+
 static struct spi_driver wm8310_spi_driver = {
         .driver = {
                 .name   = "wm8310",
                 .bus    = &spi_bus_type,
                 .owner  = THIS_MODULE,
+                .pm     = &wm831x_spi_pm,
         },
         .probe          = wm831x_spi_probe,
         .remove         = __devexit_p(wm831x_spi_remove),
-        .suspend        = wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8311_spi_driver = {
@@ -136,10 +142,10 @@ static struct spi_driver wm8311_spi_driver = {
                 .name   = "wm8311",
                 .bus    = &spi_bus_type,
                 .owner  = THIS_MODULE,
+                .pm     = &wm831x_spi_pm,
         },
         .probe          = wm831x_spi_probe,
         .remove         = __devexit_p(wm831x_spi_remove),
-        .suspend        = wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8312_spi_driver = {
@@ -147,10 +153,10 @@ static struct spi_driver wm8312_spi_driver = {
                 .name   = "wm8312",
                 .bus    = &spi_bus_type,
                 .owner  = THIS_MODULE,
+                .pm     = &wm831x_spi_pm,
         },
         .probe          = wm831x_spi_probe,
         .remove         = __devexit_p(wm831x_spi_remove),
-        .suspend        = wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8320_spi_driver = {
@@ -158,10 +164,10 @@ static struct spi_driver wm8320_spi_driver = {
                 .name   = "wm8320",
                 .bus    = &spi_bus_type,
                 .owner  = THIS_MODULE,
+                .pm     = &wm831x_spi_pm,
         },
         .probe          = wm831x_spi_probe,
         .remove         = __devexit_p(wm831x_spi_remove),
-        .suspend        = wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8321_spi_driver = {
@@ -169,10 +175,10 @@ static struct spi_driver wm8321_spi_driver = {
                 .name   = "wm8321",
                 .bus    = &spi_bus_type,
                 .owner  = THIS_MODULE,
+                .pm     = &wm831x_spi_pm,
         },
         .probe          = wm831x_spi_probe,
         .remove         = __devexit_p(wm831x_spi_remove),
-        .suspend        = wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8325_spi_driver = {
@@ -180,10 +186,10 @@ static struct spi_driver wm8325_spi_driver = {
                 .name   = "wm8325",
                 .bus    = &spi_bus_type,
                 .owner  = THIS_MODULE,
+                .pm     = &wm831x_spi_pm,
         },
         .probe          = wm831x_spi_probe,
         .remove         = __devexit_p(wm831x_spi_remove),
-        .suspend        = wm831x_spi_suspend,
 };
 
 static struct spi_driver wm8326_spi_driver = {
@@ -191,10 +197,10 @@ static struct spi_driver wm8326_spi_driver = {
                 .name   = "wm8326",
                 .bus    = &spi_bus_type,
                 .owner  = THIS_MODULE,
+                .pm     = &wm831x_spi_pm,
         },
         .probe          = wm831x_spi_probe,
         .remove         = __devexit_p(wm831x_spi_remove),
-        .suspend        = wm831x_spi_suspend,
 };
 
 static int __init wm831x_spi_init(void)
diff --git a/drivers/mfd/wm8400-core.c b/drivers/mfd/wm8400-core.c
index 1bfef4846b07..3a6e78cb0384 100644
--- a/drivers/mfd/wm8400-core.c
+++ b/drivers/mfd/wm8400-core.c
@@ -245,7 +245,7 @@ static int wm8400_register_codec(struct wm8400 *wm8400)
 {
         struct mfd_cell cell = {
                 .name = "wm8400-codec",
-                .driver_data = wm8400,
+                .mfd_data = wm8400,
         };
 
         return mfd_add_devices(wm8400->dev, -1, &cell, 1, NULL, 0);
diff --git a/drivers/mfd/wm8994-core.c b/drivers/mfd/wm8994-core.c
index f4016a075fd6..e198d40292e7 100644
--- a/drivers/mfd/wm8994-core.c
+++ b/drivers/mfd/wm8994-core.c
@@ -40,10 +40,8 @@ static int wm8994_read(struct wm8994 *wm8994, unsigned short reg,
                 return ret;
 
         for (i = 0; i < bytes / 2; i++) {
-                buf[i] = be16_to_cpu(buf[i]);
-
                 dev_vdbg(wm8994->dev, "Read %04x from R%d(0x%x)\n",
-                         buf[i], reg + i, reg + i);
+                         be16_to_cpu(buf[i]), reg + i, reg + i);
         }
 
         return 0;
@@ -69,7 +67,7 @@ int wm8994_reg_read(struct wm8994 *wm8994, unsigned short reg)
         if (ret < 0)
                 return ret;
         else
-                return val;
+                return be16_to_cpu(val);
 }
 EXPORT_SYMBOL_GPL(wm8994_reg_read);
 
@@ -79,7 +77,7 @@ EXPORT_SYMBOL_GPL(wm8994_reg_read);
  * @wm8994: Device to read from
  * @reg: First register
  * @count: Number of registers
- * @buf: Buffer to fill.
+ * @buf: Buffer to fill.  The data will be returned big endian.
  */
 int wm8994_bulk_read(struct wm8994 *wm8994, unsigned short reg,
                      int count, u16 *buf)
@@ -97,9 +95,9 @@ int wm8994_bulk_read(struct wm8994 *wm8994, unsigned short reg,
 EXPORT_SYMBOL_GPL(wm8994_bulk_read);
 
 static int wm8994_write(struct wm8994 *wm8994, unsigned short reg,
-                        int bytes, void *src)
+                        int bytes, const void *src)
 {
-        u16 *buf = src;
+        const u16 *buf = src;
         int i;
 
         BUG_ON(bytes % 2);
@@ -107,9 +105,7 @@ static int wm8994_write(struct wm8994 *wm8994, unsigned short reg,
 
         for (i = 0; i < bytes / 2; i++) {
                 dev_vdbg(wm8994->dev, "Write %04x to R%d(0x%x)\n",
-                         buf[i], reg + i, reg + i);
-
-                buf[i] = cpu_to_be16(buf[i]);
+                         be16_to_cpu(buf[i]), reg + i, reg + i);
         }
 
         return wm8994->write_dev(wm8994, reg, bytes, src);
@@ -127,6 +123,8 @@ int wm8994_reg_write(struct wm8994 *wm8994, unsigned short reg,
 {
         int ret;
 
+        val = cpu_to_be16(val);
+
         mutex_lock(&wm8994->io_lock);
 
         ret = wm8994_write(wm8994, reg, 2, &val);
@@ -138,6 +136,29 @@ int wm8994_reg_write(struct wm8994 *wm8994, unsigned short reg,
 EXPORT_SYMBOL_GPL(wm8994_reg_write);
 
 /**
+ * wm8994_bulk_write: Write multiple WM8994 registers
+ *
+ * @wm8994: Device to write to
+ * @reg: First register
+ * @count: Number of registers
+ * @buf: Buffer to write from.  Data must be big-endian formatted.
+ */
+int wm8994_bulk_write(struct wm8994 *wm8994, unsigned short reg,
+                      int count, const u16 *buf)
+{
+        int ret;
+
+        mutex_lock(&wm8994->io_lock);
+
+        ret = wm8994_write(wm8994, reg, count * 2, buf);
+
+        mutex_unlock(&wm8994->io_lock);
+
+        return ret;
+}
+EXPORT_SYMBOL_GPL(wm8994_bulk_write);
+
+/**
  * wm8994_set_bits: Set the value of a bitfield in a WM8994 register
  *
  * @wm8994: Device to write to.
@@ -157,9 +178,13 @@ int wm8994_set_bits(struct wm8994 *wm8994, unsigned short reg,
         if (ret < 0)
                 goto out;
 
+        r = be16_to_cpu(r);
+
         r &= ~mask;
         r |= val;
 
+        r = cpu_to_be16(r);
+
         ret = wm8994_write(wm8994, reg, 2, &r);
 
 out:
@@ -271,6 +296,11 @@ static int wm8994_suspend(struct device *dev)
         if (ret < 0)
                 dev_err(dev, "Failed to save LDO registers: %d\n", ret);
 
+        /* Explicitly put the device into reset in case regulators
+         * don't get disabled in order to ensure consistent restart.
+         */
+        wm8994_reg_write(wm8994, WM8994_SOFTWARE_RESET, 0x8994);
+
         wm8994->suspended = true;
 
         ret = regulator_bulk_disable(wm8994->num_supplies,
@@ -552,25 +582,29 @@ static int wm8994_i2c_read_device(struct wm8994 *wm8994, unsigned short reg,
         return 0;
 }
 
-/* Currently we allocate the write buffer on the stack; this is OK for
- * small writes - if we need to do large writes this will need to be
- * revised.
- */
 static int wm8994_i2c_write_device(struct wm8994 *wm8994, unsigned short reg,
-                                   int bytes, void *src)
+                                   int bytes, const void *src)
 {
         struct i2c_client *i2c = wm8994->control_data;
-        unsigned char msg[bytes + 2];
+        struct i2c_msg xfer[2];
         int ret;
 
         reg = cpu_to_be16(reg);
-        memcpy(&msg[0], &reg, 2);
-        memcpy(&msg[2], src, bytes);
 
-        ret = i2c_master_send(i2c, msg, bytes + 2);
+        xfer[0].addr = i2c->addr;
+        xfer[0].flags = 0;
+        xfer[0].len = 2;
+        xfer[0].buf = (char *)&reg;
+
+        xfer[1].addr = i2c->addr;
+        xfer[1].flags = I2C_M_NOSTART;
+        xfer[1].len = bytes;
+        xfer[1].buf = (char *)src;
+
+        ret = i2c_transfer(i2c->adapter, xfer, 2);
         if (ret < 0)
                 return ret;
-        if (ret < bytes + 2)
+        if (ret != 2)
                 return -EIO;
 
         return 0;
@@ -612,7 +646,8 @@ static const struct i2c_device_id wm8994_i2c_id[] = {
 };
 MODULE_DEVICE_TABLE(i2c, wm8994_i2c_id);
 
-UNIVERSAL_DEV_PM_OPS(wm8994_pm_ops, wm8994_suspend, wm8994_resume, NULL);
+static UNIVERSAL_DEV_PM_OPS(wm8994_pm_ops, wm8994_suspend, wm8994_resume,
+                            NULL);
 
 static struct i2c_driver wm8994_i2c_driver = {
         .driver = {
diff --git a/drivers/mfd/wm8994-irq.c b/drivers/mfd/wm8994-irq.c
index 29e8faf9c01c..1e3bf4a2ff8e 100644
--- a/drivers/mfd/wm8994-irq.c
+++ b/drivers/mfd/wm8994-irq.c
@@ -182,7 +182,7 @@ static void wm8994_irq_sync_unlock(struct irq_data *data)
         mutex_unlock(&wm8994->irq_lock);
 }
 
-static void wm8994_irq_unmask(struct irq_data *data)
+static void wm8994_irq_enable(struct irq_data *data)
 {
         struct wm8994 *wm8994 = irq_data_get_irq_chip_data(data);
         struct wm8994_irq_data *irq_data = irq_to_wm8994_irq(wm8994,
@@ -191,7 +191,7 @@ static void wm8994_irq_unmask(struct irq_data *data)
         wm8994->irq_masks_cur[irq_data->reg - 1] &= ~irq_data->mask;
 }
 
-static void wm8994_irq_mask(struct irq_data *data)
+static void wm8994_irq_disable(struct irq_data *data)
 {
         struct wm8994 *wm8994 = irq_data_get_irq_chip_data(data);
         struct wm8994_irq_data *irq_data = irq_to_wm8994_irq(wm8994,
@@ -204,8 +204,8 @@ static struct irq_chip wm8994_irq_chip = {
         .name                   = "wm8994",
         .irq_bus_lock           = wm8994_irq_lock,
         .irq_bus_sync_unlock    = wm8994_irq_sync_unlock,
-        .irq_mask               = wm8994_irq_mask,
-        .irq_unmask             = wm8994_irq_unmask,
+        .irq_disable            = wm8994_irq_disable,
+        .irq_enable             = wm8994_irq_enable,
 };
 
 /* The processing of the primary interrupt occurs in a thread so that
@@ -225,9 +225,11 @@ static irqreturn_t wm8994_irq_thread(int irq, void *data)
                 return IRQ_NONE;
         }
 
-        /* Apply masking */
-        for (i = 0; i < WM8994_NUM_IRQ_REGS; i++)
+        /* Bit swap and apply masking */
+        for (i = 0; i < WM8994_NUM_IRQ_REGS; i++) {
+                status[i] = be16_to_cpu(status[i]);
                 status[i] &= ~wm8994->irq_masks_cur[i];
+        }
 
         /* Report */
         for (i = 0; i < ARRAY_SIZE(wm8994_irqs); i++) {
diff --git a/drivers/mmc/host/tmio_mmc.c b/drivers/mmc/host/tmio_mmc.c
index ac52eb65395e..ab1adeabdd22 100644
--- a/drivers/mmc/host/tmio_mmc.c
+++ b/drivers/mmc/host/tmio_mmc.c
@@ -303,8 +303,7 @@ static void tmio_mmc_set_clock(struct tmio_mmc_host *host, int new_clock)
 
 static void tmio_mmc_clk_stop(struct tmio_mmc_host *host)
 {
-        struct mfd_cell *cell = host->pdev->dev.platform_data;
-        struct tmio_mmc_data *pdata = cell->driver_data;
+        struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
         /*
          * Testing on sh-mobile showed that SDIO IRQs are unmasked when
@@ -327,8 +326,7 @@ static void tmio_mmc_clk_stop(struct tmio_mmc_host *host)
 
 static void tmio_mmc_clk_start(struct tmio_mmc_host *host)
 {
-        struct mfd_cell *cell = host->pdev->dev.platform_data;
-        struct tmio_mmc_data *pdata = cell->driver_data;
+        struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
         sd_ctrl_write16(host, CTL_SD_CARD_CLK_CTL, 0x0100 |
                 sd_ctrl_read16(host, CTL_SD_CARD_CLK_CTL));
@@ -669,8 +667,7 @@ out:
 static irqreturn_t tmio_mmc_irq(int irq, void *devid)
 {
         struct tmio_mmc_host *host = devid;
-        struct mfd_cell *cell = host->pdev->dev.platform_data;
-        struct tmio_mmc_data *pdata = cell->driver_data;
+        struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
         unsigned int ireg, irq_mask, status;
         unsigned int sdio_ireg, sdio_irq_mask, sdio_status;
 
@@ -799,8 +796,7 @@ static void tmio_mmc_start_dma_rx(struct tmio_mmc_host *host)
         struct scatterlist *sg = host->sg_ptr, *sg_tmp;
         struct dma_async_tx_descriptor *desc = NULL;
         struct dma_chan *chan = host->chan_rx;
-        struct mfd_cell *cell = host->pdev->dev.platform_data;
-        struct tmio_mmc_data *pdata = cell->driver_data;
+        struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
         dma_cookie_t cookie;
         int ret, i;
         bool aligned = true, multiple = true;
@@ -869,8 +865,7 @@ static void tmio_mmc_start_dma_tx(struct tmio_mmc_host *host)
         struct scatterlist *sg = host->sg_ptr, *sg_tmp;
         struct dma_async_tx_descriptor *desc = NULL;
         struct dma_chan *chan = host->chan_tx;
-        struct mfd_cell *cell = host->pdev->dev.platform_data;
-        struct tmio_mmc_data *pdata = cell->driver_data;
+        struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
         dma_cookie_t cookie;
         int ret, i;
         bool aligned = true, multiple = true;
@@ -1063,8 +1058,7 @@ static void tmio_mmc_release_dma(struct tmio_mmc_host *host)
 static int tmio_mmc_start_data(struct tmio_mmc_host *host,
         struct mmc_data *data)
 {
-        struct mfd_cell *cell = host->pdev->dev.platform_data;
-        struct tmio_mmc_data *pdata = cell->driver_data;
+        struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
         pr_debug("setup data transfer: blocksize %08x  nr_blocks %d\n",
                  data->blksz, data->blocks);
@@ -1169,8 +1163,7 @@ static void tmio_mmc_set_ios(struct mmc_host *mmc, struct mmc_ios *ios)
 static int tmio_mmc_get_ro(struct mmc_host *mmc)
 {
         struct tmio_mmc_host *host = mmc_priv(mmc);
-        struct mfd_cell *cell = host->pdev->dev.platform_data;
-        struct tmio_mmc_data *pdata = cell->driver_data;
+        struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
         return ((pdata->flags & TMIO_MMC_WRPROTECT_DISABLE) ||
                 (sd_ctrl_read32(host, CTL_STATUS) & TMIO_STAT_WRPROTECT)) ? 0 : 1;
@@ -1179,8 +1172,7 @@ static int tmio_mmc_get_ro(struct mmc_host *mmc)
 static int tmio_mmc_get_cd(struct mmc_host *mmc)
 {
         struct tmio_mmc_host *host = mmc_priv(mmc);
-        struct mfd_cell *cell = host->pdev->dev.platform_data;
-        struct tmio_mmc_data *pdata = cell->driver_data;
+        struct tmio_mmc_data *pdata = mfd_get_data(host->pdev);
 
         if (!pdata->get_cd)
                 return -ENOSYS;
@@ -1199,7 +1191,7 @@ static const struct mmc_host_ops tmio_mmc_ops = {
 #ifdef CONFIG_PM
 static int tmio_mmc_suspend(struct platform_device *dev, pm_message_t state)
 {
-        struct mfd_cell *cell = (struct mfd_cell *)dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct mmc_host *mmc = platform_get_drvdata(dev);
         int ret;
 
@@ -1214,7 +1206,7 @@ static int tmio_mmc_suspend(struct platform_device *dev, pm_message_t state)
 
 static int tmio_mmc_resume(struct platform_device *dev)
 {
-        struct mfd_cell *cell = (struct mfd_cell *)dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct mmc_host *mmc = platform_get_drvdata(dev);
         int ret = 0;
 
@@ -1237,7 +1229,7 @@ out:
 
 static int __devinit tmio_mmc_probe(struct platform_device *dev)
 {
-        struct mfd_cell *cell = (struct mfd_cell *)dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct tmio_mmc_data *pdata;
         struct resource *res_ctl;
         struct tmio_mmc_host *host;
@@ -1252,7 +1244,7 @@ static int __devinit tmio_mmc_probe(struct platform_device *dev)
         if (!res_ctl)
                 goto out;
 
-        pdata = cell->driver_data;
+        pdata = mfd_get_data(dev);
         if (!pdata || !pdata->hclk)
                 goto out;
 
@@ -1352,7 +1344,7 @@ out:
 
 static int __devexit tmio_mmc_remove(struct platform_device *dev)
 {
-        struct mfd_cell *cell = (struct mfd_cell *)dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct mmc_host *mmc = platform_get_drvdata(dev);
 
         platform_set_drvdata(dev, NULL);
diff --git a/drivers/mtd/nand/tmio_nand.c b/drivers/mtd/nand/tmio_nand.c
index 3041d1f7ae3f..38fb16771f85 100644
--- a/drivers/mtd/nand/tmio_nand.c
+++ b/drivers/mtd/nand/tmio_nand.c
@@ -319,7 +319,7 @@ static int tmio_nand_correct_data(struct mtd_info *mtd, unsigned char *buf,
 
 static int tmio_hw_init(struct platform_device *dev, struct tmio_nand *tmio)
 {
-        struct mfd_cell *cell = dev_get_platdata(&dev->dev);
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         int ret;
 
         if (cell->enable) {
@@ -363,7 +363,7 @@ static int tmio_hw_init(struct platform_device *dev, struct tmio_nand *tmio)
 
 static void tmio_hw_stop(struct platform_device *dev, struct tmio_nand *tmio)
 {
-        struct mfd_cell *cell = dev_get_platdata(&dev->dev);
+        const struct mfd_cell *cell = mfd_get_cell(dev);
 
         tmio_iowrite8(FCR_MODE_POWER_OFF, tmio->fcr + FCR_MODE);
         if (cell->disable)
@@ -372,8 +372,7 @@ static void tmio_hw_stop(struct platform_device *dev, struct tmio_nand *tmio)
 
 static int tmio_probe(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev_get_platdata(&dev->dev);
-        struct tmio_nand_data *data = cell->driver_data;
+        struct tmio_nand_data *data = mfd_get_data(dev);
         struct resource *fcr = platform_get_resource(dev,
                         IORESOURCE_MEM, 0);
         struct resource *ccr = platform_get_resource(dev,
@@ -516,7 +515,7 @@ static int tmio_remove(struct platform_device *dev)
 #ifdef CONFIG_PM
 static int tmio_suspend(struct platform_device *dev, pm_message_t state)
 {
-        struct mfd_cell *cell = dev_get_platdata(&dev->dev);
+        const struct mfd_cell *cell = mfd_get_cell(dev);
 
         if (cell->suspend)
                 cell->suspend(dev);
@@ -527,7 +526,7 @@ static int tmio_suspend(struct platform_device *dev, pm_message_t state)
 
 static int tmio_resume(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev_get_platdata(&dev->dev);
+        const struct mfd_cell *cell = mfd_get_cell(dev);
 
         /* FIXME - is this required or merely another attack of the broken
          * SHARP platform? Looks suspicious.
diff --git a/drivers/mtd/ubi/io.c b/drivers/mtd/ubi/io.c
index aaa6e1e83b29..eededf94f5a6 100644
--- a/drivers/mtd/ubi/io.c
+++ b/drivers/mtd/ubi/io.c
@@ -1345,7 +1345,7 @@ int ubi_dbg_check_write(struct ubi_device *ubi, const void *buf, int pnum,
         if (!(ubi_chk_flags & UBI_CHK_IO))
                 return 0;
 
-        buf1 = __vmalloc(len, GFP_KERNEL | GFP_NOFS, PAGE_KERNEL);
+        buf1 = __vmalloc(len, GFP_NOFS, PAGE_KERNEL);
         if (!buf1) {
                 ubi_err("cannot allocate memory to check writes");
                 return 0;
@@ -1409,7 +1409,7 @@ int ubi_dbg_check_all_ff(struct ubi_device *ubi, int pnum, int offset, int len)
         if (!(ubi_chk_flags & UBI_CHK_IO))
                 return 0;
 
-        buf = __vmalloc(len, GFP_KERNEL | GFP_NOFS, PAGE_KERNEL);
+        buf = __vmalloc(len, GFP_NOFS, PAGE_KERNEL);
         if (!buf) {
                 ubi_err("cannot allocate memory to check for 0xFFs");
                 return 0;
diff --git a/drivers/net/can/janz-ican3.c b/drivers/net/can/janz-ican3.c
index 366f5cc050ae..102b16c6cc97 100644
--- a/drivers/net/can/janz-ican3.c
+++ b/drivers/net/can/janz-ican3.c
@@ -15,6 +15,7 @@
 #include <linux/interrupt.h>
 #include <linux/delay.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 
 #include <linux/netdevice.h>
 #include <linux/can.h>
@@ -1643,7 +1644,7 @@ static int __devinit ican3_probe(struct platform_device *pdev)
         struct device *dev;
         int ret;
 
-        pdata = pdev->dev.platform_data;
+        pdata = mfd_get_data(pdev);
         if (!pdata)
                 return -ENXIO;
 
diff --git a/drivers/net/ks8842.c b/drivers/net/ks8842.c
index 928b2b83cef5..efd44afeae83 100644
--- a/drivers/net/ks8842.c
+++ b/drivers/net/ks8842.c
@@ -26,6 +26,7 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/netdevice.h>
 #include <linux/etherdevice.h>
 #include <linux/ethtool.h>
@@ -1145,7 +1146,7 @@ static int __devinit ks8842_probe(struct platform_device *pdev)
         struct resource *iomem;
         struct net_device *netdev;
         struct ks8842_adapter *adapter;
-        struct ks8842_platform_data *pdata = pdev->dev.platform_data;
+        struct ks8842_platform_data *pdata = mfd_get_data(pdev);
         u16 id;
         unsigned i;
 
diff --git a/drivers/net/mlx4/main.c b/drivers/net/mlx4/main.c
index 2765a3ce9c24..c83501122d77 100644
--- a/drivers/net/mlx4/main.c
+++ b/drivers/net/mlx4/main.c
@@ -1109,6 +1109,9 @@ static int __mlx4_init_one(struct pci_dev *pdev, const struct pci_device_id *id)
                 }
         }
 
+        /* Allow large DMA segments, up to the firmware limit of 1 GB */
+        dma_set_max_seg_size(&pdev->dev, 1024 * 1024 * 1024);
+
         priv = kzalloc(sizeof *priv, GFP_KERNEL);
         if (!priv) {
                 dev_err(&pdev->dev, "Device struct alloc failed, "
diff --git a/drivers/net/rionet.c b/drivers/net/rionet.c
index 44150f2f7bfd..26afbaae23f0 100644
--- a/drivers/net/rionet.c
+++ b/drivers/net/rionet.c
@@ -382,7 +382,7 @@ static void rionet_remove(struct rio_dev *rdev)
         struct rionet_peer *peer, *tmp;
 
         free_pages((unsigned long)rionet_active, rdev->net->hport->sys_size ?
-                                        __ilog2(sizeof(void *)) + 4 : 0);
+                                        __fls(sizeof(void *)) + 4 : 0);
         unregister_netdev(ndev);
         free_netdev(ndev);
 
@@ -450,7 +450,7 @@ static int rionet_setup_netdev(struct rio_mport *mport)
         }
 
         rionet_active = (struct rio_dev **)__get_free_pages(GFP_KERNEL,
-                        mport->sys_size ? __ilog2(sizeof(void *)) + 4 : 0);
+                        mport->sys_size ? __fls(sizeof(void *)) + 4 : 0);
         if (!rionet_active) {
                 rc = -ENOMEM;
                 goto out;
@@ -571,5 +571,5 @@ static void __exit rionet_exit(void)
         rio_unregister_driver(&rionet_driver);
 }
 
-module_init(rionet_init);
+late_initcall(rionet_init);
 module_exit(rionet_exit);
diff --git a/drivers/pci/pci-sysfs.c b/drivers/pci/pci-sysfs.c
index c85438a367d5..a8a277a2e0d0 100644
--- a/drivers/pci/pci-sysfs.c
+++ b/drivers/pci/pci-sysfs.c
@@ -369,7 +369,7 @@ pci_read_config(struct file *filp, struct kobject *kobj,
         u8 *data = (u8*) buf;
 
         /* Several chips lock up trying to read undefined config space */
-        if (security_capable(filp->f_cred, CAP_SYS_ADMIN) == 0) {
+        if (security_capable(&init_user_ns, filp->f_cred, CAP_SYS_ADMIN) == 0) {
                 size = dev->cfg_size;
         } else if (dev->hdr_type == PCI_HEADER_TYPE_CARDBUS) {
                 size = 128;
diff --git a/drivers/power/jz4740-battery.c b/drivers/power/jz4740-battery.c
index 02414db6a94c..763f894ed188 100644
--- a/drivers/power/jz4740-battery.c
+++ b/drivers/power/jz4740-battery.c
@@ -39,7 +39,7 @@ struct jz_battery {
         int irq;
         int charge_irq;
 
-        struct mfd_cell *cell;
+        const struct mfd_cell *cell;
 
         int status;
         long voltage;
@@ -258,7 +258,7 @@ static int __devinit jz_battery_probe(struct platform_device *pdev)
                 return -ENOMEM;
         }
 
-        jz_battery->cell = pdev->dev.platform_data;
+        jz_battery->cell = mfd_get_cell(pdev);
 
         jz_battery->irq = platform_get_irq(pdev, 0);
         if (jz_battery->irq < 0) {
diff --git a/drivers/rapidio/rio-scan.c b/drivers/rapidio/rio-scan.c
index a50391b6ba2a..3a59d5f018d3 100644
--- a/drivers/rapidio/rio-scan.c
+++ b/drivers/rapidio/rio-scan.c
@@ -517,7 +517,7 @@ static struct rio_dev __devinit *rio_setup_device(struct rio_net *net,
         return rdev;
 
 cleanup:
-        if (rswitch->route_table)
+        if (rio_is_switch(rdev))
                 kfree(rswitch->route_table);
 
         kfree(rdev);
diff --git a/drivers/rapidio/rio-sysfs.c b/drivers/rapidio/rio-sysfs.c
index 1269fbd2deca..4dbe360989be 100644
--- a/drivers/rapidio/rio-sysfs.c
+++ b/drivers/rapidio/rio-sysfs.c
@@ -14,6 +14,7 @@
 #include <linux/rio.h>
 #include <linux/rio_drv.h>
 #include <linux/stat.h>
+#include <linux/capability.h>
 
 #include "rio.h"
 
@@ -33,6 +34,8 @@ rio_config_attr(device_rev, "0x%08x\n");
 rio_config_attr(asm_did, "0x%04x\n");
 rio_config_attr(asm_vid, "0x%04x\n");
 rio_config_attr(asm_rev, "0x%04x\n");
+rio_config_attr(destid, "0x%04x\n");
+rio_config_attr(hopcount, "0x%02x\n");
 
 static ssize_t routes_show(struct device *dev, struct device_attribute *attr, char *buf)
 {
@@ -52,6 +55,35 @@ static ssize_t routes_show(struct device *dev, struct device_attribute *attr, ch
         return (str - buf);
 }
 
+static ssize_t lprev_show(struct device *dev,
+                          struct device_attribute *attr, char *buf)
+{
+        struct rio_dev *rdev = to_rio_dev(dev);
+
+        return sprintf(buf, "%s\n",
+                        (rdev->prev) ? rio_name(rdev->prev) : "root");
+}
+
+static ssize_t lnext_show(struct device *dev,
+                          struct device_attribute *attr, char *buf)
+{
+        struct rio_dev *rdev = to_rio_dev(dev);
+        char *str = buf;
+        int i;
+
+        if (rdev->pef & RIO_PEF_SWITCH) {
+                for (i = 0; i < RIO_GET_TOTAL_PORTS(rdev->swpinfo); i++) {
+                        if (rdev->rswitch->nextdev[i])
+                                str += sprintf(str, "%s\n",
+                                        rio_name(rdev->rswitch->nextdev[i]));
+                        else
+                                str += sprintf(str, "null\n");
+                }
+        }
+
+        return str - buf;
+}
+
 struct device_attribute rio_dev_attrs[] = {
         __ATTR_RO(did),
         __ATTR_RO(vid),
@@ -59,10 +91,14 @@ struct device_attribute rio_dev_attrs[] = {
         __ATTR_RO(asm_did),
         __ATTR_RO(asm_vid),
         __ATTR_RO(asm_rev),
+        __ATTR_RO(lprev),
+        __ATTR_RO(destid),
         __ATTR_NULL,
 };
 
 static DEVICE_ATTR(routes, S_IRUGO, routes_show, NULL);
+static DEVICE_ATTR(lnext, S_IRUGO, lnext_show, NULL);
+static DEVICE_ATTR(hopcount, S_IRUGO, hopcount_show, NULL);
 
 static ssize_t
 rio_read_config(struct file *filp, struct kobject *kobj,
@@ -218,7 +254,9 @@ int rio_create_sysfs_dev_files(struct rio_dev *rdev)
         err = device_create_bin_file(&rdev->dev, &rio_config_attr);
 
         if (!err && (rdev->pef & RIO_PEF_SWITCH)) {
-                err = device_create_file(&rdev->dev, &dev_attr_routes);
+                err |= device_create_file(&rdev->dev, &dev_attr_routes);
+                err |= device_create_file(&rdev->dev, &dev_attr_lnext);
+                err |= device_create_file(&rdev->dev, &dev_attr_hopcount);
                 if (!err && rdev->rswitch->sw_sysfs)
                         err = rdev->rswitch->sw_sysfs(rdev, RIO_SW_SYSFS_CREATE);
         }
@@ -241,6 +279,8 @@ void rio_remove_sysfs_dev_files(struct rio_dev *rdev)
         device_remove_bin_file(&rdev->dev, &rio_config_attr);
         if (rdev->pef & RIO_PEF_SWITCH) {
                 device_remove_file(&rdev->dev, &dev_attr_routes);
+                device_remove_file(&rdev->dev, &dev_attr_lnext);
+                device_remove_file(&rdev->dev, &dev_attr_hopcount);
                 if (rdev->rswitch->sw_sysfs)
                         rdev->rswitch->sw_sysfs(rdev, RIO_SW_SYSFS_REMOVE);
         }
diff --git a/drivers/rapidio/rio.c b/drivers/rapidio/rio.c
index cc2a3b74d0f0..c29719cacbca 100644
--- a/drivers/rapidio/rio.c
+++ b/drivers/rapidio/rio.c
@@ -32,6 +32,7 @@
 #include "rio.h"
 
 static LIST_HEAD(rio_mports);
+static unsigned char next_portid;
 
 /**
  * rio_local_get_device_id - Get the base/extended device id for a port
@@ -68,9 +69,13 @@ int rio_request_inb_mbox(struct rio_mport *mport,
                          void (*minb) (struct rio_mport * mport, void *dev_id, int mbox,
                                        int slot))
 {
-        int rc = 0;
+        int rc = -ENOSYS;
+        struct resource *res;
 
-        struct resource *res = kmalloc(sizeof(struct resource), GFP_KERNEL);
+        if (mport->ops->open_inb_mbox == NULL)
+                goto out;
+
+        res = kmalloc(sizeof(struct resource), GFP_KERNEL);
 
         if (res) {
                 rio_init_mbox_res(res, mbox, mbox);
@@ -88,7 +93,7 @@ int rio_request_inb_mbox(struct rio_mport *mport,
                 /* Hook the inbound message callback */
                 mport->inb_msg[mbox].mcback = minb;
 
-                rc = rio_open_inb_mbox(mport, dev_id, mbox, entries);
+                rc = mport->ops->open_inb_mbox(mport, dev_id, mbox, entries);
         } else
                 rc = -ENOMEM;
 
@@ -106,10 +111,13 @@ int rio_request_inb_mbox(struct rio_mport *mport,
  */
 int rio_release_inb_mbox(struct rio_mport *mport, int mbox)
 {
-        rio_close_inb_mbox(mport, mbox);
+        if (mport->ops->close_inb_mbox) {
+                mport->ops->close_inb_mbox(mport, mbox);
 
-        /* Release the mailbox resource */
-        return release_resource(mport->inb_msg[mbox].res);
+                /* Release the mailbox resource */
+                return release_resource(mport->inb_msg[mbox].res);
+        } else
+                return -ENOSYS;
 }
 
 /**
@@ -129,9 +137,13 @@ int rio_request_outb_mbox(struct rio_mport *mport,
                           int entries,
                           void (*moutb) (struct rio_mport * mport, void *dev_id, int mbox, int slot))
 {
-        int rc = 0;
+        int rc = -ENOSYS;
+        struct resource *res;
 
-        struct resource *res = kmalloc(sizeof(struct resource), GFP_KERNEL);
+        if (mport->ops->open_outb_mbox == NULL)
+                goto out;
+
+        res = kmalloc(sizeof(struct resource), GFP_KERNEL);
 
         if (res) {
                 rio_init_mbox_res(res, mbox, mbox);
@@ -149,7 +161,7 @@ int rio_request_outb_mbox(struct rio_mport *mport,
                 /* Hook the inbound message callback */
                 mport->outb_msg[mbox].mcback = moutb;
 
-                rc = rio_open_outb_mbox(mport, dev_id, mbox, entries);
+                rc = mport->ops->open_outb_mbox(mport, dev_id, mbox, entries);
         } else
                 rc = -ENOMEM;
 
@@ -167,10 +179,13 @@ int rio_request_outb_mbox(struct rio_mport *mport,
  */
 int rio_release_outb_mbox(struct rio_mport *mport, int mbox)
 {
-        rio_close_outb_mbox(mport, mbox);
+        if (mport->ops->close_outb_mbox) {
+                mport->ops->close_outb_mbox(mport, mbox);
 
-        /* Release the mailbox resource */
-        return release_resource(mport->outb_msg[mbox].res);
+                /* Release the mailbox resource */
+                return release_resource(mport->outb_msg[mbox].res);
+        } else
+                return -ENOSYS;
 }
 
 /**
@@ -1120,36 +1135,51 @@ static int __devinit rio_init(void)
         return 0;
 }
 
-device_initcall(rio_init);
-
 int __devinit rio_init_mports(void)
 {
-        int rc = 0;
         struct rio_mport *port;
 
         list_for_each_entry(port, &rio_mports, node) {
-                if (!request_mem_region(port->iores.start,
-                                        resource_size(&port->iores),
-                                        port->name)) {
-                        printk(KERN_ERR
-                               "RIO: Error requesting master port region 0x%016llx-0x%016llx\n",
-                               (u64)port->iores.start, (u64)port->iores.end);
-                        rc = -ENOMEM;
-                        goto out;
-                }
-
                 if (port->host_deviceid >= 0)
                         rio_enum_mport(port);
                 else
                         rio_disc_mport(port);
         }
 
-      out:
-        return rc;
+        rio_init();
+
+        return 0;
 }
 
+device_initcall_sync(rio_init_mports);
+
+static int hdids[RIO_MAX_MPORTS + 1];
+
+static int rio_get_hdid(int index)
+{
+        if (!hdids[0] || hdids[0] <= index || index >= RIO_MAX_MPORTS)
+                return -1;
+
+        return hdids[index + 1];
+}
+
+static int rio_hdid_setup(char *str)
+{
+        (void)get_options(str, ARRAY_SIZE(hdids), hdids);
+        return 1;
+}
+
+__setup("riohdid=", rio_hdid_setup);
+
 void rio_register_mport(struct rio_mport *port)
 {
+        if (next_portid >= RIO_MAX_MPORTS) {
+                pr_err("RIO: reached specified max number of mports\n");
+                return;
+        }
+
+        port->id = next_portid++;
+        port->host_deviceid = rio_get_hdid(port->id);
         list_add_tail(&port->node, &rio_mports);
 }
 
diff --git a/drivers/regulator/88pm8607.c b/drivers/regulator/88pm8607.c
index dd6308499bd4..859251250b55 100644
--- a/drivers/regulator/88pm8607.c
+++ b/drivers/regulator/88pm8607.c
@@ -15,6 +15,7 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/regulator/machine.h>
+#include <linux/mfd/core.h>
 #include <linux/mfd/88pm860x.h>
 
 struct pm8607_regulator_info {
@@ -394,47 +395,48 @@ static struct pm8607_regulator_info pm8607_regulator_info[] = {
         PM8607_LDO(14,        LDO14, 0, 4, SUPPLIES_EN12, 6),
 };
 
-static inline struct pm8607_regulator_info *find_regulator_info(int id)
+static int __devinit pm8607_regulator_probe(struct platform_device *pdev)
 {
-        struct pm8607_regulator_info *info;
+        struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
+        struct pm8607_regulator_info *info = NULL;
+        struct regulator_init_data *pdata;
+        struct mfd_cell *cell;
         int i;
 
+        cell = pdev->dev.platform_data;
+        if (cell == NULL)
+                return -ENODEV;
+        pdata = cell->mfd_data;
+        if (pdata == NULL)
+                return -EINVAL;
+
         for (i = 0; i < ARRAY_SIZE(pm8607_regulator_info); i++) {
                 info = &pm8607_regulator_info[i];
-                if (info->desc.id == id)
-                        return info;
+                if (!strcmp(info->desc.name, pdata->constraints.name))
+                        break;
         }
-        return NULL;
-}
-
-static int __devinit pm8607_regulator_probe(struct platform_device *pdev)
-{
-        struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
-        struct pm860x_platform_data *pdata = chip->dev->platform_data;
-        struct pm8607_regulator_info *info = NULL;
-
-        info = find_regulator_info(pdev->id);
-        if (info == NULL) {
-                dev_err(&pdev->dev, "invalid regulator ID specified\n");
+        if (i > ARRAY_SIZE(pm8607_regulator_info)) {
+                dev_err(&pdev->dev, "Failed to find regulator %s\n",
+                        pdata->constraints.name);
                 return -EINVAL;
         }
 
         info->i2c = (chip->id == CHIP_PM8607) ? chip->client : chip->companion;
         info->chip = chip;
 
+        /* check DVC ramp slope double */
+        if (!strcmp(info->desc.name, "BUCK3"))
+                if (info->chip->buck3_double)
+                        info->slope_double = 1;
+
         info->regulator = regulator_register(&info->desc, &pdev->dev,
-                                             pdata->regulator[pdev->id], info);
+                                             pdata, info);
         if (IS_ERR(info->regulator)) {
                 dev_err(&pdev->dev, "failed to register regulator %s\n",
                         info->desc.name);
                 return PTR_ERR(info->regulator);
         }
 
-        /* check DVC ramp slope double */
-        if (info->desc.id == PM8607_ID_BUCK3)
-                if (info->chip->buck3_double)
-                        info->slope_double = 1;
-
         platform_set_drvdata(pdev, info);
         return 0;
 }
diff --git a/drivers/regulator/Kconfig b/drivers/regulator/Kconfig
index e1d943619ab8..de75f67f4cc3 100644
--- a/drivers/regulator/Kconfig
+++ b/drivers/regulator/Kconfig
@@ -108,6 +108,15 @@ config REGULATOR_MAX8952
           via I2C bus. Maxim 8952 has one voltage output and supports 4 DVS
           modes ranging from 0.77V to 1.40V by 0.01V steps.
 
+config REGULATOR_MAX8997
+        tristate "Maxim 8997/8966 regulator"
+        depends on MFD_MAX8997
+        help
+          This driver controls a Maxim 8997/8966 regulator
+          via I2C bus. The provided regulator is suitable for S5PC110,
+          S5PV210, and Exynos-4 chips to control VCC_CORE and
+          VCC_USIM voltages.
+
 config REGULATOR_MAX8998
         tristate "Maxim 8998 voltage regulator"
         depends on MFD_MAX8998
@@ -214,6 +223,15 @@ config REGULATOR_AB3100
          AB3100 analog baseband dealing with power regulators
          for the system.
 
+config REGULATOR_TPS6105X
+        tristate "TI TPS6105X Power regulators"
+        depends on TPS6105X
+        default y if TPS6105X
+        help
+          This driver supports TPS61050/TPS61052 voltage regulator chips.
+          It is a single boost converter primarily for white LEDs and
+          audio amplifiers.
+
 config REGULATOR_TPS65023
         tristate "TI TPS65023 Power regulators"
         depends on I2C
diff --git a/drivers/regulator/Makefile b/drivers/regulator/Makefile
index 0b5e88c2b8d7..d72a42756778 100644
--- a/drivers/regulator/Makefile
+++ b/drivers/regulator/Makefile
@@ -18,6 +18,7 @@ obj-$(CONFIG_REGULATOR_MAX8649)	+= max8649.o
 obj-$(CONFIG_REGULATOR_MAX8660) += max8660.o
 obj-$(CONFIG_REGULATOR_MAX8925) += max8925-regulator.o
 obj-$(CONFIG_REGULATOR_MAX8952) += max8952.o
+obj-$(CONFIG_REGULATOR_MAX8997) += max8997.o
 obj-$(CONFIG_REGULATOR_MAX8998) += max8998.o
 obj-$(CONFIG_REGULATOR_WM831X) += wm831x-dcdc.o
 obj-$(CONFIG_REGULATOR_WM831X) += wm831x-isink.o
@@ -33,7 +34,7 @@ obj-$(CONFIG_REGULATOR_MC13783) += mc13783-regulator.o
 obj-$(CONFIG_REGULATOR_MC13892) += mc13892-regulator.o
 obj-$(CONFIG_REGULATOR_MC13XXX_CORE) +=  mc13xxx-regulator-core.o
 obj-$(CONFIG_REGULATOR_AB3100) += ab3100.o
-
+obj-$(CONFIG_REGULATOR_TPS6105X) += tps6105x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS65023) += tps65023-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6507X) += tps6507x-regulator.o
 obj-$(CONFIG_REGULATOR_TPS6524X) += tps6524x-regulator.o
diff --git a/drivers/regulator/ab3100.c b/drivers/regulator/ab3100.c
index ed6feaf9398d..2dec589a8908 100644
--- a/drivers/regulator/ab3100.c
+++ b/drivers/regulator/ab3100.c
@@ -17,6 +17,7 @@
 #include <linux/platform_device.h>
 #include <linux/regulator/driver.h>
 #include <linux/mfd/abx500.h>
+#include <linux/mfd/core.h>
 
 /* LDO registers and some handy masking definitions for AB3100 */
 #define AB3100_LDO_A            0x40
@@ -576,7 +577,7 @@ ab3100_regulator_desc[AB3100_NUM_REGULATORS] = {
 
 static int __devinit ab3100_regulators_probe(struct platform_device *pdev)
 {
-        struct ab3100_platform_data *plfdata = pdev->dev.platform_data;
+        struct ab3100_platform_data *plfdata = mfd_get_data(pdev);
         int err = 0;
         u8 data;
         int i;
diff --git a/drivers/regulator/max8997.c b/drivers/regulator/max8997.c
new file mode 100644
index 000000000000..01ef7e9903bb
--- /dev/null
+++ b/drivers/regulator/max8997.c
@@ -0,0 +1,1213 @@
+/*
+ * max8997.c - Regulator driver for the Maxim 8997/8966
+ *
+ * Copyright (C) 2011 Samsung Electronics
+ * MyungJoo Ham <myungjoo.ham@smasung.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.
+ *
+ * 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
+ *
+ * This driver is based on max8998.c
+ */
+
+#include <linux/bug.h>
+#include <linux/delay.h>
+#include <linux/err.h>
+#include <linux/gpio.h>
+#include <linux/slab.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/regulator/machine.h>
+#include <linux/mfd/max8997.h>
+#include <linux/mfd/max8997-private.h>
+
+struct max8997_data {
+        struct device *dev;
+        struct max8997_dev *iodev;
+        int num_regulators;
+        struct regulator_dev **rdev;
+        int ramp_delay; /* in mV/us */
+
+        u8 buck1_vol[8];
+        u8 buck2_vol[8];
+        u8 buck5_vol[8];
+        int buck125_gpioindex;
+
+        u8 saved_states[MAX8997_REG_MAX];
+};
+
+static inline void max8997_set_gpio(struct max8997_data *max8997)
+{
+        struct max8997_platform_data *pdata =
+                dev_get_platdata(max8997->iodev->dev);
+        int set3 = (max8997->buck125_gpioindex) & 0x1;
+        int set2 = ((max8997->buck125_gpioindex) >> 1) & 0x1;
+        int set1 = ((max8997->buck125_gpioindex) >> 2) & 0x1;
+
+        gpio_set_value(pdata->buck125_gpios[0], set1);
+        gpio_set_value(pdata->buck125_gpios[1], set2);
+        gpio_set_value(pdata->buck125_gpios[2], set3);
+}
+
+struct voltage_map_desc {
+        int min;
+        int max;
+        int step;
+        unsigned int n_bits;
+};
+
+/* Voltage maps in mV */
+static const struct voltage_map_desc ldo_voltage_map_desc = {
+        .min = 800,     .max = 3950,    .step = 50,     .n_bits = 6,
+}; /* LDO1 ~ 18, 21 all */
+
+static const struct voltage_map_desc buck1245_voltage_map_desc = {
+        .min = 650,     .max = 2225,    .step = 25,     .n_bits = 6,
+}; /* Buck1, 2, 4, 5 */
+
+static const struct voltage_map_desc buck37_voltage_map_desc = {
+        .min = 750,     .max = 3900,    .step = 50,     .n_bits = 6,
+}; /* Buck3, 7 */
+
+/* current map in mA */
+static const struct voltage_map_desc charger_current_map_desc = {
+        .min = 200,     .max = 950,     .step = 50,     .n_bits = 4,
+};
+
+static const struct voltage_map_desc topoff_current_map_desc = {
+        .min = 50,      .max = 200,     .step = 10,     .n_bits = 4,
+};
+
+static const struct voltage_map_desc *reg_voltage_map[] = {
+        [MAX8997_LDO1] = &ldo_voltage_map_desc,
+        [MAX8997_LDO2] = &ldo_voltage_map_desc,
+        [MAX8997_LDO3] = &ldo_voltage_map_desc,
+        [MAX8997_LDO4] = &ldo_voltage_map_desc,
+        [MAX8997_LDO5] = &ldo_voltage_map_desc,
+        [MAX8997_LDO6] = &ldo_voltage_map_desc,
+        [MAX8997_LDO7] = &ldo_voltage_map_desc,
+        [MAX8997_LDO8] = &ldo_voltage_map_desc,
+        [MAX8997_LDO9] = &ldo_voltage_map_desc,
+        [MAX8997_LDO10] = &ldo_voltage_map_desc,
+        [MAX8997_LDO11] = &ldo_voltage_map_desc,
+        [MAX8997_LDO12] = &ldo_voltage_map_desc,
+        [MAX8997_LDO13] = &ldo_voltage_map_desc,
+        [MAX8997_LDO14] = &ldo_voltage_map_desc,
+        [MAX8997_LDO15] = &ldo_voltage_map_desc,
+        [MAX8997_LDO16] = &ldo_voltage_map_desc,
+        [MAX8997_LDO17] = &ldo_voltage_map_desc,
+        [MAX8997_LDO18] = &ldo_voltage_map_desc,
+        [MAX8997_LDO21] = &ldo_voltage_map_desc,
+        [MAX8997_BUCK1] = &buck1245_voltage_map_desc,
+        [MAX8997_BUCK2] = &buck1245_voltage_map_desc,
+        [MAX8997_BUCK3] = &buck37_voltage_map_desc,
+        [MAX8997_BUCK4] = &buck1245_voltage_map_desc,
+        [MAX8997_BUCK5] = &buck1245_voltage_map_desc,
+        [MAX8997_BUCK6] = NULL,
+        [MAX8997_BUCK7] = &buck37_voltage_map_desc,
+        [MAX8997_EN32KHZ_AP] = NULL,
+        [MAX8997_EN32KHZ_CP] = NULL,
+        [MAX8997_ENVICHG] = NULL,
+        [MAX8997_ESAFEOUT1] = NULL,
+        [MAX8997_ESAFEOUT2] = NULL,
+        [MAX8997_CHARGER_CV] = NULL,
+        [MAX8997_CHARGER] = &charger_current_map_desc,
+        [MAX8997_CHARGER_TOPOFF] = &topoff_current_map_desc,
+};
+
+static inline int max8997_get_rid(struct regulator_dev *rdev)
+{
+        return rdev_get_id(rdev);
+}
+
+static int max8997_list_voltage_safeout(struct regulator_dev *rdev,
+                unsigned int selector)
+{
+        int rid = max8997_get_rid(rdev);
+
+        if (rid == MAX8997_ESAFEOUT1 || rid == MAX8997_ESAFEOUT2) {
+                switch (selector) {
+                case 0:
+                        return 4850000;
+                case 1:
+                        return 4900000;
+                case 2:
+                        return 4950000;
+                case 3:
+                        return 3300000;
+                default:
+                        return -EINVAL;
+                }
+        }
+
+        return -EINVAL;
+}
+
+static int max8997_list_voltage_charger_cv(struct regulator_dev *rdev,
+                unsigned int selector)
+{
+        int rid = max8997_get_rid(rdev);
+
+        if (rid != MAX8997_CHARGER_CV)
+                goto err;
+
+        switch (selector) {
+        case 0x00:
+                return 4200000;
+        case 0x01 ... 0x0E:
+                return 4000000 + 20000 * (selector - 0x01);
+        case 0x0F:
+                return 4350000;
+        default:
+                return -EINVAL;
+        }
+err:
+        return -EINVAL;
+}
+
+static int max8997_list_voltage(struct regulator_dev *rdev,
+                unsigned int selector)
+{
+        const struct voltage_map_desc *desc;
+        int rid = max8997_get_rid(rdev);
+        int val;
+
+        if (rid >= ARRAY_SIZE(reg_voltage_map) ||
+                        rid < 0)
+                return -EINVAL;
+
+        desc = reg_voltage_map[rid];
+        if (desc == NULL)
+                return -EINVAL;
+
+        val = desc->min + desc->step * selector;
+        if (val > desc->max)
+                return -EINVAL;
+
+        return val * 1000;
+}
+
+static int max8997_get_enable_register(struct regulator_dev *rdev,
+                int *reg, int *mask, int *pattern)
+{
+        int rid = max8997_get_rid(rdev);
+
+        switch (rid) {
+        case MAX8997_LDO1 ... MAX8997_LDO21:
+                *reg = MAX8997_REG_LDO1CTRL + (rid - MAX8997_LDO1);
+                *mask = 0xC0;
+                *pattern = 0xC0;
+                break;
+        case MAX8997_BUCK1:
+                *reg = MAX8997_REG_BUCK1CTRL;
+                *mask = 0x01;
+                *pattern = 0x01;
+                break;
+        case MAX8997_BUCK2:
+                *reg = MAX8997_REG_BUCK2CTRL;
+                *mask = 0x01;
+                *pattern = 0x01;
+                break;
+        case MAX8997_BUCK3:
+                *reg = MAX8997_REG_BUCK3CTRL;
+                *mask = 0x01;
+                *pattern = 0x01;
+                break;
+        case MAX8997_BUCK4:
+                *reg = MAX8997_REG_BUCK4CTRL;
+                *mask = 0x01;
+                *pattern = 0x01;
+                break;
+        case MAX8997_BUCK5:
+                *reg = MAX8997_REG_BUCK5CTRL;
+                *mask = 0x01;
+                *pattern = 0x01;
+                break;
+        case MAX8997_BUCK6:
+                *reg = MAX8997_REG_BUCK6CTRL;
+                *mask = 0x01;
+                *pattern = 0x01;
+                break;
+        case MAX8997_BUCK7:
+                *reg = MAX8997_REG_BUCK7CTRL;
+                *mask = 0x01;
+                *pattern = 0x01;
+                break;
+        case MAX8997_EN32KHZ_AP ... MAX8997_EN32KHZ_CP:
+                *reg = MAX8997_REG_MAINCON1;
+                *mask = 0x01 << (rid - MAX8997_EN32KHZ_AP);
+                *pattern = 0x01 << (rid - MAX8997_EN32KHZ_AP);
+                break;
+        case MAX8997_ENVICHG:
+                *reg = MAX8997_REG_MBCCTRL1;
+                *mask = 0x80;
+                *pattern = 0x80;
+                break;
+        case MAX8997_ESAFEOUT1 ... MAX8997_ESAFEOUT2:
+                *reg = MAX8997_REG_SAFEOUTCTRL;
+                *mask = 0x40 << (rid - MAX8997_ESAFEOUT1);
+                *pattern = 0x40 << (rid - MAX8997_ESAFEOUT1);
+                break;
+        case MAX8997_CHARGER:
+                *reg = MAX8997_REG_MBCCTRL2;
+                *mask = 0x40;
+                *pattern = 0x40;
+                break;
+        default:
+                /* Not controllable or not exists */
+                return -EINVAL;
+                break;
+        }
+
+        return 0;
+}
+
+static int max8997_reg_is_enabled(struct regulator_dev *rdev)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct i2c_client *i2c = max8997->iodev->i2c;
+        int ret, reg, mask, pattern;
+        u8 val;
+
+        ret = max8997_get_enable_register(rdev, &reg, &mask, &pattern);
+        if (ret == -EINVAL)
+                return 1; /* "not controllable" */
+        else if (ret)
+                return ret;
+
+        ret = max8997_read_reg(i2c, reg, &val);
+        if (ret)
+                return ret;
+
+        return (val & mask) == pattern;
+}
+
+static int max8997_reg_enable(struct regulator_dev *rdev)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct i2c_client *i2c = max8997->iodev->i2c;
+        int ret, reg, mask, pattern;
+
+        ret = max8997_get_enable_register(rdev, &reg, &mask, &pattern);
+        if (ret)
+                return ret;
+
+        return max8997_update_reg(i2c, reg, pattern, mask);
+}
+
+static int max8997_reg_disable(struct regulator_dev *rdev)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct i2c_client *i2c = max8997->iodev->i2c;
+        int ret, reg, mask, pattern;
+
+        ret = max8997_get_enable_register(rdev, &reg, &mask, &pattern);
+        if (ret)
+                return ret;
+
+        return max8997_update_reg(i2c, reg, ~pattern, mask);
+}
+
+static int max8997_get_voltage_register(struct regulator_dev *rdev,
+                int *_reg, int *_shift, int *_mask)
+{
+        int rid = max8997_get_rid(rdev);
+        int reg, shift = 0, mask = 0x3f;
+
+        switch (rid) {
+        case MAX8997_LDO1 ... MAX8997_LDO21:
+                reg = MAX8997_REG_LDO1CTRL + (rid - MAX8997_LDO1);
+                break;
+        case MAX8997_BUCK1:
+                reg = MAX8997_REG_BUCK1DVS1;
+                break;
+        case MAX8997_BUCK2:
+                reg = MAX8997_REG_BUCK2DVS1;
+                break;
+        case MAX8997_BUCK3:
+                reg = MAX8997_REG_BUCK3DVS;
+                break;
+        case MAX8997_BUCK4:
+                reg = MAX8997_REG_BUCK4DVS;
+                break;
+        case MAX8997_BUCK5:
+                reg = MAX8997_REG_BUCK5DVS1;
+                break;
+        case MAX8997_BUCK7:
+                reg = MAX8997_REG_BUCK7DVS;
+                break;
+        case MAX8997_ESAFEOUT1 ...  MAX8997_ESAFEOUT2:
+                reg = MAX8997_REG_SAFEOUTCTRL;
+                shift = (rid == MAX8997_ESAFEOUT2) ? 2 : 0;
+                mask = 0x3;
+                break;
+        case MAX8997_CHARGER_CV:
+                reg = MAX8997_REG_MBCCTRL3;
+                shift = 0;
+                mask = 0xf;
+                break;
+        case MAX8997_CHARGER:
+                reg = MAX8997_REG_MBCCTRL4;
+                shift = 0;
+                mask = 0xf;
+                break;
+        case MAX8997_CHARGER_TOPOFF:
+                reg = MAX8997_REG_MBCCTRL5;
+                shift = 0;
+                mask = 0xf;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        *_reg = reg;
+        *_shift = shift;
+        *_mask = mask;
+
+        return 0;
+}
+
+static int max8997_get_voltage(struct regulator_dev *rdev)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct max8997_platform_data *pdata =
+                dev_get_platdata(max8997->iodev->dev);
+        struct i2c_client *i2c = max8997->iodev->i2c;
+        int reg, shift, mask, ret;
+        int rid = max8997_get_rid(rdev);
+        u8 val;
+
+        ret = max8997_get_voltage_register(rdev, &reg, &shift, &mask);
+        if (ret)
+                return ret;
+
+        if ((rid == MAX8997_BUCK1 && pdata->buck1_gpiodvs) ||
+                        (rid == MAX8997_BUCK2 && pdata->buck2_gpiodvs) ||
+                        (rid == MAX8997_BUCK5 && pdata->buck5_gpiodvs))
+                reg += max8997->buck125_gpioindex;
+
+        ret = max8997_read_reg(i2c, reg, &val);
+        if (ret)
+                return ret;
+
+        val >>= shift;
+        val &= mask;
+
+        if (rdev->desc && rdev->desc->ops && rdev->desc->ops->list_voltage)
+                return rdev->desc->ops->list_voltage(rdev, val);
+
+        /*
+         * max8997_list_voltage returns value for any rdev with voltage_map,
+         * which works for "CHARGER" and "CHARGER TOPOFF" that do not have
+         * list_voltage ops (they are current regulators).
+         */
+        return max8997_list_voltage(rdev, val);
+}
+
+static inline int max8997_get_voltage_proper_val(
+                const struct voltage_map_desc *desc,
+                int min_vol, int max_vol)
+{
+        int i = 0;
+
+        if (desc == NULL)
+                return -EINVAL;
+
+        if (max_vol < desc->min || min_vol > desc->max)
+                return -EINVAL;
+
+        while (desc->min + desc->step * i < min_vol &&
+                        desc->min + desc->step * i < desc->max)
+                i++;
+
+        if (desc->min + desc->step * i > max_vol)
+                return -EINVAL;
+
+        if (i >= (1 << desc->n_bits))
+                return -EINVAL;
+
+        return i;
+}
+
+static int max8997_set_voltage_charger_cv(struct regulator_dev *rdev,
+                int min_uV, int max_uV, unsigned *selector)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct i2c_client *i2c = max8997->iodev->i2c;
+        int rid = max8997_get_rid(rdev);
+        int lb, ub;
+        int reg, shift = 0, mask, ret = 0;
+        u8 val = 0x0;
+
+        if (rid != MAX8997_CHARGER_CV)
+                return -EINVAL;
+
+        ret = max8997_get_voltage_register(rdev, &reg, &shift, &mask);
+        if (ret)
+                return ret;
+
+        if (max_uV < 4000000 || min_uV > 4350000)
+                return -EINVAL;
+
+        if (min_uV <= 4000000) {
+                if (max_uV >= 4000000)
+                        return -EINVAL;
+                else
+                        val = 0x1;
+        } else if (min_uV <= 4200000 && max_uV >= 4200000)
+                val = 0x0;
+        else {
+                lb = (min_uV - 4000001) / 20000 + 2;
+                ub = (max_uV - 4000000) / 20000 + 1;
+
+                if (lb > ub)
+                        return -EINVAL;
+
+                if (lb < 0xf)
+                        val = lb;
+                else {
+                        if (ub >= 0xf)
+                                val = 0xf;
+                        else
+                                return -EINVAL;
+                }
+        }
+
+        *selector = val;
+
+        ret = max8997_update_reg(i2c, reg, val << shift, mask);
+
+        return ret;
+}
+
+/*
+ * For LDO1 ~ LDO21, BUCK1~5, BUCK7, CHARGER, CHARGER_TOPOFF
+ * BUCK1, 2, and 5 are available if they are not controlled by gpio
+ */
+static int max8997_set_voltage_ldobuck(struct regulator_dev *rdev,
+                int min_uV, int max_uV, unsigned *selector)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct i2c_client *i2c = max8997->iodev->i2c;
+        int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
+        const struct voltage_map_desc *desc;
+        int rid = max8997_get_rid(rdev);
+        int reg, shift = 0, mask, ret;
+        int i;
+        u8 org;
+
+        switch (rid) {
+        case MAX8997_LDO1 ... MAX8997_LDO21:
+                break;
+        case MAX8997_BUCK1 ... MAX8997_BUCK5:
+                break;
+        case MAX8997_BUCK6:
+                return -EINVAL;
+        case MAX8997_BUCK7:
+                break;
+        case MAX8997_CHARGER:
+                break;
+        case MAX8997_CHARGER_TOPOFF:
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        desc = reg_voltage_map[rid];
+
+        i = max8997_get_voltage_proper_val(desc, min_vol, max_vol);
+        if (i < 0)
+                return i;
+
+        ret = max8997_get_voltage_register(rdev, &reg, &shift, &mask);
+        if (ret)
+                return ret;
+
+        max8997_read_reg(i2c, reg, &org);
+        org = (org & mask) >> shift;
+
+        ret = max8997_update_reg(i2c, reg, i << shift, mask << shift);
+        *selector = i;
+
+        if (rid == MAX8997_BUCK1 || rid == MAX8997_BUCK2 ||
+                        rid == MAX8997_BUCK4 || rid == MAX8997_BUCK5) {
+                /* If the voltage is increasing */
+                if (org < i)
+                        udelay(desc->step * (i - org) / max8997->ramp_delay);
+        }
+
+        return ret;
+}
+
+/*
+ * Assess the damage on the voltage setting of BUCK1,2,5 by the change.
+ *
+ * When GPIO-DVS mode is used for multiple bucks, changing the voltage value
+ * of one of the bucks may affect that of another buck, which is the side
+ * effect of the change (set_voltage). This function examines the GPIO-DVS
+ * configurations and checks whether such side-effect exists.
+ */
+static int max8997_assess_side_effect(struct regulator_dev *rdev,
+                u8 new_val, int *best)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct max8997_platform_data *pdata =
+                dev_get_platdata(max8997->iodev->dev);
+        int rid = max8997_get_rid(rdev);
+        u8 *buckx_val[3];
+        bool buckx_gpiodvs[3];
+        int side_effect[8];
+        int min_side_effect = INT_MAX;
+        int i;
+
+        *best = -1;
+
+        switch (rid) {
+        case MAX8997_BUCK1:
+                rid = 0;
+                break;
+        case MAX8997_BUCK2:
+                rid = 1;
+                break;
+        case MAX8997_BUCK5:
+                rid = 2;
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        buckx_val[0] = max8997->buck1_vol;
+        buckx_val[1] = max8997->buck2_vol;
+        buckx_val[2] = max8997->buck5_vol;
+        buckx_gpiodvs[0] = pdata->buck1_gpiodvs;
+        buckx_gpiodvs[1] = pdata->buck2_gpiodvs;
+        buckx_gpiodvs[2] = pdata->buck5_gpiodvs;
+
+        for (i = 0; i < 8; i++) {
+                int others;
+
+                if (new_val != (buckx_val[rid])[i]) {
+                        side_effect[i] = -1;
+                        continue;
+                }
+
+                side_effect[i] = 0;
+                for (others = 0; others < 3; others++) {
+                        int diff;
+
+                        if (others == rid)
+                                continue;
+                        if (buckx_gpiodvs[others] == false)
+                                continue; /* Not affected */
+                        diff = (buckx_val[others])[i] -
+                                (buckx_val[others])[max8997->buck125_gpioindex];
+                        if (diff > 0)
+                                side_effect[i] += diff;
+                        else if (diff < 0)
+                                side_effect[i] -= diff;
+                }
+                if (side_effect[i] == 0) {
+                        *best = i;
+                        return 0; /* NO SIDE EFFECT! Use This! */
+                }
+                if (side_effect[i] < min_side_effect) {
+                        min_side_effect = side_effect[i];
+                        *best = i;
+                }
+        }
+
+        if (*best == -1)
+                return -EINVAL;
+
+        return side_effect[*best];
+}
+
+/*
+ * For Buck 1 ~ 5 and 7. If it is not controlled by GPIO, this calls
+ * max8997_set_voltage_ldobuck to do the job.
+ */
+static int max8997_set_voltage_buck(struct regulator_dev *rdev,
+                int min_uV, int max_uV, unsigned *selector)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct max8997_platform_data *pdata =
+                dev_get_platdata(max8997->iodev->dev);
+        int rid = max8997_get_rid(rdev);
+        const struct voltage_map_desc *desc;
+        int new_val, new_idx, damage, tmp_val, tmp_idx, tmp_dmg;
+        bool gpio_dvs_mode = false;
+        int min_vol = min_uV / 1000, max_vol = max_uV / 1000;
+
+        if (rid < MAX8997_BUCK1 || rid > MAX8997_BUCK7)
+                return -EINVAL;
+
+        switch (rid) {
+        case MAX8997_BUCK1:
+                if (pdata->buck1_gpiodvs)
+                        gpio_dvs_mode = true;
+                break;
+        case MAX8997_BUCK2:
+                if (pdata->buck2_gpiodvs)
+                        gpio_dvs_mode = true;
+                break;
+        case MAX8997_BUCK5:
+                if (pdata->buck5_gpiodvs)
+                        gpio_dvs_mode = true;
+                break;
+        }
+
+        if (!gpio_dvs_mode)
+                return max8997_set_voltage_ldobuck(rdev, min_uV, max_uV,
+                                                selector);
+
+        desc = reg_voltage_map[rid];
+        new_val = max8997_get_voltage_proper_val(desc, min_vol, max_vol);
+        if (new_val < 0)
+                return new_val;
+
+        tmp_dmg = INT_MAX;
+        tmp_idx = -1;
+        tmp_val = -1;
+        do {
+                damage = max8997_assess_side_effect(rdev, new_val, &new_idx);
+                if (damage == 0)
+                        goto out;
+
+                if (tmp_dmg > damage) {
+                        tmp_idx = new_idx;
+                        tmp_val = new_val;
+                        tmp_dmg = damage;
+                }
+
+                new_val++;
+        } while (desc->min + desc->step + new_val <= desc->max);
+
+        new_idx = tmp_idx;
+        new_val = tmp_val;
+
+        if (pdata->ignore_gpiodvs_side_effect == false)
+                return -EINVAL;
+
+        dev_warn(&rdev->dev, "MAX8997 GPIO-DVS Side Effect Warning: GPIO SET:"
+                        " %d -> %d\n", max8997->buck125_gpioindex, tmp_idx);
+
+out:
+        if (new_idx < 0 || new_val < 0)
+                return -EINVAL;
+
+        max8997->buck125_gpioindex = new_idx;
+        max8997_set_gpio(max8997);
+        *selector = new_val;
+
+        return 0;
+}
+
+static const int safeoutvolt[] = {
+        3300000,
+        4850000,
+        4900000,
+        4950000,
+};
+
+/* For SAFEOUT1 and SAFEOUT2 */
+static int max8997_set_voltage_safeout(struct regulator_dev *rdev,
+                int min_uV, int max_uV, unsigned *selector)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct i2c_client *i2c = max8997->iodev->i2c;
+        int rid = max8997_get_rid(rdev);
+        int reg, shift = 0, mask, ret;
+        int i = 0;
+        u8 val;
+
+        if (rid != MAX8997_ESAFEOUT1 && rid != MAX8997_ESAFEOUT2)
+                return -EINVAL;
+
+        for (i = 0; i < ARRAY_SIZE(safeoutvolt); i++) {
+                if (min_uV <= safeoutvolt[i] &&
+                                max_uV >= safeoutvolt[i])
+                        break;
+        }
+
+        if (i >= ARRAY_SIZE(safeoutvolt))
+                return -EINVAL;
+
+        if (i == 0)
+                val = 0x3;
+        else
+                val = i - 1;
+
+        ret = max8997_get_voltage_register(rdev, &reg, &shift, &mask);
+        if (ret)
+                return ret;
+
+        ret = max8997_update_reg(i2c, reg, val << shift, mask << shift);
+        *selector = val;
+
+        return ret;
+}
+
+static int max8997_reg_enable_suspend(struct regulator_dev *rdev)
+{
+        return 0;
+}
+
+static int max8997_reg_disable_suspend(struct regulator_dev *rdev)
+{
+        struct max8997_data *max8997 = rdev_get_drvdata(rdev);
+        struct i2c_client *i2c = max8997->iodev->i2c;
+        int ret, reg, mask, pattern;
+        int rid = max8997_get_rid(rdev);
+
+        ret = max8997_get_enable_register(rdev, &reg, &mask, &pattern);
+        if (ret)
+                return ret;
+
+        max8997_read_reg(i2c, reg, &max8997->saved_states[rid]);
+
+        if (rid == MAX8997_LDO1 ||
+                        rid == MAX8997_LDO10 ||
+                        rid == MAX8997_LDO21) {
+                dev_dbg(&rdev->dev, "Conditional Power-Off for %s\n",
+                                rdev->desc->name);
+                return max8997_update_reg(i2c, reg, 0x40, mask);
+        }
+
+        dev_dbg(&rdev->dev, "Full Power-Off for %s (%xh -> %xh)\n",
+                        rdev->desc->name, max8997->saved_states[rid] & mask,
+                        (~pattern) & mask);
+        return max8997_update_reg(i2c, reg, ~pattern, mask);
+}
+
+static struct regulator_ops max8997_ldo_ops = {
+        .list_voltage           = max8997_list_voltage,
+        .is_enabled             = max8997_reg_is_enabled,
+        .enable                 = max8997_reg_enable,
+        .disable                = max8997_reg_disable,
+        .get_voltage            = max8997_get_voltage,
+        .set_voltage            = max8997_set_voltage_ldobuck,
+        .set_suspend_enable     = max8997_reg_enable_suspend,
+        .set_suspend_disable    = max8997_reg_disable_suspend,
+};
+
+static struct regulator_ops max8997_buck_ops = {
+        .list_voltage           = max8997_list_voltage,
+        .is_enabled             = max8997_reg_is_enabled,
+        .enable                 = max8997_reg_enable,
+        .disable                = max8997_reg_disable,
+        .get_voltage            = max8997_get_voltage,
+        .set_voltage            = max8997_set_voltage_buck,
+        .set_suspend_enable     = max8997_reg_enable_suspend,
+        .set_suspend_disable    = max8997_reg_disable_suspend,
+};
+
+static struct regulator_ops max8997_fixedvolt_ops = {
+        .list_voltage           = max8997_list_voltage,
+        .is_enabled             = max8997_reg_is_enabled,
+        .enable                 = max8997_reg_enable,
+        .disable                = max8997_reg_disable,
+        .set_suspend_enable     = max8997_reg_enable_suspend,
+        .set_suspend_disable    = max8997_reg_disable_suspend,
+};
+
+static struct regulator_ops max8997_safeout_ops = {
+        .list_voltage           = max8997_list_voltage_safeout,
+        .is_enabled             = max8997_reg_is_enabled,
+        .enable                 = max8997_reg_enable,
+        .disable                = max8997_reg_disable,
+        .get_voltage            = max8997_get_voltage,
+        .set_voltage            = max8997_set_voltage_safeout,
+        .set_suspend_enable     = max8997_reg_enable_suspend,
+        .set_suspend_disable    = max8997_reg_disable_suspend,
+};
+
+static struct regulator_ops max8997_fixedstate_ops = {
+        .list_voltage           = max8997_list_voltage_charger_cv,
+        .get_voltage            = max8997_get_voltage,
+        .set_voltage            = max8997_set_voltage_charger_cv,
+};
+
+static int max8997_set_voltage_ldobuck_wrap(struct regulator_dev *rdev,
+                int min_uV, int max_uV)
+{
+        unsigned dummy;
+
+        return max8997_set_voltage_ldobuck(rdev, min_uV, max_uV, &dummy);
+}
+
+
+static struct regulator_ops max8997_charger_ops = {
+        .is_enabled             = max8997_reg_is_enabled,
+        .enable                 = max8997_reg_enable,
+        .disable                = max8997_reg_disable,
+        .get_current_limit      = max8997_get_voltage,
+        .set_current_limit      = max8997_set_voltage_ldobuck_wrap,
+};
+
+static struct regulator_ops max8997_charger_fixedstate_ops = {
+        .is_enabled             = max8997_reg_is_enabled,
+        .get_current_limit      = max8997_get_voltage,
+        .set_current_limit      = max8997_set_voltage_ldobuck_wrap,
+};
+
+#define regulator_desc_ldo(num)         {       \
+        .name           = "LDO"#num,            \
+        .id             = MAX8997_LDO##num,     \
+        .ops            = &max8997_ldo_ops,     \
+        .type           = REGULATOR_VOLTAGE,    \
+        .owner          = THIS_MODULE,          \
+}
+#define regulator_desc_buck(num)                {       \
+        .name           = "BUCK"#num,           \
+        .id             = MAX8997_BUCK##num,    \
+        .ops            = &max8997_buck_ops,    \
+        .type           = REGULATOR_VOLTAGE,    \
+        .owner          = THIS_MODULE,          \
+}
+
+static struct regulator_desc regulators[] = {
+        regulator_desc_ldo(1),
+        regulator_desc_ldo(2),
+        regulator_desc_ldo(3),
+        regulator_desc_ldo(4),
+        regulator_desc_ldo(5),
+        regulator_desc_ldo(6),
+        regulator_desc_ldo(7),
+        regulator_desc_ldo(8),
+        regulator_desc_ldo(9),
+        regulator_desc_ldo(10),
+        regulator_desc_ldo(11),
+        regulator_desc_ldo(12),
+        regulator_desc_ldo(13),
+        regulator_desc_ldo(14),
+        regulator_desc_ldo(15),
+        regulator_desc_ldo(16),
+        regulator_desc_ldo(17),
+        regulator_desc_ldo(18),
+        regulator_desc_ldo(21),
+        regulator_desc_buck(1),
+        regulator_desc_buck(2),
+        regulator_desc_buck(3),
+        regulator_desc_buck(4),
+        regulator_desc_buck(5),
+        {
+                .name   = "BUCK6",
+                .id     = MAX8997_BUCK6,
+                .ops    = &max8997_fixedvolt_ops,
+                .type   = REGULATOR_VOLTAGE,
+                .owner  = THIS_MODULE,
+        },
+        regulator_desc_buck(7),
+        {
+                .name   = "EN32KHz AP",
+                .id     = MAX8997_EN32KHZ_AP,
+                .ops    = &max8997_fixedvolt_ops,
+                .type   = REGULATOR_VOLTAGE,
+                .owner  = THIS_MODULE,
+        }, {
+                .name   = "EN32KHz CP",
+                .id     = MAX8997_EN32KHZ_CP,
+                .ops    = &max8997_fixedvolt_ops,
+                .type   = REGULATOR_VOLTAGE,
+                .owner  = THIS_MODULE,
+        }, {
+                .name   = "ENVICHG",
+                .id     = MAX8997_ENVICHG,
+                .ops    = &max8997_fixedvolt_ops,
+                .type   = REGULATOR_VOLTAGE,
+                .owner  = THIS_MODULE,
+        }, {
+                .name   = "ESAFEOUT1",
+                .id     = MAX8997_ESAFEOUT1,
+                .ops    = &max8997_safeout_ops,
+                .type   = REGULATOR_VOLTAGE,
+                .owner   = THIS_MODULE,
+        }, {
+                .name   = "ESAFEOUT2",
+                .id     = MAX8997_ESAFEOUT2,
+                .ops    = &max8997_safeout_ops,
+                .type   = REGULATOR_VOLTAGE,
+                .owner   = THIS_MODULE,
+        }, {
+                .name   = "CHARGER CV",
+                .id     = MAX8997_CHARGER_CV,
+                .ops    = &max8997_fixedstate_ops,
+                .type   = REGULATOR_VOLTAGE,
+                .owner   = THIS_MODULE,
+        }, {
+                .name   = "CHARGER",
+                .id     = MAX8997_CHARGER,
+                .ops    = &max8997_charger_ops,
+                .type   = REGULATOR_CURRENT,
+                .owner   = THIS_MODULE,
+        }, {
+                .name   = "CHARGER TOPOFF",
+                .id     = MAX8997_CHARGER_TOPOFF,
+                .ops    = &max8997_charger_fixedstate_ops,
+                .type   = REGULATOR_CURRENT,
+                .owner   = THIS_MODULE,
+        },
+};
+
+static __devinit int max8997_pmic_probe(struct platform_device *pdev)
+{
+        struct max8997_dev *iodev = dev_get_drvdata(pdev->dev.parent);
+        struct max8997_platform_data *pdata = dev_get_platdata(iodev->dev);
+        struct regulator_dev **rdev;
+        struct max8997_data *max8997;
+        struct i2c_client *i2c;
+        int i, ret, size;
+        u8 max_buck1 = 0, max_buck2 = 0, max_buck5 = 0;
+
+        if (!pdata) {
+                dev_err(pdev->dev.parent, "No platform init data supplied.\n");
+                return -ENODEV;
+        }
+
+        max8997 = kzalloc(sizeof(struct max8997_data), GFP_KERNEL);
+        if (!max8997)
+                return -ENOMEM;
+
+        size = sizeof(struct regulator_dev *) * pdata->num_regulators;
+        max8997->rdev = kzalloc(size, GFP_KERNEL);
+        if (!max8997->rdev) {
+                kfree(max8997);
+                return -ENOMEM;
+        }
+
+        rdev = max8997->rdev;
+        max8997->dev = &pdev->dev;
+        max8997->iodev = iodev;
+        max8997->num_regulators = pdata->num_regulators;
+        platform_set_drvdata(pdev, max8997);
+        i2c = max8997->iodev->i2c;
+
+        max8997->buck125_gpioindex = pdata->buck125_default_idx;
+
+        for (i = 0; i < 8; i++) {
+                max8997->buck1_vol[i] = ret =
+                        max8997_get_voltage_proper_val(
+                                        &buck1245_voltage_map_desc,
+                                        pdata->buck1_voltage[i] / 1000,
+                                        pdata->buck1_voltage[i] / 1000 +
+                                        buck1245_voltage_map_desc.step);
+                if (ret < 0)
+                        goto err_alloc;
+
+                max8997->buck2_vol[i] = ret =
+                        max8997_get_voltage_proper_val(
+                                        &buck1245_voltage_map_desc,
+                                        pdata->buck2_voltage[i] / 1000,
+                                        pdata->buck2_voltage[i] / 1000 +
+                                        buck1245_voltage_map_desc.step);
+                if (ret < 0)
+                        goto err_alloc;
+
+                max8997->buck5_vol[i] = ret =
+                        max8997_get_voltage_proper_val(
+                                        &buck1245_voltage_map_desc,
+                                        pdata->buck5_voltage[i] / 1000,
+                                        pdata->buck5_voltage[i] / 1000 +
+                                        buck1245_voltage_map_desc.step);
+                if (ret < 0)
+                        goto err_alloc;
+
+                if (max_buck1 < max8997->buck1_vol[i])
+                        max_buck1 = max8997->buck1_vol[i];
+                if (max_buck2 < max8997->buck2_vol[i])
+                        max_buck2 = max8997->buck2_vol[i];
+                if (max_buck5 < max8997->buck5_vol[i])
+                        max_buck5 = max8997->buck5_vol[i];
+        }
+
+        /* For the safety, set max voltage before setting up */
+        for (i = 0; i < 8; i++) {
+                max8997_update_reg(i2c, MAX8997_REG_BUCK1DVS(i + 1),
+                                max_buck1, 0x3f);
+                max8997_update_reg(i2c, MAX8997_REG_BUCK2DVS(i + 1),
+                                max_buck2, 0x3f);
+                max8997_update_reg(i2c, MAX8997_REG_BUCK5DVS(i + 1),
+                                max_buck5, 0x3f);
+        }
+
+        /*
+         * If buck 1, 2, and 5 do not care DVS GPIO settings, ignore them.
+         * If at least one of them cares, set gpios.
+         */
+        if (pdata->buck1_gpiodvs || pdata->buck2_gpiodvs ||
+                        pdata->buck5_gpiodvs) {
+                bool gpio1set = false, gpio2set = false;
+
+                if (!gpio_is_valid(pdata->buck125_gpios[0]) ||
+                                !gpio_is_valid(pdata->buck125_gpios[1]) ||
+                                !gpio_is_valid(pdata->buck125_gpios[2])) {
+                        dev_err(&pdev->dev, "GPIO NOT VALID\n");
+                        ret = -EINVAL;
+                        goto err_alloc;
+                }
+
+                ret = gpio_request(pdata->buck125_gpios[0],
+                                "MAX8997 SET1");
+                if (ret == -EBUSY)
+                        dev_warn(&pdev->dev, "Duplicated gpio request"
+                                        " on SET1\n");
+                else if (ret)
+                        goto err_alloc;
+                else
+                        gpio1set = true;
+
+                ret = gpio_request(pdata->buck125_gpios[1],
+                                "MAX8997 SET2");
+                if (ret == -EBUSY)
+                        dev_warn(&pdev->dev, "Duplicated gpio request"
+                                        " on SET2\n");
+                else if (ret) {
+                        if (gpio1set)
+                                gpio_free(pdata->buck125_gpios[0]);
+                        goto err_alloc;
+                } else
+                        gpio2set = true;
+
+                ret = gpio_request(pdata->buck125_gpios[2],
+                                "MAX8997 SET3");
+                if (ret == -EBUSY)
+                        dev_warn(&pdev->dev, "Duplicated gpio request"
+                                        " on SET3\n");
+                else if (ret) {
+                        if (gpio1set)
+                                gpio_free(pdata->buck125_gpios[0]);
+                        if (gpio2set)
+                                gpio_free(pdata->buck125_gpios[1]);
+                        goto err_alloc;
+                }
+
+                gpio_direction_output(pdata->buck125_gpios[0],
+                                (max8997->buck125_gpioindex >> 2)
+                                & 0x1); /* SET1 */
+                gpio_direction_output(pdata->buck125_gpios[1],
+                                (max8997->buck125_gpioindex >> 1)
+                                & 0x1); /* SET2 */
+                gpio_direction_output(pdata->buck125_gpios[2],
+                                (max8997->buck125_gpioindex >> 0)
+                                & 0x1); /* SET3 */
+                ret = 0;
+        }
+
+        /* DVS-GPIO disabled */
+        max8997_update_reg(i2c, MAX8997_REG_BUCK1CTRL, (pdata->buck1_gpiodvs) ?
+                        (1 << 1) : (0 << 1), 1 << 1);
+        max8997_update_reg(i2c, MAX8997_REG_BUCK2CTRL, (pdata->buck2_gpiodvs) ?
+                        (1 << 1) : (0 << 1), 1 << 1);
+        max8997_update_reg(i2c, MAX8997_REG_BUCK5CTRL, (pdata->buck5_gpiodvs) ?
+                        (1 << 1) : (0 << 1), 1 << 1);
+
+        /* Initialize all the DVS related BUCK registers */
+        for (i = 0; i < 8; i++) {
+                max8997_update_reg(i2c, MAX8997_REG_BUCK1DVS(i + 1),
+                                max8997->buck1_vol[i],
+                                0x3f);
+                max8997_update_reg(i2c, MAX8997_REG_BUCK2DVS(i + 1),
+                                max8997->buck2_vol[i],
+                                0x3f);
+                max8997_update_reg(i2c, MAX8997_REG_BUCK5DVS(i + 1),
+                                max8997->buck5_vol[i],
+                                0x3f);
+        }
+
+        for (i = 0; i < pdata->num_regulators; i++) {
+                const struct voltage_map_desc *desc;
+                int id = pdata->regulators[i].id;
+
+                desc = reg_voltage_map[id];
+                if (desc)
+                        regulators[id].n_voltages =
+                                (desc->max - desc->min) / desc->step + 1;
+                else if (id == MAX8997_ESAFEOUT1 || id == MAX8997_ESAFEOUT2)
+                        regulators[id].n_voltages = 4;
+                else if (id == MAX8997_CHARGER_CV)
+                        regulators[id].n_voltages = 16;
+
+                rdev[i] = regulator_register(&regulators[id], max8997->dev,
+                                pdata->regulators[i].initdata, max8997);
+                if (IS_ERR(rdev[i])) {
+                        ret = PTR_ERR(rdev[i]);
+                        dev_err(max8997->dev, "regulator init failed for %d\n",
+                                        id);
+                        rdev[i] = NULL;
+                        goto err;
+                }
+        }
+
+        /* Misc Settings */
+        max8997->ramp_delay = 10; /* set 10mV/us, which is the default */
+        max8997_write_reg(i2c, MAX8997_REG_BUCKRAMP, (0xf << 4) | 0x9);
+
+        return 0;
+err:
+        for (i = 0; i < max8997->num_regulators; i++)
+                if (rdev[i])
+                        regulator_unregister(rdev[i]);
+err_alloc:
+        kfree(max8997->rdev);
+        kfree(max8997);
+
+        return ret;
+}
+
+static int __devexit max8997_pmic_remove(struct platform_device *pdev)
+{
+        struct max8997_data *max8997 = platform_get_drvdata(pdev);
+        struct regulator_dev **rdev = max8997->rdev;
+        int i;
+
+        for (i = 0; i < max8997->num_regulators; i++)
+                if (rdev[i])
+                        regulator_unregister(rdev[i]);
+
+        kfree(max8997->rdev);
+        kfree(max8997);
+
+        return 0;
+}
+
+static const struct platform_device_id max8997_pmic_id[] = {
+        { "max8997-pmic", 0},
+        { },
+};
+
+static struct platform_driver max8997_pmic_driver = {
+        .driver = {
+                .name = "max8997-pmic",
+                .owner = THIS_MODULE,
+        },
+        .probe = max8997_pmic_probe,
+        .remove = __devexit_p(max8997_pmic_remove),
+        .id_table = max8997_pmic_id,
+};
+
+static int __init max8997_pmic_init(void)
+{
+        return platform_driver_register(&max8997_pmic_driver);
+}
+subsys_initcall(max8997_pmic_init);
+
+static void __exit max8997_pmic_cleanup(void)
+{
+        platform_driver_unregister(&max8997_pmic_driver);
+}
+module_exit(max8997_pmic_cleanup);
+
+MODULE_DESCRIPTION("MAXIM 8997/8966 Regulator Driver");
+MODULE_AUTHOR("MyungJoo Ham <myungjoo.ham@samsung.com>");
+MODULE_LICENSE("GPL");
diff --git a/drivers/regulator/mc13783-regulator.c b/drivers/regulator/mc13783-regulator.c
index 3e5d0c3b4e53..23249cb0a8bd 100644
--- a/drivers/regulator/mc13783-regulator.c
+++ b/drivers/regulator/mc13783-regulator.c
@@ -15,6 +15,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/platform_device.h>
 #include <linux/kernel.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/err.h>
@@ -336,8 +337,7 @@ static int __devinit mc13783_regulator_probe(struct platform_device *pdev)
 {
         struct mc13xxx_regulator_priv *priv;
         struct mc13xxx *mc13783 = dev_get_drvdata(pdev->dev.parent);
-        struct mc13783_regulator_platform_data *pdata =
-                dev_get_platdata(&pdev->dev);
+        struct mc13783_regulator_platform_data *pdata = mfd_get_data(pdev);
         struct mc13783_regulator_init_data *init_data;
         int i, ret;
 
@@ -381,8 +381,7 @@ err:
 static int __devexit mc13783_regulator_remove(struct platform_device *pdev)
 {
         struct mc13xxx_regulator_priv *priv = platform_get_drvdata(pdev);
-        struct mc13783_regulator_platform_data *pdata =
-                dev_get_platdata(&pdev->dev);
+        struct mc13783_regulator_platform_data *pdata = mfd_get_data(pdev);
         int i;
 
         platform_set_drvdata(pdev, NULL);
diff --git a/drivers/regulator/mc13892-regulator.c b/drivers/regulator/mc13892-regulator.c
index 1b8f7398a4a8..6f15168e5ed4 100644
--- a/drivers/regulator/mc13892-regulator.c
+++ b/drivers/regulator/mc13892-regulator.c
@@ -15,6 +15,7 @@
 #include <linux/regulator/driver.h>
 #include <linux/platform_device.h>
 #include <linux/kernel.h>
+#include <linux/mfd/core.h>
 #include <linux/slab.h>
 #include <linux/init.h>
 #include <linux/err.h>
@@ -520,8 +521,7 @@ static int __devinit mc13892_regulator_probe(struct platform_device *pdev)
 {
         struct mc13xxx_regulator_priv *priv;
         struct mc13xxx *mc13892 = dev_get_drvdata(pdev->dev.parent);
-        struct mc13xxx_regulator_platform_data *pdata =
-                dev_get_platdata(&pdev->dev);
+        struct mc13xxx_regulator_platform_data *pdata = mfd_get_data(pdev);
         struct mc13xxx_regulator_init_data *init_data;
         int i, ret;
         u32 val;
@@ -595,8 +595,7 @@ err_free:
 static int __devexit mc13892_regulator_remove(struct platform_device *pdev)
 {
         struct mc13xxx_regulator_priv *priv = platform_get_drvdata(pdev);
-        struct mc13xxx_regulator_platform_data *pdata =
-                dev_get_platdata(&pdev->dev);
+        struct mc13xxx_regulator_platform_data *pdata = mfd_get_data(pdev);
         int i;
 
         platform_set_drvdata(pdev, NULL);
diff --git a/drivers/regulator/tps6105x-regulator.c b/drivers/regulator/tps6105x-regulator.c
new file mode 100644
index 000000000000..1661499feda4
--- /dev/null
+++ b/drivers/regulator/tps6105x-regulator.c
@@ -0,0 +1,196 @@
+/*
+ * Driver for TPS61050/61052 boost converters, typically used for white LEDs
+ * or audio amplifiers.
+ *
+ * Copyright (C) 2011 ST-Ericsson SA
+ * Written on behalf of Linaro for ST-Ericsson
+ *
+ * Author: Linus Walleij <linus.walleij@linaro.org>
+ *
+ * License terms: GNU General Public License (GPL) version 2
+ */
+
+#include <linux/module.h>
+#include <linux/kernel.h>
+#include <linux/init.h>
+#include <linux/err.h>
+#include <linux/i2c.h>
+#include <linux/platform_device.h>
+#include <linux/regulator/driver.h>
+#include <linux/mfd/core.h>
+#include <linux/mfd/tps6105x.h>
+
+static const int tps6105x_voltages[] = {
+        4500000,
+        5000000,
+        5250000,
+        5000000, /* There is an additional 5V */
+};
+
+static int tps6105x_regulator_enable(struct regulator_dev *rdev)
+{
+        struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
+        int ret;
+
+        /* Activate voltage mode */
+        ret = tps6105x_mask_and_set(tps6105x, TPS6105X_REG_0,
+                TPS6105X_REG0_MODE_MASK,
+                TPS6105X_REG0_MODE_VOLTAGE << TPS6105X_REG0_MODE_SHIFT);
+        if (ret)
+                return ret;
+
+        return 0;
+}
+
+static int tps6105x_regulator_disable(struct regulator_dev *rdev)
+{
+        struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
+        int ret;
+
+        /* Set into shutdown mode */
+        ret = tps6105x_mask_and_set(tps6105x, TPS6105X_REG_0,
+                TPS6105X_REG0_MODE_MASK,
+                TPS6105X_REG0_MODE_SHUTDOWN << TPS6105X_REG0_MODE_SHIFT);
+        if (ret)
+                return ret;
+
+        return 0;
+}
+
+static int tps6105x_regulator_is_enabled(struct regulator_dev *rdev)
+{
+        struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
+        u8 regval;
+        int ret;
+
+        ret = tps6105x_get(tps6105x, TPS6105X_REG_0, &regval);
+        if (ret)
+                return ret;
+        regval &= TPS6105X_REG0_MODE_MASK;
+        regval >>= TPS6105X_REG0_MODE_SHIFT;
+
+        if (regval == TPS6105X_REG0_MODE_VOLTAGE)
+                return 1;
+
+        return 0;
+}
+
+static int tps6105x_regulator_get_voltage_sel(struct regulator_dev *rdev)
+{
+        struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
+        u8 regval;
+        int ret;
+
+        ret = tps6105x_get(tps6105x, TPS6105X_REG_0, &regval);
+        if (ret)
+                return ret;
+
+        regval &= TPS6105X_REG0_VOLTAGE_MASK;
+        regval >>= TPS6105X_REG0_VOLTAGE_SHIFT;
+        return (int) regval;
+}
+
+static int tps6105x_regulator_set_voltage_sel(struct regulator_dev *rdev,
+                                              unsigned selector)
+{
+        struct tps6105x *tps6105x = rdev_get_drvdata(rdev);
+        int ret;
+
+        ret = tps6105x_mask_and_set(tps6105x, TPS6105X_REG_0,
+                                    TPS6105X_REG0_VOLTAGE_MASK,
+                                    selector << TPS6105X_REG0_VOLTAGE_SHIFT);
+        if (ret)
+                return ret;
+
+        return 0;
+}
+
+static int tps6105x_regulator_list_voltage(struct regulator_dev *rdev,
+                                           unsigned selector)
+{
+        if (selector >= ARRAY_SIZE(tps6105x_voltages))
+                return -EINVAL;
+
+        return tps6105x_voltages[selector];
+}
+
+static struct regulator_ops tps6105x_regulator_ops = {
+        .enable         = tps6105x_regulator_enable,
+        .disable        = tps6105x_regulator_disable,
+        .is_enabled     = tps6105x_regulator_is_enabled,
+        .get_voltage_sel = tps6105x_regulator_get_voltage_sel,
+        .set_voltage_sel = tps6105x_regulator_set_voltage_sel,
+        .list_voltage   = tps6105x_regulator_list_voltage,
+};
+
+static struct regulator_desc tps6105x_regulator_desc = {
+        .name           = "tps6105x-boost",
+        .ops            = &tps6105x_regulator_ops,
+        .type           = REGULATOR_VOLTAGE,
+        .id             = 0,
+        .owner          = THIS_MODULE,
+        .n_voltages     = ARRAY_SIZE(tps6105x_voltages),
+};
+
+/*
+ * Registers the chip as a voltage regulator
+ */
+static int __devinit tps6105x_regulator_probe(struct platform_device *pdev)
+{
+        struct tps6105x *tps6105x = mfd_get_data(pdev);
+        struct tps6105x_platform_data *pdata = tps6105x->pdata;
+        int ret;
+
+        /* This instance is not set for regulator mode so bail out */
+        if (pdata->mode != TPS6105X_MODE_VOLTAGE) {
+                dev_info(&pdev->dev,
+                         "chip not in voltage mode mode, exit probe \n");
+                return 0;
+        }
+
+        /* Register regulator with framework */
+        tps6105x->regulator = regulator_register(&tps6105x_regulator_desc,
+                                             &tps6105x->client->dev,
+                                             pdata->regulator_data, tps6105x);
+        if (IS_ERR(tps6105x->regulator)) {
+                ret = PTR_ERR(tps6105x->regulator);
+                dev_err(&tps6105x->client->dev,
+                        "failed to register regulator\n");
+                return ret;
+        }
+
+        return 0;
+}
+
+static int __devexit tps6105x_regulator_remove(struct platform_device *pdev)
+{
+        struct tps6105x *tps6105x = platform_get_drvdata(pdev);
+        regulator_unregister(tps6105x->regulator);
+        return 0;
+}
+
+static struct platform_driver tps6105x_regulator_driver = {
+        .driver = {
+                .name  = "tps6105x-regulator",
+                .owner = THIS_MODULE,
+        },
+        .probe = tps6105x_regulator_probe,
+        .remove = __devexit_p(tps6105x_regulator_remove),
+};
+
+static __init int tps6105x_regulator_init(void)
+{
+        return platform_driver_register(&tps6105x_regulator_driver);
+}
+subsys_initcall(tps6105x_regulator_init);
+
+static __exit void tps6105x_regulator_exit(void)
+{
+        platform_driver_unregister(&tps6105x_regulator_driver);
+}
+module_exit(tps6105x_regulator_exit);
+
+MODULE_AUTHOR("Linus Walleij <linus.walleij@linaro.org>");
+MODULE_DESCRIPTION("TPS6105x regulator driver");
+MODULE_LICENSE("GPL v2");
+MODULE_ALIAS("platform:tps6105x-regulator");
diff --git a/drivers/regulator/twl-regulator.c b/drivers/regulator/twl-regulator.c
index bd332cf1cc3f..6a292852a358 100644
--- a/drivers/regulator/twl-regulator.c
+++ b/drivers/regulator/twl-regulator.c
@@ -475,6 +475,13 @@ static struct regulator_ops twlfixed_ops = {
         .get_status     = twlreg_get_status,
 };
 
+static struct regulator_ops twl6030_fixed_resource = {
+        .enable         = twlreg_enable,
+        .disable        = twlreg_disable,
+        .is_enabled     = twlreg_is_enabled,
+        .get_status     = twlreg_get_status,
+};
+
 /*----------------------------------------------------------------------*/
 
 #define TWL4030_FIXED_LDO(label, offset, mVolts, num, turnon_delay, \
@@ -538,6 +545,20 @@ static struct regulator_ops twlfixed_ops = {
                 }, \
         }
 
+#define TWL6030_FIXED_RESOURCE(label, offset, num, turnon_delay, remap_conf) { \
+        .base = offset, \
+        .id = num, \
+        .delay = turnon_delay, \
+        .remap = remap_conf, \
+        .desc = { \
+                .name = #label, \
+                .id = TWL6030_REG_##label, \
+                .ops = &twl6030_fixed_resource, \
+                .type = REGULATOR_VOLTAGE, \
+                .owner = THIS_MODULE, \
+                }, \
+        }
+
 /*
  * We list regulators here if systems need some level of
  * software control over them after boot.
@@ -577,7 +598,8 @@ static struct twlreg_info twl_regs[] = {
         TWL6030_FIXED_LDO(VANA, 0x50, 2100, 15, 0, 0x21),
         TWL6030_FIXED_LDO(VCXIO, 0x60, 1800, 16, 0, 0x21),
         TWL6030_FIXED_LDO(VDAC, 0x64, 1800, 17, 0, 0x21),
-        TWL6030_FIXED_LDO(VUSB, 0x70, 3300, 18, 0, 0x21)
+        TWL6030_FIXED_LDO(VUSB, 0x70, 3300, 18, 0, 0x21),
+        TWL6030_FIXED_RESOURCE(CLK32KG, 0x8C, 48, 0, 0x21),
 };
 
 static int __devinit twlreg_probe(struct platform_device *pdev)
diff --git a/drivers/spi/xilinx_spi.c b/drivers/spi/xilinx_spi.c
index 4d2c75df886c..c69c6f2c2c5c 100644
--- a/drivers/spi/xilinx_spi.c
+++ b/drivers/spi/xilinx_spi.c
@@ -18,6 +18,7 @@
 #include <linux/interrupt.h>
 #include <linux/of.h>
 #include <linux/platform_device.h>
+#include <linux/mfd/core.h>
 #include <linux/spi/spi.h>
 #include <linux/spi/spi_bitbang.h>
 #include <linux/spi/xilinx_spi.h>
@@ -470,7 +471,7 @@ static int __devinit xilinx_spi_probe(struct platform_device *dev)
         struct spi_master *master;
         u8 i;
 
-        pdata = dev->dev.platform_data;
+        pdata = mfd_get_data(dev);
         if (pdata) {
                 num_cs = pdata->num_chipselect;
                 little_endian = pdata->little_endian;
diff --git a/drivers/staging/tty/specialix.c b/drivers/staging/tty/specialix.c
index 47e5753f732a..17a1be536a46 100644
--- a/drivers/staging/tty/specialix.c
+++ b/drivers/staging/tty/specialix.c
@@ -1416,7 +1416,7 @@ static int sx_open(struct tty_struct *tty, struct file *filp)
                                  board, bp, port, SX_PORT(tty->index));
 
         if (sx_paranoia_check(port, tty->name, "sx_open")) {
-                func_enter();
+                func_exit();
                 return -ENODEV;
         }
 
@@ -1435,13 +1435,13 @@ static int sx_open(struct tty_struct *tty, struct file *filp)
 
         error = sx_setup_port(bp, port);
         if (error) {
-                func_enter();
+                func_exit();
                 return error;
         }
 
         error = block_til_ready(tty, filp, port);
         if (error) {
-                func_enter();
+                func_exit();
                 return error;
         }
 
@@ -1860,7 +1860,7 @@ static int sx_set_serial_info(struct specialix_port *port,
         func_enter();
 
         if (copy_from_user(&tmp, newinfo, sizeof(tmp))) {
-                func_enter();
+                func_exit();
                 return -EFAULT;
         }
 
diff --git a/drivers/tty/bfin_jtag_comm.c b/drivers/tty/bfin_jtag_comm.c
index 16402445f2b2..03c285bb2f18 100644
--- a/drivers/tty/bfin_jtag_comm.c
+++ b/drivers/tty/bfin_jtag_comm.c
@@ -251,11 +251,11 @@ static int __init bfin_jc_init(void)
         bfin_jc_write_buf.head = bfin_jc_write_buf.tail = 0;
         bfin_jc_write_buf.buf = kmalloc(CIRC_SIZE, GFP_KERNEL);
         if (!bfin_jc_write_buf.buf)
-                goto err;
+                goto err_buf;
 
         bfin_jc_driver = alloc_tty_driver(1);
         if (!bfin_jc_driver)
-                goto err;
+                goto err_driver;
 
         bfin_jc_driver->owner        = THIS_MODULE;
         bfin_jc_driver->driver_name  = DRV_NAME;
@@ -275,7 +275,9 @@ static int __init bfin_jc_init(void)
 
  err:
         put_tty_driver(bfin_jc_driver);
+ err_driver:
         kfree(bfin_jc_write_buf.buf);
+ err_buf:
         kthread_stop(bfin_jc_kthread);
         return ret;
 }
diff --git a/drivers/usb/host/ohci-tmio.c b/drivers/usb/host/ohci-tmio.c
index 8dabe8e31d8c..3558491dd87d 100644
--- a/drivers/usb/host/ohci-tmio.c
+++ b/drivers/usb/host/ohci-tmio.c
@@ -185,7 +185,7 @@ static struct platform_driver ohci_hcd_tmio_driver;
 
 static int __devinit ohci_hcd_tmio_drv_probe(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct resource *regs = platform_get_resource(dev, IORESOURCE_MEM, 0);
         struct resource *config = platform_get_resource(dev, IORESOURCE_MEM, 1);
         struct resource *sram = platform_get_resource(dev, IORESOURCE_MEM, 2);
@@ -274,7 +274,7 @@ static int __devexit ohci_hcd_tmio_drv_remove(struct platform_device *dev)
 {
         struct usb_hcd *hcd = platform_get_drvdata(dev);
         struct tmio_hcd *tmio = hcd_to_tmio(hcd);
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
 
         usb_remove_hcd(hcd);
         tmio_stop_hc(dev);
@@ -293,7 +293,7 @@ static int __devexit ohci_hcd_tmio_drv_remove(struct platform_device *dev)
 #ifdef CONFIG_PM
 static int ohci_hcd_tmio_drv_suspend(struct platform_device *dev, pm_message_t state)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct usb_hcd *hcd = platform_get_drvdata(dev);
         struct ohci_hcd *ohci = hcd_to_ohci(hcd);
         struct tmio_hcd *tmio = hcd_to_tmio(hcd);
@@ -326,7 +326,7 @@ static int ohci_hcd_tmio_drv_suspend(struct platform_device *dev, pm_message_t s
 
 static int ohci_hcd_tmio_drv_resume(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct usb_hcd *hcd = platform_get_drvdata(dev);
         struct ohci_hcd *ohci = hcd_to_ohci(hcd);
         struct tmio_hcd *tmio = hcd_to_tmio(hcd);
diff --git a/drivers/video/amba-clcd.c b/drivers/video/amba-clcd.c
index 013c8ce57205..5fc983c5b92c 100644
--- a/drivers/video/amba-clcd.c
+++ b/drivers/video/amba-clcd.c
@@ -120,8 +120,23 @@ static void clcdfb_enable(struct clcd_fb *fb, u32 cntl)
 static int
 clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
 {
+        u32 caps;
         int ret = 0;
 
+        if (fb->panel->caps && fb->board->caps)
+                caps = fb->panel->caps & fb->board->caps;
+        else {
+                /* Old way of specifying what can be used */
+                caps = fb->panel->cntl & CNTL_BGR ?
+                        CLCD_CAP_BGR : CLCD_CAP_RGB;
+                /* But mask out 444 modes as they weren't supported */
+                caps &= ~CLCD_CAP_444;
+        }
+
+        /* Only TFT panels can do RGB888/BGR888 */
+        if (!(fb->panel->cntl & CNTL_LCDTFT))
+                caps &= ~CLCD_CAP_888;
+
         memset(&var->transp, 0, sizeof(var->transp));
 
         var->red.msb_right = 0;
@@ -133,6 +148,13 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
         case 2:
         case 4:
         case 8:
+                /* If we can't do 5551, reject */
+                caps &= CLCD_CAP_5551;
+                if (!caps) {
+                        ret = -EINVAL;
+                        break;
+                }
+
                 var->red.length         = var->bits_per_pixel;
                 var->red.offset         = 0;
                 var->green.length       = var->bits_per_pixel;
@@ -140,23 +162,61 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
                 var->blue.length        = var->bits_per_pixel;
                 var->blue.offset        = 0;
                 break;
+
         case 16:
-                var->red.length = 5;
-                var->blue.length = 5;
+                /* If we can't do 444, 5551 or 565, reject */
+                if (!(caps & (CLCD_CAP_444 | CLCD_CAP_5551 | CLCD_CAP_565))) {
+                        ret = -EINVAL;
+                        break;
+                }
+
                 /*
-                 * Green length can be 5 or 6 depending whether
-                 * we're operating in RGB555 or RGB565 mode.
+                 * Green length can be 4, 5 or 6 depending whether
+                 * we're operating in 444, 5551 or 565 mode.
                  */
-                if (var->green.length != 5 && var->green.length != 6)
-                        var->green.length = 6;
+                if (var->green.length == 4 && caps & CLCD_CAP_444)
+                        caps &= CLCD_CAP_444;
+                if (var->green.length == 5 && caps & CLCD_CAP_5551)
+                        caps &= CLCD_CAP_5551;
+                else if (var->green.length == 6 && caps & CLCD_CAP_565)
+                        caps &= CLCD_CAP_565;
+                else {
+                        /*
+                         * PL110 officially only supports RGB555,
+                         * but may be wired up to allow RGB565.
+                         */
+                        if (caps & CLCD_CAP_565) {
+                                var->green.length = 6;
+                                caps &= CLCD_CAP_565;
+                        } else if (caps & CLCD_CAP_5551) {
+                                var->green.length = 5;
+                                caps &= CLCD_CAP_5551;
+                        } else {
+                                var->green.length = 4;
+                                caps &= CLCD_CAP_444;
+                        }
+                }
+
+                if (var->green.length >= 5) {
+                        var->red.length = 5;
+                        var->blue.length = 5;
+                } else {
+                        var->red.length = 4;
+                        var->blue.length = 4;
+                }
                 break;
         case 32:
-                if (fb->panel->cntl & CNTL_LCDTFT) {
-                        var->red.length         = 8;
-                        var->green.length       = 8;
-                        var->blue.length        = 8;
+                /* If we can't do 888, reject */
+                caps &= CLCD_CAP_888;
+                if (!caps) {
+                        ret = -EINVAL;
                         break;
                 }
+
+                var->red.length = 8;
+                var->green.length = 8;
+                var->blue.length = 8;
+                break;
         default:
                 ret = -EINVAL;
                 break;
@@ -168,7 +228,20 @@ clcdfb_set_bitfields(struct clcd_fb *fb, struct fb_var_screeninfo *var)
          * the bitfield length defined above.
          */
         if (ret == 0 && var->bits_per_pixel >= 16) {
-                if (fb->panel->cntl & CNTL_BGR) {
+                bool bgr, rgb;
+
+                bgr = caps & CLCD_CAP_BGR && var->blue.offset == 0;
+                rgb = caps & CLCD_CAP_RGB && var->red.offset == 0;
+
+                if (!bgr && !rgb)
+                        /*
+                         * The requested format was not possible, try just
+                         * our capabilities.  One of BGR or RGB must be
+                         * supported.
+                         */
+                        bgr = caps & CLCD_CAP_BGR;
+
+                if (bgr) {
                         var->blue.offset = 0;
                         var->green.offset = var->blue.offset + var->blue.length;
                         var->red.offset = var->green.offset + var->green.length;
@@ -443,8 +516,8 @@ static int clcdfb_register(struct clcd_fb *fb)
 
         fb_set_var(&fb->fb, &fb->fb.var);
 
-        printk(KERN_INFO "CLCD: %s hardware, %s display\n",
-               fb->board->name, fb->panel->mode.name);
+        dev_info(&fb->dev->dev, "%s hardware, %s display\n",
+                 fb->board->name, fb->panel->mode.name);
 
         ret = register_framebuffer(&fb->fb);
         if (ret == 0)
@@ -486,6 +559,10 @@ static int clcdfb_probe(struct amba_device *dev, const struct amba_id *id)
         fb->dev = dev;
         fb->board = board;
 
+        dev_info(&fb->dev->dev, "PL%03x rev%u at 0x%08llx\n",
+                amba_part(dev), amba_rev(dev),
+                (unsigned long long)dev->res.start);
+
         ret = fb->board->setup(fb);
         if (ret)
                 goto free_fb;
diff --git a/drivers/video/arkfb.c b/drivers/video/arkfb.c
index 391ac939f011..8686429cbdf0 100644
--- a/drivers/video/arkfb.c
+++ b/drivers/video/arkfb.c
@@ -158,12 +158,19 @@ static void arkfb_settile(struct fb_info *info, struct fb_tilemap *map)
         }
 }
 
+static void arkfb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+        struct arkfb_info *par = info->par;
+
+        svga_tilecursor(par->state.vgabase, info, cursor);
+}
+
 static struct fb_tile_ops arkfb_tile_ops = {
         .fb_settile     = arkfb_settile,
         .fb_tilecopy    = svga_tilecopy,
         .fb_tilefill    = svga_tilefill,
         .fb_tileblit    = svga_tileblit,
-        .fb_tilecursor  = svga_tilecursor,
+        .fb_tilecursor  = arkfb_tilecursor,
         .fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -466,32 +473,40 @@ static unsigned short dac_regs[4] = {0x3c8, 0x3c9, 0x3c6, 0x3c7};
 
 static void ark_dac_read_regs(void *data, u8 *code, int count)
 {
-        u8 regval = vga_rseq(NULL, 0x1C);
+        struct fb_info *info = data;
+        struct arkfb_info *par;
+        u8 regval;
 
+        par = info->par;
+        regval = vga_rseq(par->state.vgabase, 0x1C);
         while (count != 0)
         {
-                vga_wseq(NULL, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
-                code[1] = vga_r(NULL, dac_regs[code[0] & 3]);
+                vga_wseq(par->state.vgabase, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
+                code[1] = vga_r(par->state.vgabase, dac_regs[code[0] & 3]);
                 count--;
                 code += 2;
         }
 
-        vga_wseq(NULL, 0x1C, regval);
+        vga_wseq(par->state.vgabase, 0x1C, regval);
 }
 
 static void ark_dac_write_regs(void *data, u8 *code, int count)
 {
-        u8 regval = vga_rseq(NULL, 0x1C);
+        struct fb_info *info = data;
+        struct arkfb_info *par;
+        u8 regval;
 
+        par = info->par;
+        regval = vga_rseq(par->state.vgabase, 0x1C);
         while (count != 0)
         {
-                vga_wseq(NULL, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
-                vga_w(NULL, dac_regs[code[0] & 3], code[1]);
+                vga_wseq(par->state.vgabase, 0x1C, regval | (code[0] & 4 ? 0x80 : 0));
+                vga_w(par->state.vgabase, dac_regs[code[0] & 3], code[1]);
                 count--;
                 code += 2;
         }
 
-        vga_wseq(NULL, 0x1C, regval);
+        vga_wseq(par->state.vgabase, 0x1C, regval);
 }
 
 
@@ -507,8 +522,8 @@ static void ark_set_pixclock(struct fb_info *info, u32 pixclock)
         }
 
         /* Set VGA misc register  */
-        regval = vga_r(NULL, VGA_MIS_R);
-        vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+        regval = vga_r(par->state.vgabase, VGA_MIS_R);
+        vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 }
 
 
@@ -520,7 +535,10 @@ static int arkfb_open(struct fb_info *info, int user)
 
         mutex_lock(&(par->open_lock));
         if (par->ref_count == 0) {
+                void __iomem *vgabase = par->state.vgabase;
+
                 memset(&(par->state), 0, sizeof(struct vgastate));
+                par->state.vgabase = vgabase;
                 par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
                 par->state.num_crtc = 0x60;
                 par->state.num_seq = 0x30;
@@ -646,50 +664,50 @@ static int arkfb_set_par(struct fb_info *info)
         info->var.activate = FB_ACTIVATE_NOW;
 
         /* Unlock registers */
-        svga_wcrt_mask(0x11, 0x00, 0x80);
+        svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
 
         /* Blank screen and turn off sync */
-        svga_wseq_mask(0x01, 0x20, 0x20);
-        svga_wcrt_mask(0x17, 0x00, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
         /* Set default values */
-        svga_set_default_gfx_regs();
-        svga_set_default_atc_regs();
-        svga_set_default_seq_regs();
-        svga_set_default_crt_regs();
-        svga_wcrt_multi(ark_line_compare_regs, 0xFFFFFFFF);
-        svga_wcrt_multi(ark_start_address_regs, 0);
+        svga_set_default_gfx_regs(par->state.vgabase);
+        svga_set_default_atc_regs(par->state.vgabase);
+        svga_set_default_seq_regs(par->state.vgabase);
+        svga_set_default_crt_regs(par->state.vgabase);
+        svga_wcrt_multi(par->state.vgabase, ark_line_compare_regs, 0xFFFFFFFF);
+        svga_wcrt_multi(par->state.vgabase, ark_start_address_regs, 0);
 
         /* ARK specific initialization */
-        svga_wseq_mask(0x10, 0x1F, 0x1F); /* enable linear framebuffer and full memory access */
-        svga_wseq_mask(0x12, 0x03, 0x03); /* 4 MB linear framebuffer size */
+        svga_wseq_mask(par->state.vgabase, 0x10, 0x1F, 0x1F); /* enable linear framebuffer and full memory access */
+        svga_wseq_mask(par->state.vgabase, 0x12, 0x03, 0x03); /* 4 MB linear framebuffer size */
 
-        vga_wseq(NULL, 0x13, info->fix.smem_start >> 16);
-        vga_wseq(NULL, 0x14, info->fix.smem_start >> 24);
-        vga_wseq(NULL, 0x15, 0);
-        vga_wseq(NULL, 0x16, 0);
+        vga_wseq(par->state.vgabase, 0x13, info->fix.smem_start >> 16);
+        vga_wseq(par->state.vgabase, 0x14, info->fix.smem_start >> 24);
+        vga_wseq(par->state.vgabase, 0x15, 0);
+        vga_wseq(par->state.vgabase, 0x16, 0);
 
         /* Set the FIFO threshold register */
         /* It is fascinating way to store 5-bit value in 8-bit register */
         regval = 0x10 | ((threshold & 0x0E) >> 1) | (threshold & 0x01) << 7 | (threshold & 0x10) << 1;
-        vga_wseq(NULL, 0x18, regval);
+        vga_wseq(par->state.vgabase, 0x18, regval);
 
         /* Set the offset register */
         pr_debug("fb%d: offset register       : %d\n", info->node, offset_value);
-        svga_wcrt_multi(ark_offset_regs, offset_value);
+        svga_wcrt_multi(par->state.vgabase, ark_offset_regs, offset_value);
 
         /* fix for hi-res textmode */
-        svga_wcrt_mask(0x40, 0x08, 0x08);
+        svga_wcrt_mask(par->state.vgabase, 0x40, 0x08, 0x08);
 
         if (info->var.vmode & FB_VMODE_DOUBLE)
-                svga_wcrt_mask(0x09, 0x80, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
         else
-                svga_wcrt_mask(0x09, 0x00, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
         if (info->var.vmode & FB_VMODE_INTERLACED)
-                svga_wcrt_mask(0x44, 0x04, 0x04);
+                svga_wcrt_mask(par->state.vgabase, 0x44, 0x04, 0x04);
         else
-                svga_wcrt_mask(0x44, 0x00, 0x04);
+                svga_wcrt_mask(par->state.vgabase, 0x44, 0x00, 0x04);
 
         hmul = 1;
         hdiv = 1;
@@ -699,40 +717,40 @@ static int arkfb_set_par(struct fb_info *info)
         switch (mode) {
         case 0:
                 pr_debug("fb%d: text mode\n", info->node);
-                svga_set_textmode_vga_regs();
+                svga_set_textmode_vga_regs(par->state.vgabase);
 
-                vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-                svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
                 dac_set_mode(par->dac, DAC_PSEUDO8_8);
 
                 break;
         case 1:
                 pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-                vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
+                vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
 
-                vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-                svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
                 dac_set_mode(par->dac, DAC_PSEUDO8_8);
                 break;
         case 2:
                 pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x10); /* basic VGA mode */
-                svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x10); /* basic VGA mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
                 dac_set_mode(par->dac, DAC_PSEUDO8_8);
                 break;
         case 3:
                 pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x16); /* 8bpp accel mode */
+                vga_wseq(par->state.vgabase, 0x11, 0x16); /* 8bpp accel mode */
 
                 if (info->var.pixclock > 20000) {
                         pr_debug("fb%d: not using multiplex\n", info->node);
-                        svga_wcrt_mask(0x46, 0x00, 0x04); /* 8bit pixel path */
+                        svga_wcrt_mask(par->state.vgabase, 0x46, 0x00, 0x04); /* 8bit pixel path */
                         dac_set_mode(par->dac, DAC_PSEUDO8_8);
                 } else {
                         pr_debug("fb%d: using multiplex\n", info->node);
-                        svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                        svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                         dac_set_mode(par->dac, DAC_PSEUDO8_16);
                         hdiv = 2;
                 }
@@ -740,22 +758,22 @@ static int arkfb_set_par(struct fb_info *info)
         case 4:
                 pr_debug("fb%d: 5/5/5 truecolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x1A); /* 16bpp accel mode */
-                svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x1A); /* 16bpp accel mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                 dac_set_mode(par->dac, DAC_RGB1555_16);
                 break;
         case 5:
                 pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x1A); /* 16bpp accel mode */
-                svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x1A); /* 16bpp accel mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                 dac_set_mode(par->dac, DAC_RGB0565_16);
                 break;
         case 6:
                 pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x16); /* 8bpp accel mode ??? */
-                svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x16); /* 8bpp accel mode ??? */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                 dac_set_mode(par->dac, DAC_RGB0888_16);
                 hmul = 3;
                 hdiv = 2;
@@ -763,8 +781,8 @@ static int arkfb_set_par(struct fb_info *info)
         case 7:
                 pr_debug("fb%d: 8/8/8/8 truecolor\n", info->node);
 
-                vga_wseq(NULL, 0x11, 0x1E); /* 32bpp accel mode */
-                svga_wcrt_mask(0x46, 0x04, 0x04); /* 16bit pixel path */
+                vga_wseq(par->state.vgabase, 0x11, 0x1E); /* 32bpp accel mode */
+                svga_wcrt_mask(par->state.vgabase, 0x46, 0x04, 0x04); /* 16bit pixel path */
                 dac_set_mode(par->dac, DAC_RGB8888_16);
                 hmul = 2;
                 break;
@@ -774,7 +792,7 @@ static int arkfb_set_par(struct fb_info *info)
         }
 
         ark_set_pixclock(info, (hdiv * info->var.pixclock) / hmul);
-        svga_set_timings(&ark_timing_regs, &(info->var), hmul, hdiv,
+        svga_set_timings(par->state.vgabase, &ark_timing_regs, &(info->var), hmul, hdiv,
                          (info->var.vmode & FB_VMODE_DOUBLE)     ? 2 : 1,
                          (info->var.vmode & FB_VMODE_INTERLACED) ? 2 : 1,
                           hmul, info->node);
@@ -782,12 +800,12 @@ static int arkfb_set_par(struct fb_info *info)
         /* Set interlaced mode start/end register */
         value = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
         value = ((value * hmul / hdiv) / 8) - 5;
-        vga_wcrt(NULL, 0x42, (value + 1) / 2);
+        vga_wcrt(par->state.vgabase, 0x42, (value + 1) / 2);
 
         memset_io(info->screen_base, 0x00, screen_size);
         /* Device and screen back on */
-        svga_wcrt_mask(0x17, 0x80, 0x80);
-        svga_wseq_mask(0x01, 0x00, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
         return 0;
 }
@@ -857,23 +875,25 @@ static int arkfb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int arkfb_blank(int blank_mode, struct fb_info *info)
 {
+        struct arkfb_info *par = info->par;
+
         switch (blank_mode) {
         case FB_BLANK_UNBLANK:
                 pr_debug("fb%d: unblank\n", info->node);
-                svga_wseq_mask(0x01, 0x00, 0x20);
-                svga_wcrt_mask(0x17, 0x80, 0x80);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
                 break;
         case FB_BLANK_NORMAL:
                 pr_debug("fb%d: blank\n", info->node);
-                svga_wseq_mask(0x01, 0x20, 0x20);
-                svga_wcrt_mask(0x17, 0x80, 0x80);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
                 break;
         case FB_BLANK_POWERDOWN:
         case FB_BLANK_HSYNC_SUSPEND:
         case FB_BLANK_VSYNC_SUSPEND:
                 pr_debug("fb%d: sync down\n", info->node);
-                svga_wseq_mask(0x01, 0x20, 0x20);
-                svga_wcrt_mask(0x17, 0x00, 0x80);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
                 break;
         }
         return 0;
@@ -884,6 +904,7 @@ static int arkfb_blank(int blank_mode, struct fb_info *info)
 
 static int arkfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
+        struct arkfb_info *par = info->par;
         unsigned int offset;
 
         /* Calculate the offset */
@@ -897,7 +918,7 @@ static int arkfb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info
         }
 
         /* Set the offset */
-        svga_wcrt_multi(ark_start_address_regs, offset);
+        svga_wcrt_multi(par->state.vgabase, ark_start_address_regs, offset);
 
         return 0;
 }
@@ -930,6 +951,8 @@ static struct fb_ops arkfb_ops = {
 /* PCI probe */
 static int __devinit ark_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+        struct pci_bus_region bus_reg;
+        struct resource vga_res;
         struct fb_info *info;
         struct arkfb_info *par;
         int rc;
@@ -985,8 +1008,17 @@ static int __devinit ark_pci_probe(struct pci_dev *dev, const struct pci_device_
                 goto err_iomap;
         }
 
+        bus_reg.start = 0;
+        bus_reg.end = 64 * 1024;
+
+        vga_res.flags = IORESOURCE_IO;
+
+        pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+        par->state.vgabase = (void __iomem *) vga_res.start;
+
         /* FIXME get memsize */
-        regval = vga_rseq(NULL, 0x10);
+        regval = vga_rseq(par->state.vgabase, 0x10);
         info->screen_size = (1 << (regval >> 6)) << 20;
         info->fix.smem_len = info->screen_size;
 
diff --git a/drivers/video/atmel_lcdfb.c b/drivers/video/atmel_lcdfb.c
index 4b4e8dadd6b2..ccecf9974587 100644
--- a/drivers/video/atmel_lcdfb.c
+++ b/drivers/video/atmel_lcdfb.c
@@ -68,7 +68,7 @@ static void atmel_lcdfb_update_dma2d(struct atmel_lcdfb_info *sinfo,
 }
 #endif
 
-static const u32 contrast_ctr = ATMEL_LCDC_PS_DIV8
+static u32 contrast_ctr = ATMEL_LCDC_PS_DIV8
                 | ATMEL_LCDC_POL_POSITIVE
                 | ATMEL_LCDC_ENA_PWMENABLE;
 
@@ -164,6 +164,10 @@ static void exit_backlight(struct atmel_lcdfb_info *sinfo)
 
 static void init_contrast(struct atmel_lcdfb_info *sinfo)
 {
+        /* contrast pwm can be 'inverted' */
+        if (sinfo->lcdcon_pol_negative)
+                        contrast_ctr &= ~(ATMEL_LCDC_POL_POSITIVE);
+
         /* have some default contrast/backlight settings */
         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_CTR, contrast_ctr);
         lcdc_writel(sinfo, ATMEL_LCDC_CONTRAST_VAL, ATMEL_LCDC_CVAL_DEFAULT);
@@ -711,11 +715,35 @@ static int atmel_lcdfb_pan_display(struct fb_var_screeninfo *var,
         return 0;
 }
 
+static int atmel_lcdfb_blank(int blank_mode, struct fb_info *info)
+{
+        struct atmel_lcdfb_info *sinfo = info->par;
+
+        switch (blank_mode) {
+        case FB_BLANK_UNBLANK:
+        case FB_BLANK_NORMAL:
+                atmel_lcdfb_start(sinfo);
+                break;
+        case FB_BLANK_VSYNC_SUSPEND:
+        case FB_BLANK_HSYNC_SUSPEND:
+                break;
+        case FB_BLANK_POWERDOWN:
+                atmel_lcdfb_stop(sinfo);
+                break;
+        default:
+                return -EINVAL;
+        }
+
+        /* let fbcon do a soft blank for us */
+        return ((blank_mode == FB_BLANK_NORMAL) ? 1 : 0);
+}
+
 static struct fb_ops atmel_lcdfb_ops = {
         .owner          = THIS_MODULE,
         .fb_check_var   = atmel_lcdfb_check_var,
         .fb_set_par     = atmel_lcdfb_set_par,
         .fb_setcolreg   = atmel_lcdfb_setcolreg,
+        .fb_blank       = atmel_lcdfb_blank,
         .fb_pan_display = atmel_lcdfb_pan_display,
         .fb_fillrect    = cfb_fillrect,
         .fb_copyarea    = cfb_copyarea,
@@ -817,6 +845,7 @@ static int __init atmel_lcdfb_probe(struct platform_device *pdev)
                 sinfo->guard_time = pdata_sinfo->guard_time;
                 sinfo->smem_len = pdata_sinfo->smem_len;
                 sinfo->lcdcon_is_backlight = pdata_sinfo->lcdcon_is_backlight;
+                sinfo->lcdcon_pol_negative = pdata_sinfo->lcdcon_pol_negative;
                 sinfo->lcd_wiring_mode = pdata_sinfo->lcd_wiring_mode;
         } else {
                 dev_err(dev, "cannot get default configuration\n");
diff --git a/drivers/video/aty/radeon_base.c b/drivers/video/aty/radeon_base.c
index 3c1e13ed1cba..32f8cf6200a7 100644
--- a/drivers/video/aty/radeon_base.c
+++ b/drivers/video/aty/radeon_base.c
@@ -1248,7 +1248,7 @@ static void radeon_write_pll_regs(struct radeonfb_info *rinfo, struct radeon_reg
 
         /* Workaround from XFree */
         if (rinfo->is_mobility) {
-                /* A temporal workaround for the occational blanking on certain laptop
+                /* A temporal workaround for the occasional blanking on certain laptop
                  * panels. This appears to related to the PLL divider registers
                  * (fail to lock?). It occurs even when all dividers are the same
                  * with their old settings. In this case we really don't need to
diff --git a/drivers/video/aty/radeon_i2c.c b/drivers/video/aty/radeon_i2c.c
index 78d1f4cd1fe0..ab1d0fd76316 100644
--- a/drivers/video/aty/radeon_i2c.c
+++ b/drivers/video/aty/radeon_i2c.c
@@ -100,6 +100,9 @@ void radeon_create_i2c_busses(struct radeonfb_info *rinfo)
 {
         rinfo->i2c[0].rinfo     = rinfo;
         rinfo->i2c[0].ddc_reg   = GPIO_MONID;
+#ifndef CONFIG_PPC
+        rinfo->i2c[0].adapter.class = I2C_CLASS_HWMON;
+#endif
         radeon_setup_i2c_bus(&rinfo->i2c[0], "monid");
 
         rinfo->i2c[1].rinfo     = rinfo;
diff --git a/drivers/video/backlight/88pm860x_bl.c b/drivers/video/backlight/88pm860x_bl.c
index e59623a15f3f..c8b520e9a11a 100644
--- a/drivers/video/backlight/88pm860x_bl.c
+++ b/drivers/video/backlight/88pm860x_bl.c
@@ -12,11 +12,12 @@
 #include <linux/init.h>
 #include <linux/kernel.h>
 #include <linux/platform_device.h>
+#include <linux/slab.h>
 #include <linux/fb.h>
 #include <linux/i2c.h>
 #include <linux/backlight.h>
+#include <linux/mfd/core.h>
 #include <linux/mfd/88pm860x.h>
-#include <linux/slab.h>
 
 #define MAX_BRIGHTNESS          (0xFF)
 #define MIN_BRIGHTNESS          (0)
@@ -161,32 +162,13 @@ static const struct backlight_ops pm860x_backlight_ops = {
         .get_brightness = pm860x_backlight_get_brightness,
 };
 
-static int __check_device(struct pm860x_backlight_pdata *pdata, char *name)
-{
-        struct pm860x_backlight_pdata *p = pdata;
-        int ret = -EINVAL;
-
-        while (p && p->id) {
-                if ((p->id != PM8606_ID_BACKLIGHT) || (p->flags < 0))
-                        break;
-
-                if (!strncmp(name, pm860x_backlight_name[p->flags],
-                        MFD_NAME_SIZE)) {
-                        ret = (int)p->flags;
-                        break;
-                }
-                p++;
-        }
-        return ret;
-}
-
 static int pm860x_backlight_probe(struct platform_device *pdev)
 {
         struct pm860x_chip *chip = dev_get_drvdata(pdev->dev.parent);
-        struct pm860x_platform_data *pm860x_pdata;
         struct pm860x_backlight_pdata *pdata = NULL;
         struct pm860x_backlight_data *data;
         struct backlight_device *bl;
+        struct mfd_cell *cell;
         struct resource *res;
         struct backlight_properties props;
         unsigned char value;
@@ -199,10 +181,10 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
                 return -EINVAL;
         }
 
-        if (pdev->dev.parent->platform_data) {
-                pm860x_pdata = pdev->dev.parent->platform_data;
-                pdata = pm860x_pdata->backlight;
-        }
+        cell = pdev->dev.platform_data;
+        if (cell == NULL)
+                return -ENODEV;
+        pdata = cell->mfd_data;
         if (pdata == NULL) {
                 dev_err(&pdev->dev, "platform data isn't assigned to "
                         "backlight\n");
@@ -219,7 +201,7 @@ static int pm860x_backlight_probe(struct platform_device *pdev)
         data->current_brightness = MAX_BRIGHTNESS;
         data->pwm = pdata->pwm;
         data->iset = pdata->iset;
-        data->port = __check_device(pdata, name);
+        data->port = pdata->flags;
         if (data->port < 0) {
                 dev_err(&pdev->dev, "wrong platform data is assigned");
                 kfree(data);
diff --git a/drivers/video/cg14.c b/drivers/video/cg14.c
index e2c85b0db632..f18895006627 100644
--- a/drivers/video/cg14.c
+++ b/drivers/video/cg14.c
@@ -565,6 +565,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
         cg14_unmap_regs(op, info, par);
+        framebuffer_release(info);
 
 out_err:
         return err;
diff --git a/drivers/video/cg6.c b/drivers/video/cg6.c
index 4ffad90bde42..179e96cdb323 100644
--- a/drivers/video/cg6.c
+++ b/drivers/video/cg6.c
@@ -821,6 +821,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
         cg6_unmap_regs(op, info, par);
+        framebuffer_release(info);
 
 out_err:
         return err;
diff --git a/drivers/video/console/fbcon.c b/drivers/video/console/fbcon.c
index 9c092b8d64e6..c58393402da2 100644
--- a/drivers/video/console/fbcon.c
+++ b/drivers/video/console/fbcon.c
@@ -823,10 +823,10 @@ static int set_con2fb_map(int unit, int newidx, int user)
         if (oldidx == newidx)
                 return 0;
 
-        if (!info || fbcon_has_exited)
+        if (!info)
                 return -EINVAL;
 
-        if (!err && !search_for_mapped_con()) {
+        if (!search_for_mapped_con() || !con_is_bound(&fb_con)) {
                 info_idx = newidx;
                 return fbcon_takeover(0);
         }
diff --git a/drivers/video/console/tileblit.c b/drivers/video/console/tileblit.c
index 0056a41e5c35..15e8e1a89c45 100644
--- a/drivers/video/console/tileblit.c
+++ b/drivers/video/console/tileblit.c
@@ -83,7 +83,7 @@ static void tile_cursor(struct vc_data *vc, struct fb_info *info, int mode,
                         int softback_lines, int fg, int bg)
 {
         struct fb_tilecursor cursor;
-        int use_sw = (vc->vc_cursor_type & 0x01);
+        int use_sw = (vc->vc_cursor_type & 0x10);
 
         cursor.sx = vc->vc_x;
         cursor.sy = vc->vc_y;
diff --git a/drivers/video/edid.h b/drivers/video/edid.h
index bd89fb3be8c2..d03a232d90b2 100644
--- a/drivers/video/edid.h
+++ b/drivers/video/edid.h
@@ -101,8 +101,8 @@
 #define V_SYNC_WIDTH       COMBINE_HI_4LO( V_SYNC_WIDTH_HI, V_SYNC_WIDTH_LO )
 #define V_SYNC_OFFSET      COMBINE_HI_4LO( V_SYNC_OFFSET_HI, V_SYNC_OFFSET_LO )
 
-#define H_SYNC_WIDTH       COMBINE_HI_4LO( H_SYNC_WIDTH_HI, H_SYNC_WIDTH_LO )
-#define H_SYNC_OFFSET      COMBINE_HI_4LO( H_SYNC_OFFSET_HI, H_SYNC_OFFSET_LO )
+#define H_SYNC_WIDTH       COMBINE_HI_8LO( H_SYNC_WIDTH_HI, H_SYNC_WIDTH_LO )
+#define H_SYNC_OFFSET      COMBINE_HI_8LO( H_SYNC_OFFSET_HI, H_SYNC_OFFSET_LO )
 
 #define H_SIZE_LO          (unsigned)block[ 12 ]
 #define V_SIZE_LO          (unsigned)block[ 13 ]
diff --git a/drivers/video/ffb.c b/drivers/video/ffb.c
index 910c5e6f6702..14102a3f70f5 100644
--- a/drivers/video/ffb.c
+++ b/drivers/video/ffb.c
@@ -1010,7 +1010,7 @@ out_dealloc_cmap:
         fb_dealloc_cmap(&info->cmap);
 
 out_unmap_dac:
-        of_iounmap(&op->resource[2], par->fbc, sizeof(struct ffb_fbc));
+        of_iounmap(&op->resource[1], par->dac, sizeof(struct ffb_dac));
 
 out_unmap_fbc:
         of_iounmap(&op->resource[2], par->fbc, sizeof(struct ffb_fbc));
diff --git a/drivers/video/hecubafb.c b/drivers/video/hecubafb.c
index c77bcc6ab463..1b94643ecbcf 100644
--- a/drivers/video/hecubafb.c
+++ b/drivers/video/hecubafb.c
@@ -299,7 +299,7 @@ static int __devexit hecubafb_remove(struct platform_device *dev)
 
 static struct platform_driver hecubafb_driver = {
         .probe  = hecubafb_probe,
-        .remove = hecubafb_remove,
+        .remove = __devexit_p(hecubafb_remove),
         .driver = {
                 .owner  = THIS_MODULE,
                 .name   = "hecubafb",
diff --git a/drivers/video/hpfb.c b/drivers/video/hpfb.c
index c8e280f1bb0b..ebf8495ff198 100644
--- a/drivers/video/hpfb.c
+++ b/drivers/video/hpfb.c
@@ -321,11 +321,11 @@ static int __devinit hpfb_dio_probe(struct dio_dev * d, const struct dio_device_
         unsigned long paddr, vaddr;
 
         paddr = d->resource.start;
-        if (!request_mem_region(d->resource.start, d->resource.end - d->resource.start, d->name))
+        if (!request_mem_region(d->resource.start, resource_size(&d->resource), d->name))
                 return -EBUSY;
 
         if (d->scode >= DIOII_SCBASE) {
-                vaddr = (unsigned long)ioremap(paddr, d->resource.end - d->resource.start);
+                vaddr = (unsigned long)ioremap(paddr, resource_size(&d->resource));
         } else {
                 vaddr = paddr + DIO_VIRADDRBASE;
         }
@@ -344,7 +344,7 @@ static void __devexit hpfb_remove_one(struct dio_dev *d)
         unregister_framebuffer(&fb_info);
         if (d->scode >= DIOII_SCBASE)
                 iounmap((void *)fb_regs);
-        release_mem_region(d->resource.start, d->resource.end - d->resource.start);
+        release_mem_region(d->resource.start, resource_size(&d->resource));
 }
 
 static struct dio_device_id hpfb_dio_tbl[] = {
diff --git a/drivers/video/metronomefb.c b/drivers/video/metronomefb.c
index 63ed3b72b01c..ed64edfd2c43 100644
--- a/drivers/video/metronomefb.c
+++ b/drivers/video/metronomefb.c
@@ -765,7 +765,7 @@ static int __devexit metronomefb_remove(struct platform_device *dev)
 
 static struct platform_driver metronomefb_driver = {
         .probe  = metronomefb_probe,
-        .remove = metronomefb_remove,
+        .remove = __devexit_p(metronomefb_remove),
         .driver = {
                 .owner  = THIS_MODULE,
                 .name   = "metronomefb",
diff --git a/drivers/video/omap/Kconfig b/drivers/video/omap/Kconfig
index 083c8fe53e24..15e7f1912af9 100644
--- a/drivers/video/omap/Kconfig
+++ b/drivers/video/omap/Kconfig
@@ -5,13 +5,18 @@ config FB_OMAP
         select FB_CFB_FILLRECT
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
+        select TWL4030_CORE if MACH_OMAP_2430SDP
         help
           Frame buffer driver for OMAP based boards.
 
 config FB_OMAP_LCD_VGA
         bool "Use LCD in VGA mode"
                 depends on MACH_OMAP_3430SDP || MACH_OMAP_LDP
-
+                help
+                  Set LCD resolution as VGA (640 X 480).
+                  Default resolution without this option is QVGA(320 X 240).
+                  Please take a look at drivers/video/omap/lcd_ldp.c file
+                  for lcd driver code.
 choice
         depends on FB_OMAP && MACH_OVERO
         prompt "Screen resolution"
diff --git a/drivers/video/omap/blizzard.c b/drivers/video/omap/blizzard.c
index 87785c215a52..c0504a8a5079 100644
--- a/drivers/video/omap/blizzard.c
+++ b/drivers/video/omap/blizzard.c
@@ -397,8 +397,7 @@ static inline void free_req(struct blizzard_request *req)
 
         spin_lock_irqsave(&blizzard.req_lock, flags);
 
-        list_del(&req->entry);
-        list_add(&req->entry, &blizzard.free_req_list);
+        list_move(&req->entry, &blizzard.free_req_list);
         if (!(req->flags & REQ_FROM_IRQ_POOL))
                 up(&blizzard.req_sema);
 
diff --git a/drivers/video/omap/hwa742.c b/drivers/video/omap/hwa742.c
index 0016f77cd13f..084aa0ac562b 100644
--- a/drivers/video/omap/hwa742.c
+++ b/drivers/video/omap/hwa742.c
@@ -269,8 +269,7 @@ static inline void free_req(struct hwa742_request *req)
 
         spin_lock_irqsave(&hwa742.req_lock, flags);
 
-        list_del(&req->entry);
-        list_add(&req->entry, &hwa742.free_req_list);
+        list_move(&req->entry, &hwa742.free_req_list);
         if (!(req->flags & REQ_FROM_IRQ_POOL))
                 up(&hwa742.req_sema);
 
diff --git a/drivers/video/omap2/displays/Kconfig b/drivers/video/omap2/displays/Kconfig
index 940cab394c2e..d18ad6b2372a 100644
--- a/drivers/video/omap2/displays/Kconfig
+++ b/drivers/video/omap2/displays/Kconfig
@@ -9,6 +9,12 @@ config PANEL_GENERIC_DPI
           Supports LCD Panel used in TI SDP3430 and EVM boards,
           OMAP3517 EVM boards and CM-T35.
 
+config PANEL_LGPHILIPS_LB035Q02
+        tristate "LG.Philips LB035Q02 LCD Panel"
+        depends on OMAP2_DSS && SPI
+        help
+          LCD Panel used on the Gumstix Overo Palo35
+
 config PANEL_SHARP_LS037V7DW01
         tristate "Sharp LS037V7DW01 LCD Panel"
         depends on OMAP2_DSS
diff --git a/drivers/video/omap2/displays/Makefile b/drivers/video/omap2/displays/Makefile
index 861f0255ec6b..0f601ab3abf4 100644
--- a/drivers/video/omap2/displays/Makefile
+++ b/drivers/video/omap2/displays/Makefile
@@ -1,4 +1,5 @@
 obj-$(CONFIG_PANEL_GENERIC_DPI) += panel-generic-dpi.o
+obj-$(CONFIG_PANEL_LGPHILIPS_LB035Q02) += panel-lgphilips-lb035q02.o
 obj-$(CONFIG_PANEL_SHARP_LS037V7DW01) += panel-sharp-ls037v7dw01.o
 obj-$(CONFIG_PANEL_NEC_NL8048HL11_01B) += panel-nec-nl8048hl11-01b.o
 
diff --git a/drivers/video/omap2/displays/panel-generic-dpi.c b/drivers/video/omap2/displays/panel-generic-dpi.c
index 07eb30ee59c8..4a9b9ff59467 100644
--- a/drivers/video/omap2/displays/panel-generic-dpi.c
+++ b/drivers/video/omap2/displays/panel-generic-dpi.c
@@ -156,6 +156,31 @@ static struct panel_config generic_dpi_panels[] = {
                 .power_off_delay        = 0,
                 .name                   = "toppoly_tdo35s",
         },
+
+        /* Samsung LTE430WQ-F0C */
+        {
+                {
+                        .x_res          = 480,
+                        .y_res          = 272,
+
+                        .pixel_clock    = 9200,
+
+                        .hfp            = 8,
+                        .hsw            = 41,
+                        .hbp            = 45 - 41,
+
+                        .vfp            = 4,
+                        .vsw            = 10,
+                        .vbp            = 12 - 10,
+                },
+                .acbi                   = 0x0,
+                .acb                    = 0x0,
+                .config                 = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                                                OMAP_DSS_LCD_IHS,
+                .power_on_delay         = 0,
+                .power_off_delay        = 0,
+                .name                   = "samsung_lte430wq_f0c",
+        },
 };
 
 struct panel_drv_data {
diff --git a/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
new file mode 100644
index 000000000000..271324db2436
--- /dev/null
+++ b/drivers/video/omap2/displays/panel-lgphilips-lb035q02.c
@@ -0,0 +1,279 @@
+/*
+ * LCD panel driver for LG.Philips LB035Q02
+ *
+ * Author: Steve Sakoman <steve@sakoman.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.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/module.h>
+#include <linux/delay.h>
+#include <linux/spi/spi.h>
+#include <linux/mutex.h>
+
+#include <plat/display.h>
+
+struct lb035q02_data {
+        struct mutex lock;
+};
+
+static struct omap_video_timings lb035q02_timings = {
+        .x_res = 320,
+        .y_res = 240,
+
+        .pixel_clock    = 6500,
+
+        .hsw            = 2,
+        .hfp            = 20,
+        .hbp            = 68,
+
+        .vsw            = 2,
+        .vfp            = 4,
+        .vbp            = 18,
+};
+
+static int lb035q02_panel_power_on(struct omap_dss_device *dssdev)
+{
+        int r;
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                return 0;
+
+        r = omapdss_dpi_display_enable(dssdev);
+        if (r)
+                goto err0;
+
+        if (dssdev->platform_enable) {
+                r = dssdev->platform_enable(dssdev);
+                if (r)
+                        goto err1;
+        }
+
+        return 0;
+err1:
+        omapdss_dpi_display_disable(dssdev);
+err0:
+        return r;
+}
+
+static void lb035q02_panel_power_off(struct omap_dss_device *dssdev)
+{
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
+                return;
+
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
+
+        omapdss_dpi_display_disable(dssdev);
+}
+
+static int lb035q02_panel_probe(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld;
+        int r;
+
+        dssdev->panel.config = OMAP_DSS_LCD_TFT | OMAP_DSS_LCD_IVS |
+                OMAP_DSS_LCD_IHS;
+        dssdev->panel.timings = lb035q02_timings;
+
+        ld = kzalloc(sizeof(*ld), GFP_KERNEL);
+        if (!ld) {
+                r = -ENOMEM;
+                goto err;
+        }
+        mutex_init(&ld->lock);
+        dev_set_drvdata(&dssdev->dev, ld);
+        return 0;
+err:
+        return r;
+}
+
+static void lb035q02_panel_remove(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+        kfree(ld);
+}
+
+static int lb035q02_panel_enable(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+        int r;
+
+        mutex_lock(&ld->lock);
+
+        r = lb035q02_panel_power_on(dssdev);
+        if (r)
+                goto err;
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        mutex_unlock(&ld->lock);
+        return 0;
+err:
+        mutex_unlock(&ld->lock);
+        return r;
+}
+
+static void lb035q02_panel_disable(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+        mutex_lock(&ld->lock);
+
+        lb035q02_panel_power_off(dssdev);
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+        mutex_unlock(&ld->lock);
+}
+
+static int lb035q02_panel_suspend(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+
+        mutex_lock(&ld->lock);
+
+        lb035q02_panel_power_off(dssdev);
+        dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
+
+        mutex_unlock(&ld->lock);
+        return 0;
+}
+
+static int lb035q02_panel_resume(struct omap_dss_device *dssdev)
+{
+        struct lb035q02_data *ld = dev_get_drvdata(&dssdev->dev);
+        int r;
+
+        mutex_lock(&ld->lock);
+
+        r = lb035q02_panel_power_on(dssdev);
+        if (r)
+                goto err;
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+        mutex_unlock(&ld->lock);
+        return 0;
+err:
+        mutex_unlock(&ld->lock);
+        return r;
+}
+
+static struct omap_dss_driver lb035q02_driver = {
+        .probe          = lb035q02_panel_probe,
+        .remove         = lb035q02_panel_remove,
+
+        .enable         = lb035q02_panel_enable,
+        .disable        = lb035q02_panel_disable,
+        .suspend        = lb035q02_panel_suspend,
+        .resume         = lb035q02_panel_resume,
+
+        .driver         = {
+                .name   = "lgphilips_lb035q02_panel",
+                .owner  = THIS_MODULE,
+        },
+};
+
+static int lb035q02_write_reg(struct spi_device *spi, u8 reg, u16 val)
+{
+        struct spi_message msg;
+        struct spi_transfer index_xfer = {
+                .len            = 3,
+                .cs_change      = 1,
+        };
+        struct spi_transfer value_xfer = {
+                .len            = 3,
+        };
+        u8      buffer[16];
+
+        spi_message_init(&msg);
+
+        /* register index */
+        buffer[0] = 0x70;
+        buffer[1] = 0x00;
+        buffer[2] = reg & 0x7f;
+        index_xfer.tx_buf = buffer;
+        spi_message_add_tail(&index_xfer, &msg);
+
+        /* register value */
+        buffer[4] = 0x72;
+        buffer[5] = val >> 8;
+        buffer[6] = val;
+        value_xfer.tx_buf = buffer + 4;
+        spi_message_add_tail(&value_xfer, &msg);
+
+        return spi_sync(spi, &msg);
+}
+
+static void init_lb035q02_panel(struct spi_device *spi)
+{
+        /* Init sequence from page 28 of the lb035q02 spec */
+        lb035q02_write_reg(spi, 0x01, 0x6300);
+        lb035q02_write_reg(spi, 0x02, 0x0200);
+        lb035q02_write_reg(spi, 0x03, 0x0177);
+        lb035q02_write_reg(spi, 0x04, 0x04c7);
+        lb035q02_write_reg(spi, 0x05, 0xffc0);
+        lb035q02_write_reg(spi, 0x06, 0xe806);
+        lb035q02_write_reg(spi, 0x0a, 0x4008);
+        lb035q02_write_reg(spi, 0x0b, 0x0000);
+        lb035q02_write_reg(spi, 0x0d, 0x0030);
+        lb035q02_write_reg(spi, 0x0e, 0x2800);
+        lb035q02_write_reg(spi, 0x0f, 0x0000);
+        lb035q02_write_reg(spi, 0x16, 0x9f80);
+        lb035q02_write_reg(spi, 0x17, 0x0a0f);
+        lb035q02_write_reg(spi, 0x1e, 0x00c1);
+        lb035q02_write_reg(spi, 0x30, 0x0300);
+        lb035q02_write_reg(spi, 0x31, 0x0007);
+        lb035q02_write_reg(spi, 0x32, 0x0000);
+        lb035q02_write_reg(spi, 0x33, 0x0000);
+        lb035q02_write_reg(spi, 0x34, 0x0707);
+        lb035q02_write_reg(spi, 0x35, 0x0004);
+        lb035q02_write_reg(spi, 0x36, 0x0302);
+        lb035q02_write_reg(spi, 0x37, 0x0202);
+        lb035q02_write_reg(spi, 0x3a, 0x0a0d);
+        lb035q02_write_reg(spi, 0x3b, 0x0806);
+}
+
+static int __devinit lb035q02_panel_spi_probe(struct spi_device *spi)
+{
+        init_lb035q02_panel(spi);
+        return omap_dss_register_driver(&lb035q02_driver);
+}
+
+static int __devexit lb035q02_panel_spi_remove(struct spi_device *spi)
+{
+        omap_dss_unregister_driver(&lb035q02_driver);
+        return 0;
+}
+
+static struct spi_driver lb035q02_spi_driver = {
+        .driver         = {
+                .name   = "lgphilips_lb035q02_panel-spi",
+                .owner  = THIS_MODULE,
+        },
+        .probe          = lb035q02_panel_spi_probe,
+        .remove         = __devexit_p(lb035q02_panel_spi_remove),
+};
+
+static int __init lb035q02_panel_drv_init(void)
+{
+        return spi_register_driver(&lb035q02_spi_driver);
+}
+
+static void __exit lb035q02_panel_drv_exit(void)
+{
+        spi_unregister_driver(&lb035q02_spi_driver);
+}
+
+module_init(lb035q02_panel_drv_init);
+module_exit(lb035q02_panel_drv_exit);
+MODULE_LICENSE("GPL");
diff --git a/drivers/video/omap2/displays/panel-taal.c b/drivers/video/omap2/displays/panel-taal.c
index c74e8b778ba1..adc9900458e1 100644
--- a/drivers/video/omap2/displays/panel-taal.c
+++ b/drivers/video/omap2/displays/panel-taal.c
@@ -218,6 +218,8 @@ struct taal_data {
                 u16 w;
                 u16 h;
         } update_region;
+        int channel;
+
         struct delayed_work te_timeout_work;
 
         bool use_dsi_bl;
@@ -257,12 +259,12 @@ static void hw_guard_wait(struct taal_data *td)
         }
 }
 
-static int taal_dcs_read_1(u8 dcs_cmd, u8 *data)
+static int taal_dcs_read_1(struct taal_data *td, u8 dcs_cmd, u8 *data)
 {
         int r;
         u8 buf[1];
 
-        r = dsi_vc_dcs_read(TCH, dcs_cmd, buf, 1);
+        r = dsi_vc_dcs_read(td->channel, dcs_cmd, buf, 1);
 
         if (r < 0)
                 return r;
@@ -272,17 +274,17 @@ static int taal_dcs_read_1(u8 dcs_cmd, u8 *data)
         return 0;
 }
 
-static int taal_dcs_write_0(u8 dcs_cmd)
+static int taal_dcs_write_0(struct taal_data *td, u8 dcs_cmd)
 {
-        return dsi_vc_dcs_write(TCH, &dcs_cmd, 1);
+        return dsi_vc_dcs_write(td->channel, &dcs_cmd, 1);
 }
 
-static int taal_dcs_write_1(u8 dcs_cmd, u8 param)
+static int taal_dcs_write_1(struct taal_data *td, u8 dcs_cmd, u8 param)
 {
         u8 buf[2];
         buf[0] = dcs_cmd;
         buf[1] = param;
-        return dsi_vc_dcs_write(TCH, buf, 2);
+        return dsi_vc_dcs_write(td->channel, buf, 2);
 }
 
 static int taal_sleep_in(struct taal_data *td)
@@ -294,7 +296,7 @@ static int taal_sleep_in(struct taal_data *td)
         hw_guard_wait(td);
 
         cmd = DCS_SLEEP_IN;
-        r = dsi_vc_dcs_write_nosync(TCH, &cmd, 1);
+        r = dsi_vc_dcs_write_nosync(td->channel, &cmd, 1);
         if (r)
                 return r;
 
@@ -312,7 +314,7 @@ static int taal_sleep_out(struct taal_data *td)
 
         hw_guard_wait(td);
 
-        r = taal_dcs_write_0(DCS_SLEEP_OUT);
+        r = taal_dcs_write_0(td, DCS_SLEEP_OUT);
         if (r)
                 return r;
 
@@ -324,30 +326,30 @@ static int taal_sleep_out(struct taal_data *td)
         return 0;
 }
 
-static int taal_get_id(u8 *id1, u8 *id2, u8 *id3)
+static int taal_get_id(struct taal_data *td, u8 *id1, u8 *id2, u8 *id3)
 {
         int r;
 
-        r = taal_dcs_read_1(DCS_GET_ID1, id1);
+        r = taal_dcs_read_1(td, DCS_GET_ID1, id1);
         if (r)
                 return r;
-        r = taal_dcs_read_1(DCS_GET_ID2, id2);
+        r = taal_dcs_read_1(td, DCS_GET_ID2, id2);
         if (r)
                 return r;
-        r = taal_dcs_read_1(DCS_GET_ID3, id3);
+        r = taal_dcs_read_1(td, DCS_GET_ID3, id3);
         if (r)
                 return r;
 
         return 0;
 }
 
-static int taal_set_addr_mode(u8 rotate, bool mirror)
+static int taal_set_addr_mode(struct taal_data *td, u8 rotate, bool mirror)
 {
         int r;
         u8 mode;
         int b5, b6, b7;
 
-        r = taal_dcs_read_1(DCS_READ_MADCTL, &mode);
+        r = taal_dcs_read_1(td, DCS_READ_MADCTL, &mode);
         if (r)
                 return r;
 
@@ -381,10 +383,11 @@ static int taal_set_addr_mode(u8 rotate, bool mirror)
         mode &= ~((1<<7) | (1<<6) | (1<<5));
         mode |= (b7 << 7) | (b6 << 6) | (b5 << 5);
 
-        return taal_dcs_write_1(DCS_MEM_ACC_CTRL, mode);
+        return taal_dcs_write_1(td, DCS_MEM_ACC_CTRL, mode);
 }
 
-static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
+static int taal_set_update_window(struct taal_data *td,
+                u16 x, u16 y, u16 w, u16 h)
 {
         int r;
         u16 x1 = x;
@@ -399,7 +402,7 @@ static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
         buf[3] = (x2 >> 8) & 0xff;
         buf[4] = (x2 >> 0) & 0xff;
 
-        r = dsi_vc_dcs_write_nosync(TCH, buf, sizeof(buf));
+        r = dsi_vc_dcs_write_nosync(td->channel, buf, sizeof(buf));
         if (r)
                 return r;
 
@@ -409,11 +412,11 @@ static int taal_set_update_window(u16 x, u16 y, u16 w, u16 h)
         buf[3] = (y2 >> 8) & 0xff;
         buf[4] = (y2 >> 0) & 0xff;
 
-        r = dsi_vc_dcs_write_nosync(TCH, buf, sizeof(buf));
+        r = dsi_vc_dcs_write_nosync(td->channel, buf, sizeof(buf));
         if (r)
                 return r;
 
-        dsi_vc_send_bta_sync(TCH);
+        dsi_vc_send_bta_sync(td->channel);
 
         return r;
 }
@@ -439,7 +442,7 @@ static int taal_bl_update_status(struct backlight_device *dev)
         if (td->use_dsi_bl) {
                 if (td->enabled) {
                         dsi_bus_lock();
-                        r = taal_dcs_write_1(DCS_BRIGHTNESS, level);
+                        r = taal_dcs_write_1(td, DCS_BRIGHTNESS, level);
                         dsi_bus_unlock();
                 } else {
                         r = 0;
@@ -502,7 +505,7 @@ static ssize_t taal_num_errors_show(struct device *dev,
 
         if (td->enabled) {
                 dsi_bus_lock();
-                r = taal_dcs_read_1(DCS_READ_NUM_ERRORS, &errors);
+                r = taal_dcs_read_1(td, DCS_READ_NUM_ERRORS, &errors);
                 dsi_bus_unlock();
         } else {
                 r = -ENODEV;
@@ -528,7 +531,7 @@ static ssize_t taal_hw_revision_show(struct device *dev,
 
         if (td->enabled) {
                 dsi_bus_lock();
-                r = taal_get_id(&id1, &id2, &id3);
+                r = taal_get_id(td, &id1, &id2, &id3);
                 dsi_bus_unlock();
         } else {
                 r = -ENODEV;
@@ -590,7 +593,7 @@ static ssize_t store_cabc_mode(struct device *dev,
         if (td->enabled) {
                 dsi_bus_lock();
                 if (!td->cabc_broken)
-                        taal_dcs_write_1(DCS_WRITE_CABC, i);
+                        taal_dcs_write_1(td, DCS_WRITE_CABC, i);
                 dsi_bus_unlock();
         }
 
@@ -776,14 +779,29 @@ static int taal_probe(struct omap_dss_device *dssdev)
                 dev_dbg(&dssdev->dev, "Using GPIO TE\n");
         }
 
+        r = omap_dsi_request_vc(dssdev, &td->channel);
+        if (r) {
+                dev_err(&dssdev->dev, "failed to get virtual channel\n");
+                goto err_req_vc;
+        }
+
+        r = omap_dsi_set_vc_id(dssdev, td->channel, TCH);
+        if (r) {
+                dev_err(&dssdev->dev, "failed to set VC_ID\n");
+                goto err_vc_id;
+        }
+
         r = sysfs_create_group(&dssdev->dev.kobj, &taal_attr_group);
         if (r) {
                 dev_err(&dssdev->dev, "failed to create sysfs files\n");
-                goto err_sysfs;
+                goto err_vc_id;
         }
 
         return 0;
-err_sysfs:
+
+err_vc_id:
+        omap_dsi_release_vc(dssdev, td->channel);
+err_req_vc:
         if (panel_data->use_ext_te)
                 free_irq(gpio_to_irq(panel_data->ext_te_gpio), dssdev);
 err_irq:
@@ -810,6 +828,7 @@ static void taal_remove(struct omap_dss_device *dssdev)
         dev_dbg(&dssdev->dev, "remove\n");
 
         sysfs_remove_group(&dssdev->dev.kobj, &taal_attr_group);
+        omap_dsi_release_vc(dssdev, td->channel);
 
         if (panel_data->use_ext_te) {
                 int gpio = panel_data->ext_te_gpio;
@@ -848,13 +867,13 @@ static int taal_power_on(struct omap_dss_device *dssdev)
 
         taal_hw_reset(dssdev);
 
-        omapdss_dsi_vc_enable_hs(TCH, false);
+        omapdss_dsi_vc_enable_hs(td->channel, false);
 
         r = taal_sleep_out(td);
         if (r)
                 goto err;
 
-        r = taal_get_id(&id1, &id2, &id3);
+        r = taal_get_id(td, &id1, &id2, &id3);
         if (r)
                 goto err;
 
@@ -863,30 +882,30 @@ static int taal_power_on(struct omap_dss_device *dssdev)
                 (id2 == 0x00 || id2 == 0xff || id2 == 0x81))
                 td->cabc_broken = true;
 
-        r = taal_dcs_write_1(DCS_BRIGHTNESS, 0xff);
+        r = taal_dcs_write_1(td, DCS_BRIGHTNESS, 0xff);
         if (r)
                 goto err;
 
-        r = taal_dcs_write_1(DCS_CTRL_DISPLAY,
+        r = taal_dcs_write_1(td, DCS_CTRL_DISPLAY,
                         (1<<2) | (1<<5));       /* BL | BCTRL */
         if (r)
                 goto err;
 
-        r = taal_dcs_write_1(DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
+        r = taal_dcs_write_1(td, DCS_PIXEL_FORMAT, 0x7); /* 24bit/pixel */
         if (r)
                 goto err;
 
-        r = taal_set_addr_mode(td->rotate, td->mirror);
+        r = taal_set_addr_mode(td, td->rotate, td->mirror);
         if (r)
                 goto err;
 
         if (!td->cabc_broken) {
-                r = taal_dcs_write_1(DCS_WRITE_CABC, td->cabc_mode);
+                r = taal_dcs_write_1(td, DCS_WRITE_CABC, td->cabc_mode);
                 if (r)
                         goto err;
         }
 
-        r = taal_dcs_write_0(DCS_DISPLAY_ON);
+        r = taal_dcs_write_0(td, DCS_DISPLAY_ON);
         if (r)
                 goto err;
 
@@ -905,7 +924,7 @@ static int taal_power_on(struct omap_dss_device *dssdev)
                 td->intro_printed = true;
         }
 
-        omapdss_dsi_vc_enable_hs(TCH, true);
+        omapdss_dsi_vc_enable_hs(td->channel, true);
 
         return 0;
 err:
@@ -923,7 +942,7 @@ static void taal_power_off(struct omap_dss_device *dssdev)
         struct taal_data *td = dev_get_drvdata(&dssdev->dev);
         int r;
 
-        r = taal_dcs_write_0(DCS_DISPLAY_OFF);
+        r = taal_dcs_write_0(td, DCS_DISPLAY_OFF);
         if (!r) {
                 r = taal_sleep_in(td);
                 /* HACK: wait a bit so that the message goes through */
@@ -1091,7 +1110,7 @@ static irqreturn_t taal_te_isr(int irq, void *data)
         if (old) {
                 cancel_delayed_work(&td->te_timeout_work);
 
-                r = omap_dsi_update(dssdev, TCH,
+                r = omap_dsi_update(dssdev, td->channel,
                                 td->update_region.x,
                                 td->update_region.y,
                                 td->update_region.w,
@@ -1141,7 +1160,7 @@ static int taal_update(struct omap_dss_device *dssdev,
         if (r)
                 goto err;
 
-        r = taal_set_update_window(x, y, w, h);
+        r = taal_set_update_window(td, x, y, w, h);
         if (r)
                 goto err;
 
@@ -1155,7 +1174,7 @@ static int taal_update(struct omap_dss_device *dssdev,
                                 msecs_to_jiffies(250));
                 atomic_set(&td->do_update, 1);
         } else {
-                r = omap_dsi_update(dssdev, TCH, x, y, w, h,
+                r = omap_dsi_update(dssdev, td->channel, x, y, w, h,
                                 taal_framedone_cb, dssdev);
                 if (r)
                         goto err;
@@ -1193,9 +1212,9 @@ static int _taal_enable_te(struct omap_dss_device *dssdev, bool enable)
         int r;
 
         if (enable)
-                r = taal_dcs_write_1(DCS_TEAR_ON, 0);
+                r = taal_dcs_write_1(td, DCS_TEAR_ON, 0);
         else
-                r = taal_dcs_write_0(DCS_TEAR_OFF);
+                r = taal_dcs_write_0(td, DCS_TEAR_OFF);
 
         if (!panel_data->use_ext_te)
                 omapdss_dsi_enable_te(dssdev, enable);
@@ -1265,7 +1284,7 @@ static int taal_rotate(struct omap_dss_device *dssdev, u8 rotate)
         dsi_bus_lock();
 
         if (td->enabled) {
-                r = taal_set_addr_mode(rotate, td->mirror);
+                r = taal_set_addr_mode(td, rotate, td->mirror);
                 if (r)
                         goto err;
         }
@@ -1308,7 +1327,7 @@ static int taal_mirror(struct omap_dss_device *dssdev, bool enable)
 
         dsi_bus_lock();
         if (td->enabled) {
-                r = taal_set_addr_mode(td->rotate, enable);
+                r = taal_set_addr_mode(td, td->rotate, enable);
                 if (r)
                         goto err;
         }
@@ -1352,13 +1371,13 @@ static int taal_run_test(struct omap_dss_device *dssdev, int test_num)
 
         dsi_bus_lock();
 
-        r = taal_dcs_read_1(DCS_GET_ID1, &id1);
+        r = taal_dcs_read_1(td, DCS_GET_ID1, &id1);
         if (r)
                 goto err2;
-        r = taal_dcs_read_1(DCS_GET_ID2, &id2);
+        r = taal_dcs_read_1(td, DCS_GET_ID2, &id2);
         if (r)
                 goto err2;
-        r = taal_dcs_read_1(DCS_GET_ID3, &id3);
+        r = taal_dcs_read_1(td, DCS_GET_ID3, &id3);
         if (r)
                 goto err2;
 
@@ -1406,9 +1425,9 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
         else
                 plen = 2;
 
-        taal_set_update_window(x, y, w, h);
+        taal_set_update_window(td, x, y, w, h);
 
-        r = dsi_vc_set_max_rx_packet_size(TCH, plen);
+        r = dsi_vc_set_max_rx_packet_size(td->channel, plen);
         if (r)
                 goto err2;
 
@@ -1416,7 +1435,7 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
                 u8 dcs_cmd = first ? 0x2e : 0x3e;
                 first = 0;
 
-                r = dsi_vc_dcs_read(TCH, dcs_cmd,
+                r = dsi_vc_dcs_read(td->channel, dcs_cmd,
                                 buf + buf_used, size - buf_used);
 
                 if (r < 0) {
@@ -1442,7 +1461,7 @@ static int taal_memory_read(struct omap_dss_device *dssdev,
         r = buf_used;
 
 err3:
-        dsi_vc_set_max_rx_packet_size(TCH, 1);
+        dsi_vc_set_max_rx_packet_size(td->channel, 1);
 err2:
         dsi_bus_unlock();
 err1:
@@ -1468,7 +1487,7 @@ static void taal_esd_work(struct work_struct *work)
 
         dsi_bus_lock();
 
-        r = taal_dcs_read_1(DCS_RDDSDR, &state1);
+        r = taal_dcs_read_1(td, DCS_RDDSDR, &state1);
         if (r) {
                 dev_err(&dssdev->dev, "failed to read Taal status\n");
                 goto err;
@@ -1481,7 +1500,7 @@ static void taal_esd_work(struct work_struct *work)
                 goto err;
         }
 
-        r = taal_dcs_read_1(DCS_RDDSDR, &state2);
+        r = taal_dcs_read_1(td, DCS_RDDSDR, &state2);
         if (r) {
                 dev_err(&dssdev->dev, "failed to read Taal status\n");
                 goto err;
@@ -1497,7 +1516,7 @@ static void taal_esd_work(struct work_struct *work)
         /* Self-diagnostics result is also shown on TE GPIO line. We need
          * to re-enable TE after self diagnostics */
         if (td->te_enabled && panel_data->use_ext_te) {
-                r = taal_dcs_write_1(DCS_TEAR_ON, 0);
+                r = taal_dcs_write_1(td, DCS_TEAR_ON, 0);
                 if (r)
                         goto err;
         }
diff --git a/drivers/video/omap2/dss/Kconfig b/drivers/video/omap2/dss/Kconfig
index 43b64403eaa4..bfc5da0e9700 100644
--- a/drivers/video/omap2/dss/Kconfig
+++ b/drivers/video/omap2/dss/Kconfig
@@ -1,8 +1,8 @@
 menuconfig OMAP2_DSS
-        tristate "OMAP2/3 Display Subsystem support (EXPERIMENTAL)"
-        depends on ARCH_OMAP2 || ARCH_OMAP3
+        tristate "OMAP2+ Display Subsystem support (EXPERIMENTAL)"
+        depends on ARCH_OMAP2PLUS
         help
-          OMAP2/3 Display Subsystem support.
+          OMAP2+ Display Subsystem support.
 
 if OMAP2_DSS
 
@@ -60,6 +60,14 @@ config OMAP2_DSS_VENC
         help
           OMAP Video Encoder support for S-Video and composite TV-out.
 
+config OMAP4_DSS_HDMI
+        bool "HDMI support"
+        depends on ARCH_OMAP4
+        default y
+        help
+          HDMI Interface. This adds the High Definition Multimedia Interface.
+          See http://www.hdmi.org/ for HDMI specification.
+
 config OMAP2_DSS_SDI
         bool "SDI support"
         depends on ARCH_OMAP3
diff --git a/drivers/video/omap2/dss/Makefile b/drivers/video/omap2/dss/Makefile
index 7db17b5e570c..10d9d3bb3e24 100644
--- a/drivers/video/omap2/dss/Makefile
+++ b/drivers/video/omap2/dss/Makefile
@@ -5,3 +5,5 @@ omapdss-$(CONFIG_OMAP2_DSS_RFBI) += rfbi.o
 omapdss-$(CONFIG_OMAP2_DSS_VENC) += venc.o
 omapdss-$(CONFIG_OMAP2_DSS_SDI) += sdi.o
 omapdss-$(CONFIG_OMAP2_DSS_DSI) += dsi.o
+omapdss-$(CONFIG_OMAP4_DSS_HDMI) += hdmi.o \
+                                    hdmi_omap4_panel.o
diff --git a/drivers/video/omap2/dss/core.c b/drivers/video/omap2/dss/core.c
index 8e89f6049280..1aa2ed1e786e 100644
--- a/drivers/video/omap2/dss/core.c
+++ b/drivers/video/omap2/dss/core.c
@@ -34,332 +34,26 @@
 #include <linux/regulator/consumer.h>
 
 #include <plat/display.h>
-#include <plat/clock.h>
 
 #include "dss.h"
 #include "dss_features.h"
 
 static struct {
         struct platform_device *pdev;
-        int             ctx_id;
-
-        struct clk      *dss_ick;
-        struct clk      *dss1_fck;
-        struct clk      *dss2_fck;
-        struct clk      *dss_54m_fck;
-        struct clk      *dss_96m_fck;
-        unsigned        num_clks_enabled;
 
         struct regulator *vdds_dsi_reg;
         struct regulator *vdds_sdi_reg;
-        struct regulator *vdda_dac_reg;
 } core;
 
-static void dss_clk_enable_all_no_ctx(void);
-static void dss_clk_disable_all_no_ctx(void);
-static void dss_clk_enable_no_ctx(enum dss_clock clks);
-static void dss_clk_disable_no_ctx(enum dss_clock clks);
-
 static char *def_disp_name;
 module_param_named(def_disp, def_disp_name, charp, 0);
-MODULE_PARM_DESC(def_disp_name, "default display name");
+MODULE_PARM_DESC(def_disp, "default display name");
 
 #ifdef DEBUG
 unsigned int dss_debug;
 module_param_named(debug, dss_debug, bool, 0644);
 #endif
 
-/* CONTEXT */
-static int dss_get_ctx_id(void)
-{
-        struct omap_dss_board_info *pdata = core.pdev->dev.platform_data;
-        int r;
-
-        if (!pdata->get_last_off_on_transaction_id)
-                return 0;
-        r = pdata->get_last_off_on_transaction_id(&core.pdev->dev);
-        if (r < 0) {
-                dev_err(&core.pdev->dev, "getting transaction ID failed, "
-                                "will force context restore\n");
-                r = -1;
-        }
-        return r;
-}
-
-int dss_need_ctx_restore(void)
-{
-        int id = dss_get_ctx_id();
-
-        if (id < 0 || id != core.ctx_id) {
-                DSSDBG("ctx id %d -> id %d\n",
-                                core.ctx_id, id);
-                core.ctx_id = id;
-                return 1;
-        } else {
-                return 0;
-        }
-}
-
-static void save_all_ctx(void)
-{
-        DSSDBG("save context\n");
-
-        dss_clk_enable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK1);
-
-        dss_save_context();
-        dispc_save_context();
-#ifdef CONFIG_OMAP2_DSS_DSI
-        dsi_save_context();
-#endif
-
-        dss_clk_disable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK1);
-}
-
-static void restore_all_ctx(void)
-{
-        DSSDBG("restore context\n");
-
-        dss_clk_enable_all_no_ctx();
-
-        dss_restore_context();
-        dispc_restore_context();
-#ifdef CONFIG_OMAP2_DSS_DSI
-        dsi_restore_context();
-#endif
-
-        dss_clk_disable_all_no_ctx();
-}
-
-#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
-/* CLOCKS */
-static void core_dump_clocks(struct seq_file *s)
-{
-        int i;
-        struct clk *clocks[5] = {
-                core.dss_ick,
-                core.dss1_fck,
-                core.dss2_fck,
-                core.dss_54m_fck,
-                core.dss_96m_fck
-        };
-
-        seq_printf(s, "- CORE -\n");
-
-        seq_printf(s, "internal clk count\t\t%u\n", core.num_clks_enabled);
-
-        for (i = 0; i < 5; i++) {
-                if (!clocks[i])
-                        continue;
-                seq_printf(s, "%-15s\t%lu\t%d\n",
-                                clocks[i]->name,
-                                clk_get_rate(clocks[i]),
-                                clocks[i]->usecount);
-        }
-}
-#endif /* defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT) */
-
-static int dss_get_clock(struct clk **clock, const char *clk_name)
-{
-        struct clk *clk;
-
-        clk = clk_get(&core.pdev->dev, clk_name);
-
-        if (IS_ERR(clk)) {
-                DSSERR("can't get clock %s", clk_name);
-                return PTR_ERR(clk);
-        }
-
-        *clock = clk;
-
-        DSSDBG("clk %s, rate %ld\n", clk_name, clk_get_rate(clk));
-
-        return 0;
-}
-
-static int dss_get_clocks(void)
-{
-        int r;
-
-        core.dss_ick = NULL;
-        core.dss1_fck = NULL;
-        core.dss2_fck = NULL;
-        core.dss_54m_fck = NULL;
-        core.dss_96m_fck = NULL;
-
-        r = dss_get_clock(&core.dss_ick, "ick");
-        if (r)
-                goto err;
-
-        r = dss_get_clock(&core.dss1_fck, "dss1_fck");
-        if (r)
-                goto err;
-
-        r = dss_get_clock(&core.dss2_fck, "dss2_fck");
-        if (r)
-                goto err;
-
-        r = dss_get_clock(&core.dss_54m_fck, "tv_fck");
-        if (r)
-                goto err;
-
-        r = dss_get_clock(&core.dss_96m_fck, "video_fck");
-        if (r)
-                goto err;
-
-        return 0;
-
-err:
-        if (core.dss_ick)
-                clk_put(core.dss_ick);
-        if (core.dss1_fck)
-                clk_put(core.dss1_fck);
-        if (core.dss2_fck)
-                clk_put(core.dss2_fck);
-        if (core.dss_54m_fck)
-                clk_put(core.dss_54m_fck);
-        if (core.dss_96m_fck)
-                clk_put(core.dss_96m_fck);
-
-        return r;
-}
-
-static void dss_put_clocks(void)
-{
-        if (core.dss_96m_fck)
-                clk_put(core.dss_96m_fck);
-        clk_put(core.dss_54m_fck);
-        clk_put(core.dss1_fck);
-        clk_put(core.dss2_fck);
-        clk_put(core.dss_ick);
-}
-
-unsigned long dss_clk_get_rate(enum dss_clock clk)
-{
-        switch (clk) {
-        case DSS_CLK_ICK:
-                return clk_get_rate(core.dss_ick);
-        case DSS_CLK_FCK1:
-                return clk_get_rate(core.dss1_fck);
-        case DSS_CLK_FCK2:
-                return clk_get_rate(core.dss2_fck);
-        case DSS_CLK_54M:
-                return clk_get_rate(core.dss_54m_fck);
-        case DSS_CLK_96M:
-                return clk_get_rate(core.dss_96m_fck);
-        }
-
-        BUG();
-        return 0;
-}
-
-static unsigned count_clk_bits(enum dss_clock clks)
-{
-        unsigned num_clks = 0;
-
-        if (clks & DSS_CLK_ICK)
-                ++num_clks;
-        if (clks & DSS_CLK_FCK1)
-                ++num_clks;
-        if (clks & DSS_CLK_FCK2)
-                ++num_clks;
-        if (clks & DSS_CLK_54M)
-                ++num_clks;
-        if (clks & DSS_CLK_96M)
-                ++num_clks;
-
-        return num_clks;
-}
-
-static void dss_clk_enable_no_ctx(enum dss_clock clks)
-{
-        unsigned num_clks = count_clk_bits(clks);
-
-        if (clks & DSS_CLK_ICK)
-                clk_enable(core.dss_ick);
-        if (clks & DSS_CLK_FCK1)
-                clk_enable(core.dss1_fck);
-        if (clks & DSS_CLK_FCK2)
-                clk_enable(core.dss2_fck);
-        if (clks & DSS_CLK_54M)
-                clk_enable(core.dss_54m_fck);
-        if (clks & DSS_CLK_96M)
-                clk_enable(core.dss_96m_fck);
-
-        core.num_clks_enabled += num_clks;
-}
-
-void dss_clk_enable(enum dss_clock clks)
-{
-        bool check_ctx = core.num_clks_enabled == 0;
-
-        dss_clk_enable_no_ctx(clks);
-
-        if (check_ctx && cpu_is_omap34xx() && dss_need_ctx_restore())
-                restore_all_ctx();
-}
-
-static void dss_clk_disable_no_ctx(enum dss_clock clks)
-{
-        unsigned num_clks = count_clk_bits(clks);
-
-        if (clks & DSS_CLK_ICK)
-                clk_disable(core.dss_ick);
-        if (clks & DSS_CLK_FCK1)
-                clk_disable(core.dss1_fck);
-        if (clks & DSS_CLK_FCK2)
-                clk_disable(core.dss2_fck);
-        if (clks & DSS_CLK_54M)
-                clk_disable(core.dss_54m_fck);
-        if (clks & DSS_CLK_96M)
-                clk_disable(core.dss_96m_fck);
-
-        core.num_clks_enabled -= num_clks;
-}
-
-void dss_clk_disable(enum dss_clock clks)
-{
-        if (cpu_is_omap34xx()) {
-                unsigned num_clks = count_clk_bits(clks);
-
-                BUG_ON(core.num_clks_enabled < num_clks);
-
-                if (core.num_clks_enabled == num_clks)
-                        save_all_ctx();
-        }
-
-        dss_clk_disable_no_ctx(clks);
-}
-
-static void dss_clk_enable_all_no_ctx(void)
-{
-        enum dss_clock clks;
-
-        clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-        if (cpu_is_omap34xx())
-                clks |= DSS_CLK_96M;
-        dss_clk_enable_no_ctx(clks);
-}
-
-static void dss_clk_disable_all_no_ctx(void)
-{
-        enum dss_clock clks;
-
-        clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-        if (cpu_is_omap34xx())
-                clks |= DSS_CLK_96M;
-        dss_clk_disable_no_ctx(clks);
-}
-
-static void dss_clk_disable_all(void)
-{
-        enum dss_clock clks;
-
-        clks = DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_FCK2 | DSS_CLK_54M;
-        if (cpu_is_omap34xx())
-                clks |= DSS_CLK_96M;
-        dss_clk_disable(clks);
-}
-
 /* REGULATORS */
 
 struct regulator *dss_get_vdds_dsi(void)
@@ -390,32 +84,7 @@ struct regulator *dss_get_vdds_sdi(void)
         return reg;
 }
 
-struct regulator *dss_get_vdda_dac(void)
-{
-        struct regulator *reg;
-
-        if (core.vdda_dac_reg != NULL)
-                return core.vdda_dac_reg;
-
-        reg = regulator_get(&core.pdev->dev, "vdda_dac");
-        if (!IS_ERR(reg))
-                core.vdda_dac_reg = reg;
-
-        return reg;
-}
-
-/* DEBUGFS */
 #if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
-static void dss_debug_dump_clocks(struct seq_file *s)
-{
-        core_dump_clocks(s);
-        dss_dump_clocks(s);
-        dispc_dump_clocks(s);
-#ifdef CONFIG_OMAP2_DSS_DSI
-        dsi_dump_clocks(s);
-#endif
-}
-
 static int dss_debug_show(struct seq_file *s, void *unused)
 {
         void (*func)(struct seq_file *) = s->private;
@@ -497,7 +166,6 @@ static inline void dss_uninitialize_debugfs(void)
 static int omap_dss_probe(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
-        int skip_init = 0;
         int r;
         int i;
 
@@ -508,63 +176,43 @@ static int omap_dss_probe(struct platform_device *pdev)
         dss_init_overlay_managers(pdev);
         dss_init_overlays(pdev);
 
-        r = dss_get_clocks();
-        if (r)
-                goto err_clocks;
-
-        dss_clk_enable_all_no_ctx();
-
-        core.ctx_id = dss_get_ctx_id();
-        DSSDBG("initial ctx id %u\n", core.ctx_id);
-
-#ifdef CONFIG_FB_OMAP_BOOTLOADER_INIT
-        /* DISPC_CONTROL */
-        if (omap_readl(0x48050440) & 1) /* LCD enabled? */
-                skip_init = 1;
-#endif
-
-        r = dss_init(skip_init);
+        r = dss_init_platform_driver();
         if (r) {
-                DSSERR("Failed to initialize DSS\n");
+                DSSERR("Failed to initialize DSS platform driver\n");
                 goto err_dss;
         }
 
-        r = rfbi_init();
-        if (r) {
-                DSSERR("Failed to initialize rfbi\n");
-                goto err_rfbi;
-        }
+        /* keep clocks enabled to prevent context saves/restores during init */
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
-        r = dpi_init(pdev);
+        r = rfbi_init_platform_driver();
         if (r) {
-                DSSERR("Failed to initialize dpi\n");
-                goto err_dpi;
+                DSSERR("Failed to initialize rfbi platform driver\n");
+                goto err_rfbi;
         }
 
-        r = dispc_init();
+        r = dispc_init_platform_driver();
         if (r) {
-                DSSERR("Failed to initialize dispc\n");
+                DSSERR("Failed to initialize dispc platform driver\n");
                 goto err_dispc;
         }
 
-        r = venc_init(pdev);
+        r = venc_init_platform_driver();
         if (r) {
-                DSSERR("Failed to initialize venc\n");
+                DSSERR("Failed to initialize venc platform driver\n");
                 goto err_venc;
         }
 
-        if (cpu_is_omap34xx()) {
-                r = sdi_init(skip_init);
-                if (r) {
-                        DSSERR("Failed to initialize SDI\n");
-                        goto err_sdi;
-                }
+        r = dsi_init_platform_driver();
+        if (r) {
+                DSSERR("Failed to initialize DSI platform driver\n");
+                goto err_dsi;
+        }
 
-                r = dsi_init(pdev);
-                if (r) {
-                        DSSERR("Failed to initialize DSI\n");
-                        goto err_dsi;
-                }
+        r = hdmi_init_platform_driver();
+        if (r) {
+                DSSERR("Failed to initialize hdmi\n");
+                goto err_hdmi;
         }
 
         r = dss_initialize_debugfs();
@@ -589,32 +237,25 @@ static int omap_dss_probe(struct platform_device *pdev)
                         pdata->default_device = dssdev;
         }
 
-        dss_clk_disable_all();
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         return 0;
 
 err_register:
         dss_uninitialize_debugfs();
 err_debugfs:
-        if (cpu_is_omap34xx())
-                dsi_exit();
+        hdmi_uninit_platform_driver();
+err_hdmi:
+        dsi_uninit_platform_driver();
 err_dsi:
-        if (cpu_is_omap34xx())
-                sdi_exit();
-err_sdi:
-        venc_exit();
+        venc_uninit_platform_driver();
 err_venc:
-        dispc_exit();
+        dispc_uninit_platform_driver();
 err_dispc:
-        dpi_exit();
-err_dpi:
-        rfbi_exit();
+        rfbi_uninit_platform_driver();
 err_rfbi:
-        dss_exit();
+        dss_uninit_platform_driver();
 err_dss:
-        dss_clk_disable_all_no_ctx();
-        dss_put_clocks();
-err_clocks:
 
         return r;
 }
@@ -623,61 +264,15 @@ static int omap_dss_remove(struct platform_device *pdev)
 {
         struct omap_dss_board_info *pdata = pdev->dev.platform_data;
         int i;
-        int c;
 
         dss_uninitialize_debugfs();
 
-        venc_exit();
-        dispc_exit();
-        dpi_exit();
-        rfbi_exit();
-        if (cpu_is_omap34xx()) {
-                dsi_exit();
-                sdi_exit();
-        }
-
-        dss_exit();
-
-        /* these should be removed at some point */
-        c = core.dss_ick->usecount;
-        if (c > 0) {
-                DSSERR("warning: dss_ick usecount %d, disabling\n", c);
-                while (c-- > 0)
-                        clk_disable(core.dss_ick);
-        }
-
-        c = core.dss1_fck->usecount;
-        if (c > 0) {
-                DSSERR("warning: dss1_fck usecount %d, disabling\n", c);
-                while (c-- > 0)
-                        clk_disable(core.dss1_fck);
-        }
-
-        c = core.dss2_fck->usecount;
-        if (c > 0) {
-                DSSERR("warning: dss2_fck usecount %d, disabling\n", c);
-                while (c-- > 0)
-                        clk_disable(core.dss2_fck);
-        }
-
-        c = core.dss_54m_fck->usecount;
-        if (c > 0) {
-                DSSERR("warning: dss_54m_fck usecount %d, disabling\n", c);
-                while (c-- > 0)
-                        clk_disable(core.dss_54m_fck);
-        }
-
-        if (core.dss_96m_fck) {
-                c = core.dss_96m_fck->usecount;
-                if (c > 0) {
-                        DSSERR("warning: dss_96m_fck usecount %d, disabling\n",
-                                        c);
-                        while (c-- > 0)
-                                clk_disable(core.dss_96m_fck);
-                }
-        }
-
-        dss_put_clocks();
+        venc_uninit_platform_driver();
+        dispc_uninit_platform_driver();
+        rfbi_uninit_platform_driver();
+        dsi_uninit_platform_driver();
+        hdmi_uninit_platform_driver();
+        dss_uninit_platform_driver();
 
         dss_uninit_overlays(pdev);
         dss_uninit_overlay_managers(pdev);
@@ -965,11 +560,6 @@ static void __exit omap_dss_exit(void)
                 core.vdds_sdi_reg = NULL;
         }
 
-        if (core.vdda_dac_reg != NULL) {
-                regulator_put(core.vdda_dac_reg);
-                core.vdda_dac_reg = NULL;
-        }
-
         platform_driver_unregister(&omap_dss_driver);
 
         omap_dss_bus_unregister();
diff --git a/drivers/video/omap2/dss/dispc.c b/drivers/video/omap2/dss/dispc.c
index 9f8c69f16e61..7804779c9da1 100644
--- a/drivers/video/omap2/dss/dispc.c
+++ b/drivers/video/omap2/dss/dispc.c
@@ -32,6 +32,7 @@
 #include <linux/delay.h>
 #include <linux/workqueue.h>
 #include <linux/hardirq.h>
+#include <linux/interrupt.h>
 
 #include <plat/sram.h>
 #include <plat/clock.h>
@@ -42,8 +43,6 @@
 #include "dss_features.h"
 
 /* DISPC */
-#define DISPC_BASE                      0x48050400
-
 #define DISPC_SZ_REGS                   SZ_4K
 
 struct dispc_reg { u16 idx; };
@@ -74,7 +73,7 @@ struct dispc_reg { u16 idx; };
 #define DISPC_TIMING_H(ch)              DISPC_REG(ch != 2 ? 0x0064 : 0x0400)
 #define DISPC_TIMING_V(ch)              DISPC_REG(ch != 2 ? 0x0068 : 0x0404)
 #define DISPC_POL_FREQ(ch)              DISPC_REG(ch != 2 ? 0x006C : 0x0408)
-#define DISPC_DIVISOR(ch)               DISPC_REG(ch != 2 ? 0x0070 : 0x040C)
+#define DISPC_DIVISORo(ch)              DISPC_REG(ch != 2 ? 0x0070 : 0x040C)
 #define DISPC_GLOBAL_ALPHA              DISPC_REG(0x0074)
 #define DISPC_SIZE_DIG                  DISPC_REG(0x0078)
 #define DISPC_SIZE_LCD(ch)              DISPC_REG(ch != 2 ? 0x007C : 0x03CC)
@@ -129,6 +128,7 @@ struct dispc_reg { u16 idx; };
 
 #define DISPC_VID_PRELOAD(n)            DISPC_REG(0x230 + (n)*0x04)
 
+#define DISPC_DIVISOR                   DISPC_REG(0x0804)
 
 #define DISPC_IRQ_MASK_ERROR            (DISPC_IRQ_GFX_FIFO_UNDERFLOW | \
                                          DISPC_IRQ_OCP_ERR | \
@@ -178,7 +178,9 @@ struct dispc_irq_stats {
 };
 
 static struct {
+        struct platform_device *pdev;
         void __iomem    *base;
+        int irq;
 
         u32     fifo_size[3];
 
@@ -230,7 +232,7 @@ void dispc_save_context(void)
         SR(TIMING_H(0));
         SR(TIMING_V(0));
         SR(POL_FREQ(0));
-        SR(DIVISOR(0));
+        SR(DIVISORo(0));
         SR(GLOBAL_ALPHA);
         SR(SIZE_DIG);
         SR(SIZE_LCD(0));
@@ -242,7 +244,7 @@ void dispc_save_context(void)
                 SR(TIMING_H(2));
                 SR(TIMING_V(2));
                 SR(POL_FREQ(2));
-                SR(DIVISOR(2));
+                SR(DIVISORo(2));
                 SR(CONFIG2);
         }
 
@@ -373,6 +375,9 @@ void dispc_save_context(void)
         SR(VID_FIR_COEF_V(1, 7));
 
         SR(VID_PRELOAD(1));
+
+        if (dss_has_feature(FEAT_CORE_CLK_DIV))
+                SR(DIVISOR);
 }
 
 void dispc_restore_context(void)
@@ -389,7 +394,7 @@ void dispc_restore_context(void)
         RR(TIMING_H(0));
         RR(TIMING_V(0));
         RR(POL_FREQ(0));
-        RR(DIVISOR(0));
+        RR(DIVISORo(0));
         RR(GLOBAL_ALPHA);
         RR(SIZE_DIG);
         RR(SIZE_LCD(0));
@@ -400,7 +405,7 @@ void dispc_restore_context(void)
                 RR(TIMING_H(2));
                 RR(TIMING_V(2));
                 RR(POL_FREQ(2));
-                RR(DIVISOR(2));
+                RR(DIVISORo(2));
                 RR(CONFIG2);
         }
 
@@ -532,6 +537,9 @@ void dispc_restore_context(void)
 
         RR(VID_PRELOAD(1));
 
+        if (dss_has_feature(FEAT_CORE_CLK_DIV))
+                RR(DIVISOR);
+
         /* enable last, because LCD & DIGIT enable are here */
         RR(CONTROL);
         if (dss_has_feature(FEAT_MGR_LCD2))
@@ -552,9 +560,9 @@ void dispc_restore_context(void)
 static inline void enable_clocks(bool enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         else
-                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 bool dispc_go_busy(enum omap_channel channel)
@@ -1000,6 +1008,20 @@ void dispc_set_burst_size(enum omap_plane plane,
         enable_clocks(0);
 }
 
+void dispc_enable_gamma_table(bool enable)
+{
+        /*
+         * This is partially implemented to support only disabling of
+         * the gamma table.
+         */
+        if (enable) {
+                DSSWARN("Gamma table enabling for TV not yet supported");
+                return;
+        }
+
+        REG_FLD_MOD(DISPC_CONFIG, enable, 9, 9);
+}
+
 static void _dispc_set_vid_color_conv(enum omap_plane plane, bool enable)
 {
         u32 val;
@@ -1129,10 +1151,16 @@ static void _dispc_set_vid_accu0(enum omap_plane plane, int haccu, int vaccu)
         u32 val;
         const struct dispc_reg ac0_reg[] = { DISPC_VID_ACCU0(0),
                                       DISPC_VID_ACCU0(1) };
+        u8 hor_start, hor_end, vert_start, vert_end;
 
         BUG_ON(plane == OMAP_DSS_GFX);
 
-        val = FLD_VAL(vaccu, 25, 16) | FLD_VAL(haccu, 9, 0);
+        dss_feat_get_reg_field(FEAT_REG_HORIZONTALACCU, &hor_start, &hor_end);
+        dss_feat_get_reg_field(FEAT_REG_VERTICALACCU, &vert_start, &vert_end);
+
+        val = FLD_VAL(vaccu, vert_start, vert_end) |
+                        FLD_VAL(haccu, hor_start, hor_end);
+
         dispc_write_reg(ac0_reg[plane-1], val);
 }
 
@@ -1141,10 +1169,16 @@ static void _dispc_set_vid_accu1(enum omap_plane plane, int haccu, int vaccu)
         u32 val;
         const struct dispc_reg ac1_reg[] = { DISPC_VID_ACCU1(0),
                                       DISPC_VID_ACCU1(1) };
+        u8 hor_start, hor_end, vert_start, vert_end;
 
         BUG_ON(plane == OMAP_DSS_GFX);
 
-        val = FLD_VAL(vaccu, 25, 16) | FLD_VAL(haccu, 9, 0);
+        dss_feat_get_reg_field(FEAT_REG_HORIZONTALACCU, &hor_start, &hor_end);
+        dss_feat_get_reg_field(FEAT_REG_VERTICALACCU, &vert_start, &vert_end);
+
+        val = FLD_VAL(vaccu, vert_start, vert_end) |
+                        FLD_VAL(haccu, hor_start, hor_end);
+
         dispc_write_reg(ac1_reg[plane-1], val);
 }
 
@@ -1182,16 +1216,25 @@ static void _dispc_set_scaling(enum omap_plane plane,
         _dispc_set_fir(plane, fir_hinc, fir_vinc);
 
         l = dispc_read_reg(dispc_reg_att[plane]);
-        l &= ~((0x0f << 5) | (0x3 << 21));
 
+        /* RESIZEENABLE and VERTICALTAPS */
+        l &= ~((0x3 << 5) | (0x1 << 21));
         l |= fir_hinc ? (1 << 5) : 0;
         l |= fir_vinc ? (1 << 6) : 0;
+        l |= five_taps ? (1 << 21) : 0;
 
-        l |= hscaleup ? 0 : (1 << 7);
-        l |= vscaleup ? 0 : (1 << 8);
+        /* VRESIZECONF and HRESIZECONF */
+        if (dss_has_feature(FEAT_RESIZECONF)) {
+                l &= ~(0x3 << 7);
+                l |= hscaleup ? 0 : (1 << 7);
+                l |= vscaleup ? 0 : (1 << 8);
+        }
 
-        l |= five_taps ? (1 << 21) : 0;
-        l |= five_taps ? (1 << 22) : 0;
+        /* LINEBUFFERSPLIT */
+        if (dss_has_feature(FEAT_LINEBUFFERSPLIT)) {
+                l &= ~(0x1 << 22);
+                l |= five_taps ? (1 << 22) : 0;
+        }
 
         dispc_write_reg(dispc_reg_att[plane], l);
 
@@ -1215,9 +1258,11 @@ static void _dispc_set_scaling(enum omap_plane plane,
 static void _dispc_set_rotation_attrs(enum omap_plane plane, u8 rotation,
                 bool mirroring, enum omap_color_mode color_mode)
 {
+        bool row_repeat = false;
+        int vidrot = 0;
+
         if (color_mode == OMAP_DSS_COLOR_YUV2 ||
                         color_mode == OMAP_DSS_COLOR_UYVY) {
-                int vidrot = 0;
 
                 if (mirroring) {
                         switch (rotation) {
@@ -1251,16 +1296,15 @@ static void _dispc_set_rotation_attrs(enum omap_plane plane, u8 rotation,
                         }
                 }
 
-                REG_FLD_MOD(dispc_reg_att[plane], vidrot, 13, 12);
-
                 if (rotation == OMAP_DSS_ROT_90 || rotation == OMAP_DSS_ROT_270)
-                        REG_FLD_MOD(dispc_reg_att[plane], 0x1, 18, 18);
+                        row_repeat = true;
                 else
-                        REG_FLD_MOD(dispc_reg_att[plane], 0x0, 18, 18);
-        } else {
-                REG_FLD_MOD(dispc_reg_att[plane], 0, 13, 12);
-                REG_FLD_MOD(dispc_reg_att[plane], 0, 18, 18);
+                        row_repeat = false;
         }
+
+        REG_FLD_MOD(dispc_reg_att[plane], vidrot, 13, 12);
+        if (dss_has_feature(FEAT_ROWREPEATENABLE))
+                REG_FLD_MOD(dispc_reg_att[plane], row_repeat ? 1 : 0, 18, 18);
 }
 
 static int color_mode_to_bpp(enum omap_color_mode color_mode)
@@ -2293,7 +2337,7 @@ static void dispc_set_lcd_divisor(enum omap_channel channel, u16 lck_div,
         BUG_ON(pck_div < 2);
 
         enable_clocks(1);
-        dispc_write_reg(DISPC_DIVISOR(channel),
+        dispc_write_reg(DISPC_DIVISORo(channel),
                         FLD_VAL(lck_div, 23, 16) | FLD_VAL(pck_div, 7, 0));
         enable_clocks(0);
 }
@@ -2302,7 +2346,7 @@ static void dispc_get_lcd_divisor(enum omap_channel channel, int *lck_div,
                 int *pck_div)
 {
         u32 l;
-        l = dispc_read_reg(DISPC_DIVISOR(channel));
+        l = dispc_read_reg(DISPC_DIVISORo(channel));
         *lck_div = FLD_GET(l, 23, 16);
         *pck_div = FLD_GET(l, 7, 0);
 }
@@ -2311,14 +2355,17 @@ unsigned long dispc_fclk_rate(void)
 {
         unsigned long r = 0;
 
-        if (dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK)
-                r = dss_clk_get_rate(DSS_CLK_FCK1);
-        else
-#ifdef CONFIG_OMAP2_DSS_DSI
-                r = dsi_get_dsi1_pll_rate();
-#else
-        BUG();
-#endif
+        switch (dss_get_dispc_clk_source()) {
+        case DSS_CLK_SRC_FCK:
+                r = dss_clk_get_rate(DSS_CLK_FCK);
+                break;
+        case DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                r = dsi_get_pll_hsdiv_dispc_rate();
+                break;
+        default:
+                BUG();
+        }
+
         return r;
 }
 
@@ -2328,47 +2375,72 @@ unsigned long dispc_lclk_rate(enum omap_channel channel)
         unsigned long r;
         u32 l;
 
-        l = dispc_read_reg(DISPC_DIVISOR(channel));
+        l = dispc_read_reg(DISPC_DIVISORo(channel));
 
         lcd = FLD_GET(l, 23, 16);
 
-        r = dispc_fclk_rate();
+        switch (dss_get_lcd_clk_source(channel)) {
+        case DSS_CLK_SRC_FCK:
+                r = dss_clk_get_rate(DSS_CLK_FCK);
+                break;
+        case DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                r = dsi_get_pll_hsdiv_dispc_rate();
+                break;
+        default:
+                BUG();
+        }
 
         return r / lcd;
 }
 
 unsigned long dispc_pclk_rate(enum omap_channel channel)
 {
-        int lcd, pcd;
+        int pcd;
         unsigned long r;
         u32 l;
 
-        l = dispc_read_reg(DISPC_DIVISOR(channel));
+        l = dispc_read_reg(DISPC_DIVISORo(channel));
 
-        lcd = FLD_GET(l, 23, 16);
         pcd = FLD_GET(l, 7, 0);
 
-        r = dispc_fclk_rate();
+        r = dispc_lclk_rate(channel);
 
-        return r / lcd / pcd;
+        return r / pcd;
 }
 
 void dispc_dump_clocks(struct seq_file *s)
 {
         int lcd, pcd;
+        u32 l;
+        enum dss_clk_source dispc_clk_src = dss_get_dispc_clk_source();
+        enum dss_clk_source lcd_clk_src;
 
         enable_clocks(1);
 
         seq_printf(s, "- DISPC -\n");
 
-        seq_printf(s, "dispc fclk source = %s\n",
-                        dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
-                        "dss1_alwon_fclk" : "dsi1_pll_fclk");
+        seq_printf(s, "dispc fclk source = %s (%s)\n",
+                        dss_get_generic_clk_source_name(dispc_clk_src),
+                        dss_feat_get_clk_source_name(dispc_clk_src));
 
         seq_printf(s, "fck\t\t%-16lu\n", dispc_fclk_rate());
 
+        if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+                seq_printf(s, "- DISPC-CORE-CLK -\n");
+                l = dispc_read_reg(DISPC_DIVISOR);
+                lcd = FLD_GET(l, 23, 16);
+
+                seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
+                                (dispc_fclk_rate()/lcd), lcd);
+        }
         seq_printf(s, "- LCD1 -\n");
 
+        lcd_clk_src = dss_get_lcd_clk_source(OMAP_DSS_CHANNEL_LCD);
+
+        seq_printf(s, "lcd1_clk source = %s (%s)\n",
+                dss_get_generic_clk_source_name(lcd_clk_src),
+                dss_feat_get_clk_source_name(lcd_clk_src));
+
         dispc_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD, &lcd, &pcd);
 
         seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
@@ -2378,6 +2450,12 @@ void dispc_dump_clocks(struct seq_file *s)
         if (dss_has_feature(FEAT_MGR_LCD2)) {
                 seq_printf(s, "- LCD2 -\n");
 
+                lcd_clk_src = dss_get_lcd_clk_source(OMAP_DSS_CHANNEL_LCD2);
+
+                seq_printf(s, "lcd2_clk source = %s (%s)\n",
+                        dss_get_generic_clk_source_name(lcd_clk_src),
+                        dss_feat_get_clk_source_name(lcd_clk_src));
+
                 dispc_get_lcd_divisor(OMAP_DSS_CHANNEL_LCD2, &lcd, &pcd);
 
                 seq_printf(s, "lck\t\t%-16lulck div\t%u\n",
@@ -2440,7 +2518,7 @@ void dispc_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dispc_read_reg(r))
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         DUMPREG(DISPC_REVISION);
         DUMPREG(DISPC_SYSCONFIG);
@@ -2459,7 +2537,7 @@ void dispc_dump_regs(struct seq_file *s)
         DUMPREG(DISPC_TIMING_H(0));
         DUMPREG(DISPC_TIMING_V(0));
         DUMPREG(DISPC_POL_FREQ(0));
-        DUMPREG(DISPC_DIVISOR(0));
+        DUMPREG(DISPC_DIVISORo(0));
         DUMPREG(DISPC_GLOBAL_ALPHA);
         DUMPREG(DISPC_SIZE_DIG);
         DUMPREG(DISPC_SIZE_LCD(0));
@@ -2471,7 +2549,7 @@ void dispc_dump_regs(struct seq_file *s)
                 DUMPREG(DISPC_TIMING_H(2));
                 DUMPREG(DISPC_TIMING_V(2));
                 DUMPREG(DISPC_POL_FREQ(2));
-                DUMPREG(DISPC_DIVISOR(2));
+                DUMPREG(DISPC_DIVISORo(2));
                 DUMPREG(DISPC_SIZE_LCD(2));
         }
 
@@ -2597,7 +2675,7 @@ void dispc_dump_regs(struct seq_file *s)
         DUMPREG(DISPC_VID_PRELOAD(0));
         DUMPREG(DISPC_VID_PRELOAD(1));
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
@@ -2713,8 +2791,8 @@ int dispc_get_clock_div(enum omap_channel channel,
 
         fck = dispc_fclk_rate();
 
-        cinfo->lck_div = REG_GET(DISPC_DIVISOR(channel), 23, 16);
-        cinfo->pck_div = REG_GET(DISPC_DIVISOR(channel), 7, 0);
+        cinfo->lck_div = REG_GET(DISPC_DIVISORo(channel), 23, 16);
+        cinfo->pck_div = REG_GET(DISPC_DIVISORo(channel), 7, 0);
 
         cinfo->lck = fck / cinfo->lck_div;
         cinfo->pck = cinfo->lck / cinfo->pck_div;
@@ -2791,6 +2869,9 @@ int omap_dispc_register_isr(omap_dispc_isr_t isr, void *arg, u32 mask)
                 break;
         }
 
+        if (ret)
+                goto err;
+
         _omap_dispc_set_irqs();
 
         spin_unlock_irqrestore(&dispc.irq_lock, flags);
@@ -2866,10 +2947,10 @@ static void print_irq_status(u32 status)
  * but we presume they are on because we got an IRQ. However,
  * an irq handler may turn the clocks off, so we may not have
  * clock later in the function. */
-void dispc_irq_handler(void)
+static irqreturn_t omap_dispc_irq_handler(int irq, void *arg)
 {
         int i;
-        u32 irqstatus;
+        u32 irqstatus, irqenable;
         u32 handledirqs = 0;
         u32 unhandled_errors;
         struct omap_dispc_isr_data *isr_data;
@@ -2878,6 +2959,13 @@ void dispc_irq_handler(void)
         spin_lock(&dispc.irq_lock);
 
         irqstatus = dispc_read_reg(DISPC_IRQSTATUS);
+        irqenable = dispc_read_reg(DISPC_IRQENABLE);
+
+        /* IRQ is not for us */
+        if (!(irqstatus & irqenable)) {
+                spin_unlock(&dispc.irq_lock);
+                return IRQ_NONE;
+        }
 
 #ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
         spin_lock(&dispc.irq_stats_lock);
@@ -2929,6 +3017,8 @@ void dispc_irq_handler(void)
         }
 
         spin_unlock(&dispc.irq_lock);
+
+        return IRQ_HANDLED;
 }
 
 static void dispc_error_worker(struct work_struct *work)
@@ -3253,6 +3343,15 @@ static void _omap_dispc_initial_config(void)
         l = FLD_MOD(l, 1, 0, 0);        /* AUTOIDLE */
         dispc_write_reg(DISPC_SYSCONFIG, l);
 
+        /* Exclusively enable DISPC_CORE_CLK and set divider to 1 */
+        if (dss_has_feature(FEAT_CORE_CLK_DIV)) {
+                l = dispc_read_reg(DISPC_DIVISOR);
+                /* Use DISPC_DIVISOR.LCD, instead of DISPC_DIVISOR1.LCD */
+                l = FLD_MOD(l, 1, 0, 0);
+                l = FLD_MOD(l, 1, 23, 16);
+                dispc_write_reg(DISPC_DIVISOR, l);
+        }
+
         /* FUNCGATED */
         if (dss_has_feature(FEAT_FUNCGATED))
                 REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
@@ -3269,47 +3368,6 @@ static void _omap_dispc_initial_config(void)
         dispc_read_plane_fifo_sizes();
 }
 
-int dispc_init(void)
-{
-        u32 rev;
-
-        spin_lock_init(&dispc.irq_lock);
-
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-        spin_lock_init(&dispc.irq_stats_lock);
-        dispc.irq_stats.last_reset = jiffies;
-#endif
-
-        INIT_WORK(&dispc.error_work, dispc_error_worker);
-
-        dispc.base = ioremap(DISPC_BASE, DISPC_SZ_REGS);
-        if (!dispc.base) {
-                DSSERR("can't ioremap DISPC\n");
-                return -ENOMEM;
-        }
-
-        enable_clocks(1);
-
-        _omap_dispc_initial_config();
-
-        _omap_dispc_initialize_irq();
-
-        dispc_save_context();
-
-        rev = dispc_read_reg(DISPC_REVISION);
-        printk(KERN_INFO "OMAP DISPC rev %d.%d\n",
-               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
-
-        enable_clocks(0);
-
-        return 0;
-}
-
-void dispc_exit(void)
-{
-        iounmap(dispc.base);
-}
-
 int dispc_enable_plane(enum omap_plane plane, bool enable)
 {
         DSSDBG("dispc_enable_plane %d, %d\n", plane, enable);
@@ -3359,3 +3417,94 @@ int dispc_setup_plane(enum omap_plane plane,
 
         return r;
 }
+
+/* DISPC HW IP initialisation */
+static int omap_dispchw_probe(struct platform_device *pdev)
+{
+        u32 rev;
+        int r = 0;
+        struct resource *dispc_mem;
+
+        dispc.pdev = pdev;
+
+        spin_lock_init(&dispc.irq_lock);
+
+#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
+        spin_lock_init(&dispc.irq_stats_lock);
+        dispc.irq_stats.last_reset = jiffies;
+#endif
+
+        INIT_WORK(&dispc.error_work, dispc_error_worker);
+
+        dispc_mem = platform_get_resource(dispc.pdev, IORESOURCE_MEM, 0);
+        if (!dispc_mem) {
+                DSSERR("can't get IORESOURCE_MEM DISPC\n");
+                r = -EINVAL;
+                goto fail0;
+        }
+        dispc.base = ioremap(dispc_mem->start, resource_size(dispc_mem));
+        if (!dispc.base) {
+                DSSERR("can't ioremap DISPC\n");
+                r = -ENOMEM;
+                goto fail0;
+        }
+        dispc.irq = platform_get_irq(dispc.pdev, 0);
+        if (dispc.irq < 0) {
+                DSSERR("platform_get_irq failed\n");
+                r = -ENODEV;
+                goto fail1;
+        }
+
+        r = request_irq(dispc.irq, omap_dispc_irq_handler, IRQF_SHARED,
+                "OMAP DISPC", dispc.pdev);
+        if (r < 0) {
+                DSSERR("request_irq failed\n");
+                goto fail1;
+        }
+
+        enable_clocks(1);
+
+        _omap_dispc_initial_config();
+
+        _omap_dispc_initialize_irq();
+
+        dispc_save_context();
+
+        rev = dispc_read_reg(DISPC_REVISION);
+        dev_dbg(&pdev->dev, "OMAP DISPC rev %d.%d\n",
+               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+        enable_clocks(0);
+
+        return 0;
+fail1:
+        iounmap(dispc.base);
+fail0:
+        return r;
+}
+
+static int omap_dispchw_remove(struct platform_device *pdev)
+{
+        free_irq(dispc.irq, dispc.pdev);
+        iounmap(dispc.base);
+        return 0;
+}
+
+static struct platform_driver omap_dispchw_driver = {
+        .probe          = omap_dispchw_probe,
+        .remove         = omap_dispchw_remove,
+        .driver         = {
+                .name   = "omapdss_dispc",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int dispc_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dispchw_driver);
+}
+
+void dispc_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omap_dispchw_driver);
+}
diff --git a/drivers/video/omap2/dss/display.c b/drivers/video/omap2/dss/display.c
index 22dd7a474f79..a85a6f38b40c 100644
--- a/drivers/video/omap2/dss/display.c
+++ b/drivers/video/omap2/dss/display.c
@@ -25,14 +25,11 @@
 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/jiffies.h>
-#include <linux/list.h>
 #include <linux/platform_device.h>
 
 #include <plat/display.h>
 #include "dss.h"
 
-static LIST_HEAD(display_list);
-
 static ssize_t display_enabled_show(struct device *dev,
                 struct device_attribute *attr, char *buf)
 {
@@ -345,6 +342,7 @@ int omapdss_default_get_recommended_bpp(struct omap_dss_device *dssdev)
                         return 16;
         case OMAP_DISPLAY_TYPE_VENC:
         case OMAP_DISPLAY_TYPE_SDI:
+        case OMAP_DISPLAY_TYPE_HDMI:
                 return 24;
         default:
                 BUG();
@@ -371,6 +369,7 @@ bool dss_use_replication(struct omap_dss_device *dssdev,
         case OMAP_DISPLAY_TYPE_DPI:
                 bpp = dssdev->phy.dpi.data_lines;
                 break;
+        case OMAP_DISPLAY_TYPE_HDMI:
         case OMAP_DISPLAY_TYPE_VENC:
         case OMAP_DISPLAY_TYPE_SDI:
                 bpp = 24;
@@ -396,29 +395,6 @@ void dss_init_device(struct platform_device *pdev,
         switch (dssdev->type) {
 #ifdef CONFIG_OMAP2_DSS_DPI
         case OMAP_DISPLAY_TYPE_DPI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_RFBI
-        case OMAP_DISPLAY_TYPE_DBI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_SDI
-        case OMAP_DISPLAY_TYPE_SDI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_DSI
-        case OMAP_DISPLAY_TYPE_DSI:
-#endif
-#ifdef CONFIG_OMAP2_DSS_VENC
-        case OMAP_DISPLAY_TYPE_VENC:
-#endif
-                break;
-        default:
-                DSSERR("Support for display '%s' not compiled in.\n",
-                                dssdev->name);
-                return;
-        }
-
-        switch (dssdev->type) {
-#ifdef CONFIG_OMAP2_DSS_DPI
-        case OMAP_DISPLAY_TYPE_DPI:
                 r = dpi_init_display(dssdev);
                 break;
 #endif
@@ -442,8 +418,13 @@ void dss_init_device(struct platform_device *pdev,
                 r = dsi_init_display(dssdev);
                 break;
 #endif
+        case OMAP_DISPLAY_TYPE_HDMI:
+                r = hdmi_init_display(dssdev);
+                break;
         default:
-                BUG();
+                DSSERR("Support for display '%s' not compiled in.\n",
+                                dssdev->name);
+                return;
         }
 
         if (r) {
diff --git a/drivers/video/omap2/dss/dpi.c b/drivers/video/omap2/dss/dpi.c
index 75fb0a515430..2d3ca4ca4a05 100644
--- a/drivers/video/omap2/dss/dpi.c
+++ b/drivers/video/omap2/dss/dpi.c
@@ -57,13 +57,13 @@ static int dpi_set_dsi_clk(struct omap_dss_device *dssdev, bool is_tft,
         if (r)
                 return r;
 
-        dss_select_dispc_clk_source(DSS_SRC_DSI1_PLL_FCLK);
+        dss_select_dispc_clk_source(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC);
 
         r = dispc_set_clock_div(dssdev->manager->id, &dispc_cinfo);
         if (r)
                 return r;
 
-        *fck = dsi_cinfo.dsi1_pll_fclk;
+        *fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
         *lck_div = dispc_cinfo.lck_div;
         *pck_div = dispc_cinfo.pck_div;
 
@@ -107,7 +107,7 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
         bool is_tft;
         int r = 0;
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         dispc_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
                         dssdev->panel.acbi, dssdev->panel.acb);
@@ -137,7 +137,7 @@ static int dpi_set_mode(struct omap_dss_device *dssdev)
         dispc_set_lcd_timings(dssdev->manager->id, t);
 
 err0:
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
         return r;
 }
 
@@ -173,14 +173,14 @@ int omapdss_dpi_display_enable(struct omap_dss_device *dssdev)
                         goto err1;
         }
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         r = dpi_basic_init(dssdev);
         if (r)
                 goto err2;
 
 #ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-        dss_clk_enable(DSS_CLK_FCK2);
+        dss_clk_enable(DSS_CLK_SYSCK);
         r = dsi_pll_init(dssdev, 0, 1);
         if (r)
                 goto err3;
@@ -199,10 +199,10 @@ err4:
 #ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
         dsi_pll_uninit();
 err3:
-        dss_clk_disable(DSS_CLK_FCK2);
+        dss_clk_disable(DSS_CLK_SYSCK);
 #endif
 err2:
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
         if (cpu_is_omap34xx())
                 regulator_disable(dpi.vdds_dsi_reg);
 err1:
@@ -217,12 +217,12 @@ void omapdss_dpi_display_disable(struct omap_dss_device *dssdev)
         dssdev->manager->disable(dssdev->manager);
 
 #ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
-        dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
+        dss_select_dispc_clk_source(DSS_CLK_SRC_FCK);
         dsi_pll_uninit();
-        dss_clk_disable(DSS_CLK_FCK2);
+        dss_clk_disable(DSS_CLK_SYSCK);
 #endif
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         if (cpu_is_omap34xx())
                 regulator_disable(dpi.vdds_dsi_reg);
@@ -271,7 +271,7 @@ int dpi_check_timings(struct omap_dss_device *dssdev,
                 if (r)
                         return r;
 
-                fck = dsi_cinfo.dsi1_pll_fclk;
+                fck = dsi_cinfo.dsi_pll_hsdiv_dispc_clk;
                 lck_div = dispc_cinfo.lck_div;
                 pck_div = dispc_cinfo.pck_div;
         }
@@ -303,22 +303,27 @@ int dpi_init_display(struct omap_dss_device *dssdev)
 {
         DSSDBG("init_display\n");
 
-        return 0;
-}
+        if (cpu_is_omap34xx() && dpi.vdds_dsi_reg == NULL) {
+                struct regulator *vdds_dsi;
 
-int dpi_init(struct platform_device *pdev)
-{
-        if (cpu_is_omap34xx()) {
-                dpi.vdds_dsi_reg = dss_get_vdds_dsi();
-                if (IS_ERR(dpi.vdds_dsi_reg)) {
+                vdds_dsi = dss_get_vdds_dsi();
+
+                if (IS_ERR(vdds_dsi)) {
                         DSSERR("can't get VDDS_DSI regulator\n");
-                        return PTR_ERR(dpi.vdds_dsi_reg);
+                        return PTR_ERR(vdds_dsi);
                 }
+
+                dpi.vdds_dsi_reg = vdds_dsi;
         }
 
         return 0;
 }
 
+int dpi_init(void)
+{
+        return 0;
+}
+
 void dpi_exit(void)
 {
 }
diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index ddf3a0560822..0a7f1a47f8e3 100644
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -38,12 +38,11 @@
 #include <plat/clock.h>
 
 #include "dss.h"
+#include "dss_features.h"
 
 /*#define VERBOSE_IRQ*/
 #define DSI_CATCH_MISSING_TE
 
-#define DSI_BASE                0x4804FC00
-
 struct dsi_reg { u16 idx; };
 
 #define DSI_REG(idx)            ((const struct dsi_reg) { idx })
@@ -186,13 +185,15 @@ struct dsi_reg { u16 idx; };
 #define DSI_DT_RX_SHORT_READ_1          0x21
 #define DSI_DT_RX_SHORT_READ_2          0x22
 
-#define FINT_MAX 2100000
-#define FINT_MIN 750000
-#define REGN_MAX (1 << 7)
-#define REGM_MAX ((1 << 11) - 1)
-#define REGM3_MAX (1 << 4)
-#define REGM4_MAX (1 << 4)
-#define LP_DIV_MAX ((1 << 13) - 1)
+typedef void (*omap_dsi_isr_t) (void *arg, u32 mask);
+
+#define DSI_MAX_NR_ISRS                2
+
+struct dsi_isr_data {
+        omap_dsi_isr_t  isr;
+        void            *arg;
+        u32             mask;
+};
 
 enum fifo_size {
         DSI_FIFO_SIZE_0         = 0,
@@ -220,9 +221,17 @@ struct dsi_irq_stats {
         unsigned cio_irqs[32];
 };
 
+struct dsi_isr_tables {
+        struct dsi_isr_data isr_table[DSI_MAX_NR_ISRS];
+        struct dsi_isr_data isr_table_vc[4][DSI_MAX_NR_ISRS];
+        struct dsi_isr_data isr_table_cio[DSI_MAX_NR_ISRS];
+};
+
 static struct
 {
+        struct platform_device *pdev;
         void __iomem    *base;
+        int irq;
 
         struct dsi_clock_info current_cinfo;
 
@@ -232,6 +241,7 @@ static struct
                 enum dsi_vc_mode mode;
                 struct omap_dss_device *dssdev;
                 enum fifo_size fifo_size;
+                int vc_id;
         } vc[4];
 
         struct mutex lock;
@@ -239,8 +249,10 @@ static struct
 
         unsigned pll_locked;
 
-        struct completion bta_completion;
-        void (*bta_callback)(void);
+        spinlock_t irq_lock;
+        struct dsi_isr_tables isr_tables;
+        /* space for a copy used by the interrupt handler */
+        struct dsi_isr_tables isr_tables_copy;
 
         int update_channel;
         struct dsi_update_region update_region;
@@ -275,6 +287,11 @@ static struct
         spinlock_t irq_stats_lock;
         struct dsi_irq_stats irq_stats;
 #endif
+        /* DSI PLL Parameter Ranges */
+        unsigned long regm_max, regn_max;
+        unsigned long  regm_dispc_max, regm_dsi_max;
+        unsigned long  fint_min, fint_max;
+        unsigned long lpdiv_max;
 } dsi;
 
 #ifdef DEBUG
@@ -318,6 +335,11 @@ static bool dsi_bus_is_locked(void)
         return dsi.bus_lock.count == 0;
 }
 
+static void dsi_completion_handler(void *data, u32 mask)
+{
+        complete((struct completion *)data);
+}
+
 static inline int wait_for_bit_change(const struct dsi_reg idx, int bitnum,
                 int value)
 {
@@ -387,6 +409,9 @@ static void dsi_perf_show(const char *name)
 
 static void print_irq_status(u32 status)
 {
+        if (status == 0)
+                return;
+
 #ifndef VERBOSE_IRQ
         if ((status & ~DSI_IRQ_CHANNEL_MASK) == 0)
                 return;
@@ -422,6 +447,9 @@ static void print_irq_status(u32 status)
 
 static void print_irq_status_vc(int channel, u32 status)
 {
+        if (status == 0)
+                return;
+
 #ifndef VERBOSE_IRQ
         if ((status & ~DSI_VC_IRQ_PACKET_SENT) == 0)
                 return;
@@ -448,6 +476,9 @@ static void print_irq_status_vc(int channel, u32 status)
 
 static void print_irq_status_cio(u32 status)
 {
+        if (status == 0)
+                return;
+
         printk(KERN_DEBUG "DSI CIO IRQ 0x%x: ", status);
 
 #define PIS(x) \
@@ -478,22 +509,33 @@ static void print_irq_status_cio(u32 status)
         printk("\n");
 }
 
-static int debug_irq;
-
-/* called from dss */
-void dsi_irq_handler(void)
+#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
+static void dsi_collect_irq_stats(u32 irqstatus, u32 *vcstatus, u32 ciostatus)
 {
-        u32 irqstatus, vcstatus, ciostatus;
         int i;
 
-        irqstatus = dsi_read_reg(DSI_IRQSTATUS);
-
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
         spin_lock(&dsi.irq_stats_lock);
+
         dsi.irq_stats.irq_count++;
         dss_collect_irq_stats(irqstatus, dsi.irq_stats.dsi_irqs);
+
+        for (i = 0; i < 4; ++i)
+                dss_collect_irq_stats(vcstatus[i], dsi.irq_stats.vc_irqs[i]);
+
+        dss_collect_irq_stats(ciostatus, dsi.irq_stats.cio_irqs);
+
+        spin_unlock(&dsi.irq_stats_lock);
+}
+#else
+#define dsi_collect_irq_stats(irqstatus, vcstatus, ciostatus)
 #endif
 
+static int debug_irq;
+
+static void dsi_handle_irq_errors(u32 irqstatus, u32 *vcstatus, u32 ciostatus)
+{
+        int i;
+
         if (irqstatus & DSI_IRQ_ERROR_MASK) {
                 DSSERR("DSI error, irqstatus %x\n", irqstatus);
                 print_irq_status(irqstatus);
@@ -504,37 +546,88 @@ void dsi_irq_handler(void)
                 print_irq_status(irqstatus);
         }
 
-#ifdef DSI_CATCH_MISSING_TE
-        if (irqstatus & DSI_IRQ_TE_TRIGGER)
-                del_timer(&dsi.te_timer);
-#endif
+        for (i = 0; i < 4; ++i) {
+                if (vcstatus[i] & DSI_VC_IRQ_ERROR_MASK) {
+                        DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
+                                       i, vcstatus[i]);
+                        print_irq_status_vc(i, vcstatus[i]);
+                } else if (debug_irq) {
+                        print_irq_status_vc(i, vcstatus[i]);
+                }
+        }
+
+        if (ciostatus & DSI_CIO_IRQ_ERROR_MASK) {
+                DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
+                print_irq_status_cio(ciostatus);
+        } else if (debug_irq) {
+                print_irq_status_cio(ciostatus);
+        }
+}
+
+static void dsi_call_isrs(struct dsi_isr_data *isr_array,
+                unsigned isr_array_size, u32 irqstatus)
+{
+        struct dsi_isr_data *isr_data;
+        int i;
+
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
+                if (isr_data->isr && isr_data->mask & irqstatus)
+                        isr_data->isr(isr_data->arg, irqstatus);
+        }
+}
+
+static void dsi_handle_isrs(struct dsi_isr_tables *isr_tables,
+                u32 irqstatus, u32 *vcstatus, u32 ciostatus)
+{
+        int i;
+
+        dsi_call_isrs(isr_tables->isr_table,
+                        ARRAY_SIZE(isr_tables->isr_table),
+                        irqstatus);
 
         for (i = 0; i < 4; ++i) {
-                if ((irqstatus & (1<<i)) == 0)
+                if (vcstatus[i] == 0)
                         continue;
+                dsi_call_isrs(isr_tables->isr_table_vc[i],
+                                ARRAY_SIZE(isr_tables->isr_table_vc[i]),
+                                vcstatus[i]);
+        }
 
-                vcstatus = dsi_read_reg(DSI_VC_IRQSTATUS(i));
+        if (ciostatus != 0)
+                dsi_call_isrs(isr_tables->isr_table_cio,
+                                ARRAY_SIZE(isr_tables->isr_table_cio),
+                                ciostatus);
+}
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-                dss_collect_irq_stats(vcstatus, dsi.irq_stats.vc_irqs[i]);
-#endif
+static irqreturn_t omap_dsi_irq_handler(int irq, void *arg)
+{
+        u32 irqstatus, vcstatus[4], ciostatus;
+        int i;
 
-                if (vcstatus & DSI_VC_IRQ_BTA) {
-                        complete(&dsi.bta_completion);
+        spin_lock(&dsi.irq_lock);
 
-                        if (dsi.bta_callback)
-                                dsi.bta_callback();
-                }
+        irqstatus = dsi_read_reg(DSI_IRQSTATUS);
 
-                if (vcstatus & DSI_VC_IRQ_ERROR_MASK) {
-                        DSSERR("DSI VC(%d) error, vc irqstatus %x\n",
-                                       i, vcstatus);
-                        print_irq_status_vc(i, vcstatus);
-                } else if (debug_irq) {
-                        print_irq_status_vc(i, vcstatus);
+        /* IRQ is not for us */
+        if (!irqstatus) {
+                spin_unlock(&dsi.irq_lock);
+                return IRQ_NONE;
+        }
+
+        dsi_write_reg(DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
+        /* flush posted write */
+        dsi_read_reg(DSI_IRQSTATUS);
+
+        for (i = 0; i < 4; ++i) {
+                if ((irqstatus & (1 << i)) == 0) {
+                        vcstatus[i] = 0;
+                        continue;
                 }
 
-                dsi_write_reg(DSI_VC_IRQSTATUS(i), vcstatus);
+                vcstatus[i] = dsi_read_reg(DSI_VC_IRQSTATUS(i));
+
+                dsi_write_reg(DSI_VC_IRQSTATUS(i), vcstatus[i]);
                 /* flush posted write */
                 dsi_read_reg(DSI_VC_IRQSTATUS(i));
         }
@@ -542,117 +635,307 @@ void dsi_irq_handler(void)
         if (irqstatus & DSI_IRQ_COMPLEXIO_ERR) {
                 ciostatus = dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-                dss_collect_irq_stats(ciostatus, dsi.irq_stats.cio_irqs);
-#endif
-
                 dsi_write_reg(DSI_COMPLEXIO_IRQ_STATUS, ciostatus);
                 /* flush posted write */
                 dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
+        } else {
+                ciostatus = 0;
+        }
 
-                if (ciostatus & DSI_CIO_IRQ_ERROR_MASK) {
-                        DSSERR("DSI CIO error, cio irqstatus %x\n", ciostatus);
-                        print_irq_status_cio(ciostatus);
-                } else if (debug_irq) {
-                        print_irq_status_cio(ciostatus);
-                }
+#ifdef DSI_CATCH_MISSING_TE
+        if (irqstatus & DSI_IRQ_TE_TRIGGER)
+                del_timer(&dsi.te_timer);
+#endif
+
+        /* make a copy and unlock, so that isrs can unregister
+         * themselves */
+        memcpy(&dsi.isr_tables_copy, &dsi.isr_tables, sizeof(dsi.isr_tables));
+
+        spin_unlock(&dsi.irq_lock);
+
+        dsi_handle_isrs(&dsi.isr_tables_copy, irqstatus, vcstatus, ciostatus);
+
+        dsi_handle_irq_errors(irqstatus, vcstatus, ciostatus);
+
+        dsi_collect_irq_stats(irqstatus, vcstatus, ciostatus);
+
+        return IRQ_HANDLED;
+}
+
+/* dsi.irq_lock has to be locked by the caller */
+static void _omap_dsi_configure_irqs(struct dsi_isr_data *isr_array,
+                unsigned isr_array_size, u32 default_mask,
+                const struct dsi_reg enable_reg,
+                const struct dsi_reg status_reg)
+{
+        struct dsi_isr_data *isr_data;
+        u32 mask;
+        u32 old_mask;
+        int i;
+
+        mask = default_mask;
+
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
+
+                if (isr_data->isr == NULL)
+                        continue;
+
+                mask |= isr_data->mask;
         }
 
-        dsi_write_reg(DSI_IRQSTATUS, irqstatus & ~DSI_IRQ_CHANNEL_MASK);
-        /* flush posted write */
-        dsi_read_reg(DSI_IRQSTATUS);
+        old_mask = dsi_read_reg(enable_reg);
+        /* clear the irqstatus for newly enabled irqs */
+        dsi_write_reg(status_reg, (mask ^ old_mask) & mask);
+        dsi_write_reg(enable_reg, mask);
 
-#ifdef CONFIG_OMAP2_DSS_COLLECT_IRQ_STATS
-        spin_unlock(&dsi.irq_stats_lock);
+        /* flush posted writes */
+        dsi_read_reg(enable_reg);
+        dsi_read_reg(status_reg);
+}
+
+/* dsi.irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs(void)
+{
+        u32 mask = DSI_IRQ_ERROR_MASK;
+#ifdef DSI_CATCH_MISSING_TE
+        mask |= DSI_IRQ_TE_TRIGGER;
 #endif
+        _omap_dsi_configure_irqs(dsi.isr_tables.isr_table,
+                        ARRAY_SIZE(dsi.isr_tables.isr_table), mask,
+                        DSI_IRQENABLE, DSI_IRQSTATUS);
+}
+
+/* dsi.irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs_vc(int vc)
+{
+        _omap_dsi_configure_irqs(dsi.isr_tables.isr_table_vc[vc],
+                        ARRAY_SIZE(dsi.isr_tables.isr_table_vc[vc]),
+                        DSI_VC_IRQ_ERROR_MASK,
+                        DSI_VC_IRQENABLE(vc), DSI_VC_IRQSTATUS(vc));
 }
 
+/* dsi.irq_lock has to be locked by the caller */
+static void _omap_dsi_set_irqs_cio(void)
+{
+        _omap_dsi_configure_irqs(dsi.isr_tables.isr_table_cio,
+                        ARRAY_SIZE(dsi.isr_tables.isr_table_cio),
+                        DSI_CIO_IRQ_ERROR_MASK,
+                        DSI_COMPLEXIO_IRQ_ENABLE, DSI_COMPLEXIO_IRQ_STATUS);
+}
 
 static void _dsi_initialize_irq(void)
 {
-        u32 l;
+        unsigned long flags;
+        int vc;
+
+        spin_lock_irqsave(&dsi.irq_lock, flags);
+
+        memset(&dsi.isr_tables, 0, sizeof(dsi.isr_tables));
+
+        _omap_dsi_set_irqs();
+        for (vc = 0; vc < 4; ++vc)
+                _omap_dsi_set_irqs_vc(vc);
+        _omap_dsi_set_irqs_cio();
+
+        spin_unlock_irqrestore(&dsi.irq_lock, flags);
+}
+
+static int _dsi_register_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+                struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+        struct dsi_isr_data *isr_data;
+        int free_idx;
         int i;
 
-        /* disable all interrupts */
-        dsi_write_reg(DSI_IRQENABLE, 0);
-        for (i = 0; i < 4; ++i)
-                dsi_write_reg(DSI_VC_IRQENABLE(i), 0);
-        dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE, 0);
+        BUG_ON(isr == NULL);
 
-        /* clear interrupt status */
-        l = dsi_read_reg(DSI_IRQSTATUS);
-        dsi_write_reg(DSI_IRQSTATUS, l & ~DSI_IRQ_CHANNEL_MASK);
+        /* check for duplicate entry and find a free slot */
+        free_idx = -1;
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
 
-        for (i = 0; i < 4; ++i) {
-                l = dsi_read_reg(DSI_VC_IRQSTATUS(i));
-                dsi_write_reg(DSI_VC_IRQSTATUS(i), l);
+                if (isr_data->isr == isr && isr_data->arg == arg &&
+                                isr_data->mask == mask) {
+                        return -EINVAL;
+                }
+
+                if (isr_data->isr == NULL && free_idx == -1)
+                        free_idx = i;
         }
 
-        l = dsi_read_reg(DSI_COMPLEXIO_IRQ_STATUS);
-        dsi_write_reg(DSI_COMPLEXIO_IRQ_STATUS, l);
+        if (free_idx == -1)
+                return -EBUSY;
 
-        /* enable error irqs */
-        l = DSI_IRQ_ERROR_MASK;
-#ifdef DSI_CATCH_MISSING_TE
-        l |= DSI_IRQ_TE_TRIGGER;
-#endif
-        dsi_write_reg(DSI_IRQENABLE, l);
+        isr_data = &isr_array[free_idx];
+        isr_data->isr = isr;
+        isr_data->arg = arg;
+        isr_data->mask = mask;
 
-        l = DSI_VC_IRQ_ERROR_MASK;
-        for (i = 0; i < 4; ++i)
-                dsi_write_reg(DSI_VC_IRQENABLE(i), l);
+        return 0;
+}
+
+static int _dsi_unregister_isr(omap_dsi_isr_t isr, void *arg, u32 mask,
+                struct dsi_isr_data *isr_array, unsigned isr_array_size)
+{
+        struct dsi_isr_data *isr_data;
+        int i;
+
+        for (i = 0; i < isr_array_size; i++) {
+                isr_data = &isr_array[i];
+                if (isr_data->isr != isr || isr_data->arg != arg ||
+                                isr_data->mask != mask)
+                        continue;
+
+                isr_data->isr = NULL;
+                isr_data->arg = NULL;
+                isr_data->mask = 0;
+
+                return 0;
+        }
 
-        l = DSI_CIO_IRQ_ERROR_MASK;
-        dsi_write_reg(DSI_COMPLEXIO_IRQ_ENABLE, l);
+        return -EINVAL;
 }
 
-static u32 dsi_get_errors(void)
+static int dsi_register_isr(omap_dsi_isr_t isr, void *arg, u32 mask)
 {
         unsigned long flags;
-        u32 e;
-        spin_lock_irqsave(&dsi.errors_lock, flags);
-        e = dsi.errors;
-        dsi.errors = 0;
-        spin_unlock_irqrestore(&dsi.errors_lock, flags);
-        return e;
+        int r;
+
+        spin_lock_irqsave(&dsi.irq_lock, flags);
+
+        r = _dsi_register_isr(isr, arg, mask, dsi.isr_tables.isr_table,
+                        ARRAY_SIZE(dsi.isr_tables.isr_table));
+
+        if (r == 0)
+                _omap_dsi_set_irqs();
+
+        spin_unlock_irqrestore(&dsi.irq_lock, flags);
+
+        return r;
 }
 
-static void dsi_vc_enable_bta_irq(int channel)
+static int dsi_unregister_isr(omap_dsi_isr_t isr, void *arg, u32 mask)
 {
-        u32 l;
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi.irq_lock, flags);
+
+        r = _dsi_unregister_isr(isr, arg, mask, dsi.isr_tables.isr_table,
+                        ARRAY_SIZE(dsi.isr_tables.isr_table));
+
+        if (r == 0)
+                _omap_dsi_set_irqs();
 
-        dsi_write_reg(DSI_VC_IRQSTATUS(channel), DSI_VC_IRQ_BTA);
+        spin_unlock_irqrestore(&dsi.irq_lock, flags);
 
-        l = dsi_read_reg(DSI_VC_IRQENABLE(channel));
-        l |= DSI_VC_IRQ_BTA;
-        dsi_write_reg(DSI_VC_IRQENABLE(channel), l);
+        return r;
 }
 
-static void dsi_vc_disable_bta_irq(int channel)
+static int dsi_register_isr_vc(int channel, omap_dsi_isr_t isr, void *arg,
+                u32 mask)
 {
-        u32 l;
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi.irq_lock, flags);
+
+        r = _dsi_register_isr(isr, arg, mask,
+                        dsi.isr_tables.isr_table_vc[channel],
+                        ARRAY_SIZE(dsi.isr_tables.isr_table_vc[channel]));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_vc(channel);
+
+        spin_unlock_irqrestore(&dsi.irq_lock, flags);
+
+        return r;
+}
+
+static int dsi_unregister_isr_vc(int channel, omap_dsi_isr_t isr, void *arg,
+                u32 mask)
+{
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi.irq_lock, flags);
+
+        r = _dsi_unregister_isr(isr, arg, mask,
+                        dsi.isr_tables.isr_table_vc[channel],
+                        ARRAY_SIZE(dsi.isr_tables.isr_table_vc[channel]));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_vc(channel);
+
+        spin_unlock_irqrestore(&dsi.irq_lock, flags);
+
+        return r;
+}
+
+static int dsi_register_isr_cio(omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi.irq_lock, flags);
+
+        r = _dsi_register_isr(isr, arg, mask, dsi.isr_tables.isr_table_cio,
+                        ARRAY_SIZE(dsi.isr_tables.isr_table_cio));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_cio();
+
+        spin_unlock_irqrestore(&dsi.irq_lock, flags);
+
+        return r;
+}
+
+static int dsi_unregister_isr_cio(omap_dsi_isr_t isr, void *arg, u32 mask)
+{
+        unsigned long flags;
+        int r;
+
+        spin_lock_irqsave(&dsi.irq_lock, flags);
+
+        r = _dsi_unregister_isr(isr, arg, mask, dsi.isr_tables.isr_table_cio,
+                        ARRAY_SIZE(dsi.isr_tables.isr_table_cio));
+
+        if (r == 0)
+                _omap_dsi_set_irqs_cio();
+
+        spin_unlock_irqrestore(&dsi.irq_lock, flags);
 
-        l = dsi_read_reg(DSI_VC_IRQENABLE(channel));
-        l &= ~DSI_VC_IRQ_BTA;
-        dsi_write_reg(DSI_VC_IRQENABLE(channel), l);
+        return r;
 }
 
-/* DSI func clock. this could also be DSI2_PLL_FCLK */
+static u32 dsi_get_errors(void)
+{
+        unsigned long flags;
+        u32 e;
+        spin_lock_irqsave(&dsi.errors_lock, flags);
+        e = dsi.errors;
+        dsi.errors = 0;
+        spin_unlock_irqrestore(&dsi.errors_lock, flags);
+        return e;
+}
+
+/* DSI func clock. this could also be dsi_pll_hsdiv_dsi_clk */
 static inline void enable_clocks(bool enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         else
-                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 /* source clock for DSI PLL. this could also be PCLKFREE */
 static inline void dsi_enable_pll_clock(bool enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_FCK2);
+                dss_clk_enable(DSS_CLK_SYSCK);
         else
-                dss_clk_disable(DSS_CLK_FCK2);
+                dss_clk_disable(DSS_CLK_SYSCK);
 
         if (enable && dsi.pll_locked) {
                 if (wait_for_bit_change(DSI_PLL_STATUS, 1, 1) != 1)
@@ -707,14 +990,14 @@ static inline int dsi_if_enable(bool enable)
         return 0;
 }
 
-unsigned long dsi_get_dsi1_pll_rate(void)
+unsigned long dsi_get_pll_hsdiv_dispc_rate(void)
 {
-        return dsi.current_cinfo.dsi1_pll_fclk;
+        return dsi.current_cinfo.dsi_pll_hsdiv_dispc_clk;
 }
 
-static unsigned long dsi_get_dsi2_pll_rate(void)
+static unsigned long dsi_get_pll_hsdiv_dsi_rate(void)
 {
-        return dsi.current_cinfo.dsi2_pll_fclk;
+        return dsi.current_cinfo.dsi_pll_hsdiv_dsi_clk;
 }
 
 static unsigned long dsi_get_txbyteclkhs(void)
@@ -726,12 +1009,12 @@ static unsigned long dsi_fclk_rate(void)
 {
         unsigned long r;
 
-        if (dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK) {
-                /* DSI FCLK source is DSS1_ALWON_FCK, which is dss1_fck */
-                r = dss_clk_get_rate(DSS_CLK_FCK1);
+        if (dss_get_dsi_clk_source() == DSS_CLK_SRC_FCK) {
+                /* DSI FCLK source is DSS_CLK_FCK */
+                r = dss_clk_get_rate(DSS_CLK_FCK);
         } else {
-                /* DSI FCLK source is DSI2_PLL_FCLK */
-                r = dsi_get_dsi2_pll_rate();
+                /* DSI FCLK source is dsi_pll_hsdiv_dsi_clk */
+                r = dsi_get_pll_hsdiv_dsi_rate();
         }
 
         return r;
@@ -745,7 +1028,7 @@ static int dsi_set_lp_clk_divisor(struct omap_dss_device *dssdev)
 
         lp_clk_div = dssdev->phy.dsi.div.lp_clk_div;
 
-        if (lp_clk_div == 0 || lp_clk_div > LP_DIV_MAX)
+        if (lp_clk_div == 0 || lp_clk_div > dsi.lpdiv_max)
                 return -EINVAL;
 
         dsi_fclk = dsi_fclk_rate();
@@ -795,22 +1078,22 @@ static int dsi_pll_power(enum dsi_pll_power_state state)
 static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
                 struct dsi_clock_info *cinfo)
 {
-        if (cinfo->regn == 0 || cinfo->regn > REGN_MAX)
+        if (cinfo->regn == 0 || cinfo->regn > dsi.regn_max)
                 return -EINVAL;
 
-        if (cinfo->regm == 0 || cinfo->regm > REGM_MAX)
+        if (cinfo->regm == 0 || cinfo->regm > dsi.regm_max)
                 return -EINVAL;
 
-        if (cinfo->regm3 > REGM3_MAX)
+        if (cinfo->regm_dispc > dsi.regm_dispc_max)
                 return -EINVAL;
 
-        if (cinfo->regm4 > REGM4_MAX)
+        if (cinfo->regm_dsi > dsi.regm_dsi_max)
                 return -EINVAL;
 
-        if (cinfo->use_dss2_fck) {
-                cinfo->clkin = dss_clk_get_rate(DSS_CLK_FCK2);
+        if (cinfo->use_sys_clk) {
+                cinfo->clkin = dss_clk_get_rate(DSS_CLK_SYSCK);
                 /* XXX it is unclear if highfreq should be used
-                 * with DSS2_FCK source also */
+                 * with DSS_SYS_CLK source also */
                 cinfo->highfreq = 0;
         } else {
                 cinfo->clkin = dispc_pclk_rate(dssdev->manager->id);
@@ -823,7 +1106,7 @@ static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
 
         cinfo->fint = cinfo->clkin / (cinfo->regn * (cinfo->highfreq ? 2 : 1));
 
-        if (cinfo->fint > FINT_MAX || cinfo->fint < FINT_MIN)
+        if (cinfo->fint > dsi.fint_max || cinfo->fint < dsi.fint_min)
                 return -EINVAL;
 
         cinfo->clkin4ddr = 2 * cinfo->regm * cinfo->fint;
@@ -831,15 +1114,17 @@ static int dsi_calc_clock_rates(struct omap_dss_device *dssdev,
         if (cinfo->clkin4ddr > 1800 * 1000 * 1000)
                 return -EINVAL;
 
-        if (cinfo->regm3 > 0)
-                cinfo->dsi1_pll_fclk = cinfo->clkin4ddr / cinfo->regm3;
+        if (cinfo->regm_dispc > 0)
+                cinfo->dsi_pll_hsdiv_dispc_clk =
+                        cinfo->clkin4ddr / cinfo->regm_dispc;
         else
-                cinfo->dsi1_pll_fclk = 0;
+                cinfo->dsi_pll_hsdiv_dispc_clk = 0;
 
-        if (cinfo->regm4 > 0)
-                cinfo->dsi2_pll_fclk = cinfo->clkin4ddr / cinfo->regm4;
+        if (cinfo->regm_dsi > 0)
+                cinfo->dsi_pll_hsdiv_dsi_clk =
+                        cinfo->clkin4ddr / cinfo->regm_dsi;
         else
-                cinfo->dsi2_pll_fclk = 0;
+                cinfo->dsi_pll_hsdiv_dsi_clk = 0;
 
         return 0;
 }
@@ -852,23 +1137,25 @@ int dsi_pll_calc_clock_div_pck(bool is_tft, unsigned long req_pck,
         struct dispc_clock_info best_dispc;
         int min_fck_per_pck;
         int match = 0;
-        unsigned long dss_clk_fck2;
+        unsigned long dss_sys_clk, max_dss_fck;
+
+        dss_sys_clk = dss_clk_get_rate(DSS_CLK_SYSCK);
 
-        dss_clk_fck2 = dss_clk_get_rate(DSS_CLK_FCK2);
+        max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
 
         if (req_pck == dsi.cache_req_pck &&
-                        dsi.cache_cinfo.clkin == dss_clk_fck2) {
+                        dsi.cache_cinfo.clkin == dss_sys_clk) {
                 DSSDBG("DSI clock info found from cache\n");
                 *dsi_cinfo = dsi.cache_cinfo;
-                dispc_find_clk_divs(is_tft, req_pck, dsi_cinfo->dsi1_pll_fclk,
-                                dispc_cinfo);
+                dispc_find_clk_divs(is_tft, req_pck,
+                        dsi_cinfo->dsi_pll_hsdiv_dispc_clk, dispc_cinfo);
                 return 0;
         }
 
         min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
 
         if (min_fck_per_pck &&
-                req_pck * min_fck_per_pck > DISPC_MAX_FCK) {
+                req_pck * min_fck_per_pck > max_dss_fck) {
                 DSSERR("Requested pixel clock not possible with the current "
                                 "OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
                                 "the constraint off.\n");
@@ -882,24 +1169,24 @@ retry:
         memset(&best_dispc, 0, sizeof(best_dispc));
 
         memset(&cur, 0, sizeof(cur));
-        cur.clkin = dss_clk_fck2;
-        cur.use_dss2_fck = 1;
+        cur.clkin = dss_sys_clk;
+        cur.use_sys_clk = 1;
         cur.highfreq = 0;
 
         /* no highfreq: 0.75MHz < Fint = clkin / regn < 2.1MHz */
         /* highfreq: 0.75MHz < Fint = clkin / (2*regn) < 2.1MHz */
         /* To reduce PLL lock time, keep Fint high (around 2 MHz) */
-        for (cur.regn = 1; cur.regn < REGN_MAX; ++cur.regn) {
+        for (cur.regn = 1; cur.regn < dsi.regn_max; ++cur.regn) {
                 if (cur.highfreq == 0)
                         cur.fint = cur.clkin / cur.regn;
                 else
                         cur.fint = cur.clkin / (2 * cur.regn);
 
-                if (cur.fint > FINT_MAX || cur.fint < FINT_MIN)
+                if (cur.fint > dsi.fint_max || cur.fint < dsi.fint_min)
                         continue;
 
                 /* DSIPHY(MHz) = (2 * regm / regn) * (clkin / (highfreq + 1)) */
-                for (cur.regm = 1; cur.regm < REGM_MAX; ++cur.regm) {
+                for (cur.regm = 1; cur.regm < dsi.regm_max; ++cur.regm) {
                         unsigned long a, b;
 
                         a = 2 * cur.regm * (cur.clkin/1000);
@@ -909,30 +1196,32 @@ retry:
                         if (cur.clkin4ddr > 1800 * 1000 * 1000)
                                 break;
 
-                        /* DSI1_PLL_FCLK(MHz) = DSIPHY(MHz) / regm3  < 173MHz */
-                        for (cur.regm3 = 1; cur.regm3 < REGM3_MAX;
-                                        ++cur.regm3) {
+                        /* dsi_pll_hsdiv_dispc_clk(MHz) =
+                         * DSIPHY(MHz) / regm_dispc  < 173MHz/186Mhz */
+                        for (cur.regm_dispc = 1; cur.regm_dispc < dsi.regm_dispc_max;
+                                        ++cur.regm_dispc) {
                                 struct dispc_clock_info cur_dispc;
-                                cur.dsi1_pll_fclk = cur.clkin4ddr / cur.regm3;
+                                cur.dsi_pll_hsdiv_dispc_clk =
+                                        cur.clkin4ddr / cur.regm_dispc;
 
                                 /* this will narrow down the search a bit,
                                  * but still give pixclocks below what was
                                  * requested */
-                                if (cur.dsi1_pll_fclk  < req_pck)
+                                if (cur.dsi_pll_hsdiv_dispc_clk  < req_pck)
                                         break;
 
-                                if (cur.dsi1_pll_fclk > DISPC_MAX_FCK)
+                                if (cur.dsi_pll_hsdiv_dispc_clk > max_dss_fck)
                                         continue;
 
                                 if (min_fck_per_pck &&
-                                        cur.dsi1_pll_fclk <
+                                        cur.dsi_pll_hsdiv_dispc_clk <
                                                 req_pck * min_fck_per_pck)
                                         continue;
 
                                 match = 1;
 
                                 dispc_find_clk_divs(is_tft, req_pck,
-                                                cur.dsi1_pll_fclk,
+                                                cur.dsi_pll_hsdiv_dispc_clk,
                                                 &cur_dispc);
 
                                 if (abs(cur_dispc.pck - req_pck) <
@@ -961,9 +1250,9 @@ found:
                 return -EINVAL;
         }
 
-        /* DSI2_PLL_FCLK (regm4) is not used */
-        best.regm4 = 0;
-        best.dsi2_pll_fclk = 0;
+        /* dsi_pll_hsdiv_dsi_clk (regm_dsi) is not used */
+        best.regm_dsi = 0;
+        best.dsi_pll_hsdiv_dsi_clk = 0;
 
         if (dsi_cinfo)
                 *dsi_cinfo = best;
@@ -982,23 +1271,27 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
         int r = 0;
         u32 l;
         int f;
+        u8 regn_start, regn_end, regm_start, regm_end;
+        u8 regm_dispc_start, regm_dispc_end, regm_dsi_start, regm_dsi_end;
 
         DSSDBGF();
 
         dsi.current_cinfo.fint = cinfo->fint;
         dsi.current_cinfo.clkin4ddr = cinfo->clkin4ddr;
-        dsi.current_cinfo.dsi1_pll_fclk = cinfo->dsi1_pll_fclk;
-        dsi.current_cinfo.dsi2_pll_fclk = cinfo->dsi2_pll_fclk;
+        dsi.current_cinfo.dsi_pll_hsdiv_dispc_clk =
+                        cinfo->dsi_pll_hsdiv_dispc_clk;
+        dsi.current_cinfo.dsi_pll_hsdiv_dsi_clk =
+                        cinfo->dsi_pll_hsdiv_dsi_clk;
 
         dsi.current_cinfo.regn = cinfo->regn;
         dsi.current_cinfo.regm = cinfo->regm;
-        dsi.current_cinfo.regm3 = cinfo->regm3;
-        dsi.current_cinfo.regm4 = cinfo->regm4;
+        dsi.current_cinfo.regm_dispc = cinfo->regm_dispc;
+        dsi.current_cinfo.regm_dsi = cinfo->regm_dsi;
 
         DSSDBG("DSI Fint %ld\n", cinfo->fint);
 
         DSSDBG("clkin (%s) rate %ld, highfreq %d\n",
-                        cinfo->use_dss2_fck ? "dss2_fck" : "pclkfree",
+                        cinfo->use_sys_clk ? "dss_sys_clk" : "pclkfree",
                         cinfo->clkin,
                         cinfo->highfreq);
 
@@ -1015,24 +1308,39 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 
         DSSDBG("Clock lane freq %ld Hz\n", cinfo->clkin4ddr / 4);
 
-        DSSDBG("regm3 = %d, dsi1_pll_fclk = %lu\n",
-                        cinfo->regm3, cinfo->dsi1_pll_fclk);
-        DSSDBG("regm4 = %d, dsi2_pll_fclk = %lu\n",
-                        cinfo->regm4, cinfo->dsi2_pll_fclk);
+        DSSDBG("regm_dispc = %d, %s (%s) = %lu\n", cinfo->regm_dispc,
+                dss_get_generic_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+                dss_feat_get_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+                cinfo->dsi_pll_hsdiv_dispc_clk);
+        DSSDBG("regm_dsi = %d, %s (%s) = %lu\n", cinfo->regm_dsi,
+                dss_get_generic_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+                dss_feat_get_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+                cinfo->dsi_pll_hsdiv_dsi_clk);
+
+        dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGN, &regn_start, &regn_end);
+        dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM, &regm_start, &regm_end);
+        dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM_DISPC, &regm_dispc_start,
+                        &regm_dispc_end);
+        dss_feat_get_reg_field(FEAT_REG_DSIPLL_REGM_DSI, &regm_dsi_start,
+                        &regm_dsi_end);
 
         REG_FLD_MOD(DSI_PLL_CONTROL, 0, 0, 0); /* DSI_PLL_AUTOMODE = manual */
 
         l = dsi_read_reg(DSI_PLL_CONFIGURATION1);
         l = FLD_MOD(l, 1, 0, 0);                /* DSI_PLL_STOPMODE */
-        l = FLD_MOD(l, cinfo->regn - 1, 7, 1);  /* DSI_PLL_REGN */
-        l = FLD_MOD(l, cinfo->regm, 18, 8);     /* DSI_PLL_REGM */
-        l = FLD_MOD(l, cinfo->regm3 > 0 ? cinfo->regm3 - 1 : 0,
-                        22, 19);                /* DSI_CLOCK_DIV */
-        l = FLD_MOD(l, cinfo->regm4 > 0 ? cinfo->regm4 - 1 : 0,
-                        26, 23);                /* DSIPROTO_CLOCK_DIV */
+        /* DSI_PLL_REGN */
+        l = FLD_MOD(l, cinfo->regn - 1, regn_start, regn_end);
+        /* DSI_PLL_REGM */
+        l = FLD_MOD(l, cinfo->regm, regm_start, regm_end);
+        /* DSI_CLOCK_DIV */
+        l = FLD_MOD(l, cinfo->regm_dispc > 0 ? cinfo->regm_dispc - 1 : 0,
+                        regm_dispc_start, regm_dispc_end);
+        /* DSIPROTO_CLOCK_DIV */
+        l = FLD_MOD(l, cinfo->regm_dsi > 0 ? cinfo->regm_dsi - 1 : 0,
+                        regm_dsi_start, regm_dsi_end);
         dsi_write_reg(DSI_PLL_CONFIGURATION1, l);
 
-        BUG_ON(cinfo->fint < 750000 || cinfo->fint > 2100000);
+        BUG_ON(cinfo->fint < dsi.fint_min || cinfo->fint > dsi.fint_max);
         if (cinfo->fint < 1000000)
                 f = 0x3;
         else if (cinfo->fint < 1250000)
@@ -1046,7 +1354,7 @@ int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo)
 
         l = dsi_read_reg(DSI_PLL_CONFIGURATION2);
         l = FLD_MOD(l, f, 4, 1);                /* DSI_PLL_FREQSEL */
-        l = FLD_MOD(l, cinfo->use_dss2_fck ? 0 : 1,
+        l = FLD_MOD(l, cinfo->use_sys_clk ? 0 : 1,
                         11, 11);                /* DSI_PLL_CLKSEL */
         l = FLD_MOD(l, cinfo->highfreq,
                         12, 12);                /* DSI_PLL_HIGHFREQ */
@@ -1101,6 +1409,26 @@ int dsi_pll_init(struct omap_dss_device *dssdev, bool enable_hsclk,
 
         DSSDBG("PLL init\n");
 
+#ifdef CONFIG_OMAP2_DSS_USE_DSI_PLL
+        /*
+         * HACK: this is just a quick hack to get the USE_DSI_PLL
+         * option working. USE_DSI_PLL is itself a big hack, and
+         * should be removed.
+         */
+        if (dsi.vdds_dsi_reg == NULL) {
+                struct regulator *vdds_dsi;
+
+                vdds_dsi = regulator_get(&dsi.pdev->dev, "vdds_dsi");
+
+                if (IS_ERR(vdds_dsi)) {
+                        DSSERR("can't get VDDS_DSI regulator\n");
+                        return PTR_ERR(vdds_dsi);
+                }
+
+                dsi.vdds_dsi_reg = vdds_dsi;
+        }
+#endif
+
         enable_clocks(1);
         dsi_enable_pll_clock(1);
 
@@ -1162,6 +1490,10 @@ void dsi_dump_clocks(struct seq_file *s)
 {
         int clksel;
         struct dsi_clock_info *cinfo = &dsi.current_cinfo;
+        enum dss_clk_source dispc_clk_src, dsi_clk_src;
+
+        dispc_clk_src = dss_get_dispc_clk_source();
+        dsi_clk_src = dss_get_dsi_clk_source();
 
         enable_clocks(1);
 
@@ -1171,30 +1503,34 @@ void dsi_dump_clocks(struct seq_file *s)
 
         seq_printf(s,   "dsi pll source = %s\n",
                         clksel == 0 ?
-                        "dss2_alwon_fclk" : "pclkfree");
+                        "dss_sys_clk" : "pclkfree");
 
         seq_printf(s,   "Fint\t\t%-16luregn %u\n", cinfo->fint, cinfo->regn);
 
         seq_printf(s,   "CLKIN4DDR\t%-16luregm %u\n",
                         cinfo->clkin4ddr, cinfo->regm);
 
-        seq_printf(s,   "dsi1_pll_fck\t%-16luregm3 %u\t(%s)\n",
-                        cinfo->dsi1_pll_fclk,
-                        cinfo->regm3,
-                        dss_get_dispc_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
+        seq_printf(s,   "%s (%s)\t%-16luregm_dispc %u\t(%s)\n",
+                        dss_get_generic_clk_source_name(dispc_clk_src),
+                        dss_feat_get_clk_source_name(dispc_clk_src),
+                        cinfo->dsi_pll_hsdiv_dispc_clk,
+                        cinfo->regm_dispc,
+                        dispc_clk_src == DSS_CLK_SRC_FCK ?
                         "off" : "on");
 
-        seq_printf(s,   "dsi2_pll_fck\t%-16luregm4 %u\t(%s)\n",
-                        cinfo->dsi2_pll_fclk,
-                        cinfo->regm4,
-                        dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
+        seq_printf(s,   "%s (%s)\t%-16luregm_dsi %u\t(%s)\n",
+                        dss_get_generic_clk_source_name(dsi_clk_src),
+                        dss_feat_get_clk_source_name(dsi_clk_src),
+                        cinfo->dsi_pll_hsdiv_dsi_clk,
+                        cinfo->regm_dsi,
+                        dsi_clk_src == DSS_CLK_SRC_FCK ?
                         "off" : "on");
 
         seq_printf(s,   "- DSI -\n");
 
-        seq_printf(s,   "dsi fclk source = %s\n",
-                        dss_get_dsi_clk_source() == DSS_SRC_DSS1_ALWON_FCLK ?
-                        "dss1_alwon_fclk" : "dsi2_pll_fclk");
+        seq_printf(s,   "dsi fclk source = %s (%s)\n",
+                        dss_get_generic_clk_source_name(dsi_clk_src),
+                        dss_feat_get_clk_source_name(dsi_clk_src));
 
         seq_printf(s,   "DSI_FCLK\t%lu\n", dsi_fclk_rate());
 
@@ -1306,7 +1642,7 @@ void dsi_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dsi_read_reg(r))
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         DUMPREG(DSI_REVISION);
         DUMPREG(DSI_SYSCONFIG);
@@ -1378,7 +1714,7 @@ void dsi_dump_regs(struct seq_file *s)
         DUMPREG(DSI_PLL_CONFIGURATION1);
         DUMPREG(DSI_PLL_CONFIGURATION2);
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
@@ -1622,20 +1958,6 @@ static int _dsi_reset(void)
         return _dsi_wait_reset();
 }
 
-static void dsi_reset_tx_fifo(int channel)
-{
-        u32 mask;
-        u32 l;
-
-        /* set fifosize of the channel to 0, then return the old size */
-        l = dsi_read_reg(DSI_TX_FIFO_VC_SIZE);
-
-        mask = FLD_MASK((8 * channel) + 7, (8 * channel) + 4);
-        dsi_write_reg(DSI_TX_FIFO_VC_SIZE, l & ~mask);
-
-        dsi_write_reg(DSI_TX_FIFO_VC_SIZE, l);
-}
-
 static void dsi_config_tx_fifo(enum fifo_size size1, enum fifo_size size2,
                 enum fifo_size size3, enum fifo_size size4)
 {
@@ -1753,8 +2075,6 @@ static void dsi_vc_initial_config(int channel)
         r = FLD_MOD(r, 4, 23, 21); /* DMA_TX_REQ_NB = no dma */
 
         dsi_write_reg(DSI_VC_CTRL(channel), r);
-
-        dsi.vc[channel].mode = DSI_VC_MODE_L4;
 }
 
 static int dsi_vc_config_l4(int channel)
@@ -1922,33 +2242,44 @@ static int dsi_vc_send_bta(int channel)
 
 int dsi_vc_send_bta_sync(int channel)
 {
+        DECLARE_COMPLETION_ONSTACK(completion);
         int r = 0;
         u32 err;
 
-        INIT_COMPLETION(dsi.bta_completion);
+        r = dsi_register_isr_vc(channel, dsi_completion_handler,
+                        &completion, DSI_VC_IRQ_BTA);
+        if (r)
+                goto err0;
 
-        dsi_vc_enable_bta_irq(channel);
+        r = dsi_register_isr(dsi_completion_handler, &completion,
+                        DSI_IRQ_ERROR_MASK);
+        if (r)
+                goto err1;
 
         r = dsi_vc_send_bta(channel);
         if (r)
-                goto err;
+                goto err2;
 
-        if (wait_for_completion_timeout(&dsi.bta_completion,
+        if (wait_for_completion_timeout(&completion,
                                 msecs_to_jiffies(500)) == 0) {
                 DSSERR("Failed to receive BTA\n");
                 r = -EIO;
-                goto err;
+                goto err2;
         }
 
         err = dsi_get_errors();
         if (err) {
                 DSSERR("Error while sending BTA: %x\n", err);
                 r = -EIO;
-                goto err;
+                goto err2;
         }
-err:
-        dsi_vc_disable_bta_irq(channel);
-
+err2:
+        dsi_unregister_isr(dsi_completion_handler, &completion,
+                        DSI_IRQ_ERROR_MASK);
+err1:
+        dsi_unregister_isr_vc(channel, dsi_completion_handler,
+                        &completion, DSI_VC_IRQ_BTA);
+err0:
         return r;
 }
 EXPORT_SYMBOL(dsi_vc_send_bta_sync);
@@ -1961,7 +2292,7 @@ static inline void dsi_vc_write_long_header(int channel, u8 data_type,
 
         WARN_ON(!dsi_bus_is_locked());
 
-        data_id = data_type | channel << 6;
+        data_id = data_type | dsi.vc[channel].vc_id << 6;
 
         val = FLD_VAL(data_id, 7, 0) | FLD_VAL(len, 23, 8) |
                 FLD_VAL(ecc, 31, 24);
@@ -2064,7 +2395,7 @@ static int dsi_vc_send_short(int channel, u8 data_type, u16 data, u8 ecc)
                 return -EINVAL;
         }
 
-        data_id = data_type | channel << 6;
+        data_id = data_type | dsi.vc[channel].vc_id << 6;
 
         r = (data_id << 0) | (data << 8) | (ecc << 24);
 
@@ -2762,19 +3093,20 @@ static void dsi_te_timeout(unsigned long arg)
 }
 #endif
 
+static void dsi_framedone_bta_callback(void *data, u32 mask);
+
 static void dsi_handle_framedone(int error)
 {
         const int channel = dsi.update_channel;
 
-        cancel_delayed_work(&dsi.framedone_timeout_work);
+        dsi_unregister_isr_vc(channel, dsi_framedone_bta_callback,
+                        NULL, DSI_VC_IRQ_BTA);
 
-        dsi_vc_disable_bta_irq(channel);
+        cancel_delayed_work(&dsi.framedone_timeout_work);
 
         /* SIDLEMODE back to smart-idle */
         dispc_enable_sidle();
 
-        dsi.bta_callback = NULL;
-
         if (dsi.te_enabled) {
                 /* enable LP_RX_TO again after the TE */
                 REG_FLD_MOD(DSI_TIMING2, 1, 15, 15); /* LP_RX_TO */
@@ -2808,7 +3140,7 @@ static void dsi_framedone_timeout_work_callback(struct work_struct *work)
         dsi_handle_framedone(-ETIMEDOUT);
 }
 
-static void dsi_framedone_bta_callback(void)
+static void dsi_framedone_bta_callback(void *data, u32 mask)
 {
         dsi_handle_framedone(0);
 
@@ -2848,15 +3180,19 @@ static void dsi_framedone_irq_callback(void *data, u32 mask)
          * asynchronously.
          * */
 
-        dsi.bta_callback = dsi_framedone_bta_callback;
-
-        barrier();
-
-        dsi_vc_enable_bta_irq(channel);
+        r = dsi_register_isr_vc(channel, dsi_framedone_bta_callback,
+                        NULL, DSI_VC_IRQ_BTA);
+        if (r) {
+                DSSERR("Failed to register BTA ISR\n");
+                dsi_handle_framedone(-EIO);
+                return;
+        }
 
         r = dsi_vc_send_bta(channel);
         if (r) {
                 DSSERR("BTA after framedone failed\n");
+                dsi_unregister_isr_vc(channel, dsi_framedone_bta_callback,
+                                NULL, DSI_VC_IRQ_BTA);
                 dsi_handle_framedone(-EIO);
         }
 }
@@ -2984,12 +3320,12 @@ static int dsi_configure_dsi_clocks(struct omap_dss_device *dssdev)
         struct dsi_clock_info cinfo;
         int r;
 
-        /* we always use DSS2_FCK as input clock */
-        cinfo.use_dss2_fck = true;
+        /* we always use DSS_CLK_SYSCK as input clock */
+        cinfo.use_sys_clk = true;
         cinfo.regn  = dssdev->phy.dsi.div.regn;
         cinfo.regm  = dssdev->phy.dsi.div.regm;
-        cinfo.regm3 = dssdev->phy.dsi.div.regm3;
-        cinfo.regm4 = dssdev->phy.dsi.div.regm4;
+        cinfo.regm_dispc = dssdev->phy.dsi.div.regm_dispc;
+        cinfo.regm_dsi = dssdev->phy.dsi.div.regm_dsi;
         r = dsi_calc_clock_rates(dssdev, &cinfo);
         if (r) {
                 DSSERR("Failed to calc dsi clocks\n");
@@ -3011,7 +3347,7 @@ static int dsi_configure_dispc_clocks(struct omap_dss_device *dssdev)
         int r;
         unsigned long long fck;
 
-        fck = dsi_get_dsi1_pll_rate();
+        fck = dsi_get_pll_hsdiv_dispc_rate();
 
         dispc_cinfo.lck_div = dssdev->phy.dsi.div.lck_div;
         dispc_cinfo.pck_div = dssdev->phy.dsi.div.pck_div;
@@ -3045,8 +3381,8 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
         if (r)
                 goto err1;
 
-        dss_select_dispc_clk_source(DSS_SRC_DSI1_PLL_FCLK);
-        dss_select_dsi_clk_source(DSS_SRC_DSI2_PLL_FCLK);
+        dss_select_dispc_clk_source(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC);
+        dss_select_dsi_clk_source(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI);
 
         DSSDBG("PLL OK\n");
 
@@ -3082,8 +3418,8 @@ static int dsi_display_init_dsi(struct omap_dss_device *dssdev)
 err3:
         dsi_complexio_uninit();
 err2:
-        dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-        dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
+        dss_select_dispc_clk_source(DSS_CLK_SRC_FCK);
+        dss_select_dsi_clk_source(DSS_CLK_SRC_FCK);
 err1:
         dsi_pll_uninit();
 err0:
@@ -3099,8 +3435,8 @@ static void dsi_display_uninit_dsi(struct omap_dss_device *dssdev)
         dsi_vc_enable(2, 0);
         dsi_vc_enable(3, 0);
 
-        dss_select_dispc_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
-        dss_select_dsi_clk_source(DSS_SRC_DSS1_ALWON_FCLK);
+        dss_select_dispc_clk_source(DSS_CLK_SRC_FCK);
+        dss_select_dsi_clk_source(DSS_CLK_SRC_FCK);
         dsi_complexio_uninit();
         dsi_pll_uninit();
 }
@@ -3220,29 +3556,107 @@ int dsi_init_display(struct omap_dss_device *dssdev)
         dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE |
                 OMAP_DSS_DISPLAY_CAP_TEAR_ELIM;
 
-        dsi.vc[0].dssdev = dssdev;
-        dsi.vc[1].dssdev = dssdev;
+        if (dsi.vdds_dsi_reg == NULL) {
+                struct regulator *vdds_dsi;
+
+                vdds_dsi = regulator_get(&dsi.pdev->dev, "vdds_dsi");
+
+                if (IS_ERR(vdds_dsi)) {
+                        DSSERR("can't get VDDS_DSI regulator\n");
+                        return PTR_ERR(vdds_dsi);
+                }
+
+                dsi.vdds_dsi_reg = vdds_dsi;
+        }
 
         return 0;
 }
 
-void dsi_wait_dsi1_pll_active(void)
+int omap_dsi_request_vc(struct omap_dss_device *dssdev, int *channel)
+{
+        int i;
+
+        for (i = 0; i < ARRAY_SIZE(dsi.vc); i++) {
+                if (!dsi.vc[i].dssdev) {
+                        dsi.vc[i].dssdev = dssdev;
+                        *channel = i;
+                        return 0;
+                }
+        }
+
+        DSSERR("cannot get VC for display %s", dssdev->name);
+        return -ENOSPC;
+}
+EXPORT_SYMBOL(omap_dsi_request_vc);
+
+int omap_dsi_set_vc_id(struct omap_dss_device *dssdev, int channel, int vc_id)
+{
+        if (vc_id < 0 || vc_id > 3) {
+                DSSERR("VC ID out of range\n");
+                return -EINVAL;
+        }
+
+        if (channel < 0 || channel > 3) {
+                DSSERR("Virtual Channel out of range\n");
+                return -EINVAL;
+        }
+
+        if (dsi.vc[channel].dssdev != dssdev) {
+                DSSERR("Virtual Channel not allocated to display %s\n",
+                        dssdev->name);
+                return -EINVAL;
+        }
+
+        dsi.vc[channel].vc_id = vc_id;
+
+        return 0;
+}
+EXPORT_SYMBOL(omap_dsi_set_vc_id);
+
+void omap_dsi_release_vc(struct omap_dss_device *dssdev, int channel)
+{
+        if ((channel >= 0 && channel <= 3) &&
+                dsi.vc[channel].dssdev == dssdev) {
+                dsi.vc[channel].dssdev = NULL;
+                dsi.vc[channel].vc_id = 0;
+        }
+}
+EXPORT_SYMBOL(omap_dsi_release_vc);
+
+void dsi_wait_pll_hsdiv_dispc_active(void)
 {
         if (wait_for_bit_change(DSI_PLL_STATUS, 7, 1) != 1)
-                DSSERR("DSI1 PLL clock not active\n");
+                DSSERR("%s (%s) not active\n",
+                        dss_get_generic_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC),
+                        dss_feat_get_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC));
 }
 
-void dsi_wait_dsi2_pll_active(void)
+void dsi_wait_pll_hsdiv_dsi_active(void)
 {
         if (wait_for_bit_change(DSI_PLL_STATUS, 8, 1) != 1)
-                DSSERR("DSI2 PLL clock not active\n");
+                DSSERR("%s (%s) not active\n",
+                        dss_get_generic_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI),
+                        dss_feat_get_clk_source_name(DSS_CLK_SRC_DSI_PLL_HSDIV_DSI));
+}
+
+static void dsi_calc_clock_param_ranges(void)
+{
+        dsi.regn_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGN);
+        dsi.regm_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM);
+        dsi.regm_dispc_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM_DISPC);
+        dsi.regm_dsi_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_REGM_DSI);
+        dsi.fint_min = dss_feat_get_param_min(FEAT_PARAM_DSIPLL_FINT);
+        dsi.fint_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_FINT);
+        dsi.lpdiv_max = dss_feat_get_param_max(FEAT_PARAM_DSIPLL_LPDIV);
 }
 
-int dsi_init(struct platform_device *pdev)
+static int dsi_init(struct platform_device *pdev)
 {
         u32 rev;
-        int r;
+        int r, i;
+        struct resource *dsi_mem;
 
+        spin_lock_init(&dsi.irq_lock);
         spin_lock_init(&dsi.errors_lock);
         dsi.errors = 0;
 
@@ -3251,8 +3665,6 @@ int dsi_init(struct platform_device *pdev)
         dsi.irq_stats.last_reset = jiffies;
 #endif
 
-        init_completion(&dsi.bta_completion);
-
         mutex_init(&dsi.lock);
         sema_init(&dsi.bus_lock, 1);
 
@@ -3268,24 +3680,45 @@ int dsi_init(struct platform_device *pdev)
         dsi.te_timer.function = dsi_te_timeout;
         dsi.te_timer.data = 0;
 #endif
-        dsi.base = ioremap(DSI_BASE, DSI_SZ_REGS);
+        dsi_mem = platform_get_resource(dsi.pdev, IORESOURCE_MEM, 0);
+        if (!dsi_mem) {
+                DSSERR("can't get IORESOURCE_MEM DSI\n");
+                r = -EINVAL;
+                goto err1;
+        }
+        dsi.base = ioremap(dsi_mem->start, resource_size(dsi_mem));
         if (!dsi.base) {
                 DSSERR("can't ioremap DSI\n");
                 r = -ENOMEM;
                 goto err1;
         }
+        dsi.irq = platform_get_irq(dsi.pdev, 0);
+        if (dsi.irq < 0) {
+                DSSERR("platform_get_irq failed\n");
+                r = -ENODEV;
+                goto err2;
+        }
 
-        dsi.vdds_dsi_reg = dss_get_vdds_dsi();
-        if (IS_ERR(dsi.vdds_dsi_reg)) {
-                DSSERR("can't get VDDS_DSI regulator\n");
-                r = PTR_ERR(dsi.vdds_dsi_reg);
+        r = request_irq(dsi.irq, omap_dsi_irq_handler, IRQF_SHARED,
+                "OMAP DSI1", dsi.pdev);
+        if (r < 0) {
+                DSSERR("request_irq failed\n");
                 goto err2;
         }
 
+        /* DSI VCs initialization */
+        for (i = 0; i < ARRAY_SIZE(dsi.vc); i++) {
+                dsi.vc[i].mode = DSI_VC_MODE_L4;
+                dsi.vc[i].dssdev = NULL;
+                dsi.vc[i].vc_id = 0;
+        }
+
+        dsi_calc_clock_param_ranges();
+
         enable_clocks(1);
 
         rev = dsi_read_reg(DSI_REVISION);
-        printk(KERN_INFO "OMAP DSI rev %d.%d\n",
+        dev_dbg(&pdev->dev, "OMAP DSI rev %d.%d\n",
                FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
 
         enable_clocks(0);
@@ -3298,8 +3731,14 @@ err1:
         return r;
 }
 
-void dsi_exit(void)
+static void dsi_exit(void)
 {
+        if (dsi.vdds_dsi_reg != NULL) {
+                regulator_put(dsi.vdds_dsi_reg);
+                dsi.vdds_dsi_reg = NULL;
+        }
+
+        free_irq(dsi.irq, dsi.pdev);
         iounmap(dsi.base);
 
         destroy_workqueue(dsi.workqueue);
@@ -3307,3 +3746,41 @@ void dsi_exit(void)
         DSSDBG("omap_dsi_exit\n");
 }
 
+/* DSI1 HW IP initialisation */
+static int omap_dsi1hw_probe(struct platform_device *pdev)
+{
+        int r;
+        dsi.pdev = pdev;
+        r = dsi_init(pdev);
+        if (r) {
+                DSSERR("Failed to initialize DSI\n");
+                goto err_dsi;
+        }
+err_dsi:
+        return r;
+}
+
+static int omap_dsi1hw_remove(struct platform_device *pdev)
+{
+        dsi_exit();
+        return 0;
+}
+
+static struct platform_driver omap_dsi1hw_driver = {
+        .probe          = omap_dsi1hw_probe,
+        .remove         = omap_dsi1hw_remove,
+        .driver         = {
+                .name   = "omapdss_dsi1",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int dsi_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dsi1hw_driver);
+}
+
+void dsi_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omap_dsi1hw_driver);
+}
diff --git a/drivers/video/omap2/dss/dss.c b/drivers/video/omap2/dss/dss.c
index 77c3621c9171..3f1fee63c678 100644
--- a/drivers/video/omap2/dss/dss.c
+++ b/drivers/video/omap2/dss/dss.c
@@ -26,14 +26,13 @@
 #include <linux/io.h>
 #include <linux/err.h>
 #include <linux/delay.h>
-#include <linux/interrupt.h>
 #include <linux/seq_file.h>
 #include <linux/clk.h>
 
 #include <plat/display.h>
+#include <plat/clock.h>
 #include "dss.h"
-
-#define DSS_BASE                        0x48050000
+#include "dss_features.h"
 
 #define DSS_SZ_REGS                     SZ_512
 
@@ -59,9 +58,17 @@ struct dss_reg {
         dss_write_reg(idx, FLD_MOD(dss_read_reg(idx), val, start, end))
 
 static struct {
+        struct platform_device *pdev;
         void __iomem    *base;
+        int             ctx_id;
 
         struct clk      *dpll4_m4_ck;
+        struct clk      *dss_ick;
+        struct clk      *dss_fck;
+        struct clk      *dss_sys_clk;
+        struct clk      *dss_tv_fck;
+        struct clk      *dss_video_fck;
+        unsigned        num_clks_enabled;
 
         unsigned long   cache_req_pck;
         unsigned long   cache_prate;
@@ -70,10 +77,22 @@ static struct {
 
         enum dss_clk_source dsi_clk_source;
         enum dss_clk_source dispc_clk_source;
+        enum dss_clk_source lcd_clk_source[MAX_DSS_LCD_MANAGERS];
 
         u32             ctx[DSS_SZ_REGS / sizeof(u32)];
 } dss;
 
+static const char * const dss_generic_clk_source_names[] = {
+        [DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]       = "DSI_PLL_HSDIV_DISPC",
+        [DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]         = "DSI_PLL_HSDIV_DSI",
+        [DSS_CLK_SRC_FCK]                       = "DSS_FCK",
+};
+
+static void dss_clk_enable_all_no_ctx(void);
+static void dss_clk_disable_all_no_ctx(void);
+static void dss_clk_enable_no_ctx(enum dss_clock clks);
+static void dss_clk_disable_no_ctx(enum dss_clock clks);
+
 static int _omap_dss_wait_reset(void);
 
 static inline void dss_write_reg(const struct dss_reg idx, u32 val)
@@ -99,10 +118,11 @@ void dss_save_context(void)
         SR(SYSCONFIG);
         SR(CONTROL);
 
-#ifdef CONFIG_OMAP2_DSS_SDI
-        SR(SDI_CONTROL);
-        SR(PLL_CONTROL);
-#endif
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                SR(SDI_CONTROL);
+                SR(PLL_CONTROL);
+        }
 }
 
 void dss_restore_context(void)
@@ -113,10 +133,11 @@ void dss_restore_context(void)
         RR(SYSCONFIG);
         RR(CONTROL);
 
-#ifdef CONFIG_OMAP2_DSS_SDI
-        RR(SDI_CONTROL);
-        RR(PLL_CONTROL);
-#endif
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                RR(SDI_CONTROL);
+                RR(PLL_CONTROL);
+        }
 }
 
 #undef SR
@@ -209,66 +230,96 @@ void dss_sdi_disable(void)
         REG_FLD_MOD(DSS_PLL_CONTROL, 0, 18, 18); /* SDI_PLL_SYSRESET */
 }
 
+const char *dss_get_generic_clk_source_name(enum dss_clk_source clk_src)
+{
+        return dss_generic_clk_source_names[clk_src];
+}
+
 void dss_dump_clocks(struct seq_file *s)
 {
         unsigned long dpll4_ck_rate;
         unsigned long dpll4_m4_ck_rate;
+        const char *fclk_name, *fclk_real_name;
+        unsigned long fclk_rate;
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
-
-        dpll4_ck_rate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-        dpll4_m4_ck_rate = clk_get_rate(dss.dpll4_m4_ck);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         seq_printf(s, "- DSS -\n");
 
-        seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
+        fclk_name = dss_get_generic_clk_source_name(DSS_CLK_SRC_FCK);
+        fclk_real_name = dss_feat_get_clk_source_name(DSS_CLK_SRC_FCK);
+        fclk_rate = dss_clk_get_rate(DSS_CLK_FCK);
 
-        if (cpu_is_omap3630())
-                seq_printf(s, "dss1_alwon_fclk = %lu / %lu  = %lu\n",
-                        dpll4_ck_rate,
-                        dpll4_ck_rate / dpll4_m4_ck_rate,
-                        dss_clk_get_rate(DSS_CLK_FCK1));
-        else
-                seq_printf(s, "dss1_alwon_fclk = %lu / %lu * 2 = %lu\n",
-                        dpll4_ck_rate,
-                        dpll4_ck_rate / dpll4_m4_ck_rate,
-                        dss_clk_get_rate(DSS_CLK_FCK1));
+        if (dss.dpll4_m4_ck) {
+                dpll4_ck_rate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
+                dpll4_m4_ck_rate = clk_get_rate(dss.dpll4_m4_ck);
+
+                seq_printf(s, "dpll4_ck %lu\n", dpll4_ck_rate);
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                if (cpu_is_omap3630() || cpu_is_omap44xx())
+                        seq_printf(s, "%s (%s) = %lu / %lu  = %lu\n",
+                                        fclk_name, fclk_real_name,
+                                        dpll4_ck_rate,
+                                        dpll4_ck_rate / dpll4_m4_ck_rate,
+                                        fclk_rate);
+                else
+                        seq_printf(s, "%s (%s) = %lu / %lu * 2 = %lu\n",
+                                        fclk_name, fclk_real_name,
+                                        dpll4_ck_rate,
+                                        dpll4_ck_rate / dpll4_m4_ck_rate,
+                                        fclk_rate);
+        } else {
+                seq_printf(s, "%s (%s) = %lu\n",
+                                fclk_name, fclk_real_name,
+                                fclk_rate);
+        }
+
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 void dss_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, dss_read_reg(r))
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         DUMPREG(DSS_REVISION);
         DUMPREG(DSS_SYSCONFIG);
         DUMPREG(DSS_SYSSTATUS);
         DUMPREG(DSS_IRQSTATUS);
         DUMPREG(DSS_CONTROL);
-        DUMPREG(DSS_SDI_CONTROL);
-        DUMPREG(DSS_PLL_CONTROL);
-        DUMPREG(DSS_SDI_STATUS);
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        if (dss_feat_get_supported_displays(OMAP_DSS_CHANNEL_LCD) &
+                        OMAP_DISPLAY_TYPE_SDI) {
+                DUMPREG(DSS_SDI_CONTROL);
+                DUMPREG(DSS_PLL_CONTROL);
+                DUMPREG(DSS_SDI_STATUS);
+        }
+
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
 void dss_select_dispc_clk_source(enum dss_clk_source clk_src)
 {
         int b;
+        u8 start, end;
+
+        switch (clk_src) {
+        case DSS_CLK_SRC_FCK:
+                b = 0;
+                break;
+        case DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                b = 1;
+                dsi_wait_pll_hsdiv_dispc_active();
+                break;
+        default:
+                BUG();
+        }
 
-        BUG_ON(clk_src != DSS_SRC_DSI1_PLL_FCLK &&
-                        clk_src != DSS_SRC_DSS1_ALWON_FCLK);
-
-        b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
-
-        if (clk_src == DSS_SRC_DSI1_PLL_FCLK)
-                dsi_wait_dsi1_pll_active();
+        dss_feat_get_reg_field(FEAT_REG_DISPC_CLK_SWITCH, &start, &end);
 
-        REG_FLD_MOD(DSS_CONTROL, b, 0, 0);      /* DISPC_CLK_SWITCH */
+        REG_FLD_MOD(DSS_CONTROL, b, start, end);        /* DISPC_CLK_SWITCH */
 
         dss.dispc_clk_source = clk_src;
 }
@@ -277,19 +328,51 @@ void dss_select_dsi_clk_source(enum dss_clk_source clk_src)
 {
         int b;
 
-        BUG_ON(clk_src != DSS_SRC_DSI2_PLL_FCLK &&
-                        clk_src != DSS_SRC_DSS1_ALWON_FCLK);
-
-        b = clk_src == DSS_SRC_DSS1_ALWON_FCLK ? 0 : 1;
-
-        if (clk_src == DSS_SRC_DSI2_PLL_FCLK)
-                dsi_wait_dsi2_pll_active();
+        switch (clk_src) {
+        case DSS_CLK_SRC_FCK:
+                b = 0;
+                break;
+        case DSS_CLK_SRC_DSI_PLL_HSDIV_DSI:
+                b = 1;
+                dsi_wait_pll_hsdiv_dsi_active();
+                break;
+        default:
+                BUG();
+        }
 
         REG_FLD_MOD(DSS_CONTROL, b, 1, 1);      /* DSI_CLK_SWITCH */
 
         dss.dsi_clk_source = clk_src;
 }
 
+void dss_select_lcd_clk_source(enum omap_channel channel,
+                enum dss_clk_source clk_src)
+{
+        int b, ix, pos;
+
+        if (!dss_has_feature(FEAT_LCD_CLK_SRC))
+                return;
+
+        switch (clk_src) {
+        case DSS_CLK_SRC_FCK:
+                b = 0;
+                break;
+        case DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC:
+                BUG_ON(channel != OMAP_DSS_CHANNEL_LCD);
+                b = 1;
+                dsi_wait_pll_hsdiv_dispc_active();
+                break;
+        default:
+                BUG();
+        }
+
+        pos = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 12;
+        REG_FLD_MOD(DSS_CONTROL, b, pos, pos);  /* LCDx_CLK_SWITCH */
+
+        ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 1;
+        dss.lcd_clk_source[ix] = clk_src;
+}
+
 enum dss_clk_source dss_get_dispc_clk_source(void)
 {
         return dss.dispc_clk_source;
@@ -300,34 +383,52 @@ enum dss_clk_source dss_get_dsi_clk_source(void)
         return dss.dsi_clk_source;
 }
 
+enum dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel)
+{
+        int ix = channel == OMAP_DSS_CHANNEL_LCD ? 0 : 1;
+        return dss.lcd_clk_source[ix];
+}
+
 /* calculate clock rates using dividers in cinfo */
 int dss_calc_clock_rates(struct dss_clock_info *cinfo)
 {
-        unsigned long prate;
+        if (dss.dpll4_m4_ck) {
+                unsigned long prate;
+                u16 fck_div_max = 16;
 
-        if (cinfo->fck_div > (cpu_is_omap3630() ? 32 : 16) ||
-                                                cinfo->fck_div == 0)
-                return -EINVAL;
+                if (cpu_is_omap3630() || cpu_is_omap44xx())
+                        fck_div_max = 32;
+
+                if (cinfo->fck_div > fck_div_max || cinfo->fck_div == 0)
+                        return -EINVAL;
 
-        prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
+                prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
 
-        cinfo->fck = prate / cinfo->fck_div;
+                cinfo->fck = prate / cinfo->fck_div;
+        } else {
+                if (cinfo->fck_div != 0)
+                        return -EINVAL;
+                cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK);
+        }
 
         return 0;
 }
 
 int dss_set_clock_div(struct dss_clock_info *cinfo)
 {
-        unsigned long prate;
-        int r;
+        if (dss.dpll4_m4_ck) {
+                unsigned long prate;
+                int r;
 
-        if (cpu_is_omap34xx()) {
                 prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
                 DSSDBG("dpll4_m4 = %ld\n", prate);
 
                 r = clk_set_rate(dss.dpll4_m4_ck, prate / cinfo->fck_div);
                 if (r)
                         return r;
+        } else {
+                if (cinfo->fck_div != 0)
+                        return -EINVAL;
         }
 
         DSSDBG("fck = %ld (%d)\n", cinfo->fck, cinfo->fck_div);
@@ -337,12 +438,14 @@ int dss_set_clock_div(struct dss_clock_info *cinfo)
 
 int dss_get_clock_div(struct dss_clock_info *cinfo)
 {
-        cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK1);
+        cinfo->fck = dss_clk_get_rate(DSS_CLK_FCK);
 
-        if (cpu_is_omap34xx()) {
+        if (dss.dpll4_m4_ck) {
                 unsigned long prate;
+
                 prate = clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
-                if (cpu_is_omap3630())
+
+                if (cpu_is_omap3630() || cpu_is_omap44xx())
                         cinfo->fck_div = prate / (cinfo->fck);
                 else
                         cinfo->fck_div = prate / (cinfo->fck / 2);
@@ -355,7 +458,7 @@ int dss_get_clock_div(struct dss_clock_info *cinfo)
 
 unsigned long dss_get_dpll4_rate(void)
 {
-        if (cpu_is_omap34xx())
+        if (dss.dpll4_m4_ck)
                 return clk_get_rate(clk_get_parent(dss.dpll4_m4_ck));
         else
                 return 0;
@@ -369,16 +472,18 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
         struct dss_clock_info best_dss;
         struct dispc_clock_info best_dispc;
 
-        unsigned long fck;
+        unsigned long fck, max_dss_fck;
 
-        u16 fck_div;
+        u16 fck_div, fck_div_max = 16;
 
         int match = 0;
         int min_fck_per_pck;
 
         prate = dss_get_dpll4_rate();
 
-        fck = dss_clk_get_rate(DSS_CLK_FCK1);
+        max_dss_fck = dss_feat_get_param_max(FEAT_PARAM_DSS_FCK);
+
+        fck = dss_clk_get_rate(DSS_CLK_FCK);
         if (req_pck == dss.cache_req_pck &&
                         ((cpu_is_omap34xx() && prate == dss.cache_prate) ||
                          dss.cache_dss_cinfo.fck == fck)) {
@@ -391,7 +496,7 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
         min_fck_per_pck = CONFIG_OMAP2_DSS_MIN_FCK_PER_PCK;
 
         if (min_fck_per_pck &&
-                req_pck * min_fck_per_pck > DISPC_MAX_FCK) {
+                req_pck * min_fck_per_pck > max_dss_fck) {
                 DSSERR("Requested pixel clock not possible with the current "
                                 "OMAP2_DSS_MIN_FCK_PER_PCK setting. Turning "
                                 "the constraint off.\n");
@@ -402,10 +507,10 @@ retry:
         memset(&best_dss, 0, sizeof(best_dss));
         memset(&best_dispc, 0, sizeof(best_dispc));
 
-        if (cpu_is_omap24xx()) {
+        if (dss.dpll4_m4_ck == NULL) {
                 struct dispc_clock_info cur_dispc;
                 /* XXX can we change the clock on omap2? */
-                fck = dss_clk_get_rate(DSS_CLK_FCK1);
+                fck = dss_clk_get_rate(DSS_CLK_FCK);
                 fck_div = 1;
 
                 dispc_find_clk_divs(is_tft, req_pck, fck, &cur_dispc);
@@ -417,17 +522,19 @@ retry:
                 best_dispc = cur_dispc;
 
                 goto found;
-        } else if (cpu_is_omap34xx()) {
-                for (fck_div = (cpu_is_omap3630() ? 32 : 16);
-                                        fck_div > 0; --fck_div) {
+        } else {
+                if (cpu_is_omap3630() || cpu_is_omap44xx())
+                        fck_div_max = 32;
+
+                for (fck_div = fck_div_max; fck_div > 0; --fck_div) {
                         struct dispc_clock_info cur_dispc;
 
-                        if (cpu_is_omap3630())
+                        if (fck_div_max == 32)
                                 fck = prate / fck_div;
                         else
                                 fck = prate / fck_div * 2;
 
-                        if (fck > DISPC_MAX_FCK)
+                        if (fck > max_dss_fck)
                                 continue;
 
                         if (min_fck_per_pck &&
@@ -450,8 +557,6 @@ retry:
                                         goto found;
                         }
                 }
-        } else {
-                BUG();
         }
 
 found:
@@ -482,31 +587,6 @@ found:
         return 0;
 }
 
-
-
-static irqreturn_t dss_irq_handler_omap2(int irq, void *arg)
-{
-        dispc_irq_handler();
-
-        return IRQ_HANDLED;
-}
-
-static irqreturn_t dss_irq_handler_omap3(int irq, void *arg)
-{
-        u32 irqstatus;
-
-        irqstatus = dss_read_reg(DSS_IRQSTATUS);
-
-        if (irqstatus & (1<<0)) /* DISPC_IRQ */
-                dispc_irq_handler();
-#ifdef CONFIG_OMAP2_DSS_DSI
-        if (irqstatus & (1<<1)) /* DSI_IRQ */
-                dsi_irq_handler();
-#endif
-
-        return IRQ_HANDLED;
-}
-
 static int _omap_dss_wait_reset(void)
 {
         int t = 0;
@@ -549,34 +629,45 @@ void dss_set_dac_pwrdn_bgz(bool enable)
         REG_FLD_MOD(DSS_CONTROL, enable, 5, 5); /* DAC Power-Down Control */
 }
 
-int dss_init(bool skip_init)
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select hdmi)
+{
+        REG_FLD_MOD(DSS_CONTROL, hdmi, 15, 15); /* VENC_HDMI_SWITCH */
+}
+
+static int dss_init(void)
 {
         int r;
         u32 rev;
+        struct resource *dss_mem;
+        struct clk *dpll4_m4_ck;
 
-        dss.base = ioremap(DSS_BASE, DSS_SZ_REGS);
+        dss_mem = platform_get_resource(dss.pdev, IORESOURCE_MEM, 0);
+        if (!dss_mem) {
+                DSSERR("can't get IORESOURCE_MEM DSS\n");
+                r = -EINVAL;
+                goto fail0;
+        }
+        dss.base = ioremap(dss_mem->start, resource_size(dss_mem));
         if (!dss.base) {
                 DSSERR("can't ioremap DSS\n");
                 r = -ENOMEM;
                 goto fail0;
         }
 
-        if (!skip_init) {
-                /* disable LCD and DIGIT output. This seems to fix the synclost
-                 * problem that we get, if the bootloader starts the DSS and
-                 * the kernel resets it */
-                omap_writel(omap_readl(0x48050440) & ~0x3, 0x48050440);
+        /* disable LCD and DIGIT output. This seems to fix the synclost
+         * problem that we get, if the bootloader starts the DSS and
+         * the kernel resets it */
+        omap_writel(omap_readl(0x48050440) & ~0x3, 0x48050440);
 
-                /* We need to wait here a bit, otherwise we sometimes start to
-                 * get synclost errors, and after that only power cycle will
-                 * restore DSS functionality. I have no idea why this happens.
-                 * And we have to wait _before_ resetting the DSS, but after
-                 * enabling clocks.
-                 */
-                msleep(50);
+        /* We need to wait here a bit, otherwise we sometimes start to
+         * get synclost errors, and after that only power cycle will
+         * restore DSS functionality. I have no idea why this happens.
+         * And we have to wait _before_ resetting the DSS, but after
+         * enabling clocks.
+         */
+        msleep(50);
 
-                _omap_dss_reset();
-        }
+        _omap_dss_reset();
 
         /* autoidle */
         REG_FLD_MOD(DSS_SYSCONFIG, 1, 0, 0);
@@ -589,29 +680,30 @@ int dss_init(bool skip_init)
         REG_FLD_MOD(DSS_CONTROL, 1, 3, 3);      /* venc clock 4x enable */
         REG_FLD_MOD(DSS_CONTROL, 0, 2, 2);      /* venc clock mode = normal */
 #endif
-
-        r = request_irq(INT_24XX_DSS_IRQ,
-                        cpu_is_omap24xx()
-                        ? dss_irq_handler_omap2
-                        : dss_irq_handler_omap3,
-                        0, "OMAP DSS", NULL);
-
-        if (r < 0) {
-                DSSERR("omap2 dss: request_irq failed\n");
-                goto fail1;
-        }
-
         if (cpu_is_omap34xx()) {
-                dss.dpll4_m4_ck = clk_get(NULL, "dpll4_m4_ck");
-                if (IS_ERR(dss.dpll4_m4_ck)) {
+                dpll4_m4_ck = clk_get(NULL, "dpll4_m4_ck");
+                if (IS_ERR(dpll4_m4_ck)) {
                         DSSERR("Failed to get dpll4_m4_ck\n");
-                        r = PTR_ERR(dss.dpll4_m4_ck);
-                        goto fail2;
+                        r = PTR_ERR(dpll4_m4_ck);
+                        goto fail1;
                 }
+        } else if (cpu_is_omap44xx()) {
+                dpll4_m4_ck = clk_get(NULL, "dpll_per_m5x2_ck");
+                if (IS_ERR(dpll4_m4_ck)) {
+                        DSSERR("Failed to get dpll4_m4_ck\n");
+                        r = PTR_ERR(dpll4_m4_ck);
+                        goto fail1;
+                }
+        } else { /* omap24xx */
+                dpll4_m4_ck = NULL;
         }
 
-        dss.dsi_clk_source = DSS_SRC_DSS1_ALWON_FCLK;
-        dss.dispc_clk_source = DSS_SRC_DSS1_ALWON_FCLK;
+        dss.dpll4_m4_ck = dpll4_m4_ck;
+
+        dss.dsi_clk_source = DSS_CLK_SRC_FCK;
+        dss.dispc_clk_source = DSS_CLK_SRC_FCK;
+        dss.lcd_clk_source[0] = DSS_CLK_SRC_FCK;
+        dss.lcd_clk_source[1] = DSS_CLK_SRC_FCK;
 
         dss_save_context();
 
@@ -621,21 +713,416 @@ int dss_init(bool skip_init)
 
         return 0;
 
-fail2:
-        free_irq(INT_24XX_DSS_IRQ, NULL);
 fail1:
         iounmap(dss.base);
 fail0:
         return r;
 }
 
-void dss_exit(void)
+static void dss_exit(void)
 {
-        if (cpu_is_omap34xx())
+        if (dss.dpll4_m4_ck)
                 clk_put(dss.dpll4_m4_ck);
 
-        free_irq(INT_24XX_DSS_IRQ, NULL);
-
         iounmap(dss.base);
 }
 
+/* CONTEXT */
+static int dss_get_ctx_id(void)
+{
+        struct omap_display_platform_data *pdata = dss.pdev->dev.platform_data;
+        int r;
+
+        if (!pdata->board_data->get_last_off_on_transaction_id)
+                return 0;
+        r = pdata->board_data->get_last_off_on_transaction_id(&dss.pdev->dev);
+        if (r < 0) {
+                dev_err(&dss.pdev->dev, "getting transaction ID failed, "
+                                "will force context restore\n");
+                r = -1;
+        }
+        return r;
+}
+
+int dss_need_ctx_restore(void)
+{
+        int id = dss_get_ctx_id();
+
+        if (id < 0 || id != dss.ctx_id) {
+                DSSDBG("ctx id %d -> id %d\n",
+                                dss.ctx_id, id);
+                dss.ctx_id = id;
+                return 1;
+        } else {
+                return 0;
+        }
+}
+
+static void save_all_ctx(void)
+{
+        DSSDBG("save context\n");
+
+        dss_clk_enable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK);
+
+        dss_save_context();
+        dispc_save_context();
+#ifdef CONFIG_OMAP2_DSS_DSI
+        dsi_save_context();
+#endif
+
+        dss_clk_disable_no_ctx(DSS_CLK_ICK | DSS_CLK_FCK);
+}
+
+static void restore_all_ctx(void)
+{
+        DSSDBG("restore context\n");
+
+        dss_clk_enable_all_no_ctx();
+
+        dss_restore_context();
+        dispc_restore_context();
+#ifdef CONFIG_OMAP2_DSS_DSI
+        dsi_restore_context();
+#endif
+
+        dss_clk_disable_all_no_ctx();
+}
+
+static int dss_get_clock(struct clk **clock, const char *clk_name)
+{
+        struct clk *clk;
+
+        clk = clk_get(&dss.pdev->dev, clk_name);
+
+        if (IS_ERR(clk)) {
+                DSSERR("can't get clock %s", clk_name);
+                return PTR_ERR(clk);
+        }
+
+        *clock = clk;
+
+        DSSDBG("clk %s, rate %ld\n", clk_name, clk_get_rate(clk));
+
+        return 0;
+}
+
+static int dss_get_clocks(void)
+{
+        int r;
+        struct omap_display_platform_data *pdata = dss.pdev->dev.platform_data;
+
+        dss.dss_ick = NULL;
+        dss.dss_fck = NULL;
+        dss.dss_sys_clk = NULL;
+        dss.dss_tv_fck = NULL;
+        dss.dss_video_fck = NULL;
+
+        r = dss_get_clock(&dss.dss_ick, "ick");
+        if (r)
+                goto err;
+
+        r = dss_get_clock(&dss.dss_fck, "fck");
+        if (r)
+                goto err;
+
+        if (!pdata->opt_clock_available) {
+                r = -ENODEV;
+                goto err;
+        }
+
+        if (pdata->opt_clock_available("sys_clk")) {
+                r = dss_get_clock(&dss.dss_sys_clk, "sys_clk");
+                if (r)
+                        goto err;
+        }
+
+        if (pdata->opt_clock_available("tv_clk")) {
+                r = dss_get_clock(&dss.dss_tv_fck, "tv_clk");
+                if (r)
+                        goto err;
+        }
+
+        if (pdata->opt_clock_available("video_clk")) {
+                r = dss_get_clock(&dss.dss_video_fck, "video_clk");
+                if (r)
+                        goto err;
+        }
+
+        return 0;
+
+err:
+        if (dss.dss_ick)
+                clk_put(dss.dss_ick);
+        if (dss.dss_fck)
+                clk_put(dss.dss_fck);
+        if (dss.dss_sys_clk)
+                clk_put(dss.dss_sys_clk);
+        if (dss.dss_tv_fck)
+                clk_put(dss.dss_tv_fck);
+        if (dss.dss_video_fck)
+                clk_put(dss.dss_video_fck);
+
+        return r;
+}
+
+static void dss_put_clocks(void)
+{
+        if (dss.dss_video_fck)
+                clk_put(dss.dss_video_fck);
+        if (dss.dss_tv_fck)
+                clk_put(dss.dss_tv_fck);
+        if (dss.dss_sys_clk)
+                clk_put(dss.dss_sys_clk);
+        clk_put(dss.dss_fck);
+        clk_put(dss.dss_ick);
+}
+
+unsigned long dss_clk_get_rate(enum dss_clock clk)
+{
+        switch (clk) {
+        case DSS_CLK_ICK:
+                return clk_get_rate(dss.dss_ick);
+        case DSS_CLK_FCK:
+                return clk_get_rate(dss.dss_fck);
+        case DSS_CLK_SYSCK:
+                return clk_get_rate(dss.dss_sys_clk);
+        case DSS_CLK_TVFCK:
+                return clk_get_rate(dss.dss_tv_fck);
+        case DSS_CLK_VIDFCK:
+                return clk_get_rate(dss.dss_video_fck);
+        }
+
+        BUG();
+        return 0;
+}
+
+static unsigned count_clk_bits(enum dss_clock clks)
+{
+        unsigned num_clks = 0;
+
+        if (clks & DSS_CLK_ICK)
+                ++num_clks;
+        if (clks & DSS_CLK_FCK)
+                ++num_clks;
+        if (clks & DSS_CLK_SYSCK)
+                ++num_clks;
+        if (clks & DSS_CLK_TVFCK)
+                ++num_clks;
+        if (clks & DSS_CLK_VIDFCK)
+                ++num_clks;
+
+        return num_clks;
+}
+
+static void dss_clk_enable_no_ctx(enum dss_clock clks)
+{
+        unsigned num_clks = count_clk_bits(clks);
+
+        if (clks & DSS_CLK_ICK)
+                clk_enable(dss.dss_ick);
+        if (clks & DSS_CLK_FCK)
+                clk_enable(dss.dss_fck);
+        if ((clks & DSS_CLK_SYSCK) && dss.dss_sys_clk)
+                clk_enable(dss.dss_sys_clk);
+        if ((clks & DSS_CLK_TVFCK) && dss.dss_tv_fck)
+                clk_enable(dss.dss_tv_fck);
+        if ((clks & DSS_CLK_VIDFCK) && dss.dss_video_fck)
+                clk_enable(dss.dss_video_fck);
+
+        dss.num_clks_enabled += num_clks;
+}
+
+void dss_clk_enable(enum dss_clock clks)
+{
+        bool check_ctx = dss.num_clks_enabled == 0;
+
+        dss_clk_enable_no_ctx(clks);
+
+        /*
+         * HACK: On omap4 the registers may not be accessible right after
+         * enabling the clocks. At some point this will be handled by
+         * pm_runtime, but for the time begin this should make things work.
+         */
+        if (cpu_is_omap44xx() && check_ctx)
+                udelay(10);
+
+        if (check_ctx && cpu_is_omap34xx() && dss_need_ctx_restore())
+                restore_all_ctx();
+}
+
+static void dss_clk_disable_no_ctx(enum dss_clock clks)
+{
+        unsigned num_clks = count_clk_bits(clks);
+
+        if (clks & DSS_CLK_ICK)
+                clk_disable(dss.dss_ick);
+        if (clks & DSS_CLK_FCK)
+                clk_disable(dss.dss_fck);
+        if ((clks & DSS_CLK_SYSCK) && dss.dss_sys_clk)
+                clk_disable(dss.dss_sys_clk);
+        if ((clks & DSS_CLK_TVFCK) && dss.dss_tv_fck)
+                clk_disable(dss.dss_tv_fck);
+        if ((clks & DSS_CLK_VIDFCK) && dss.dss_video_fck)
+                clk_disable(dss.dss_video_fck);
+
+        dss.num_clks_enabled -= num_clks;
+}
+
+void dss_clk_disable(enum dss_clock clks)
+{
+        if (cpu_is_omap34xx()) {
+                unsigned num_clks = count_clk_bits(clks);
+
+                BUG_ON(dss.num_clks_enabled < num_clks);
+
+                if (dss.num_clks_enabled == num_clks)
+                        save_all_ctx();
+        }
+
+        dss_clk_disable_no_ctx(clks);
+}
+
+static void dss_clk_enable_all_no_ctx(void)
+{
+        enum dss_clock clks;
+
+        clks = DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_SYSCK | DSS_CLK_TVFCK;
+        if (cpu_is_omap34xx())
+                clks |= DSS_CLK_VIDFCK;
+        dss_clk_enable_no_ctx(clks);
+}
+
+static void dss_clk_disable_all_no_ctx(void)
+{
+        enum dss_clock clks;
+
+        clks = DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_SYSCK | DSS_CLK_TVFCK;
+        if (cpu_is_omap34xx())
+                clks |= DSS_CLK_VIDFCK;
+        dss_clk_disable_no_ctx(clks);
+}
+
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+/* CLOCKS */
+static void core_dump_clocks(struct seq_file *s)
+{
+        int i;
+        struct clk *clocks[5] = {
+                dss.dss_ick,
+                dss.dss_fck,
+                dss.dss_sys_clk,
+                dss.dss_tv_fck,
+                dss.dss_video_fck
+        };
+
+        seq_printf(s, "- CORE -\n");
+
+        seq_printf(s, "internal clk count\t\t%u\n", dss.num_clks_enabled);
+
+        for (i = 0; i < 5; i++) {
+                if (!clocks[i])
+                        continue;
+                seq_printf(s, "%-15s\t%lu\t%d\n",
+                                clocks[i]->name,
+                                clk_get_rate(clocks[i]),
+                                clocks[i]->usecount);
+        }
+}
+#endif /* defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT) */
+
+/* DEBUGFS */
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+void dss_debug_dump_clocks(struct seq_file *s)
+{
+        core_dump_clocks(s);
+        dss_dump_clocks(s);
+        dispc_dump_clocks(s);
+#ifdef CONFIG_OMAP2_DSS_DSI
+        dsi_dump_clocks(s);
+#endif
+}
+#endif
+
+
+/* DSS HW IP initialisation */
+static int omap_dsshw_probe(struct platform_device *pdev)
+{
+        int r;
+
+        dss.pdev = pdev;
+
+        r = dss_get_clocks();
+        if (r)
+                goto err_clocks;
+
+        dss_clk_enable_all_no_ctx();
+
+        dss.ctx_id = dss_get_ctx_id();
+        DSSDBG("initial ctx id %u\n", dss.ctx_id);
+
+        r = dss_init();
+        if (r) {
+                DSSERR("Failed to initialize DSS\n");
+                goto err_dss;
+        }
+
+        r = dpi_init();
+        if (r) {
+                DSSERR("Failed to initialize DPI\n");
+                goto err_dpi;
+        }
+
+        r = sdi_init();
+        if (r) {
+                DSSERR("Failed to initialize SDI\n");
+                goto err_sdi;
+        }
+
+        dss_clk_disable_all_no_ctx();
+        return 0;
+err_sdi:
+        dpi_exit();
+err_dpi:
+        dss_exit();
+err_dss:
+        dss_clk_disable_all_no_ctx();
+        dss_put_clocks();
+err_clocks:
+        return r;
+}
+
+static int omap_dsshw_remove(struct platform_device *pdev)
+{
+
+        dss_exit();
+
+        /*
+         * As part of hwmod changes, DSS is not the only controller of dss
+         * clocks; hwmod framework itself will also enable clocks during hwmod
+         * init for dss, and autoidle is set in h/w for DSS. Hence, there's no
+         * need to disable clocks if their usecounts > 1.
+         */
+        WARN_ON(dss.num_clks_enabled > 0);
+
+        dss_put_clocks();
+        return 0;
+}
+
+static struct platform_driver omap_dsshw_driver = {
+        .probe          = omap_dsshw_probe,
+        .remove         = omap_dsshw_remove,
+        .driver         = {
+                .name   = "omapdss_dss",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int dss_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_dsshw_driver);
+}
+
+void dss_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omap_dsshw_driver);
+}
diff --git a/drivers/video/omap2/dss/dss.h b/drivers/video/omap2/dss/dss.h
index b394951120ac..c2f582bb19c0 100644
--- a/drivers/video/omap2/dss/dss.h
+++ b/drivers/video/omap2/dss/dss.h
@@ -97,8 +97,6 @@ extern unsigned int dss_debug;
 #define FLD_MOD(orig, val, start, end) \
         (((orig) & ~FLD_MASK(start, end)) | FLD_VAL(val, start, end))
 
-#define DISPC_MAX_FCK 173000000
-
 enum omap_burst_size {
         OMAP_DSS_BURST_4x32 = 0,
         OMAP_DSS_BURST_8x32 = 1,
@@ -112,17 +110,25 @@ enum omap_parallel_interface_mode {
 };
 
 enum dss_clock {
-        DSS_CLK_ICK     = 1 << 0,
-        DSS_CLK_FCK1    = 1 << 1,
-        DSS_CLK_FCK2    = 1 << 2,
-        DSS_CLK_54M     = 1 << 3,
-        DSS_CLK_96M     = 1 << 4,
+        DSS_CLK_ICK     = 1 << 0,       /* DSS_L3_ICLK and DSS_L4_ICLK */
+        DSS_CLK_FCK     = 1 << 1,       /* DSS1_ALWON_FCLK */
+        DSS_CLK_SYSCK   = 1 << 2,       /* DSS2_ALWON_FCLK */
+        DSS_CLK_TVFCK   = 1 << 3,       /* DSS_TV_FCLK */
+        DSS_CLK_VIDFCK  = 1 << 4,       /* DSS_96M_FCLK*/
 };
 
 enum dss_clk_source {
-        DSS_SRC_DSI1_PLL_FCLK,
-        DSS_SRC_DSI2_PLL_FCLK,
-        DSS_SRC_DSS1_ALWON_FCLK,
+        DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC,        /* OMAP3: DSI1_PLL_FCLK
+                                                 * OMAP4: PLL1_CLK1 */
+        DSS_CLK_SRC_DSI_PLL_HSDIV_DSI,          /* OMAP3: DSI2_PLL_FCLK
+                                                 * OMAP4: PLL1_CLK2 */
+        DSS_CLK_SRC_FCK,                        /* OMAP2/3: DSS1_ALWON_FCLK
+                                                 * OMAP4: DSS_FCLK */
+};
+
+enum dss_hdmi_venc_clk_source_select {
+        DSS_VENC_TV_CLK = 0,
+        DSS_HDMI_M_PCLK = 1,
 };
 
 struct dss_clock_info {
@@ -148,36 +154,42 @@ struct dsi_clock_info {
         unsigned long fint;
         unsigned long clkin4ddr;
         unsigned long clkin;
-        unsigned long dsi1_pll_fclk;
-        unsigned long dsi2_pll_fclk;
-
+        unsigned long dsi_pll_hsdiv_dispc_clk;  /* OMAP3: DSI1_PLL_CLK
+                                                 * OMAP4: PLLx_CLK1 */
+        unsigned long dsi_pll_hsdiv_dsi_clk;    /* OMAP3: DSI2_PLL_CLK
+                                                 * OMAP4: PLLx_CLK2 */
         unsigned long lp_clk;
 
         /* dividers */
         u16 regn;
         u16 regm;
-        u16 regm3;
-        u16 regm4;
-
+        u16 regm_dispc; /* OMAP3: REGM3
+                         * OMAP4: REGM4 */
+        u16 regm_dsi;   /* OMAP3: REGM4
+                         * OMAP4: REGM5 */
         u16 lp_clk_div;
 
         u8 highfreq;
-        bool use_dss2_fck;
+        bool use_sys_clk;
+};
+
+/* HDMI PLL structure */
+struct hdmi_pll_info {
+        u16 regn;
+        u16 regm;
+        u32 regmf;
+        u16 regm2;
+        u16 regsd;
+        u16 dcofreq;
 };
 
 struct seq_file;
 struct platform_device;
 
 /* core */
-void dss_clk_enable(enum dss_clock clks);
-void dss_clk_disable(enum dss_clock clks);
-unsigned long dss_clk_get_rate(enum dss_clock clk);
-int dss_need_ctx_restore(void);
-void dss_dump_clocks(struct seq_file *s);
 struct bus_type *dss_get_bus(void);
 struct regulator *dss_get_vdds_dsi(void);
 struct regulator *dss_get_vdds_sdi(void);
-struct regulator *dss_get_vdda_dac(void);
 
 /* display */
 int dss_suspend_all_devices(void);
@@ -214,13 +226,23 @@ void dss_overlay_setup_l4_manager(struct omap_overlay_manager *mgr);
 void dss_recheck_connections(struct omap_dss_device *dssdev, bool force);
 
 /* DSS */
-int dss_init(bool skip_init);
-void dss_exit(void);
+int dss_init_platform_driver(void);
+void dss_uninit_platform_driver(void);
 
+void dss_select_hdmi_venc_clk_source(enum dss_hdmi_venc_clk_source_select);
 void dss_save_context(void);
 void dss_restore_context(void);
+void dss_clk_enable(enum dss_clock clks);
+void dss_clk_disable(enum dss_clock clks);
+unsigned long dss_clk_get_rate(enum dss_clock clk);
+int dss_need_ctx_restore(void);
+const char *dss_get_generic_clk_source_name(enum dss_clk_source clk_src);
+void dss_dump_clocks(struct seq_file *s);
 
 void dss_dump_regs(struct seq_file *s);
+#if defined(CONFIG_DEBUG_FS) && defined(CONFIG_OMAP2_DSS_DEBUG_SUPPORT)
+void dss_debug_dump_clocks(struct seq_file *s);
+#endif
 
 void dss_sdi_init(u8 datapairs);
 int dss_sdi_enable(void);
@@ -228,8 +250,11 @@ void dss_sdi_disable(void);
 
 void dss_select_dispc_clk_source(enum dss_clk_source clk_src);
 void dss_select_dsi_clk_source(enum dss_clk_source clk_src);
+void dss_select_lcd_clk_source(enum omap_channel channel,
+                enum dss_clk_source clk_src);
 enum dss_clk_source dss_get_dispc_clk_source(void);
 enum dss_clk_source dss_get_dsi_clk_source(void);
+enum dss_clk_source dss_get_lcd_clk_source(enum omap_channel channel);
 
 void dss_set_venc_output(enum omap_dss_venc_type type);
 void dss_set_dac_pwrdn_bgz(bool enable);
@@ -244,11 +269,11 @@ int dss_calc_clock_div(bool is_tft, unsigned long req_pck,
 
 /* SDI */
 #ifdef CONFIG_OMAP2_DSS_SDI
-int sdi_init(bool skip_init);
+int sdi_init(void);
 void sdi_exit(void);
 int sdi_init_display(struct omap_dss_device *display);
 #else
-static inline int sdi_init(bool skip_init)
+static inline int sdi_init(void)
 {
         return 0;
 }
@@ -259,8 +284,8 @@ static inline void sdi_exit(void)
 
 /* DSI */
 #ifdef CONFIG_OMAP2_DSS_DSI
-int dsi_init(struct platform_device *pdev);
-void dsi_exit(void);
+int dsi_init_platform_driver(void);
+void dsi_uninit_platform_driver(void);
 
 void dsi_dump_clocks(struct seq_file *s);
 void dsi_dump_irqs(struct seq_file *s);
@@ -271,7 +296,7 @@ void dsi_restore_context(void);
 
 int dsi_init_display(struct omap_dss_device *display);
 void dsi_irq_handler(void);
-unsigned long dsi_get_dsi1_pll_rate(void);
+unsigned long dsi_get_pll_hsdiv_dispc_rate(void);
 int dsi_pll_set_clock_div(struct dsi_clock_info *cinfo);
 int dsi_pll_calc_clock_div_pck(bool is_tft, unsigned long req_pck,
                 struct dsi_clock_info *cinfo,
@@ -282,31 +307,36 @@ void dsi_pll_uninit(void);
 void dsi_get_overlay_fifo_thresholds(enum omap_plane plane,
                 u32 fifo_size, enum omap_burst_size *burst_size,
                 u32 *fifo_low, u32 *fifo_high);
-void dsi_wait_dsi1_pll_active(void);
-void dsi_wait_dsi2_pll_active(void);
+void dsi_wait_pll_hsdiv_dispc_active(void);
+void dsi_wait_pll_hsdiv_dsi_active(void);
 #else
-static inline int dsi_init(struct platform_device *pdev)
+static inline int dsi_init_platform_driver(void)
 {
         return 0;
 }
-static inline void dsi_exit(void)
+static inline void dsi_uninit_platform_driver(void)
 {
 }
-static inline void dsi_wait_dsi1_pll_active(void)
+static inline unsigned long dsi_get_pll_hsdiv_dispc_rate(void)
 {
+        WARN("%s: DSI not compiled in, returning rate as 0\n", __func__);
+        return 0;
 }
-static inline void dsi_wait_dsi2_pll_active(void)
+static inline void dsi_wait_pll_hsdiv_dispc_active(void)
+{
+}
+static inline void dsi_wait_pll_hsdiv_dsi_active(void)
 {
 }
 #endif
 
 /* DPI */
 #ifdef CONFIG_OMAP2_DSS_DPI
-int dpi_init(struct platform_device *pdev);
+int dpi_init(void);
 void dpi_exit(void);
 int dpi_init_display(struct omap_dss_device *dssdev);
 #else
-static inline int dpi_init(struct platform_device *pdev)
+static inline int dpi_init(void)
 {
         return 0;
 }
@@ -316,8 +346,8 @@ static inline void dpi_exit(void)
 #endif
 
 /* DISPC */
-int dispc_init(void);
-void dispc_exit(void);
+int dispc_init_platform_driver(void);
+void dispc_uninit_platform_driver(void);
 void dispc_dump_clocks(struct seq_file *s);
 void dispc_dump_irqs(struct seq_file *s);
 void dispc_dump_regs(struct seq_file *s);
@@ -350,6 +380,7 @@ void dispc_set_plane_size(enum omap_plane plane, u16 width, u16 height);
 void dispc_set_channel_out(enum omap_plane plane,
                 enum omap_channel channel_out);
 
+void dispc_enable_gamma_table(bool enable);
 int dispc_setup_plane(enum omap_plane plane,
                       u32 paddr, u16 screen_width,
                       u16 pos_x, u16 pos_y,
@@ -409,24 +440,50 @@ int dispc_get_clock_div(enum omap_channel channel,
 
 /* VENC */
 #ifdef CONFIG_OMAP2_DSS_VENC
-int venc_init(struct platform_device *pdev);
-void venc_exit(void);
+int venc_init_platform_driver(void);
+void venc_uninit_platform_driver(void);
 void venc_dump_regs(struct seq_file *s);
 int venc_init_display(struct omap_dss_device *display);
 #else
-static inline int venc_init(struct platform_device *pdev)
+static inline int venc_init_platform_driver(void)
+{
+        return 0;
+}
+static inline void venc_uninit_platform_driver(void)
+{
+}
+#endif
+
+/* HDMI */
+#ifdef CONFIG_OMAP4_DSS_HDMI
+int hdmi_init_platform_driver(void);
+void hdmi_uninit_platform_driver(void);
+int hdmi_init_display(struct omap_dss_device *dssdev);
+#else
+static inline int hdmi_init_display(struct omap_dss_device *dssdev)
+{
+        return 0;
+}
+static inline int hdmi_init_platform_driver(void)
 {
         return 0;
 }
-static inline void venc_exit(void)
+static inline void hdmi_uninit_platform_driver(void)
 {
 }
 #endif
+int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev);
+void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev);
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev);
+int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
+                                        struct omap_video_timings *timings);
+int hdmi_panel_init(void);
+void hdmi_panel_exit(void);
 
 /* RFBI */
 #ifdef CONFIG_OMAP2_DSS_RFBI
-int rfbi_init(void);
-void rfbi_exit(void);
+int rfbi_init_platform_driver(void);
+void rfbi_uninit_platform_driver(void);
 void rfbi_dump_regs(struct seq_file *s);
 
 int rfbi_configure(int rfbi_module, int bpp, int lines);
@@ -437,11 +494,11 @@ void rfbi_set_timings(int rfbi_module, struct rfbi_timings *t);
 unsigned long rfbi_get_max_tx_rate(void);
 int rfbi_init_display(struct omap_dss_device *display);
 #else
-static inline int rfbi_init(void)
+static inline int rfbi_init_platform_driver(void)
 {
         return 0;
 }
-static inline void rfbi_exit(void)
+static inline void rfbi_uninit_platform_driver(void)
 {
 }
 #endif
diff --git a/drivers/video/omap2/dss/dss_features.c b/drivers/video/omap2/dss/dss_features.c
index cf3ef696e141..aa1622241d0d 100644
--- a/drivers/video/omap2/dss/dss_features.c
+++ b/drivers/video/omap2/dss/dss_features.c
@@ -25,14 +25,18 @@
 #include <plat/display.h>
 #include <plat/cpu.h>
 
+#include "dss.h"
 #include "dss_features.h"
 
 /* Defines a generic omap register field */
 struct dss_reg_field {
-        enum dss_feat_reg_field id;
         u8 start, end;
 };
 
+struct dss_param_range {
+        int min, max;
+};
+
 struct omap_dss_features {
         const struct dss_reg_field *reg_fields;
         const int num_reg_fields;
@@ -43,29 +47,68 @@ struct omap_dss_features {
         const int num_ovls;
         const enum omap_display_type *supported_displays;
         const enum omap_color_mode *supported_color_modes;
+        const char * const *clksrc_names;
+        const struct dss_param_range *dss_params;
 };
 
 /* This struct is assigned to one of the below during initialization */
 static struct omap_dss_features *omap_current_dss_features;
 
 static const struct dss_reg_field omap2_dss_reg_fields[] = {
-        { FEAT_REG_FIRHINC, 11, 0 },
-        { FEAT_REG_FIRVINC, 27, 16 },
-        { FEAT_REG_FIFOLOWTHRESHOLD, 8, 0 },
-        { FEAT_REG_FIFOHIGHTHRESHOLD, 24, 16 },
-        { FEAT_REG_FIFOSIZE, 8, 0 },
+        [FEAT_REG_FIRHINC]                      = { 11, 0 },
+        [FEAT_REG_FIRVINC]                      = { 27, 16 },
+        [FEAT_REG_FIFOLOWTHRESHOLD]             = { 8, 0 },
+        [FEAT_REG_FIFOHIGHTHRESHOLD]            = { 24, 16 },
+        [FEAT_REG_FIFOSIZE]                     = { 8, 0 },
+        [FEAT_REG_HORIZONTALACCU]               = { 9, 0 },
+        [FEAT_REG_VERTICALACCU]                 = { 25, 16 },
+        [FEAT_REG_DISPC_CLK_SWITCH]             = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGN]                  = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGM]                  = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGM_DISPC]            = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGM_DSI]              = { 0, 0 },
 };
 
 static const struct dss_reg_field omap3_dss_reg_fields[] = {
-        { FEAT_REG_FIRHINC, 12, 0 },
-        { FEAT_REG_FIRVINC, 28, 16 },
-        { FEAT_REG_FIFOLOWTHRESHOLD, 11, 0 },
-        { FEAT_REG_FIFOHIGHTHRESHOLD, 27, 16 },
-        { FEAT_REG_FIFOSIZE, 10, 0 },
+        [FEAT_REG_FIRHINC]                      = { 12, 0 },
+        [FEAT_REG_FIRVINC]                      = { 28, 16 },
+        [FEAT_REG_FIFOLOWTHRESHOLD]             = { 11, 0 },
+        [FEAT_REG_FIFOHIGHTHRESHOLD]            = { 27, 16 },
+        [FEAT_REG_FIFOSIZE]                     = { 10, 0 },
+        [FEAT_REG_HORIZONTALACCU]               = { 9, 0 },
+        [FEAT_REG_VERTICALACCU]                 = { 25, 16 },
+        [FEAT_REG_DISPC_CLK_SWITCH]             = { 0, 0 },
+        [FEAT_REG_DSIPLL_REGN]                  = { 7, 1 },
+        [FEAT_REG_DSIPLL_REGM]                  = { 18, 8 },
+        [FEAT_REG_DSIPLL_REGM_DISPC]            = { 22, 19 },
+        [FEAT_REG_DSIPLL_REGM_DSI]              = { 26, 23 },
+};
+
+static const struct dss_reg_field omap4_dss_reg_fields[] = {
+        [FEAT_REG_FIRHINC]                      = { 12, 0 },
+        [FEAT_REG_FIRVINC]                      = { 28, 16 },
+        [FEAT_REG_FIFOLOWTHRESHOLD]             = { 15, 0 },
+        [FEAT_REG_FIFOHIGHTHRESHOLD]            = { 31, 16 },
+        [FEAT_REG_FIFOSIZE]                     = { 15, 0 },
+        [FEAT_REG_HORIZONTALACCU]               = { 10, 0 },
+        [FEAT_REG_VERTICALACCU]                 = { 26, 16 },
+        [FEAT_REG_DISPC_CLK_SWITCH]             = { 9, 8 },
+        [FEAT_REG_DSIPLL_REGN]                  = { 8, 1 },
+        [FEAT_REG_DSIPLL_REGM]                  = { 20, 9 },
+        [FEAT_REG_DSIPLL_REGM_DISPC]            = { 25, 21 },
+        [FEAT_REG_DSIPLL_REGM_DSI]              = { 30, 26 },
 };
 
 static const enum omap_display_type omap2_dss_supported_displays[] = {
         /* OMAP_DSS_CHANNEL_LCD */
+        OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI,
+
+        /* OMAP_DSS_CHANNEL_DIGIT */
+        OMAP_DISPLAY_TYPE_VENC,
+};
+
+static const enum omap_display_type omap3430_dss_supported_displays[] = {
+        /* OMAP_DSS_CHANNEL_LCD */
         OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
         OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI,
 
@@ -73,10 +116,10 @@ static const enum omap_display_type omap2_dss_supported_displays[] = {
         OMAP_DISPLAY_TYPE_VENC,
 };
 
-static const enum omap_display_type omap3_dss_supported_displays[] = {
+static const enum omap_display_type omap3630_dss_supported_displays[] = {
         /* OMAP_DSS_CHANNEL_LCD */
         OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
-        OMAP_DISPLAY_TYPE_SDI | OMAP_DISPLAY_TYPE_DSI,
+        OMAP_DISPLAY_TYPE_DSI,
 
         /* OMAP_DSS_CHANNEL_DIGIT */
         OMAP_DISPLAY_TYPE_VENC,
@@ -87,7 +130,7 @@ static const enum omap_display_type omap4_dss_supported_displays[] = {
         OMAP_DISPLAY_TYPE_DBI | OMAP_DISPLAY_TYPE_DSI,
 
         /* OMAP_DSS_CHANNEL_DIGIT */
-        OMAP_DISPLAY_TYPE_VENC,
+        OMAP_DISPLAY_TYPE_VENC | OMAP_DISPLAY_TYPE_HDMI,
 
         /* OMAP_DSS_CHANNEL_LCD2 */
         OMAP_DISPLAY_TYPE_DPI | OMAP_DISPLAY_TYPE_DBI |
@@ -134,6 +177,54 @@ static const enum omap_color_mode omap3_dss_supported_color_modes[] = {
         OMAP_DSS_COLOR_RGBA32 | OMAP_DSS_COLOR_RGBX32,
 };
 
+static const char * const omap2_dss_clk_source_names[] = {
+        [DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]       = "N/A",
+        [DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]         = "N/A",
+        [DSS_CLK_SRC_FCK]                       = "DSS_FCLK1",
+};
+
+static const char * const omap3_dss_clk_source_names[] = {
+        [DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]       = "DSI1_PLL_FCLK",
+        [DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]         = "DSI2_PLL_FCLK",
+        [DSS_CLK_SRC_FCK]                       = "DSS1_ALWON_FCLK",
+};
+
+static const char * const omap4_dss_clk_source_names[] = {
+        [DSS_CLK_SRC_DSI_PLL_HSDIV_DISPC]       = "PLL1_CLK1",
+        [DSS_CLK_SRC_DSI_PLL_HSDIV_DSI]         = "PLL1_CLK2",
+        [DSS_CLK_SRC_FCK]                       = "DSS_FCLK",
+};
+
+static const struct dss_param_range omap2_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 173000000 },
+        [FEAT_PARAM_DSIPLL_REGN]                = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_REGM]                = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_REGM_DISPC]          = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_REGM_DSI]            = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_FINT]                = { 0, 0 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 0, 0 },
+};
+
+static const struct dss_param_range omap3_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 173000000 },
+        [FEAT_PARAM_DSIPLL_REGN]                = { 0, (1 << 7) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM]                = { 0, (1 << 11) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM_DISPC]          = { 0, (1 << 4) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM_DSI]            = { 0, (1 << 4) - 1 },
+        [FEAT_PARAM_DSIPLL_FINT]                = { 750000, 2100000 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 1, (1 << 13) - 1},
+};
+
+static const struct dss_param_range omap4_dss_param_range[] = {
+        [FEAT_PARAM_DSS_FCK]                    = { 0, 186000000 },
+        [FEAT_PARAM_DSIPLL_REGN]                = { 0, (1 << 8) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM]                = { 0, (1 << 12) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM_DISPC]          = { 0, (1 << 5) - 1 },
+        [FEAT_PARAM_DSIPLL_REGM_DSI]            = { 0, (1 << 5) - 1 },
+        [FEAT_PARAM_DSIPLL_FINT]                = { 500000, 2500000 },
+        [FEAT_PARAM_DSIPLL_LPDIV]               = { 0, (1 << 13) - 1 },
+};
+
 /* OMAP2 DSS Features */
 static struct omap_dss_features omap2_dss_features = {
         .reg_fields = omap2_dss_reg_fields,
@@ -141,12 +232,15 @@ static struct omap_dss_features omap2_dss_features = {
 
         .has_feature    =
                 FEAT_LCDENABLEPOL | FEAT_LCDENABLESIGNAL |
-                FEAT_PCKFREEENABLE | FEAT_FUNCGATED,
+                FEAT_PCKFREEENABLE | FEAT_FUNCGATED |
+                FEAT_ROWREPEATENABLE | FEAT_RESIZECONF,
 
         .num_mgrs = 2,
         .num_ovls = 3,
         .supported_displays = omap2_dss_supported_displays,
         .supported_color_modes = omap2_dss_supported_color_modes,
+        .clksrc_names = omap2_dss_clk_source_names,
+        .dss_params = omap2_dss_param_range,
 };
 
 /* OMAP3 DSS Features */
@@ -157,12 +251,15 @@ static struct omap_dss_features omap3430_dss_features = {
         .has_feature    =
                 FEAT_GLOBAL_ALPHA | FEAT_LCDENABLEPOL |
                 FEAT_LCDENABLESIGNAL | FEAT_PCKFREEENABLE |
-                FEAT_FUNCGATED,
+                FEAT_FUNCGATED | FEAT_ROWREPEATENABLE |
+                FEAT_LINEBUFFERSPLIT | FEAT_RESIZECONF,
 
         .num_mgrs = 2,
         .num_ovls = 3,
-        .supported_displays = omap3_dss_supported_displays,
+        .supported_displays = omap3430_dss_supported_displays,
         .supported_color_modes = omap3_dss_supported_color_modes,
+        .clksrc_names = omap3_dss_clk_source_names,
+        .dss_params = omap3_dss_param_range,
 };
 
 static struct omap_dss_features omap3630_dss_features = {
@@ -172,27 +269,34 @@ static struct omap_dss_features omap3630_dss_features = {
         .has_feature    =
                 FEAT_GLOBAL_ALPHA | FEAT_LCDENABLEPOL |
                 FEAT_LCDENABLESIGNAL | FEAT_PCKFREEENABLE |
-                FEAT_PRE_MULT_ALPHA | FEAT_FUNCGATED,
+                FEAT_PRE_MULT_ALPHA | FEAT_FUNCGATED |
+                FEAT_ROWREPEATENABLE | FEAT_LINEBUFFERSPLIT |
+                FEAT_RESIZECONF,
 
         .num_mgrs = 2,
         .num_ovls = 3,
-        .supported_displays = omap3_dss_supported_displays,
+        .supported_displays = omap3630_dss_supported_displays,
         .supported_color_modes = omap3_dss_supported_color_modes,
+        .clksrc_names = omap3_dss_clk_source_names,
+        .dss_params = omap3_dss_param_range,
 };
 
 /* OMAP4 DSS Features */
 static struct omap_dss_features omap4_dss_features = {
-        .reg_fields = omap3_dss_reg_fields,
-        .num_reg_fields = ARRAY_SIZE(omap3_dss_reg_fields),
+        .reg_fields = omap4_dss_reg_fields,
+        .num_reg_fields = ARRAY_SIZE(omap4_dss_reg_fields),
 
         .has_feature    =
                 FEAT_GLOBAL_ALPHA | FEAT_PRE_MULT_ALPHA |
-                FEAT_MGR_LCD2,
+                FEAT_MGR_LCD2 | FEAT_GLOBAL_ALPHA_VID1 |
+                FEAT_CORE_CLK_DIV | FEAT_LCD_CLK_SRC,
 
         .num_mgrs = 3,
         .num_ovls = 3,
         .supported_displays = omap4_dss_supported_displays,
         .supported_color_modes = omap3_dss_supported_color_modes,
+        .clksrc_names = omap4_dss_clk_source_names,
+        .dss_params = omap4_dss_param_range,
 };
 
 /* Functions returning values related to a DSS feature */
@@ -206,6 +310,16 @@ int dss_feat_get_num_ovls(void)
         return omap_current_dss_features->num_ovls;
 }
 
+unsigned long dss_feat_get_param_min(enum dss_range_param param)
+{
+        return omap_current_dss_features->dss_params[param].min;
+}
+
+unsigned long dss_feat_get_param_max(enum dss_range_param param)
+{
+        return omap_current_dss_features->dss_params[param].max;
+}
+
 enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel)
 {
         return omap_current_dss_features->supported_displays[channel];
@@ -223,6 +337,11 @@ bool dss_feat_color_mode_supported(enum omap_plane plane,
                         color_mode;
 }
 
+const char *dss_feat_get_clk_source_name(enum dss_clk_source id)
+{
+        return omap_current_dss_features->clksrc_names[id];
+}
+
 /* DSS has_feature check */
 bool dss_has_feature(enum dss_feat_id id)
 {
diff --git a/drivers/video/omap2/dss/dss_features.h b/drivers/video/omap2/dss/dss_features.h
index b9c70be92588..12e9c4ef0dec 100644
--- a/drivers/video/omap2/dss/dss_features.h
+++ b/drivers/video/omap2/dss/dss_features.h
@@ -22,6 +22,7 @@
 
 #define MAX_DSS_MANAGERS        3
 #define MAX_DSS_OVERLAYS        3
+#define MAX_DSS_LCD_MANAGERS    2
 
 /* DSS has feature id */
 enum dss_feat_id {
@@ -33,6 +34,12 @@ enum dss_feat_id {
         FEAT_PCKFREEENABLE      = 1 << 5,
         FEAT_FUNCGATED          = 1 << 6,
         FEAT_MGR_LCD2           = 1 << 7,
+        FEAT_LINEBUFFERSPLIT    = 1 << 8,
+        FEAT_ROWREPEATENABLE    = 1 << 9,
+        FEAT_RESIZECONF         = 1 << 10,
+        /* Independent core clk divider */
+        FEAT_CORE_CLK_DIV       = 1 << 11,
+        FEAT_LCD_CLK_SRC        = 1 << 12,
 };
 
 /* DSS register field id */
@@ -42,15 +49,35 @@ enum dss_feat_reg_field {
         FEAT_REG_FIFOHIGHTHRESHOLD,
         FEAT_REG_FIFOLOWTHRESHOLD,
         FEAT_REG_FIFOSIZE,
+        FEAT_REG_HORIZONTALACCU,
+        FEAT_REG_VERTICALACCU,
+        FEAT_REG_DISPC_CLK_SWITCH,
+        FEAT_REG_DSIPLL_REGN,
+        FEAT_REG_DSIPLL_REGM,
+        FEAT_REG_DSIPLL_REGM_DISPC,
+        FEAT_REG_DSIPLL_REGM_DSI,
+};
+
+enum dss_range_param {
+        FEAT_PARAM_DSS_FCK,
+        FEAT_PARAM_DSIPLL_REGN,
+        FEAT_PARAM_DSIPLL_REGM,
+        FEAT_PARAM_DSIPLL_REGM_DISPC,
+        FEAT_PARAM_DSIPLL_REGM_DSI,
+        FEAT_PARAM_DSIPLL_FINT,
+        FEAT_PARAM_DSIPLL_LPDIV,
 };
 
 /* DSS Feature Functions */
 int dss_feat_get_num_mgrs(void);
 int dss_feat_get_num_ovls(void);
+unsigned long dss_feat_get_param_min(enum dss_range_param param);
+unsigned long dss_feat_get_param_max(enum dss_range_param param);
 enum omap_display_type dss_feat_get_supported_displays(enum omap_channel channel);
 enum omap_color_mode dss_feat_get_supported_color_modes(enum omap_plane plane);
 bool dss_feat_color_mode_supported(enum omap_plane plane,
                 enum omap_color_mode color_mode);
+const char *dss_feat_get_clk_source_name(enum dss_clk_source id);
 
 bool dss_has_feature(enum dss_feat_id id);
 void dss_feat_get_reg_field(enum dss_feat_reg_field id, u8 *start, u8 *end);
diff --git a/drivers/video/omap2/dss/hdmi.c b/drivers/video/omap2/dss/hdmi.c
new file mode 100644
index 000000000000..0d44f070ef36
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi.c
@@ -0,0 +1,1332 @@
+/*
+ * hdmi.c
+ *
+ * HDMI interface DSS driver setting for TI's OMAP4 family of processor.
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors: Yong Zhi
+ *      Mythri pk <mythripk@ti.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.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#define DSS_SUBSYS_NAME "HDMI"
+
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/interrupt.h>
+#include <linux/mutex.h>
+#include <linux/delay.h>
+#include <linux/string.h>
+#include <plat/display.h>
+
+#include "dss.h"
+#include "hdmi.h"
+
+static struct {
+        struct mutex lock;
+        struct omap_display_platform_data *pdata;
+        struct platform_device *pdev;
+        void __iomem *base_wp;  /* HDMI wrapper */
+        int code;
+        int mode;
+        u8 edid[HDMI_EDID_MAX_LENGTH];
+        u8 edid_set;
+        bool custom_set;
+        struct hdmi_config cfg;
+} hdmi;
+
+/*
+ * Logic for the below structure :
+ * user enters the CEA or VESA timings by specifying the HDMI/DVI code.
+ * There is a correspondence between CEA/VESA timing and code, please
+ * refer to section 6.3 in HDMI 1.3 specification for timing code.
+ *
+ * In the below structure, cea_vesa_timings corresponds to all OMAP4
+ * supported CEA and VESA timing values.code_cea corresponds to the CEA
+ * code, It is used to get the timing from cea_vesa_timing array.Similarly
+ * with code_vesa. Code_index is used for back mapping, that is once EDID
+ * is read from the TV, EDID is parsed to find the timing values and then
+ * map it to corresponding CEA or VESA index.
+ */
+
+static const struct hdmi_timings cea_vesa_timings[OMAP_HDMI_TIMINGS_NB] = {
+        { {640, 480, 25200, 96, 16, 48, 2, 10, 33} , 0 , 0},
+        { {1280, 720, 74250, 40, 440, 220, 5, 5, 20}, 1, 1},
+        { {1280, 720, 74250, 40, 110, 220, 5, 5, 20}, 1, 1},
+        { {720, 480, 27027, 62, 16, 60, 6, 9, 30}, 0, 0},
+        { {2880, 576, 108000, 256, 48, 272, 5, 5, 39}, 0, 0},
+        { {1440, 240, 27027, 124, 38, 114, 3, 4, 15}, 0, 0},
+        { {1440, 288, 27000, 126, 24, 138, 3, 2, 19}, 0, 0},
+        { {1920, 540, 74250, 44, 528, 148, 5, 2, 15}, 1, 1},
+        { {1920, 540, 74250, 44, 88, 148, 5, 2, 15}, 1, 1},
+        { {1920, 1080, 148500, 44, 88, 148, 5, 4, 36}, 1, 1},
+        { {720, 576, 27000, 64, 12, 68, 5, 5, 39}, 0, 0},
+        { {1440, 576, 54000, 128, 24, 136, 5, 5, 39}, 0, 0},
+        { {1920, 1080, 148500, 44, 528, 148, 5, 4, 36}, 1, 1},
+        { {2880, 480, 108108, 248, 64, 240, 6, 9, 30}, 0, 0},
+        { {1920, 1080, 74250, 44, 638, 148, 5, 4, 36}, 1, 1},
+        /* VESA From Here */
+        { {640, 480, 25175, 96, 16, 48, 2 , 11, 31}, 0, 0},
+        { {800, 600, 40000, 128, 40, 88, 4 , 1, 23}, 1, 1},
+        { {848, 480, 33750, 112, 16, 112, 8 , 6, 23}, 1, 1},
+        { {1280, 768, 79500, 128, 64, 192, 7 , 3, 20}, 1, 0},
+        { {1280, 800, 83500, 128, 72, 200, 6 , 3, 22}, 1, 0},
+        { {1360, 768, 85500, 112, 64, 256, 6 , 3, 18}, 1, 1},
+        { {1280, 960, 108000, 112, 96, 312, 3 , 1, 36}, 1, 1},
+        { {1280, 1024, 108000, 112, 48, 248, 3 , 1, 38}, 1, 1},
+        { {1024, 768, 65000, 136, 24, 160, 6, 3, 29}, 0, 0},
+        { {1400, 1050, 121750, 144, 88, 232, 4, 3, 32}, 1, 0},
+        { {1440, 900, 106500, 152, 80, 232, 6, 3, 25}, 1, 0},
+        { {1680, 1050, 146250, 176 , 104, 280, 6, 3, 30}, 1, 0},
+        { {1366, 768, 85500, 143, 70, 213, 3, 3, 24}, 1, 1},
+        { {1920, 1080, 148500, 44, 148, 80, 5, 4, 36}, 1, 1},
+        { {1280, 768, 68250, 32, 48, 80, 7, 3, 12}, 0, 1},
+        { {1400, 1050, 101000, 32, 48, 80, 4, 3, 23}, 0, 1},
+        { {1680, 1050, 119000, 32, 48, 80, 6, 3, 21}, 0, 1},
+        { {1280, 800, 79500, 32, 48, 80, 6, 3, 14}, 0, 1},
+        { {1280, 720, 74250, 40, 110, 220, 5, 5, 20}, 1, 1}
+};
+
+/*
+ * This is a static mapping array which maps the timing values
+ * with corresponding CEA / VESA code
+ */
+static const int code_index[OMAP_HDMI_TIMINGS_NB] = {
+        1, 19, 4, 2, 37, 6, 21, 20, 5, 16, 17, 29, 31, 35, 32,
+        /* <--15 CEA 17--> vesa*/
+        4, 9, 0xE, 0x17, 0x1C, 0x27, 0x20, 0x23, 0x10, 0x2A,
+        0X2F, 0x3A, 0X51, 0X52, 0x16, 0x29, 0x39, 0x1B
+};
+
+/*
+ * This is reverse static mapping which maps the CEA / VESA code
+ * to the corresponding timing values
+ */
+static const int code_cea[39] = {
+        -1,  0,  3,  3,  2,  8,  5,  5, -1, -1,
+        -1, -1, -1, -1, -1, -1,  9, 10, 10,  1,
+        7,   6,  6, -1, -1, -1, -1, -1, -1, 11,
+        11, 12, 14, -1, -1, 13, 13,  4,  4
+};
+
+static const int code_vesa[85] = {
+        -1, -1, -1, -1, 15, -1, -1, -1, -1, 16,
+        -1, -1, -1, -1, 17, -1, 23, -1, -1, -1,
+        -1, -1, 29, 18, -1, -1, -1, 32, 19, -1,
+        -1, -1, 21, -1, -1, 22, -1, -1, -1, 20,
+        -1, 30, 24, -1, -1, -1, -1, 25, -1, -1,
+        -1, -1, -1, -1, -1, -1, -1, 31, 26, -1,
+        -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
+        -1, -1, -1, -1, -1, -1, -1, -1, -1, -1,
+        -1, 27, 28, -1, 33};
+
+static const u8 edid_header[8] = {0x0, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0x0};
+
+static inline void hdmi_write_reg(const struct hdmi_reg idx, u32 val)
+{
+        __raw_writel(val, hdmi.base_wp + idx.idx);
+}
+
+static inline u32 hdmi_read_reg(const struct hdmi_reg idx)
+{
+        return __raw_readl(hdmi.base_wp + idx.idx);
+}
+
+static inline int hdmi_wait_for_bit_change(const struct hdmi_reg idx,
+                                int b2, int b1, u32 val)
+{
+        u32 t = 0;
+        while (val != REG_GET(idx, b2, b1)) {
+                udelay(1);
+                if (t++ > 10000)
+                        return !val;
+        }
+        return val;
+}
+
+int hdmi_init_display(struct omap_dss_device *dssdev)
+{
+        DSSDBG("init_display\n");
+
+        return 0;
+}
+
+static int hdmi_pll_init(enum hdmi_clk_refsel refsel, int dcofreq,
+                struct hdmi_pll_info *fmt, u16 sd)
+{
+        u32 r;
+
+        /* PLL start always use manual mode */
+        REG_FLD_MOD(PLLCTRL_PLL_CONTROL, 0x0, 0, 0);
+
+        r = hdmi_read_reg(PLLCTRL_CFG1);
+        r = FLD_MOD(r, fmt->regm, 20, 9); /* CFG1_PLL_REGM */
+        r = FLD_MOD(r, fmt->regn, 8, 1);  /* CFG1_PLL_REGN */
+
+        hdmi_write_reg(PLLCTRL_CFG1, r);
+
+        r = hdmi_read_reg(PLLCTRL_CFG2);
+
+        r = FLD_MOD(r, 0x0, 12, 12); /* PLL_HIGHFREQ divide by 2 */
+        r = FLD_MOD(r, 0x1, 13, 13); /* PLL_REFEN */
+        r = FLD_MOD(r, 0x0, 14, 14); /* PHY_CLKINEN de-assert during locking */
+
+        if (dcofreq) {
+                /* divider programming for frequency beyond 1000Mhz */
+                REG_FLD_MOD(PLLCTRL_CFG3, sd, 17, 10);
+                r = FLD_MOD(r, 0x4, 3, 1); /* 1000MHz and 2000MHz */
+        } else {
+                r = FLD_MOD(r, 0x2, 3, 1); /* 500MHz and 1000MHz */
+        }
+
+        hdmi_write_reg(PLLCTRL_CFG2, r);
+
+        r = hdmi_read_reg(PLLCTRL_CFG4);
+        r = FLD_MOD(r, fmt->regm2, 24, 18);
+        r = FLD_MOD(r, fmt->regmf, 17, 0);
+
+        hdmi_write_reg(PLLCTRL_CFG4, r);
+
+        /* go now */
+        REG_FLD_MOD(PLLCTRL_PLL_GO, 0x1, 0, 0);
+
+        /* wait for bit change */
+        if (hdmi_wait_for_bit_change(PLLCTRL_PLL_GO, 0, 0, 1) != 1) {
+                DSSERR("PLL GO bit not set\n");
+                return -ETIMEDOUT;
+        }
+
+        /* Wait till the lock bit is set in PLL status */
+        if (hdmi_wait_for_bit_change(PLLCTRL_PLL_STATUS, 1, 1, 1) != 1) {
+                DSSWARN("cannot lock PLL\n");
+                DSSWARN("CFG1 0x%x\n",
+                        hdmi_read_reg(PLLCTRL_CFG1));
+                DSSWARN("CFG2 0x%x\n",
+                        hdmi_read_reg(PLLCTRL_CFG2));
+                DSSWARN("CFG4 0x%x\n",
+                        hdmi_read_reg(PLLCTRL_CFG4));
+                return -ETIMEDOUT;
+        }
+
+        DSSDBG("PLL locked!\n");
+
+        return 0;
+}
+
+/* PHY_PWR_CMD */
+static int hdmi_set_phy_pwr(enum hdmi_phy_pwr val)
+{
+        /* Command for power control of HDMI PHY */
+        REG_FLD_MOD(HDMI_WP_PWR_CTRL, val, 7, 6);
+
+        /* Status of the power control of HDMI PHY */
+        if (hdmi_wait_for_bit_change(HDMI_WP_PWR_CTRL, 5, 4, val) != val) {
+                DSSERR("Failed to set PHY power mode to %d\n", val);
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+/* PLL_PWR_CMD */
+static int hdmi_set_pll_pwr(enum hdmi_pll_pwr val)
+{
+        /* Command for power control of HDMI PLL */
+        REG_FLD_MOD(HDMI_WP_PWR_CTRL, val, 3, 2);
+
+        /* wait till PHY_PWR_STATUS is set */
+        if (hdmi_wait_for_bit_change(HDMI_WP_PWR_CTRL, 1, 0, val) != val) {
+                DSSERR("Failed to set PHY_PWR_STATUS\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+static int hdmi_pll_reset(void)
+{
+        /* SYSRESET  controlled by power FSM */
+        REG_FLD_MOD(PLLCTRL_PLL_CONTROL, 0x0, 3, 3);
+
+        /* READ 0x0 reset is in progress */
+        if (hdmi_wait_for_bit_change(PLLCTRL_PLL_STATUS, 0, 0, 1) != 1) {
+                DSSERR("Failed to sysreset PLL\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+static int hdmi_phy_init(void)
+{
+        u16 r = 0;
+
+        r = hdmi_set_phy_pwr(HDMI_PHYPWRCMD_LDOON);
+        if (r)
+                return r;
+
+        r = hdmi_set_phy_pwr(HDMI_PHYPWRCMD_TXON);
+        if (r)
+                return r;
+
+        /*
+         * Read address 0 in order to get the SCP reset done completed
+         * Dummy access performed to make sure reset is done
+         */
+        hdmi_read_reg(HDMI_TXPHY_TX_CTRL);
+
+        /*
+         * Write to phy address 0 to configure the clock
+         * use HFBITCLK write HDMI_TXPHY_TX_CONTROL_FREQOUT field
+         */
+        REG_FLD_MOD(HDMI_TXPHY_TX_CTRL, 0x1, 31, 30);
+
+        /* Write to phy address 1 to start HDMI line (TXVALID and TMDSCLKEN) */
+        hdmi_write_reg(HDMI_TXPHY_DIGITAL_CTRL, 0xF0000000);
+
+        /* Setup max LDO voltage */
+        REG_FLD_MOD(HDMI_TXPHY_POWER_CTRL, 0xB, 3, 0);
+
+        /* Write to phy address 3 to change the polarity control */
+        REG_FLD_MOD(HDMI_TXPHY_PAD_CFG_CTRL, 0x1, 27, 27);
+
+        return 0;
+}
+
+static int hdmi_wait_softreset(void)
+{
+        /* reset W1 */
+        REG_FLD_MOD(HDMI_WP_SYSCONFIG, 0x1, 0, 0);
+
+        /* wait till SOFTRESET == 0 */
+        if (hdmi_wait_for_bit_change(HDMI_WP_SYSCONFIG, 0, 0, 0) != 0) {
+                DSSERR("sysconfig reset failed\n");
+                return -ETIMEDOUT;
+        }
+
+        return 0;
+}
+
+static int hdmi_pll_program(struct hdmi_pll_info *fmt)
+{
+        u16 r = 0;
+        enum hdmi_clk_refsel refsel;
+
+        /* wait for wrapper reset */
+        r = hdmi_wait_softreset();
+        if (r)
+                return r;
+
+        r = hdmi_set_pll_pwr(HDMI_PLLPWRCMD_ALLOFF);
+        if (r)
+                return r;
+
+        r = hdmi_set_pll_pwr(HDMI_PLLPWRCMD_BOTHON_ALLCLKS);
+        if (r)
+                return r;
+
+        r = hdmi_pll_reset();
+        if (r)
+                return r;
+
+        refsel = HDMI_REFSEL_SYSCLK;
+
+        r = hdmi_pll_init(refsel, fmt->dcofreq, fmt, fmt->regsd);
+        if (r)
+                return r;
+
+        return 0;
+}
+
+static void hdmi_phy_off(void)
+{
+        hdmi_set_phy_pwr(HDMI_PHYPWRCMD_OFF);
+}
+
+static int hdmi_core_ddc_edid(u8 *pedid, int ext)
+{
+        u32 i, j;
+        char checksum = 0;
+        u32 offset = 0;
+
+        /* Turn on CLK for DDC */
+        REG_FLD_MOD(HDMI_CORE_AV_DPD, 0x7, 2, 0);
+
+        /*
+         * SW HACK : Without the Delay DDC(i2c bus) reads 0 values /
+         * right shifted values( The behavior is not consistent and seen only
+         * with some TV's)
+         */
+        usleep_range(800, 1000);
+
+        if (!ext) {
+                /* Clk SCL Devices */
+                REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0xA, 3, 0);
+
+                /* HDMI_CORE_DDC_STATUS_IN_PROG */
+                if (hdmi_wait_for_bit_change(HDMI_CORE_DDC_STATUS,
+                                                4, 4, 0) != 0) {
+                        DSSERR("Failed to program DDC\n");
+                        return -ETIMEDOUT;
+                }
+
+                /* Clear FIFO */
+                REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x9, 3, 0);
+
+                /* HDMI_CORE_DDC_STATUS_IN_PROG */
+                if (hdmi_wait_for_bit_change(HDMI_CORE_DDC_STATUS,
+                                                4, 4, 0) != 0) {
+                        DSSERR("Failed to program DDC\n");
+                        return -ETIMEDOUT;
+                }
+
+        } else {
+                if (ext % 2 != 0)
+                        offset = 0x80;
+        }
+
+        /* Load Segment Address Register */
+        REG_FLD_MOD(HDMI_CORE_DDC_SEGM, ext/2, 7, 0);
+
+        /* Load Slave Address Register */
+        REG_FLD_MOD(HDMI_CORE_DDC_ADDR, 0xA0 >> 1, 7, 1);
+
+        /* Load Offset Address Register */
+        REG_FLD_MOD(HDMI_CORE_DDC_OFFSET, offset, 7, 0);
+
+        /* Load Byte Count */
+        REG_FLD_MOD(HDMI_CORE_DDC_COUNT1, 0x80, 7, 0);
+        REG_FLD_MOD(HDMI_CORE_DDC_COUNT2, 0x0, 1, 0);
+
+        /* Set DDC_CMD */
+        if (ext)
+                REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x4, 3, 0);
+        else
+                REG_FLD_MOD(HDMI_CORE_DDC_CMD, 0x2, 3, 0);
+
+        /* HDMI_CORE_DDC_STATUS_BUS_LOW */
+        if (REG_GET(HDMI_CORE_DDC_STATUS, 6, 6) == 1) {
+                DSSWARN("I2C Bus Low?\n");
+                return -EIO;
+        }
+        /* HDMI_CORE_DDC_STATUS_NO_ACK */
+        if (REG_GET(HDMI_CORE_DDC_STATUS, 5, 5) == 1) {
+                DSSWARN("I2C No Ack\n");
+                return -EIO;
+        }
+
+        i = ext * 128;
+        j = 0;
+        while (((REG_GET(HDMI_CORE_DDC_STATUS, 4, 4) == 1) ||
+                        (REG_GET(HDMI_CORE_DDC_STATUS, 2, 2) == 0)) &&
+                        j < 128) {
+
+                if (REG_GET(HDMI_CORE_DDC_STATUS, 2, 2) == 0) {
+                        /* FIFO not empty */
+                        pedid[i++] = REG_GET(HDMI_CORE_DDC_DATA, 7, 0);
+                        j++;
+                }
+        }
+
+        for (j = 0; j < 128; j++)
+                checksum += pedid[j];
+
+        if (checksum != 0) {
+                DSSERR("E-EDID checksum failed!!\n");
+                return -EIO;
+        }
+
+        return 0;
+}
+
+static int read_edid(u8 *pedid, u16 max_length)
+{
+        int r = 0, n = 0, i = 0;
+        int max_ext_blocks = (max_length / 128) - 1;
+
+        r = hdmi_core_ddc_edid(pedid, 0);
+        if (r) {
+                return r;
+        } else {
+                n = pedid[0x7e];
+
+                /*
+                 * README: need to comply with max_length set by the caller.
+                 * Better implementation should be to allocate necessary
+                 * memory to store EDID according to nb_block field found
+                 * in first block
+                 */
+                if (n > max_ext_blocks)
+                        n = max_ext_blocks;
+
+                for (i = 1; i <= n; i++) {
+                        r = hdmi_core_ddc_edid(pedid, i);
+                        if (r)
+                                return r;
+                }
+        }
+        return 0;
+}
+
+static int get_timings_index(void)
+{
+        int code;
+
+        if (hdmi.mode == 0)
+                code = code_vesa[hdmi.code];
+        else
+                code = code_cea[hdmi.code];
+
+        if (code == -1) {
+                /* HDMI code 4 corresponds to 640 * 480 VGA */
+                hdmi.code = 4;
+                /* DVI mode 1 corresponds to HDMI 0 to DVI */
+                hdmi.mode = HDMI_DVI;
+
+                code = code_vesa[hdmi.code];
+        }
+        return code;
+}
+
+static struct hdmi_cm hdmi_get_code(struct omap_video_timings *timing)
+{
+        int i = 0, code = -1, temp_vsync = 0, temp_hsync = 0;
+        int timing_vsync = 0, timing_hsync = 0;
+        struct omap_video_timings temp;
+        struct hdmi_cm cm = {-1};
+        DSSDBG("hdmi_get_code\n");
+
+        for (i = 0; i < OMAP_HDMI_TIMINGS_NB; i++) {
+                temp = cea_vesa_timings[i].timings;
+                if ((temp.pixel_clock == timing->pixel_clock) &&
+                        (temp.x_res == timing->x_res) &&
+                        (temp.y_res == timing->y_res)) {
+
+                        temp_hsync = temp.hfp + temp.hsw + temp.hbp;
+                        timing_hsync = timing->hfp + timing->hsw + timing->hbp;
+                        temp_vsync = temp.vfp + temp.vsw + temp.vbp;
+                        timing_vsync = timing->vfp + timing->vsw + timing->vbp;
+
+                        DSSDBG("temp_hsync = %d , temp_vsync = %d"
+                                "timing_hsync = %d, timing_vsync = %d\n",
+                                temp_hsync, temp_hsync,
+                                timing_hsync, timing_vsync);
+
+                        if ((temp_hsync == timing_hsync) &&
+                                        (temp_vsync == timing_vsync)) {
+                                code = i;
+                                cm.code = code_index[i];
+                                if (code < 14)
+                                        cm.mode = HDMI_HDMI;
+                                else
+                                        cm.mode = HDMI_DVI;
+                                DSSDBG("Hdmi_code = %d mode = %d\n",
+                                         cm.code, cm.mode);
+                                break;
+                         }
+                }
+        }
+
+        return cm;
+}
+
+static void get_horz_vert_timing_info(int current_descriptor_addrs, u8 *edid ,
+                struct omap_video_timings *timings)
+{
+        /* X and Y resolution */
+        timings->x_res = (((edid[current_descriptor_addrs + 4] & 0xF0) << 4) |
+                         edid[current_descriptor_addrs + 2]);
+        timings->y_res = (((edid[current_descriptor_addrs + 7] & 0xF0) << 4) |
+                         edid[current_descriptor_addrs + 5]);
+
+        timings->pixel_clock = ((edid[current_descriptor_addrs + 1] << 8) |
+                                edid[current_descriptor_addrs]);
+
+        timings->pixel_clock = 10 * timings->pixel_clock;
+
+        /* HORIZONTAL FRONT PORCH */
+        timings->hfp = edid[current_descriptor_addrs + 8] |
+                        ((edid[current_descriptor_addrs + 11] & 0xc0) << 2);
+        /* HORIZONTAL SYNC WIDTH */
+        timings->hsw = edid[current_descriptor_addrs + 9] |
+                        ((edid[current_descriptor_addrs + 11] & 0x30) << 4);
+        /* HORIZONTAL BACK PORCH */
+        timings->hbp = (((edid[current_descriptor_addrs + 4] & 0x0F) << 8) |
+                        edid[current_descriptor_addrs + 3]) -
+                        (timings->hfp + timings->hsw);
+        /* VERTICAL FRONT PORCH */
+        timings->vfp = ((edid[current_descriptor_addrs + 10] & 0xF0) >> 4) |
+                        ((edid[current_descriptor_addrs + 11] & 0x0f) << 2);
+        /* VERTICAL SYNC WIDTH */
+        timings->vsw = (edid[current_descriptor_addrs + 10] & 0x0F) |
+                        ((edid[current_descriptor_addrs + 11] & 0x03) << 4);
+        /* VERTICAL BACK PORCH */
+        timings->vbp = (((edid[current_descriptor_addrs + 7] & 0x0F) << 8) |
+                        edid[current_descriptor_addrs + 6]) -
+                        (timings->vfp + timings->vsw);
+
+}
+
+/* Description : This function gets the resolution information from EDID */
+static void get_edid_timing_data(u8 *edid)
+{
+        u8 count;
+        u16 current_descriptor_addrs;
+        struct hdmi_cm cm;
+        struct omap_video_timings edid_timings;
+
+        /* seach block 0, there are 4 DTDs arranged in priority order */
+        for (count = 0; count < EDID_SIZE_BLOCK0_TIMING_DESCRIPTOR; count++) {
+                current_descriptor_addrs =
+                        EDID_DESCRIPTOR_BLOCK0_ADDRESS +
+                        count * EDID_TIMING_DESCRIPTOR_SIZE;
+                get_horz_vert_timing_info(current_descriptor_addrs,
+                                edid, &edid_timings);
+                cm = hdmi_get_code(&edid_timings);
+                DSSDBG("Block0[%d] value matches code = %d , mode = %d\n",
+                        count, cm.code, cm.mode);
+                if (cm.code == -1) {
+                        continue;
+                } else {
+                        hdmi.code = cm.code;
+                        hdmi.mode = cm.mode;
+                        DSSDBG("code = %d , mode = %d\n",
+                                hdmi.code, hdmi.mode);
+                        return;
+                }
+        }
+        if (edid[0x7e] != 0x00) {
+                for (count = 0; count < EDID_SIZE_BLOCK1_TIMING_DESCRIPTOR;
+                        count++) {
+                        current_descriptor_addrs =
+                        EDID_DESCRIPTOR_BLOCK1_ADDRESS +
+                        count * EDID_TIMING_DESCRIPTOR_SIZE;
+                        get_horz_vert_timing_info(current_descriptor_addrs,
+                                                edid, &edid_timings);
+                        cm = hdmi_get_code(&edid_timings);
+                        DSSDBG("Block1[%d] value matches code = %d, mode = %d",
+                                count, cm.code, cm.mode);
+                        if (cm.code == -1) {
+                                continue;
+                        } else {
+                                hdmi.code = cm.code;
+                                hdmi.mode = cm.mode;
+                                DSSDBG("code = %d , mode = %d\n",
+                                        hdmi.code, hdmi.mode);
+                                return;
+                        }
+                }
+        }
+
+        DSSINFO("no valid timing found , falling back to VGA\n");
+        hdmi.code = 4; /* setting default value of 640 480 VGA */
+        hdmi.mode = HDMI_DVI;
+}
+
+static void hdmi_read_edid(struct omap_video_timings *dp)
+{
+        int ret = 0, code;
+
+        memset(hdmi.edid, 0, HDMI_EDID_MAX_LENGTH);
+
+        if (!hdmi.edid_set)
+                ret = read_edid(hdmi.edid, HDMI_EDID_MAX_LENGTH);
+
+        if (!ret) {
+                if (!memcmp(hdmi.edid, edid_header, sizeof(edid_header))) {
+                        /* search for timings of default resolution */
+                        get_edid_timing_data(hdmi.edid);
+                        hdmi.edid_set = true;
+                }
+        } else {
+                DSSWARN("failed to read E-EDID\n");
+        }
+
+        if (!hdmi.edid_set) {
+                DSSINFO("fallback to VGA\n");
+                hdmi.code = 4; /* setting default value of 640 480 VGA */
+                hdmi.mode = HDMI_DVI;
+        }
+
+        code = get_timings_index();
+
+        *dp = cea_vesa_timings[code].timings;
+}
+
+static void hdmi_core_init(struct hdmi_core_video_config *video_cfg,
+                        struct hdmi_core_infoframe_avi *avi_cfg,
+                        struct hdmi_core_packet_enable_repeat *repeat_cfg)
+{
+        DSSDBG("Enter hdmi_core_init\n");
+
+        /* video core */
+        video_cfg->ip_bus_width = HDMI_INPUT_8BIT;
+        video_cfg->op_dither_truc = HDMI_OUTPUTTRUNCATION_8BIT;
+        video_cfg->deep_color_pkt = HDMI_DEEPCOLORPACKECTDISABLE;
+        video_cfg->pkt_mode = HDMI_PACKETMODERESERVEDVALUE;
+        video_cfg->hdmi_dvi = HDMI_DVI;
+        video_cfg->tclk_sel_clkmult = HDMI_FPLL10IDCK;
+
+        /* info frame */
+        avi_cfg->db1_format = 0;
+        avi_cfg->db1_active_info = 0;
+        avi_cfg->db1_bar_info_dv = 0;
+        avi_cfg->db1_scan_info = 0;
+        avi_cfg->db2_colorimetry = 0;
+        avi_cfg->db2_aspect_ratio = 0;
+        avi_cfg->db2_active_fmt_ar = 0;
+        avi_cfg->db3_itc = 0;
+        avi_cfg->db3_ec = 0;
+        avi_cfg->db3_q_range = 0;
+        avi_cfg->db3_nup_scaling = 0;
+        avi_cfg->db4_videocode = 0;
+        avi_cfg->db5_pixel_repeat = 0;
+        avi_cfg->db6_7_line_eoftop = 0 ;
+        avi_cfg->db8_9_line_sofbottom = 0;
+        avi_cfg->db10_11_pixel_eofleft = 0;
+        avi_cfg->db12_13_pixel_sofright = 0;
+
+        /* packet enable and repeat */
+        repeat_cfg->audio_pkt = 0;
+        repeat_cfg->audio_pkt_repeat = 0;
+        repeat_cfg->avi_infoframe = 0;
+        repeat_cfg->avi_infoframe_repeat = 0;
+        repeat_cfg->gen_cntrl_pkt = 0;
+        repeat_cfg->gen_cntrl_pkt_repeat = 0;
+        repeat_cfg->generic_pkt = 0;
+        repeat_cfg->generic_pkt_repeat = 0;
+}
+
+static void hdmi_core_powerdown_disable(void)
+{
+        DSSDBG("Enter hdmi_core_powerdown_disable\n");
+        REG_FLD_MOD(HDMI_CORE_CTRL1, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_release(void)
+{
+        DSSDBG("Enter hdmi_core_swreset_release\n");
+        REG_FLD_MOD(HDMI_CORE_SYS_SRST, 0x0, 0, 0);
+}
+
+static void hdmi_core_swreset_assert(void)
+{
+        DSSDBG("Enter hdmi_core_swreset_assert\n");
+        REG_FLD_MOD(HDMI_CORE_SYS_SRST, 0x1, 0, 0);
+}
+
+/* DSS_HDMI_CORE_VIDEO_CONFIG */
+static void hdmi_core_video_config(struct hdmi_core_video_config *cfg)
+{
+        u32 r = 0;
+
+        /* sys_ctrl1 default configuration not tunable */
+        r = hdmi_read_reg(HDMI_CORE_CTRL1);
+        r = FLD_MOD(r, HDMI_CORE_CTRL1_VEN_FOLLOWVSYNC, 5, 5);
+        r = FLD_MOD(r, HDMI_CORE_CTRL1_HEN_FOLLOWHSYNC, 4, 4);
+        r = FLD_MOD(r, HDMI_CORE_CTRL1_BSEL_24BITBUS, 2, 2);
+        r = FLD_MOD(r, HDMI_CORE_CTRL1_EDGE_RISINGEDGE, 1, 1);
+        hdmi_write_reg(HDMI_CORE_CTRL1, r);
+
+        REG_FLD_MOD(HDMI_CORE_SYS_VID_ACEN, cfg->ip_bus_width, 7, 6);
+
+        /* Vid_Mode */
+        r = hdmi_read_reg(HDMI_CORE_SYS_VID_MODE);
+
+        /* dither truncation configuration */
+        if (cfg->op_dither_truc > HDMI_OUTPUTTRUNCATION_12BIT) {
+                r = FLD_MOD(r, cfg->op_dither_truc - 3, 7, 6);
+                r = FLD_MOD(r, 1, 5, 5);
+        } else {
+                r = FLD_MOD(r, cfg->op_dither_truc, 7, 6);
+                r = FLD_MOD(r, 0, 5, 5);
+        }
+        hdmi_write_reg(HDMI_CORE_SYS_VID_MODE, r);
+
+        /* HDMI_Ctrl */
+        r = hdmi_read_reg(HDMI_CORE_AV_HDMI_CTRL);
+        r = FLD_MOD(r, cfg->deep_color_pkt, 6, 6);
+        r = FLD_MOD(r, cfg->pkt_mode, 5, 3);
+        r = FLD_MOD(r, cfg->hdmi_dvi, 0, 0);
+        hdmi_write_reg(HDMI_CORE_AV_HDMI_CTRL, r);
+
+        /* TMDS_CTRL */
+        REG_FLD_MOD(HDMI_CORE_SYS_TMDS_CTRL,
+                cfg->tclk_sel_clkmult, 6, 5);
+}
+
+static void hdmi_core_aux_infoframe_avi_config(
+                struct hdmi_core_infoframe_avi info_avi)
+{
+        u32 val;
+        char sum = 0, checksum = 0;
+
+        sum += 0x82 + 0x002 + 0x00D;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_TYPE, 0x082);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_VERS, 0x002);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_LEN, 0x00D);
+
+        val = (info_avi.db1_format << 5) |
+                (info_avi.db1_active_info << 4) |
+                (info_avi.db1_bar_info_dv << 2) |
+                (info_avi.db1_scan_info);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(0), val);
+        sum += val;
+
+        val = (info_avi.db2_colorimetry << 6) |
+                (info_avi.db2_aspect_ratio << 4) |
+                (info_avi.db2_active_fmt_ar);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(1), val);
+        sum += val;
+
+        val = (info_avi.db3_itc << 7) |
+                (info_avi.db3_ec << 4) |
+                (info_avi.db3_q_range << 2) |
+                (info_avi.db3_nup_scaling);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(2), val);
+        sum += val;
+
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(3), info_avi.db4_videocode);
+        sum += info_avi.db4_videocode;
+
+        val = info_avi.db5_pixel_repeat;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(4), val);
+        sum += val;
+
+        val = info_avi.db6_7_line_eoftop & 0x00FF;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(5), val);
+        sum += val;
+
+        val = ((info_avi.db6_7_line_eoftop >> 8) & 0x00FF);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(6), val);
+        sum += val;
+
+        val = info_avi.db8_9_line_sofbottom & 0x00FF;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(7), val);
+        sum += val;
+
+        val = ((info_avi.db8_9_line_sofbottom >> 8) & 0x00FF);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(8), val);
+        sum += val;
+
+        val = info_avi.db10_11_pixel_eofleft & 0x00FF;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(9), val);
+        sum += val;
+
+        val = ((info_avi.db10_11_pixel_eofleft >> 8) & 0x00FF);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(10), val);
+        sum += val;
+
+        val = info_avi.db12_13_pixel_sofright & 0x00FF;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(11), val);
+        sum += val;
+
+        val = ((info_avi.db12_13_pixel_sofright >> 8) & 0x00FF);
+        hdmi_write_reg(HDMI_CORE_AV_AVI_DBYTE(12), val);
+        sum += val;
+
+        checksum = 0x100 - sum;
+        hdmi_write_reg(HDMI_CORE_AV_AVI_CHSUM, checksum);
+}
+
+static void hdmi_core_av_packet_config(
+                struct hdmi_core_packet_enable_repeat repeat_cfg)
+{
+        /* enable/repeat the infoframe */
+        hdmi_write_reg(HDMI_CORE_AV_PB_CTRL1,
+                (repeat_cfg.audio_pkt << 5) |
+                (repeat_cfg.audio_pkt_repeat << 4) |
+                (repeat_cfg.avi_infoframe << 1) |
+                (repeat_cfg.avi_infoframe_repeat));
+
+        /* enable/repeat the packet */
+        hdmi_write_reg(HDMI_CORE_AV_PB_CTRL2,
+                (repeat_cfg.gen_cntrl_pkt << 3) |
+                (repeat_cfg.gen_cntrl_pkt_repeat << 2) |
+                (repeat_cfg.generic_pkt << 1) |
+                (repeat_cfg.generic_pkt_repeat));
+}
+
+static void hdmi_wp_init(struct omap_video_timings *timings,
+                        struct hdmi_video_format *video_fmt,
+                        struct hdmi_video_interface *video_int)
+{
+        DSSDBG("Enter hdmi_wp_init\n");
+
+        timings->hbp = 0;
+        timings->hfp = 0;
+        timings->hsw = 0;
+        timings->vbp = 0;
+        timings->vfp = 0;
+        timings->vsw = 0;
+
+        video_fmt->packing_mode = HDMI_PACK_10b_RGB_YUV444;
+        video_fmt->y_res = 0;
+        video_fmt->x_res = 0;
+
+        video_int->vsp = 0;
+        video_int->hsp = 0;
+
+        video_int->interlacing = 0;
+        video_int->tm = 0; /* HDMI_TIMING_SLAVE */
+
+}
+
+static void hdmi_wp_video_start(bool start)
+{
+        REG_FLD_MOD(HDMI_WP_VIDEO_CFG, start, 31, 31);
+}
+
+static void hdmi_wp_video_init_format(struct hdmi_video_format *video_fmt,
+        struct omap_video_timings *timings, struct hdmi_config *param)
+{
+        DSSDBG("Enter hdmi_wp_video_init_format\n");
+
+        video_fmt->y_res = param->timings.timings.y_res;
+        video_fmt->x_res = param->timings.timings.x_res;
+
+        timings->hbp = param->timings.timings.hbp;
+        timings->hfp = param->timings.timings.hfp;
+        timings->hsw = param->timings.timings.hsw;
+        timings->vbp = param->timings.timings.vbp;
+        timings->vfp = param->timings.timings.vfp;
+        timings->vsw = param->timings.timings.vsw;
+}
+
+static void hdmi_wp_video_config_format(
+                struct hdmi_video_format *video_fmt)
+{
+        u32 l = 0;
+
+        REG_FLD_MOD(HDMI_WP_VIDEO_CFG, video_fmt->packing_mode, 10, 8);
+
+        l |= FLD_VAL(video_fmt->y_res, 31, 16);
+        l |= FLD_VAL(video_fmt->x_res, 15, 0);
+        hdmi_write_reg(HDMI_WP_VIDEO_SIZE, l);
+}
+
+static void hdmi_wp_video_config_interface(
+                struct hdmi_video_interface *video_int)
+{
+        u32 r;
+        DSSDBG("Enter hdmi_wp_video_config_interface\n");
+
+        r = hdmi_read_reg(HDMI_WP_VIDEO_CFG);
+        r = FLD_MOD(r, video_int->vsp, 7, 7);
+        r = FLD_MOD(r, video_int->hsp, 6, 6);
+        r = FLD_MOD(r, video_int->interlacing, 3, 3);
+        r = FLD_MOD(r, video_int->tm, 1, 0);
+        hdmi_write_reg(HDMI_WP_VIDEO_CFG, r);
+}
+
+static void hdmi_wp_video_config_timing(
+                struct omap_video_timings *timings)
+{
+        u32 timing_h = 0;
+        u32 timing_v = 0;
+
+        DSSDBG("Enter hdmi_wp_video_config_timing\n");
+
+        timing_h |= FLD_VAL(timings->hbp, 31, 20);
+        timing_h |= FLD_VAL(timings->hfp, 19, 8);
+        timing_h |= FLD_VAL(timings->hsw, 7, 0);
+        hdmi_write_reg(HDMI_WP_VIDEO_TIMING_H, timing_h);
+
+        timing_v |= FLD_VAL(timings->vbp, 31, 20);
+        timing_v |= FLD_VAL(timings->vfp, 19, 8);
+        timing_v |= FLD_VAL(timings->vsw, 7, 0);
+        hdmi_write_reg(HDMI_WP_VIDEO_TIMING_V, timing_v);
+}
+
+static void hdmi_basic_configure(struct hdmi_config *cfg)
+{
+        /* HDMI */
+        struct omap_video_timings video_timing;
+        struct hdmi_video_format video_format;
+        struct hdmi_video_interface video_interface;
+        /* HDMI core */
+        struct hdmi_core_infoframe_avi avi_cfg;
+        struct hdmi_core_video_config v_core_cfg;
+        struct hdmi_core_packet_enable_repeat repeat_cfg;
+
+        hdmi_wp_init(&video_timing, &video_format,
+                &video_interface);
+
+        hdmi_core_init(&v_core_cfg,
+                &avi_cfg,
+                &repeat_cfg);
+
+        hdmi_wp_video_init_format(&video_format,
+                        &video_timing, cfg);
+
+        hdmi_wp_video_config_timing(&video_timing);
+
+        /* video config */
+        video_format.packing_mode = HDMI_PACK_24b_RGB_YUV444_YUV422;
+
+        hdmi_wp_video_config_format(&video_format);
+
+        video_interface.vsp = cfg->timings.vsync_pol;
+        video_interface.hsp = cfg->timings.hsync_pol;
+        video_interface.interlacing = cfg->interlace;
+        video_interface.tm = 1 ; /* HDMI_TIMING_MASTER_24BIT */
+
+        hdmi_wp_video_config_interface(&video_interface);
+
+        /*
+         * configure core video part
+         * set software reset in the core
+         */
+        hdmi_core_swreset_assert();
+
+        /* power down off */
+        hdmi_core_powerdown_disable();
+
+        v_core_cfg.pkt_mode = HDMI_PACKETMODE24BITPERPIXEL;
+        v_core_cfg.hdmi_dvi = cfg->cm.mode;
+
+        hdmi_core_video_config(&v_core_cfg);
+
+        /* release software reset in the core */
+        hdmi_core_swreset_release();
+
+        /*
+         * configure packet
+         * info frame video see doc CEA861-D page 65
+         */
+        avi_cfg.db1_format = HDMI_INFOFRAME_AVI_DB1Y_RGB;
+        avi_cfg.db1_active_info =
+                HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_OFF;
+        avi_cfg.db1_bar_info_dv = HDMI_INFOFRAME_AVI_DB1B_NO;
+        avi_cfg.db1_scan_info = HDMI_INFOFRAME_AVI_DB1S_0;
+        avi_cfg.db2_colorimetry = HDMI_INFOFRAME_AVI_DB2C_NO;
+        avi_cfg.db2_aspect_ratio = HDMI_INFOFRAME_AVI_DB2M_NO;
+        avi_cfg.db2_active_fmt_ar = HDMI_INFOFRAME_AVI_DB2R_SAME;
+        avi_cfg.db3_itc = HDMI_INFOFRAME_AVI_DB3ITC_NO;
+        avi_cfg.db3_ec = HDMI_INFOFRAME_AVI_DB3EC_XVYUV601;
+        avi_cfg.db3_q_range = HDMI_INFOFRAME_AVI_DB3Q_DEFAULT;
+        avi_cfg.db3_nup_scaling = HDMI_INFOFRAME_AVI_DB3SC_NO;
+        avi_cfg.db4_videocode = cfg->cm.code;
+        avi_cfg.db5_pixel_repeat = HDMI_INFOFRAME_AVI_DB5PR_NO;
+        avi_cfg.db6_7_line_eoftop = 0;
+        avi_cfg.db8_9_line_sofbottom = 0;
+        avi_cfg.db10_11_pixel_eofleft = 0;
+        avi_cfg.db12_13_pixel_sofright = 0;
+
+        hdmi_core_aux_infoframe_avi_config(avi_cfg);
+
+        /* enable/repeat the infoframe */
+        repeat_cfg.avi_infoframe = HDMI_PACKETENABLE;
+        repeat_cfg.avi_infoframe_repeat = HDMI_PACKETREPEATON;
+        /* wakeup */
+        repeat_cfg.audio_pkt = HDMI_PACKETENABLE;
+        repeat_cfg.audio_pkt_repeat = HDMI_PACKETREPEATON;
+        hdmi_core_av_packet_config(repeat_cfg);
+}
+
+static void update_hdmi_timings(struct hdmi_config *cfg,
+                struct omap_video_timings *timings, int code)
+{
+        cfg->timings.timings.x_res = timings->x_res;
+        cfg->timings.timings.y_res = timings->y_res;
+        cfg->timings.timings.hbp = timings->hbp;
+        cfg->timings.timings.hfp = timings->hfp;
+        cfg->timings.timings.hsw = timings->hsw;
+        cfg->timings.timings.vbp = timings->vbp;
+        cfg->timings.timings.vfp = timings->vfp;
+        cfg->timings.timings.vsw = timings->vsw;
+        cfg->timings.timings.pixel_clock = timings->pixel_clock;
+        cfg->timings.vsync_pol = cea_vesa_timings[code].vsync_pol;
+        cfg->timings.hsync_pol = cea_vesa_timings[code].hsync_pol;
+}
+
+static void hdmi_compute_pll(unsigned long clkin, int phy,
+        int n, struct hdmi_pll_info *pi)
+{
+        unsigned long refclk;
+        u32 mf;
+
+        /*
+         * Input clock is predivided by N + 1
+         * out put of which is reference clk
+         */
+        refclk = clkin / (n + 1);
+        pi->regn = n;
+
+        /*
+         * multiplier is pixel_clk/ref_clk
+         * Multiplying by 100 to avoid fractional part removal
+         */
+        pi->regm = (phy * 100/(refclk))/100;
+        pi->regm2 = 1;
+
+        /*
+         * fractional multiplier is remainder of the difference between
+         * multiplier and actual phy(required pixel clock thus should be
+         * multiplied by 2^18(262144) divided by the reference clock
+         */
+        mf = (phy - pi->regm * refclk) * 262144;
+        pi->regmf = mf/(refclk);
+
+        /*
+         * Dcofreq should be set to 1 if required pixel clock
+         * is greater than 1000MHz
+         */
+        pi->dcofreq = phy > 1000 * 100;
+        pi->regsd = ((pi->regm * clkin / 10) / ((n + 1) * 250) + 5) / 10;
+
+        DSSDBG("M = %d Mf = %d\n", pi->regm, pi->regmf);
+        DSSDBG("range = %d sd = %d\n", pi->dcofreq, pi->regsd);
+}
+
+static void hdmi_enable_clocks(int enable)
+{
+        if (enable)
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK |
+                                DSS_CLK_SYSCK | DSS_CLK_VIDFCK);
+        else
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK |
+                                DSS_CLK_SYSCK | DSS_CLK_VIDFCK);
+}
+
+static int hdmi_power_on(struct omap_dss_device *dssdev)
+{
+        int r, code = 0;
+        struct hdmi_pll_info pll_data;
+        struct omap_video_timings *p;
+        int clkin, n, phy;
+
+        hdmi_enable_clocks(1);
+
+        dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 0);
+
+        p = &dssdev->panel.timings;
+
+        DSSDBG("hdmi_power_on x_res= %d y_res = %d\n",
+                dssdev->panel.timings.x_res,
+                dssdev->panel.timings.y_res);
+
+        if (!hdmi.custom_set) {
+                DSSDBG("Read EDID as no EDID is not set on poweron\n");
+                hdmi_read_edid(p);
+        }
+        code = get_timings_index();
+        dssdev->panel.timings = cea_vesa_timings[code].timings;
+        update_hdmi_timings(&hdmi.cfg, p, code);
+
+        clkin = 3840; /* 38.4 MHz */
+        n = 15; /* this is a constant for our math */
+        phy = p->pixel_clock;
+
+        hdmi_compute_pll(clkin, phy, n, &pll_data);
+
+        hdmi_wp_video_start(0);
+
+        /* config the PLL and PHY first */
+        r = hdmi_pll_program(&pll_data);
+        if (r) {
+                DSSDBG("Failed to lock PLL\n");
+                goto err;
+        }
+
+        r = hdmi_phy_init();
+        if (r) {
+                DSSDBG("Failed to start PHY\n");
+                goto err;
+        }
+
+        hdmi.cfg.cm.mode = hdmi.mode;
+        hdmi.cfg.cm.code = hdmi.code;
+        hdmi_basic_configure(&hdmi.cfg);
+
+        /* Make selection of HDMI in DSS */
+        dss_select_hdmi_venc_clk_source(DSS_HDMI_M_PCLK);
+
+        /* Select the dispc clock source as PRCM clock, to ensure that it is not
+         * DSI PLL source as the clock selected by DSI PLL might not be
+         * sufficient for the resolution selected / that can be changed
+         * dynamically by user. This can be moved to single location , say
+         * Boardfile.
+         */
+        dss_select_dispc_clk_source(DSS_CLK_SRC_FCK);
+
+        /* bypass TV gamma table */
+        dispc_enable_gamma_table(0);
+
+        /* tv size */
+        dispc_set_digit_size(dssdev->panel.timings.x_res,
+                        dssdev->panel.timings.y_res);
+
+        dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 1);
+
+        hdmi_wp_video_start(1);
+
+        return 0;
+err:
+        hdmi_enable_clocks(0);
+        return -EIO;
+}
+
+static void hdmi_power_off(struct omap_dss_device *dssdev)
+{
+        dispc_enable_channel(OMAP_DSS_CHANNEL_DIGIT, 0);
+
+        hdmi_wp_video_start(0);
+        hdmi_phy_off();
+        hdmi_set_pll_pwr(HDMI_PLLPWRCMD_ALLOFF);
+        hdmi_enable_clocks(0);
+
+        hdmi.edid_set = 0;
+}
+
+int omapdss_hdmi_display_check_timing(struct omap_dss_device *dssdev,
+                                        struct omap_video_timings *timings)
+{
+        struct hdmi_cm cm;
+
+        cm = hdmi_get_code(timings);
+        if (cm.code == -1) {
+                DSSERR("Invalid timing entered\n");
+                return -EINVAL;
+        }
+
+        return 0;
+
+}
+
+void omapdss_hdmi_display_set_timing(struct omap_dss_device *dssdev)
+{
+        struct hdmi_cm cm;
+
+        hdmi.custom_set = 1;
+        cm = hdmi_get_code(&dssdev->panel.timings);
+        hdmi.code = cm.code;
+        hdmi.mode = cm.mode;
+        omapdss_hdmi_display_enable(dssdev);
+        hdmi.custom_set = 0;
+}
+
+int omapdss_hdmi_display_enable(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        DSSDBG("ENTER hdmi_display_enable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        r = omap_dss_start_device(dssdev);
+        if (r) {
+                DSSERR("failed to start device\n");
+                goto err0;
+        }
+
+        if (dssdev->platform_enable) {
+                r = dssdev->platform_enable(dssdev);
+                if (r) {
+                        DSSERR("failed to enable GPIO's\n");
+                        goto err1;
+                }
+        }
+
+        r = hdmi_power_on(dssdev);
+        if (r) {
+                DSSERR("failed to power on device\n");
+                goto err2;
+        }
+
+        mutex_unlock(&hdmi.lock);
+        return 0;
+
+err2:
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
+err1:
+        omap_dss_stop_device(dssdev);
+err0:
+        mutex_unlock(&hdmi.lock);
+        return r;
+}
+
+void omapdss_hdmi_display_disable(struct omap_dss_device *dssdev)
+{
+        DSSDBG("Enter hdmi_display_disable\n");
+
+        mutex_lock(&hdmi.lock);
+
+        hdmi_power_off(dssdev);
+
+        if (dssdev->platform_disable)
+                dssdev->platform_disable(dssdev);
+
+        omap_dss_stop_device(dssdev);
+
+        mutex_unlock(&hdmi.lock);
+}
+
+/* HDMI HW IP initialisation */
+static int omapdss_hdmihw_probe(struct platform_device *pdev)
+{
+        struct resource *hdmi_mem;
+
+        hdmi.pdata = pdev->dev.platform_data;
+        hdmi.pdev = pdev;
+
+        mutex_init(&hdmi.lock);
+
+        hdmi_mem = platform_get_resource(hdmi.pdev, IORESOURCE_MEM, 0);
+        if (!hdmi_mem) {
+                DSSERR("can't get IORESOURCE_MEM HDMI\n");
+                return -EINVAL;
+        }
+
+        /* Base address taken from platform */
+        hdmi.base_wp = ioremap(hdmi_mem->start, resource_size(hdmi_mem));
+        if (!hdmi.base_wp) {
+                DSSERR("can't ioremap WP\n");
+                return -ENOMEM;
+        }
+
+        hdmi_panel_init();
+
+        return 0;
+}
+
+static int omapdss_hdmihw_remove(struct platform_device *pdev)
+{
+        hdmi_panel_exit();
+
+        iounmap(hdmi.base_wp);
+
+        return 0;
+}
+
+static struct platform_driver omapdss_hdmihw_driver = {
+        .probe          = omapdss_hdmihw_probe,
+        .remove         = omapdss_hdmihw_remove,
+        .driver         = {
+                .name   = "omapdss_hdmi",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int hdmi_init_platform_driver(void)
+{
+        return platform_driver_register(&omapdss_hdmihw_driver);
+}
+
+void hdmi_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omapdss_hdmihw_driver);
+}
diff --git a/drivers/video/omap2/dss/hdmi.h b/drivers/video/omap2/dss/hdmi.h
new file mode 100644
index 000000000000..9887ab96da3c
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi.h
@@ -0,0 +1,415 @@
+/*
+ * hdmi.h
+ *
+ * HDMI driver definition for TI OMAP4 processors.
+ *
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.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.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#ifndef _OMAP4_DSS_HDMI_H_
+#define _OMAP4_DSS_HDMI_H_
+
+#include <linux/string.h>
+#include <plat/display.h>
+
+#define HDMI_WP         0x0
+#define HDMI_CORE_SYS           0x400
+#define HDMI_CORE_AV            0x900
+#define HDMI_PLLCTRL            0x200
+#define HDMI_PHY                0x300
+
+struct hdmi_reg { u16 idx; };
+
+#define HDMI_REG(idx)                   ((const struct hdmi_reg) { idx })
+
+/* HDMI Wrapper */
+#define HDMI_WP_REG(idx)                        HDMI_REG(HDMI_WP + idx)
+
+#define HDMI_WP_REVISION                        HDMI_WP_REG(0x0)
+#define HDMI_WP_SYSCONFIG                       HDMI_WP_REG(0x10)
+#define HDMI_WP_IRQSTATUS_RAW                   HDMI_WP_REG(0x24)
+#define HDMI_WP_IRQSTATUS                       HDMI_WP_REG(0x28)
+#define HDMI_WP_PWR_CTRL                        HDMI_WP_REG(0x40)
+#define HDMI_WP_IRQENABLE_SET                   HDMI_WP_REG(0x2C)
+#define HDMI_WP_VIDEO_CFG                       HDMI_WP_REG(0x50)
+#define HDMI_WP_VIDEO_SIZE                      HDMI_WP_REG(0x60)
+#define HDMI_WP_VIDEO_TIMING_H                  HDMI_WP_REG(0x68)
+#define HDMI_WP_VIDEO_TIMING_V                  HDMI_WP_REG(0x6C)
+#define HDMI_WP_WP_CLK                          HDMI_WP_REG(0x70)
+
+/* HDMI IP Core System */
+#define HDMI_CORE_SYS_REG(idx)                  HDMI_REG(HDMI_CORE_SYS + idx)
+
+#define HDMI_CORE_SYS_VND_IDL                   HDMI_CORE_SYS_REG(0x0)
+#define HDMI_CORE_SYS_DEV_IDL                   HDMI_CORE_SYS_REG(0x8)
+#define HDMI_CORE_SYS_DEV_IDH                   HDMI_CORE_SYS_REG(0xC)
+#define HDMI_CORE_SYS_DEV_REV                   HDMI_CORE_SYS_REG(0x10)
+#define HDMI_CORE_SYS_SRST                      HDMI_CORE_SYS_REG(0x14)
+#define HDMI_CORE_CTRL1                 HDMI_CORE_SYS_REG(0x20)
+#define HDMI_CORE_SYS_SYS_STAT                  HDMI_CORE_SYS_REG(0x24)
+#define HDMI_CORE_SYS_VID_ACEN                  HDMI_CORE_SYS_REG(0x124)
+#define HDMI_CORE_SYS_VID_MODE                  HDMI_CORE_SYS_REG(0x128)
+#define HDMI_CORE_SYS_INTR_STATE                HDMI_CORE_SYS_REG(0x1C0)
+#define HDMI_CORE_SYS_INTR1                     HDMI_CORE_SYS_REG(0x1C4)
+#define HDMI_CORE_SYS_INTR2                     HDMI_CORE_SYS_REG(0x1C8)
+#define HDMI_CORE_SYS_INTR3                     HDMI_CORE_SYS_REG(0x1CC)
+#define HDMI_CORE_SYS_INTR4                     HDMI_CORE_SYS_REG(0x1D0)
+#define HDMI_CORE_SYS_UMASK1                    HDMI_CORE_SYS_REG(0x1D4)
+#define HDMI_CORE_SYS_TMDS_CTRL         HDMI_CORE_SYS_REG(0x208)
+#define HDMI_CORE_SYS_DE_DLY                    HDMI_CORE_SYS_REG(0xC8)
+#define HDMI_CORE_SYS_DE_CTRL                   HDMI_CORE_SYS_REG(0xCC)
+#define HDMI_CORE_SYS_DE_TOP                    HDMI_CORE_SYS_REG(0xD0)
+#define HDMI_CORE_SYS_DE_CNTL                   HDMI_CORE_SYS_REG(0xD8)
+#define HDMI_CORE_SYS_DE_CNTH                   HDMI_CORE_SYS_REG(0xDC)
+#define HDMI_CORE_SYS_DE_LINL                   HDMI_CORE_SYS_REG(0xE0)
+#define HDMI_CORE_SYS_DE_LINH_1         HDMI_CORE_SYS_REG(0xE4)
+#define HDMI_CORE_CTRL1_VEN_FOLLOWVSYNC 0x1
+#define HDMI_CORE_CTRL1_HEN_FOLLOWHSYNC 0x1
+#define HDMI_CORE_CTRL1_BSEL_24BITBUS           0x1
+#define HDMI_CORE_CTRL1_EDGE_RISINGEDGE 0x1
+
+/* HDMI DDC E-DID */
+#define HDMI_CORE_DDC_CMD                       HDMI_CORE_SYS_REG(0x3CC)
+#define HDMI_CORE_DDC_STATUS                    HDMI_CORE_SYS_REG(0x3C8)
+#define HDMI_CORE_DDC_ADDR                      HDMI_CORE_SYS_REG(0x3B4)
+#define HDMI_CORE_DDC_OFFSET                    HDMI_CORE_SYS_REG(0x3BC)
+#define HDMI_CORE_DDC_COUNT1                    HDMI_CORE_SYS_REG(0x3C0)
+#define HDMI_CORE_DDC_COUNT2                    HDMI_CORE_SYS_REG(0x3C4)
+#define HDMI_CORE_DDC_DATA                      HDMI_CORE_SYS_REG(0x3D0)
+#define HDMI_CORE_DDC_SEGM                      HDMI_CORE_SYS_REG(0x3B8)
+
+/* HDMI IP Core Audio Video */
+#define HDMI_CORE_AV_REG(idx)                   HDMI_REG(HDMI_CORE_AV + idx)
+
+#define HDMI_CORE_AV_HDMI_CTRL                  HDMI_CORE_AV_REG(0xBC)
+#define HDMI_CORE_AV_DPD                        HDMI_CORE_AV_REG(0xF4)
+#define HDMI_CORE_AV_PB_CTRL1                   HDMI_CORE_AV_REG(0xF8)
+#define HDMI_CORE_AV_PB_CTRL2                   HDMI_CORE_AV_REG(0xFC)
+#define HDMI_CORE_AV_AVI_TYPE                   HDMI_CORE_AV_REG(0x100)
+#define HDMI_CORE_AV_AVI_VERS                   HDMI_CORE_AV_REG(0x104)
+#define HDMI_CORE_AV_AVI_LEN                    HDMI_CORE_AV_REG(0x108)
+#define HDMI_CORE_AV_AVI_CHSUM                  HDMI_CORE_AV_REG(0x10C)
+#define HDMI_CORE_AV_AVI_DBYTE(n)               HDMI_CORE_AV_REG(n * 4 + 0x110)
+#define HDMI_CORE_AV_AVI_DBYTE_NELEMS           HDMI_CORE_AV_REG(15)
+#define HDMI_CORE_AV_SPD_DBYTE                  HDMI_CORE_AV_REG(0x190)
+#define HDMI_CORE_AV_SPD_DBYTE_NELEMS           HDMI_CORE_AV_REG(27)
+#define HDMI_CORE_AV_MPEG_DBYTE         HDMI_CORE_AV_REG(0x290)
+#define HDMI_CORE_AV_MPEG_DBYTE_NELEMS          HDMI_CORE_AV_REG(27)
+#define HDMI_CORE_AV_GEN_DBYTE                  HDMI_CORE_AV_REG(0x300)
+#define HDMI_CORE_AV_GEN_DBYTE_NELEMS           HDMI_CORE_AV_REG(31)
+#define HDMI_CORE_AV_GEN2_DBYTE         HDMI_CORE_AV_REG(0x380)
+#define HDMI_CORE_AV_GEN2_DBYTE_NELEMS          HDMI_CORE_AV_REG(31)
+#define HDMI_CORE_AV_ACR_CTRL                   HDMI_CORE_AV_REG(0x4)
+#define HDMI_CORE_AV_FREQ_SVAL                  HDMI_CORE_AV_REG(0x8)
+#define HDMI_CORE_AV_N_SVAL1                    HDMI_CORE_AV_REG(0xC)
+#define HDMI_CORE_AV_N_SVAL2                    HDMI_CORE_AV_REG(0x10)
+#define HDMI_CORE_AV_N_SVAL3                    HDMI_CORE_AV_REG(0x14)
+#define HDMI_CORE_AV_CTS_SVAL1                  HDMI_CORE_AV_REG(0x18)
+#define HDMI_CORE_AV_CTS_SVAL2                  HDMI_CORE_AV_REG(0x1C)
+#define HDMI_CORE_AV_CTS_SVAL3                  HDMI_CORE_AV_REG(0x20)
+#define HDMI_CORE_AV_CTS_HVAL1                  HDMI_CORE_AV_REG(0x24)
+#define HDMI_CORE_AV_CTS_HVAL2                  HDMI_CORE_AV_REG(0x28)
+#define HDMI_CORE_AV_CTS_HVAL3                  HDMI_CORE_AV_REG(0x2C)
+#define HDMI_CORE_AV_AUD_MODE                   HDMI_CORE_AV_REG(0x50)
+#define HDMI_CORE_AV_SPDIF_CTRL         HDMI_CORE_AV_REG(0x54)
+#define HDMI_CORE_AV_HW_SPDIF_FS                HDMI_CORE_AV_REG(0x60)
+#define HDMI_CORE_AV_SWAP_I2S                   HDMI_CORE_AV_REG(0x64)
+#define HDMI_CORE_AV_SPDIF_ERTH         HDMI_CORE_AV_REG(0x6C)
+#define HDMI_CORE_AV_I2S_IN_MAP         HDMI_CORE_AV_REG(0x70)
+#define HDMI_CORE_AV_I2S_IN_CTRL                HDMI_CORE_AV_REG(0x74)
+#define HDMI_CORE_AV_I2S_CHST0                  HDMI_CORE_AV_REG(0x78)
+#define HDMI_CORE_AV_I2S_CHST1                  HDMI_CORE_AV_REG(0x7C)
+#define HDMI_CORE_AV_I2S_CHST2                  HDMI_CORE_AV_REG(0x80)
+#define HDMI_CORE_AV_I2S_CHST4                  HDMI_CORE_AV_REG(0x84)
+#define HDMI_CORE_AV_I2S_CHST5                  HDMI_CORE_AV_REG(0x88)
+#define HDMI_CORE_AV_ASRC                       HDMI_CORE_AV_REG(0x8C)
+#define HDMI_CORE_AV_I2S_IN_LEN         HDMI_CORE_AV_REG(0x90)
+#define HDMI_CORE_AV_HDMI_CTRL                  HDMI_CORE_AV_REG(0xBC)
+#define HDMI_CORE_AV_AUDO_TXSTAT                HDMI_CORE_AV_REG(0xC0)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_1           HDMI_CORE_AV_REG(0xCC)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_2           HDMI_CORE_AV_REG(0xD0)
+#define HDMI_CORE_AV_AUD_PAR_BUSCLK_3           HDMI_CORE_AV_REG(0xD4)
+#define HDMI_CORE_AV_TEST_TXCTRL                HDMI_CORE_AV_REG(0xF0)
+#define HDMI_CORE_AV_DPD                        HDMI_CORE_AV_REG(0xF4)
+#define HDMI_CORE_AV_PB_CTRL1                   HDMI_CORE_AV_REG(0xF8)
+#define HDMI_CORE_AV_PB_CTRL2                   HDMI_CORE_AV_REG(0xFC)
+#define HDMI_CORE_AV_AVI_TYPE                   HDMI_CORE_AV_REG(0x100)
+#define HDMI_CORE_AV_AVI_VERS                   HDMI_CORE_AV_REG(0x104)
+#define HDMI_CORE_AV_AVI_LEN                    HDMI_CORE_AV_REG(0x108)
+#define HDMI_CORE_AV_AVI_CHSUM                  HDMI_CORE_AV_REG(0x10C)
+#define HDMI_CORE_AV_SPD_TYPE                   HDMI_CORE_AV_REG(0x180)
+#define HDMI_CORE_AV_SPD_VERS                   HDMI_CORE_AV_REG(0x184)
+#define HDMI_CORE_AV_SPD_LEN                    HDMI_CORE_AV_REG(0x188)
+#define HDMI_CORE_AV_SPD_CHSUM                  HDMI_CORE_AV_REG(0x18C)
+#define HDMI_CORE_AV_MPEG_TYPE                  HDMI_CORE_AV_REG(0x280)
+#define HDMI_CORE_AV_MPEG_VERS                  HDMI_CORE_AV_REG(0x284)
+#define HDMI_CORE_AV_MPEG_LEN                   HDMI_CORE_AV_REG(0x288)
+#define HDMI_CORE_AV_MPEG_CHSUM         HDMI_CORE_AV_REG(0x28C)
+#define HDMI_CORE_AV_CP_BYTE1                   HDMI_CORE_AV_REG(0x37C)
+#define HDMI_CORE_AV_CEC_ADDR_ID                HDMI_CORE_AV_REG(0x3FC)
+#define HDMI_CORE_AV_SPD_DBYTE_ELSIZE           0x4
+#define HDMI_CORE_AV_GEN2_DBYTE_ELSIZE          0x4
+#define HDMI_CORE_AV_MPEG_DBYTE_ELSIZE          0x4
+#define HDMI_CORE_AV_GEN_DBYTE_ELSIZE           0x4
+
+/* PLL */
+#define HDMI_PLL_REG(idx)                       HDMI_REG(HDMI_PLLCTRL + idx)
+
+#define PLLCTRL_PLL_CONTROL                     HDMI_PLL_REG(0x0)
+#define PLLCTRL_PLL_STATUS                      HDMI_PLL_REG(0x4)
+#define PLLCTRL_PLL_GO                          HDMI_PLL_REG(0x8)
+#define PLLCTRL_CFG1                            HDMI_PLL_REG(0xC)
+#define PLLCTRL_CFG2                            HDMI_PLL_REG(0x10)
+#define PLLCTRL_CFG3                            HDMI_PLL_REG(0x14)
+#define PLLCTRL_CFG4                            HDMI_PLL_REG(0x20)
+
+/* HDMI PHY */
+#define HDMI_PHY_REG(idx)                       HDMI_REG(HDMI_PHY + idx)
+
+#define HDMI_TXPHY_TX_CTRL                      HDMI_PHY_REG(0x0)
+#define HDMI_TXPHY_DIGITAL_CTRL         HDMI_PHY_REG(0x4)
+#define HDMI_TXPHY_POWER_CTRL                   HDMI_PHY_REG(0x8)
+#define HDMI_TXPHY_PAD_CFG_CTRL         HDMI_PHY_REG(0xC)
+
+/* HDMI EDID Length  */
+#define HDMI_EDID_MAX_LENGTH                    256
+#define EDID_TIMING_DESCRIPTOR_SIZE             0x12
+#define EDID_DESCRIPTOR_BLOCK0_ADDRESS          0x36
+#define EDID_DESCRIPTOR_BLOCK1_ADDRESS          0x80
+#define EDID_SIZE_BLOCK0_TIMING_DESCRIPTOR      4
+#define EDID_SIZE_BLOCK1_TIMING_DESCRIPTOR      4
+
+#define OMAP_HDMI_TIMINGS_NB                    34
+
+#define REG_FLD_MOD(idx, val, start, end) \
+        hdmi_write_reg(idx, FLD_MOD(hdmi_read_reg(idx), val, start, end))
+#define REG_GET(idx, start, end) \
+        FLD_GET(hdmi_read_reg(idx), start, end)
+
+/* HDMI timing structure */
+struct hdmi_timings {
+        struct omap_video_timings timings;
+        int vsync_pol;
+        int hsync_pol;
+};
+
+enum hdmi_phy_pwr {
+        HDMI_PHYPWRCMD_OFF = 0,
+        HDMI_PHYPWRCMD_LDOON = 1,
+        HDMI_PHYPWRCMD_TXON = 2
+};
+
+enum hdmi_pll_pwr {
+        HDMI_PLLPWRCMD_ALLOFF = 0,
+        HDMI_PLLPWRCMD_PLLONLY = 1,
+        HDMI_PLLPWRCMD_BOTHON_ALLCLKS = 2,
+        HDMI_PLLPWRCMD_BOTHON_NOPHYCLK = 3
+};
+
+enum hdmi_clk_refsel {
+        HDMI_REFSEL_PCLK = 0,
+        HDMI_REFSEL_REF1 = 1,
+        HDMI_REFSEL_REF2 = 2,
+        HDMI_REFSEL_SYSCLK = 3
+};
+
+enum hdmi_core_inputbus_width {
+        HDMI_INPUT_8BIT = 0,
+        HDMI_INPUT_10BIT = 1,
+        HDMI_INPUT_12BIT = 2
+};
+
+enum hdmi_core_dither_trunc {
+        HDMI_OUTPUTTRUNCATION_8BIT = 0,
+        HDMI_OUTPUTTRUNCATION_10BIT = 1,
+        HDMI_OUTPUTTRUNCATION_12BIT = 2,
+        HDMI_OUTPUTDITHER_8BIT = 3,
+        HDMI_OUTPUTDITHER_10BIT = 4,
+        HDMI_OUTPUTDITHER_12BIT = 5
+};
+
+enum hdmi_core_deepcolor_ed {
+        HDMI_DEEPCOLORPACKECTDISABLE = 0,
+        HDMI_DEEPCOLORPACKECTENABLE = 1
+};
+
+enum hdmi_core_packet_mode {
+        HDMI_PACKETMODERESERVEDVALUE = 0,
+        HDMI_PACKETMODE24BITPERPIXEL = 4,
+        HDMI_PACKETMODE30BITPERPIXEL = 5,
+        HDMI_PACKETMODE36BITPERPIXEL = 6,
+        HDMI_PACKETMODE48BITPERPIXEL = 7
+};
+
+enum hdmi_core_hdmi_dvi {
+        HDMI_DVI = 0,
+        HDMI_HDMI = 1
+};
+
+enum hdmi_core_tclkselclkmult {
+        HDMI_FPLL05IDCK = 0,
+        HDMI_FPLL10IDCK = 1,
+        HDMI_FPLL20IDCK = 2,
+        HDMI_FPLL40IDCK = 3
+};
+
+enum hdmi_core_packet_ctrl {
+        HDMI_PACKETENABLE = 1,
+        HDMI_PACKETDISABLE = 0,
+        HDMI_PACKETREPEATON = 1,
+        HDMI_PACKETREPEATOFF = 0
+};
+
+/* INFOFRAME_AVI_ definitions */
+enum hdmi_core_infoframe {
+        HDMI_INFOFRAME_AVI_DB1Y_RGB = 0,
+        HDMI_INFOFRAME_AVI_DB1Y_YUV422 = 1,
+        HDMI_INFOFRAME_AVI_DB1Y_YUV444 = 2,
+        HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_OFF = 0,
+        HDMI_INFOFRAME_AVI_DB1A_ACTIVE_FORMAT_ON =  1,
+        HDMI_INFOFRAME_AVI_DB1B_NO = 0,
+        HDMI_INFOFRAME_AVI_DB1B_VERT = 1,
+        HDMI_INFOFRAME_AVI_DB1B_HORI = 2,
+        HDMI_INFOFRAME_AVI_DB1B_VERTHORI = 3,
+        HDMI_INFOFRAME_AVI_DB1S_0 = 0,
+        HDMI_INFOFRAME_AVI_DB1S_1 = 1,
+        HDMI_INFOFRAME_AVI_DB1S_2 = 2,
+        HDMI_INFOFRAME_AVI_DB2C_NO = 0,
+        HDMI_INFOFRAME_AVI_DB2C_ITU601 = 1,
+        HDMI_INFOFRAME_AVI_DB2C_ITU709 = 2,
+        HDMI_INFOFRAME_AVI_DB2C_EC_EXTENDED = 3,
+        HDMI_INFOFRAME_AVI_DB2M_NO = 0,
+        HDMI_INFOFRAME_AVI_DB2M_43 = 1,
+        HDMI_INFOFRAME_AVI_DB2M_169 = 2,
+        HDMI_INFOFRAME_AVI_DB2R_SAME = 8,
+        HDMI_INFOFRAME_AVI_DB2R_43 = 9,
+        HDMI_INFOFRAME_AVI_DB2R_169 = 10,
+        HDMI_INFOFRAME_AVI_DB2R_149 = 11,
+        HDMI_INFOFRAME_AVI_DB3ITC_NO = 0,
+        HDMI_INFOFRAME_AVI_DB3ITC_YES = 1,
+        HDMI_INFOFRAME_AVI_DB3EC_XVYUV601 = 0,
+        HDMI_INFOFRAME_AVI_DB3EC_XVYUV709 = 1,
+        HDMI_INFOFRAME_AVI_DB3Q_DEFAULT = 0,
+        HDMI_INFOFRAME_AVI_DB3Q_LR = 1,
+        HDMI_INFOFRAME_AVI_DB3Q_FR = 2,
+        HDMI_INFOFRAME_AVI_DB3SC_NO = 0,
+        HDMI_INFOFRAME_AVI_DB3SC_HORI = 1,
+        HDMI_INFOFRAME_AVI_DB3SC_VERT = 2,
+        HDMI_INFOFRAME_AVI_DB3SC_HORIVERT = 3,
+        HDMI_INFOFRAME_AVI_DB5PR_NO = 0,
+        HDMI_INFOFRAME_AVI_DB5PR_2 = 1,
+        HDMI_INFOFRAME_AVI_DB5PR_3 = 2,
+        HDMI_INFOFRAME_AVI_DB5PR_4 = 3,
+        HDMI_INFOFRAME_AVI_DB5PR_5 = 4,
+        HDMI_INFOFRAME_AVI_DB5PR_6 = 5,
+        HDMI_INFOFRAME_AVI_DB5PR_7 = 6,
+        HDMI_INFOFRAME_AVI_DB5PR_8 = 7,
+        HDMI_INFOFRAME_AVI_DB5PR_9 = 8,
+        HDMI_INFOFRAME_AVI_DB5PR_10 = 9
+};
+
+enum hdmi_packing_mode {
+        HDMI_PACK_10b_RGB_YUV444 = 0,
+        HDMI_PACK_24b_RGB_YUV444_YUV422 = 1,
+        HDMI_PACK_20b_YUV422 = 2,
+        HDMI_PACK_ALREADYPACKED = 7
+};
+
+struct hdmi_core_video_config {
+        enum hdmi_core_inputbus_width   ip_bus_width;
+        enum hdmi_core_dither_trunc     op_dither_truc;
+        enum hdmi_core_deepcolor_ed     deep_color_pkt;
+        enum hdmi_core_packet_mode      pkt_mode;
+        enum hdmi_core_hdmi_dvi         hdmi_dvi;
+        enum hdmi_core_tclkselclkmult   tclk_sel_clkmult;
+};
+
+/*
+ * Refer to section 8.2 in HDMI 1.3 specification for
+ * details about infoframe databytes
+ */
+struct hdmi_core_infoframe_avi {
+        u8      db1_format;
+                /* Y0, Y1 rgb,yCbCr */
+        u8      db1_active_info;
+                /* A0  Active information Present */
+        u8      db1_bar_info_dv;
+                /* B0, B1 Bar info data valid */
+        u8      db1_scan_info;
+                /* S0, S1 scan information */
+        u8      db2_colorimetry;
+                /* C0, C1 colorimetry */
+        u8      db2_aspect_ratio;
+                /* M0, M1 Aspect ratio (4:3, 16:9) */
+        u8      db2_active_fmt_ar;
+                /* R0...R3 Active format aspect ratio */
+        u8      db3_itc;
+                /* ITC IT content. */
+        u8      db3_ec;
+                /* EC0, EC1, EC2 Extended colorimetry */
+        u8      db3_q_range;
+                /* Q1, Q0 Quantization range */
+        u8      db3_nup_scaling;
+                /* SC1, SC0 Non-uniform picture scaling */
+        u8      db4_videocode;
+                /* VIC0..6 Video format identification */
+        u8      db5_pixel_repeat;
+                /* PR0..PR3 Pixel repetition factor */
+        u16     db6_7_line_eoftop;
+                /* Line number end of top bar */
+        u16     db8_9_line_sofbottom;
+                /* Line number start of bottom bar */
+        u16     db10_11_pixel_eofleft;
+                /* Pixel number end of left bar */
+        u16     db12_13_pixel_sofright;
+                /* Pixel number start of right bar */
+};
+
+struct hdmi_core_packet_enable_repeat {
+        u32     audio_pkt;
+        u32     audio_pkt_repeat;
+        u32     avi_infoframe;
+        u32     avi_infoframe_repeat;
+        u32     gen_cntrl_pkt;
+        u32     gen_cntrl_pkt_repeat;
+        u32     generic_pkt;
+        u32     generic_pkt_repeat;
+};
+
+struct hdmi_video_format {
+        enum hdmi_packing_mode  packing_mode;
+        u32                     y_res;  /* Line per panel */
+        u32                     x_res;  /* pixel per line */
+};
+
+struct hdmi_video_interface {
+        int     vsp;    /* Vsync polarity */
+        int     hsp;    /* Hsync polarity */
+        int     interlacing;
+        int     tm;     /* Timing mode */
+};
+
+struct hdmi_cm {
+        int     code;
+        int     mode;
+};
+
+struct hdmi_config {
+        struct hdmi_timings timings;
+        u16     interlace;
+        struct hdmi_cm cm;
+};
+
+#endif
diff --git a/drivers/video/omap2/dss/hdmi_omap4_panel.c b/drivers/video/omap2/dss/hdmi_omap4_panel.c
new file mode 100644
index 000000000000..ffb5de94131f
--- /dev/null
+++ b/drivers/video/omap2/dss/hdmi_omap4_panel.c
@@ -0,0 +1,222 @@
+/*
+ * hdmi_omap4_panel.c
+ *
+ * HDMI library support functions for TI OMAP4 processors.
+ *
+ * Copyright (C) 2010-2011 Texas Instruments Incorporated - http://www.ti.com/
+ * Authors:     Mythri P k <mythripk@ti.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.
+ *
+ * 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, see <http://www.gnu.org/licenses/>.
+ */
+
+#include <linux/kernel.h>
+#include <linux/err.h>
+#include <linux/io.h>
+#include <linux/mutex.h>
+#include <linux/module.h>
+#include <plat/display.h>
+
+#include "dss.h"
+
+static struct {
+        struct mutex hdmi_lock;
+} hdmi;
+
+
+static int hdmi_panel_probe(struct omap_dss_device *dssdev)
+{
+        DSSDBG("ENTER hdmi_panel_probe\n");
+
+        dssdev->panel.config = OMAP_DSS_LCD_TFT |
+                        OMAP_DSS_LCD_IVS | OMAP_DSS_LCD_IHS;
+
+        /*
+         * Initialize the timings to 640 * 480
+         * This is only for framebuffer update not for TV timing setting
+         * Setting TV timing will be done only on enable
+         */
+        dssdev->panel.timings.x_res = 640;
+        dssdev->panel.timings.y_res = 480;
+
+        DSSDBG("hdmi_panel_probe x_res= %d y_res = %d\n",
+                dssdev->panel.timings.x_res,
+                dssdev->panel.timings.y_res);
+        return 0;
+}
+
+static void hdmi_panel_remove(struct omap_dss_device *dssdev)
+{
+
+}
+
+static int hdmi_panel_enable(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+        DSSDBG("ENTER hdmi_panel_enable\n");
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_DISABLED) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        r = omapdss_hdmi_display_enable(dssdev);
+        if (r) {
+                DSSERR("failed to power on\n");
+                goto err;
+        }
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+err:
+        mutex_unlock(&hdmi.hdmi_lock);
+
+        return r;
+}
+
+static void hdmi_panel_disable(struct omap_dss_device *dssdev)
+{
+        mutex_lock(&hdmi.hdmi_lock);
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE)
+                omapdss_hdmi_display_disable(dssdev);
+
+        dssdev->state = OMAP_DSS_DISPLAY_DISABLED;
+
+        mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static int hdmi_panel_suspend(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_ACTIVE) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        dssdev->state = OMAP_DSS_DISPLAY_SUSPENDED;
+
+        omapdss_hdmi_display_disable(dssdev);
+
+err:
+        mutex_unlock(&hdmi.hdmi_lock);
+
+        return r;
+}
+
+static int hdmi_panel_resume(struct omap_dss_device *dssdev)
+{
+        int r = 0;
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        if (dssdev->state != OMAP_DSS_DISPLAY_SUSPENDED) {
+                r = -EINVAL;
+                goto err;
+        }
+
+        r = omapdss_hdmi_display_enable(dssdev);
+        if (r) {
+                DSSERR("failed to power on\n");
+                goto err;
+        }
+
+        dssdev->state = OMAP_DSS_DISPLAY_ACTIVE;
+
+err:
+        mutex_unlock(&hdmi.hdmi_lock);
+
+        return r;
+}
+
+static void hdmi_get_timings(struct omap_dss_device *dssdev,
+                        struct omap_video_timings *timings)
+{
+        mutex_lock(&hdmi.hdmi_lock);
+
+        *timings = dssdev->panel.timings;
+
+        mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static void hdmi_set_timings(struct omap_dss_device *dssdev,
+                        struct omap_video_timings *timings)
+{
+        DSSDBG("hdmi_set_timings\n");
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        dssdev->panel.timings = *timings;
+
+        if (dssdev->state == OMAP_DSS_DISPLAY_ACTIVE) {
+                /* turn the hdmi off and on to get new timings to use */
+                omapdss_hdmi_display_disable(dssdev);
+                omapdss_hdmi_display_set_timing(dssdev);
+        }
+
+        mutex_unlock(&hdmi.hdmi_lock);
+}
+
+static int hdmi_check_timings(struct omap_dss_device *dssdev,
+                        struct omap_video_timings *timings)
+{
+        int r = 0;
+
+        DSSDBG("hdmi_check_timings\n");
+
+        mutex_lock(&hdmi.hdmi_lock);
+
+        r = omapdss_hdmi_display_check_timing(dssdev, timings);
+        if (r) {
+                DSSERR("Timing cannot be applied\n");
+                goto err;
+        }
+err:
+        mutex_unlock(&hdmi.hdmi_lock);
+        return r;
+}
+
+static struct omap_dss_driver hdmi_driver = {
+        .probe          = hdmi_panel_probe,
+        .remove         = hdmi_panel_remove,
+        .enable         = hdmi_panel_enable,
+        .disable        = hdmi_panel_disable,
+        .suspend        = hdmi_panel_suspend,
+        .resume         = hdmi_panel_resume,
+        .get_timings    = hdmi_get_timings,
+        .set_timings    = hdmi_set_timings,
+        .check_timings  = hdmi_check_timings,
+        .driver                 = {
+                .name   = "hdmi_panel",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int hdmi_panel_init(void)
+{
+        mutex_init(&hdmi.hdmi_lock);
+
+        omap_dss_register_driver(&hdmi_driver);
+
+        return 0;
+}
+
+void hdmi_panel_exit(void)
+{
+        omap_dss_unregister_driver(&hdmi_driver);
+
+}
diff --git a/drivers/video/omap2/dss/manager.c b/drivers/video/omap2/dss/manager.c
index 172d4e697309..bcd37ec86952 100644
--- a/drivers/video/omap2/dss/manager.c
+++ b/drivers/video/omap2/dss/manager.c
@@ -515,6 +515,8 @@ static int dss_mgr_wait_for_vsync(struct omap_overlay_manager *mgr)
 
         if (mgr->device->type == OMAP_DISPLAY_TYPE_VENC) {
                 irq = DISPC_IRQ_EVSYNC_ODD;
+        } else if (mgr->device->type == OMAP_DISPLAY_TYPE_HDMI) {
+                irq = DISPC_IRQ_EVSYNC_EVEN;
         } else {
                 if (mgr->id == OMAP_DSS_CHANNEL_LCD)
                         irq = DISPC_IRQ_VSYNC;
@@ -536,7 +538,8 @@ static int dss_mgr_wait_for_go(struct omap_overlay_manager *mgr)
         if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
-        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+                        || dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
                 irq = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
         } else {
                 if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
@@ -613,7 +616,8 @@ int dss_mgr_wait_for_go_ovl(struct omap_overlay *ovl)
         if (!dssdev || dssdev->state != OMAP_DSS_DISPLAY_ACTIVE)
                 return 0;
 
-        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+                        || dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
                 irq = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN;
         } else {
                 if (dssdev->caps & OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE) {
@@ -1377,6 +1381,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
                 case OMAP_DISPLAY_TYPE_DBI:
                 case OMAP_DISPLAY_TYPE_SDI:
                 case OMAP_DISPLAY_TYPE_VENC:
+                case OMAP_DISPLAY_TYPE_HDMI:
                         default_get_overlay_fifo_thresholds(ovl->id, size,
                                         &oc->burst_size, &oc->fifo_low,
                                         &oc->fifo_high);
@@ -1394,7 +1399,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
         }
 
         r = 0;
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         if (!dss_cache.irq_enabled) {
                 u32 mask;
 
@@ -1407,7 +1412,7 @@ static int omap_dss_mgr_apply(struct omap_overlay_manager *mgr)
                 dss_cache.irq_enabled = true;
         }
         configure_dispc();
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         spin_unlock_irqrestore(&dss_cache.lock, flags);
 
diff --git a/drivers/video/omap2/dss/overlay.c b/drivers/video/omap2/dss/overlay.c
index 456efef03c20..f1aca6d04011 100644
--- a/drivers/video/omap2/dss/overlay.c
+++ b/drivers/video/omap2/dss/overlay.c
@@ -490,7 +490,7 @@ static int omap_dss_set_manager(struct omap_overlay *ovl,
 
         ovl->manager = mgr;
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         /* XXX: on manual update display, in auto update mode, a bug happens
          * here. When an overlay is first enabled on LCD, then it's disabled,
          * and the manager is changed to TV, we sometimes get SYNC_LOST_DIGIT
@@ -499,7 +499,7 @@ static int omap_dss_set_manager(struct omap_overlay *ovl,
          * but I don't understand how or why. */
         msleep(40);
         dispc_set_channel_out(ovl->id, mgr->id);
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         return 0;
 }
@@ -679,7 +679,8 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
                         lcd2_mgr->set_device(lcd2_mgr, dssdev);
                         mgr = lcd2_mgr;
                 }
-        } else if (dssdev->type != OMAP_DISPLAY_TYPE_VENC) {
+        } else if (dssdev->type != OMAP_DISPLAY_TYPE_VENC
+                        && dssdev->type != OMAP_DISPLAY_TYPE_HDMI) {
                 if (!lcd_mgr->device || force) {
                         if (lcd_mgr->device)
                                 lcd_mgr->unset_device(lcd_mgr);
@@ -688,7 +689,8 @@ void dss_recheck_connections(struct omap_dss_device *dssdev, bool force)
                 }
         }
 
-        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC) {
+        if (dssdev->type == OMAP_DISPLAY_TYPE_VENC
+                        || dssdev->type == OMAP_DISPLAY_TYPE_HDMI) {
                 if (!tv_mgr->device || force) {
                         if (tv_mgr->device)
                                 tv_mgr->unset_device(tv_mgr);
diff --git a/drivers/video/omap2/dss/rfbi.c b/drivers/video/omap2/dss/rfbi.c
index 10a2ffe02882..5ea17f49c611 100644
--- a/drivers/video/omap2/dss/rfbi.c
+++ b/drivers/video/omap2/dss/rfbi.c
@@ -36,8 +36,6 @@
 #include <plat/display.h>
 #include "dss.h"
 
-#define RFBI_BASE               0x48050800
-
 struct rfbi_reg { u16 idx; };
 
 #define RFBI_REG(idx)           ((const struct rfbi_reg) { idx })
@@ -100,6 +98,7 @@ static int rfbi_convert_timings(struct rfbi_timings *t);
 static void rfbi_get_clk_info(u32 *clk_period, u32 *max_clk_div);
 
 static struct {
+        struct platform_device *pdev;
         void __iomem    *base;
 
         unsigned long   l4_khz;
@@ -142,9 +141,9 @@ static inline u32 rfbi_read_reg(const struct rfbi_reg idx)
 static void rfbi_enable_clocks(bool enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
         else
-                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 }
 
 void omap_rfbi_write_command(const void *buf, u32 len)
@@ -497,7 +496,7 @@ unsigned long rfbi_get_max_tx_rate(void)
         };
 
         l4_rate = rfbi.l4_khz / 1000;
-        dss1_rate = dss_clk_get_rate(DSS_CLK_FCK1) / 1000000;
+        dss1_rate = dss_clk_get_rate(DSS_CLK_FCK) / 1000000;
 
         for (i = 0; i < ARRAY_SIZE(ftab); i++) {
                 /* Use a window instead of an exact match, to account
@@ -922,7 +921,7 @@ void rfbi_dump_regs(struct seq_file *s)
 {
 #define DUMPREG(r) seq_printf(s, "%-35s %08x\n", #r, rfbi_read_reg(r))
 
-        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         DUMPREG(RFBI_REVISION);
         DUMPREG(RFBI_SYSCONFIG);
@@ -953,54 +952,10 @@ void rfbi_dump_regs(struct seq_file *s)
         DUMPREG(RFBI_VSYNC_WIDTH);
         DUMPREG(RFBI_HSYNC_WIDTH);
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 #undef DUMPREG
 }
 
-int rfbi_init(void)
-{
-        u32 rev;
-        u32 l;
-
-        spin_lock_init(&rfbi.cmd_lock);
-
-        init_completion(&rfbi.cmd_done);
-        atomic_set(&rfbi.cmd_fifo_full, 0);
-        atomic_set(&rfbi.cmd_pending, 0);
-
-        rfbi.base = ioremap(RFBI_BASE, SZ_256);
-        if (!rfbi.base) {
-                DSSERR("can't ioremap RFBI\n");
-                return -ENOMEM;
-        }
-
-        rfbi_enable_clocks(1);
-
-        msleep(10);
-
-        rfbi.l4_khz = dss_clk_get_rate(DSS_CLK_ICK) / 1000;
-
-        /* Enable autoidle and smart-idle */
-        l = rfbi_read_reg(RFBI_SYSCONFIG);
-        l |= (1 << 0) | (2 << 3);
-        rfbi_write_reg(RFBI_SYSCONFIG, l);
-
-        rev = rfbi_read_reg(RFBI_REVISION);
-        printk(KERN_INFO "OMAP RFBI rev %d.%d\n",
-               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
-
-        rfbi_enable_clocks(0);
-
-        return 0;
-}
-
-void rfbi_exit(void)
-{
-        DSSDBG("rfbi_exit\n");
-
-        iounmap(rfbi.base);
-}
-
 int omapdss_rfbi_display_enable(struct omap_dss_device *dssdev)
 {
         int r;
@@ -1056,3 +1011,74 @@ int rfbi_init_display(struct omap_dss_device *dssdev)
         dssdev->caps = OMAP_DSS_DISPLAY_CAP_MANUAL_UPDATE;
         return 0;
 }
+
+/* RFBI HW IP initialisation */
+static int omap_rfbihw_probe(struct platform_device *pdev)
+{
+        u32 rev;
+        u32 l;
+        struct resource *rfbi_mem;
+
+        rfbi.pdev = pdev;
+
+        spin_lock_init(&rfbi.cmd_lock);
+
+        init_completion(&rfbi.cmd_done);
+        atomic_set(&rfbi.cmd_fifo_full, 0);
+        atomic_set(&rfbi.cmd_pending, 0);
+
+        rfbi_mem = platform_get_resource(rfbi.pdev, IORESOURCE_MEM, 0);
+        if (!rfbi_mem) {
+                DSSERR("can't get IORESOURCE_MEM RFBI\n");
+                return -EINVAL;
+        }
+        rfbi.base = ioremap(rfbi_mem->start, resource_size(rfbi_mem));
+        if (!rfbi.base) {
+                DSSERR("can't ioremap RFBI\n");
+                return -ENOMEM;
+        }
+
+        rfbi_enable_clocks(1);
+
+        msleep(10);
+
+        rfbi.l4_khz = dss_clk_get_rate(DSS_CLK_ICK) / 1000;
+
+        /* Enable autoidle and smart-idle */
+        l = rfbi_read_reg(RFBI_SYSCONFIG);
+        l |= (1 << 0) | (2 << 3);
+        rfbi_write_reg(RFBI_SYSCONFIG, l);
+
+        rev = rfbi_read_reg(RFBI_REVISION);
+        dev_dbg(&pdev->dev, "OMAP RFBI rev %d.%d\n",
+               FLD_GET(rev, 7, 4), FLD_GET(rev, 3, 0));
+
+        rfbi_enable_clocks(0);
+
+        return 0;
+}
+
+static int omap_rfbihw_remove(struct platform_device *pdev)
+{
+        iounmap(rfbi.base);
+        return 0;
+}
+
+static struct platform_driver omap_rfbihw_driver = {
+        .probe          = omap_rfbihw_probe,
+        .remove         = omap_rfbihw_remove,
+        .driver         = {
+                .name   = "omapdss_rfbi",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int rfbi_init_platform_driver(void)
+{
+        return platform_driver_register(&omap_rfbihw_driver);
+}
+
+void rfbi_uninit_platform_driver(void)
+{
+        return platform_driver_unregister(&omap_rfbihw_driver);
+}
diff --git a/drivers/video/omap2/dss/sdi.c b/drivers/video/omap2/dss/sdi.c
index b64adf7dfc88..54a53e648180 100644
--- a/drivers/video/omap2/dss/sdi.c
+++ b/drivers/video/omap2/dss/sdi.c
@@ -30,7 +30,6 @@
 #include "dss.h"
 
 static struct {
-        bool skip_init;
         bool update_enabled;
         struct regulator *vdds_sdi_reg;
 } sdi;
@@ -68,9 +67,7 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
         if (r)
                 goto err1;
 
-        /* In case of skip_init sdi_init has already enabled the clocks */
-        if (!sdi.skip_init)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         sdi_basic_init(dssdev);
 
@@ -80,14 +77,8 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
         dispc_set_pol_freq(dssdev->manager->id, dssdev->panel.config,
                         dssdev->panel.acbi, dssdev->panel.acb);
 
-        if (!sdi.skip_init) {
-                r = dss_calc_clock_div(1, t->pixel_clock * 1000,
-                                &dss_cinfo, &dispc_cinfo);
-        } else {
-                r = dss_get_clock_div(&dss_cinfo);
-                r = dispc_get_clock_div(dssdev->manager->id, &dispc_cinfo);
-        }
-
+        r = dss_calc_clock_div(1, t->pixel_clock * 1000,
+                        &dss_cinfo, &dispc_cinfo);
         if (r)
                 goto err2;
 
@@ -116,21 +107,17 @@ int omapdss_sdi_display_enable(struct omap_dss_device *dssdev)
         if (r)
                 goto err2;
 
-        if (!sdi.skip_init) {
-                dss_sdi_init(dssdev->phy.sdi.datapairs);
-                r = dss_sdi_enable();
-                if (r)
-                        goto err1;
-                mdelay(2);
-        }
+        dss_sdi_init(dssdev->phy.sdi.datapairs);
+        r = dss_sdi_enable();
+        if (r)
+                goto err1;
+        mdelay(2);
 
         dssdev->manager->enable(dssdev->manager);
 
-        sdi.skip_init = 0;
-
         return 0;
 err2:
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
         regulator_disable(sdi.vdds_sdi_reg);
 err1:
         omap_dss_stop_device(dssdev);
@@ -145,7 +132,7 @@ void omapdss_sdi_display_disable(struct omap_dss_device *dssdev)
 
         dss_sdi_disable();
 
-        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1);
+        dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK);
 
         regulator_disable(sdi.vdds_sdi_reg);
 
@@ -157,25 +144,24 @@ int sdi_init_display(struct omap_dss_device *dssdev)
 {
         DSSDBG("SDI init\n");
 
+        if (sdi.vdds_sdi_reg == NULL) {
+                struct regulator *vdds_sdi;
+
+                vdds_sdi = dss_get_vdds_sdi();
+
+                if (IS_ERR(vdds_sdi)) {
+                        DSSERR("can't get VDDS_SDI regulator\n");
+                        return PTR_ERR(vdds_sdi);
+                }
+
+                sdi.vdds_sdi_reg = vdds_sdi;
+        }
+
         return 0;
 }
 
-int sdi_init(bool skip_init)
+int sdi_init(void)
 {
-        /* we store this for first display enable, then clear it */
-        sdi.skip_init = skip_init;
-
-        sdi.vdds_sdi_reg = dss_get_vdds_sdi();
-        if (IS_ERR(sdi.vdds_sdi_reg)) {
-                DSSERR("can't get VDDS_SDI regulator\n");
-                return PTR_ERR(sdi.vdds_sdi_reg);
-        }
-        /*
-         * Enable clocks already here, otherwise there would be a toggle
-         * of them until sdi_display_enable is called.
-         */
-        if (skip_init)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1);
         return 0;
 }
 
diff --git a/drivers/video/omap2/dss/venc.c b/drivers/video/omap2/dss/venc.c
index eff35050e28a..8e35a5bae429 100644
--- a/drivers/video/omap2/dss/venc.c
+++ b/drivers/video/omap2/dss/venc.c
@@ -39,8 +39,6 @@
 
 #include "dss.h"
 
-#define VENC_BASE       0x48050C00
-
 /* Venc registers */
 #define VENC_REV_ID                             0x00
 #define VENC_STATUS                             0x04
@@ -289,6 +287,7 @@ const struct omap_video_timings omap_dss_ntsc_timings = {
 EXPORT_SYMBOL(omap_dss_ntsc_timings);
 
 static struct {
+        struct platform_device *pdev;
         void __iomem *base;
         struct mutex venc_lock;
         u32 wss_data;
@@ -381,11 +380,11 @@ static void venc_reset(void)
 static void venc_enable_clocks(int enable)
 {
         if (enable)
-                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_54M |
-                                DSS_CLK_96M);
+                dss_clk_enable(DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_TVFCK |
+                                DSS_CLK_VIDFCK);
         else
-                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK1 | DSS_CLK_54M |
-                                DSS_CLK_96M);
+                dss_clk_disable(DSS_CLK_ICK | DSS_CLK_FCK | DSS_CLK_TVFCK |
+                                DSS_CLK_VIDFCK);
 }
 
 static const struct venc_config *venc_timings_to_config(
@@ -641,50 +640,23 @@ static struct omap_dss_driver venc_driver = {
 };
 /* driver end */
 
-
-
-int venc_init(struct platform_device *pdev)
+int venc_init_display(struct omap_dss_device *dssdev)
 {
-        u8 rev_id;
+        DSSDBG("init_display\n");
 
-        mutex_init(&venc.venc_lock);
+        if (venc.vdda_dac_reg == NULL) {
+                struct regulator *vdda_dac;
 
-        venc.wss_data = 0;
+                vdda_dac = regulator_get(&venc.pdev->dev, "vdda_dac");
 
-        venc.base = ioremap(VENC_BASE, SZ_1K);
-        if (!venc.base) {
-                DSSERR("can't ioremap VENC\n");
-                return -ENOMEM;
-        }
+                if (IS_ERR(vdda_dac)) {
+                        DSSERR("can't get VDDA_DAC regulator\n");
+                        return PTR_ERR(vdda_dac);
+                }
 
-        venc.vdda_dac_reg = dss_get_vdda_dac();
-        if (IS_ERR(venc.vdda_dac_reg)) {
-                iounmap(venc.base);
-                DSSERR("can't get VDDA_DAC regulator\n");
-                return PTR_ERR(venc.vdda_dac_reg);
+                venc.vdda_dac_reg = vdda_dac;
         }
 
-        venc_enable_clocks(1);
-
-        rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
-        printk(KERN_INFO "OMAP VENC rev %d\n", rev_id);
-
-        venc_enable_clocks(0);
-
-        return omap_dss_register_driver(&venc_driver);
-}
-
-void venc_exit(void)
-{
-        omap_dss_unregister_driver(&venc_driver);
-
-        iounmap(venc.base);
-}
-
-int venc_init_display(struct omap_dss_device *dssdev)
-{
-        DSSDBG("init_display\n");
-
         return 0;
 }
 
@@ -740,3 +712,73 @@ void venc_dump_regs(struct seq_file *s)
 
 #undef DUMPREG
 }
+
+/* VENC HW IP initialisation */
+static int omap_venchw_probe(struct platform_device *pdev)
+{
+        u8 rev_id;
+        struct resource *venc_mem;
+
+        venc.pdev = pdev;
+
+        mutex_init(&venc.venc_lock);
+
+        venc.wss_data = 0;
+
+        venc_mem = platform_get_resource(venc.pdev, IORESOURCE_MEM, 0);
+        if (!venc_mem) {
+                DSSERR("can't get IORESOURCE_MEM VENC\n");
+                return -EINVAL;
+        }
+        venc.base = ioremap(venc_mem->start, resource_size(venc_mem));
+        if (!venc.base) {
+                DSSERR("can't ioremap VENC\n");
+                return -ENOMEM;
+        }
+
+        venc_enable_clocks(1);
+
+        rev_id = (u8)(venc_read_reg(VENC_REV_ID) & 0xff);
+        dev_dbg(&pdev->dev, "OMAP VENC rev %d\n", rev_id);
+
+        venc_enable_clocks(0);
+
+        return omap_dss_register_driver(&venc_driver);
+}
+
+static int omap_venchw_remove(struct platform_device *pdev)
+{
+        if (venc.vdda_dac_reg != NULL) {
+                regulator_put(venc.vdda_dac_reg);
+                venc.vdda_dac_reg = NULL;
+        }
+        omap_dss_unregister_driver(&venc_driver);
+
+        iounmap(venc.base);
+        return 0;
+}
+
+static struct platform_driver omap_venchw_driver = {
+        .probe          = omap_venchw_probe,
+        .remove         = omap_venchw_remove,
+        .driver         = {
+                .name   = "omapdss_venc",
+                .owner  = THIS_MODULE,
+        },
+};
+
+int venc_init_platform_driver(void)
+{
+        if (cpu_is_omap44xx())
+                return 0;
+
+        return platform_driver_register(&omap_venchw_driver);
+}
+
+void venc_uninit_platform_driver(void)
+{
+        if (cpu_is_omap44xx())
+                return;
+
+        return platform_driver_unregister(&omap_venchw_driver);
+}
diff --git a/drivers/video/omap2/omapfb/Kconfig b/drivers/video/omap2/omapfb/Kconfig
index 65149b22cf37..aa33386c81ff 100644
--- a/drivers/video/omap2/omapfb/Kconfig
+++ b/drivers/video/omap2/omapfb/Kconfig
@@ -1,5 +1,5 @@
 menuconfig FB_OMAP2
-        tristate "OMAP2/3 frame buffer support (EXPERIMENTAL)"
+        tristate "OMAP2+ frame buffer support (EXPERIMENTAL)"
         depends on FB && OMAP2_DSS
 
         select OMAP2_VRAM
@@ -8,10 +8,10 @@ menuconfig FB_OMAP2
         select FB_CFB_COPYAREA
         select FB_CFB_IMAGEBLIT
         help
-          Frame buffer driver for OMAP2/3 based boards.
+          Frame buffer driver for OMAP2+ based boards.
 
 config FB_OMAP2_DEBUG_SUPPORT
-        bool "Debug support for OMAP2/3 FB"
+        bool "Debug support for OMAP2+ FB"
         default y
         depends on FB_OMAP2
         help
diff --git a/drivers/video/omap2/omapfb/omapfb-main.c b/drivers/video/omap2/omapfb/omapfb-main.c
index 4fdab8e9c496..505ec6672049 100644
--- a/drivers/video/omap2/omapfb/omapfb-main.c
+++ b/drivers/video/omap2/omapfb/omapfb-main.c
@@ -2090,7 +2090,7 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
 {
         int r;
         u8 bpp;
-        struct omap_video_timings timings;
+        struct omap_video_timings timings, temp_timings;
 
         r = omapfb_mode_to_timings(mode_str, &timings, &bpp);
         if (r)
@@ -2100,14 +2100,23 @@ static int omapfb_set_def_mode(struct omapfb2_device *fbdev,
         fbdev->bpp_overrides[fbdev->num_bpp_overrides].bpp = bpp;
         ++fbdev->num_bpp_overrides;
 
-        if (!display->driver->check_timings || !display->driver->set_timings)
-                return -EINVAL;
+        if (display->driver->check_timings) {
+                r = display->driver->check_timings(display, &timings);
+                if (r)
+                        return r;
+        } else {
+                /* If check_timings is not present compare xres and yres */
+                if (display->driver->get_timings) {
+                        display->driver->get_timings(display, &temp_timings);
 
-        r = display->driver->check_timings(display, &timings);
-        if (r)
-                return r;
+                        if (temp_timings.x_res != timings.x_res ||
+                                temp_timings.y_res != timings.y_res)
+                                return -EINVAL;
+                }
+        }
 
-        display->driver->set_timings(display, &timings);
+        if (display->driver->set_timings)
+                        display->driver->set_timings(display, &timings);
 
         return 0;
 }
diff --git a/drivers/video/s3c-fb.c b/drivers/video/s3c-fb.c
index 83ce9a04d872..6817d187d46e 100644
--- a/drivers/video/s3c-fb.c
+++ b/drivers/video/s3c-fb.c
@@ -1340,6 +1340,7 @@ static int __devinit s3c_fb_probe(struct platform_device *pdev)
         sfb->bus_clk = clk_get(dev, "lcd");
         if (IS_ERR(sfb->bus_clk)) {
                 dev_err(dev, "failed to get bus clock\n");
+                ret = PTR_ERR(sfb->bus_clk);
                 goto err_sfb;
         }
 
diff --git a/drivers/video/s3fb.c b/drivers/video/s3fb.c
index 75738a928610..ddedad9cd069 100644
--- a/drivers/video/s3fb.c
+++ b/drivers/video/s3fb.c
@@ -64,6 +64,8 @@ static const struct svga_fb_format s3fb_formats[] = {
 
 static const struct svga_pll s3_pll = {3, 129, 3, 33, 0, 3,
         35000, 240000, 14318};
+static const struct svga_pll s3_trio3d_pll = {3, 129, 3, 31, 0, 4,
+        230000, 460000, 14318};
 
 static const int s3_memsizes[] = {4096, 0, 3072, 8192, 2048, 6144, 1024, 512};
 
@@ -72,7 +74,8 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
                         "S3 Plato/PX", "S3 Aurora64VP", "S3 Virge",
                         "S3 Virge/VX", "S3 Virge/DX", "S3 Virge/GX",
                         "S3 Virge/GX2", "S3 Virge/GX2P", "S3 Virge/GX2P",
-                        "S3 Trio3D/1X", "S3 Trio3D/2X", "S3 Trio3D/2X"};
+                        "S3 Trio3D/1X", "S3 Trio3D/2X", "S3 Trio3D/2X",
+                        "S3 Trio3D"};
 
 #define CHIP_UNKNOWN            0x00
 #define CHIP_732_TRIO32         0x01
@@ -93,6 +96,7 @@ static const char * const s3_names[] = {"S3 Unknown", "S3 Trio32", "S3 Trio64",
 #define CHIP_360_TRIO3D_1X      0x10
 #define CHIP_362_TRIO3D_2X      0x11
 #define CHIP_368_TRIO3D_2X      0x12
+#define CHIP_365_TRIO3D         0x13
 
 #define CHIP_XXX_TRIO           0x80
 #define CHIP_XXX_TRIO64V2_DXGX  0x81
@@ -119,9 +123,11 @@ static const struct vga_regset s3_v_sync_start_regs[]   = {{0x10, 0, 7}, {0x07,
 static const struct vga_regset s3_v_sync_end_regs[]     = {{0x11, 0, 3}, VGA_REGSET_END};
 
 static const struct vga_regset s3_line_compare_regs[]   = {{0x18, 0, 7}, {0x07, 4, 4}, {0x09, 6, 6}, {0x5E, 6, 6}, VGA_REGSET_END};
-static const struct vga_regset s3_start_address_regs[]  = {{0x0d, 0, 7}, {0x0c, 0, 7}, {0x31, 4, 5}, {0x51, 0, 1}, VGA_REGSET_END};
+static const struct vga_regset s3_start_address_regs[]  = {{0x0d, 0, 7}, {0x0c, 0, 7}, {0x69, 0, 4}, VGA_REGSET_END};
 static const struct vga_regset s3_offset_regs[]         = {{0x13, 0, 7}, {0x51, 4, 5}, VGA_REGSET_END}; /* set 0x43 bit 2 to 0 */
 
+static const struct vga_regset s3_dtpc_regs[]           = {{0x3B, 0, 7}, {0x5D, 6, 6}, VGA_REGSET_END};
+
 static const struct svga_timing_regs s3_timing_regs     = {
         s3_h_total_regs, s3_h_display_regs, s3_h_blank_start_regs,
         s3_h_blank_end_regs, s3_h_sync_start_regs, s3_h_sync_end_regs,
@@ -188,12 +194,19 @@ static void s3fb_settile_fast(struct fb_info *info, struct fb_tilemap *map)
         }
 }
 
+static void s3fb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+        struct s3fb_info *par = info->par;
+
+        svga_tilecursor(par->state.vgabase, info, cursor);
+}
+
 static struct fb_tile_ops s3fb_tile_ops = {
         .fb_settile     = svga_settile,
         .fb_tilecopy    = svga_tilecopy,
         .fb_tilefill    = svga_tilefill,
         .fb_tileblit    = svga_tileblit,
-        .fb_tilecursor  = svga_tilecursor,
+        .fb_tilecursor  = s3fb_tilecursor,
         .fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -202,7 +215,7 @@ static struct fb_tile_ops s3fb_fast_tile_ops = {
         .fb_tilecopy    = svga_tilecopy,
         .fb_tilefill    = svga_tilefill,
         .fb_tileblit    = svga_tileblit,
-        .fb_tilecursor  = svga_tilecursor,
+        .fb_tilecursor  = s3fb_tilecursor,
         .fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -334,33 +347,34 @@ static void s3_set_pixclock(struct fb_info *info, u32 pixclock)
         u8 regval;
         int rv;
 
-        rv = svga_compute_pll(&s3_pll, 1000000000 / pixclock, &m, &n, &r, info->node);
+        rv = svga_compute_pll((par->chip == CHIP_365_TRIO3D) ? &s3_trio3d_pll : &s3_pll,
+                              1000000000 / pixclock, &m, &n, &r, info->node);
         if (rv < 0) {
                 printk(KERN_ERR "fb%d: cannot set requested pixclock, keeping old value\n", info->node);
                 return;
         }
 
         /* Set VGA misc register  */
-        regval = vga_r(NULL, VGA_MIS_R);
-        vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+        regval = vga_r(par->state.vgabase, VGA_MIS_R);
+        vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 
         /* Set S3 clock registers */
         if (par->chip == CHIP_360_TRIO3D_1X ||
             par->chip == CHIP_362_TRIO3D_2X ||
             par->chip == CHIP_368_TRIO3D_2X) {
-                vga_wseq(NULL, 0x12, (n - 2) | ((r & 3) << 6)); /* n and two bits of r */
-                vga_wseq(NULL, 0x29, r >> 2); /* remaining highest bit of r */
+                vga_wseq(par->state.vgabase, 0x12, (n - 2) | ((r & 3) << 6));   /* n and two bits of r */
+                vga_wseq(par->state.vgabase, 0x29, r >> 2); /* remaining highest bit of r */
         } else
-                vga_wseq(NULL, 0x12, (n - 2) | (r << 5));
-        vga_wseq(NULL, 0x13, m - 2);
+                vga_wseq(par->state.vgabase, 0x12, (n - 2) | (r << 5));
+        vga_wseq(par->state.vgabase, 0x13, m - 2);
 
         udelay(1000);
 
         /* Activate clock - write 0, 1, 0 to seq/15 bit 5 */
-        regval = vga_rseq (NULL, 0x15); /* | 0x80; */
-        vga_wseq(NULL, 0x15, regval & ~(1<<5));
-        vga_wseq(NULL, 0x15, regval |  (1<<5));
-        vga_wseq(NULL, 0x15, regval & ~(1<<5));
+        regval = vga_rseq (par->state.vgabase, 0x15); /* | 0x80; */
+        vga_wseq(par->state.vgabase, 0x15, regval & ~(1<<5));
+        vga_wseq(par->state.vgabase, 0x15, regval |  (1<<5));
+        vga_wseq(par->state.vgabase, 0x15, regval & ~(1<<5));
 }
 
 
@@ -372,7 +386,10 @@ static int s3fb_open(struct fb_info *info, int user)
 
         mutex_lock(&(par->open_lock));
         if (par->ref_count == 0) {
+                void __iomem *vgabase = par->state.vgabase;
+
                 memset(&(par->state), 0, sizeof(struct vgastate));
+                par->state.vgabase = vgabase;
                 par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
                 par->state.num_crtc = 0x70;
                 par->state.num_seq = 0x20;
@@ -470,6 +487,7 @@ static int s3fb_set_par(struct fb_info *info)
         struct s3fb_info *par = info->par;
         u32 value, mode, hmul, offset_value, screen_size, multiplex, dbytes;
         u32 bpp = info->var.bits_per_pixel;
+        u32 htotal, hsstart;
 
         if (bpp != 0) {
                 info->fix.ypanstep = 1;
@@ -504,99 +522,112 @@ static int s3fb_set_par(struct fb_info *info)
         info->var.activate = FB_ACTIVATE_NOW;
 
         /* Unlock registers */
-        vga_wcrt(NULL, 0x38, 0x48);
-        vga_wcrt(NULL, 0x39, 0xA5);
-        vga_wseq(NULL, 0x08, 0x06);
-        svga_wcrt_mask(0x11, 0x00, 0x80);
+        vga_wcrt(par->state.vgabase, 0x38, 0x48);
+        vga_wcrt(par->state.vgabase, 0x39, 0xA5);
+        vga_wseq(par->state.vgabase, 0x08, 0x06);
+        svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
 
         /* Blank screen and turn off sync */
-        svga_wseq_mask(0x01, 0x20, 0x20);
-        svga_wcrt_mask(0x17, 0x00, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
         /* Set default values */
-        svga_set_default_gfx_regs();
-        svga_set_default_atc_regs();
-        svga_set_default_seq_regs();
-        svga_set_default_crt_regs();
-        svga_wcrt_multi(s3_line_compare_regs, 0xFFFFFFFF);
-        svga_wcrt_multi(s3_start_address_regs, 0);
+        svga_set_default_gfx_regs(par->state.vgabase);
+        svga_set_default_atc_regs(par->state.vgabase);
+        svga_set_default_seq_regs(par->state.vgabase);
+        svga_set_default_crt_regs(par->state.vgabase);
+        svga_wcrt_multi(par->state.vgabase, s3_line_compare_regs, 0xFFFFFFFF);
+        svga_wcrt_multi(par->state.vgabase, s3_start_address_regs, 0);
 
         /* S3 specific initialization */
-        svga_wcrt_mask(0x58, 0x10, 0x10); /* enable linear framebuffer */
-        svga_wcrt_mask(0x31, 0x08, 0x08); /* enable sequencer access to framebuffer above 256 kB */
+        svga_wcrt_mask(par->state.vgabase, 0x58, 0x10, 0x10); /* enable linear framebuffer */
+        svga_wcrt_mask(par->state.vgabase, 0x31, 0x08, 0x08); /* enable sequencer access to framebuffer above 256 kB */
 
-/*      svga_wcrt_mask(0x33, 0x08, 0x08); */ /* DDR ?   */
-/*      svga_wcrt_mask(0x43, 0x01, 0x01); */ /* DDR ?   */
-        svga_wcrt_mask(0x33, 0x00, 0x08); /* no DDR ?   */
-        svga_wcrt_mask(0x43, 0x00, 0x01); /* no DDR ?   */
+/*      svga_wcrt_mask(par->state.vgabase, 0x33, 0x08, 0x08); */ /* DDR ?       */
+/*      svga_wcrt_mask(par->state.vgabase, 0x43, 0x01, 0x01); */ /* DDR ?       */
+        svga_wcrt_mask(par->state.vgabase, 0x33, 0x00, 0x08); /* no DDR ?       */
+        svga_wcrt_mask(par->state.vgabase, 0x43, 0x00, 0x01); /* no DDR ?       */
 
-        svga_wcrt_mask(0x5D, 0x00, 0x28); /* Clear strange HSlen bits */
+        svga_wcrt_mask(par->state.vgabase, 0x5D, 0x00, 0x28); /* Clear strange HSlen bits */
 
-/*      svga_wcrt_mask(0x58, 0x03, 0x03); */
+/*      svga_wcrt_mask(par->state.vgabase, 0x58, 0x03, 0x03); */
 
-/*      svga_wcrt_mask(0x53, 0x12, 0x13); */ /* enable MMIO */
-/*      svga_wcrt_mask(0x40, 0x08, 0x08); */ /* enable write buffer */
+/*      svga_wcrt_mask(par->state.vgabase, 0x53, 0x12, 0x13); */ /* enable MMIO */
+/*      svga_wcrt_mask(par->state.vgabase, 0x40, 0x08, 0x08); */ /* enable write buffer */
 
 
         /* Set the offset register */
         pr_debug("fb%d: offset register       : %d\n", info->node, offset_value);
-        svga_wcrt_multi(s3_offset_regs, offset_value);
+        svga_wcrt_multi(par->state.vgabase, s3_offset_regs, offset_value);
 
         if (par->chip != CHIP_360_TRIO3D_1X &&
             par->chip != CHIP_362_TRIO3D_2X &&
             par->chip != CHIP_368_TRIO3D_2X) {
-                vga_wcrt(NULL, 0x54, 0x18); /* M parameter */
-                vga_wcrt(NULL, 0x60, 0xff); /* N parameter */
-                vga_wcrt(NULL, 0x61, 0xff); /* L parameter */
-                vga_wcrt(NULL, 0x62, 0xff); /* L parameter */
+                vga_wcrt(par->state.vgabase, 0x54, 0x18); /* M parameter */
+                vga_wcrt(par->state.vgabase, 0x60, 0xff); /* N parameter */
+                vga_wcrt(par->state.vgabase, 0x61, 0xff); /* L parameter */
+                vga_wcrt(par->state.vgabase, 0x62, 0xff); /* L parameter */
         }
 
-        vga_wcrt(NULL, 0x3A, 0x35);
-        svga_wattr(0x33, 0x00);
+        vga_wcrt(par->state.vgabase, 0x3A, 0x35);
+        svga_wattr(par->state.vgabase, 0x33, 0x00);
 
         if (info->var.vmode & FB_VMODE_DOUBLE)
-                svga_wcrt_mask(0x09, 0x80, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
         else
-                svga_wcrt_mask(0x09, 0x00, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
         if (info->var.vmode & FB_VMODE_INTERLACED)
-                svga_wcrt_mask(0x42, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x42, 0x20, 0x20);
         else
-                svga_wcrt_mask(0x42, 0x00, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x42, 0x00, 0x20);
 
         /* Disable hardware graphics cursor */
-        svga_wcrt_mask(0x45, 0x00, 0x01);
+        svga_wcrt_mask(par->state.vgabase, 0x45, 0x00, 0x01);
         /* Disable Streams engine */
-        svga_wcrt_mask(0x67, 0x00, 0x0C);
+        svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0x0C);
 
         mode = svga_match_format(s3fb_formats, &(info->var), &(info->fix));
 
         /* S3 virge DX hack */
         if (par->chip == CHIP_375_VIRGE_DX) {
-                vga_wcrt(NULL, 0x86, 0x80);
-                vga_wcrt(NULL, 0x90, 0x00);
+                vga_wcrt(par->state.vgabase, 0x86, 0x80);
+                vga_wcrt(par->state.vgabase, 0x90, 0x00);
         }
 
         /* S3 virge VX hack */
         if (par->chip == CHIP_988_VIRGE_VX) {
-                vga_wcrt(NULL, 0x50, 0x00);
-                vga_wcrt(NULL, 0x67, 0x50);
+                vga_wcrt(par->state.vgabase, 0x50, 0x00);
+                vga_wcrt(par->state.vgabase, 0x67, 0x50);
 
-                vga_wcrt(NULL, 0x63, (mode <= 2) ? 0x90 : 0x09);
-                vga_wcrt(NULL, 0x66, 0x90);
+                vga_wcrt(par->state.vgabase, 0x63, (mode <= 2) ? 0x90 : 0x09);
+                vga_wcrt(par->state.vgabase, 0x66, 0x90);
         }
 
         if (par->chip == CHIP_360_TRIO3D_1X ||
             par->chip == CHIP_362_TRIO3D_2X ||
-            par->chip == CHIP_368_TRIO3D_2X) {
+            par->chip == CHIP_368_TRIO3D_2X ||
+            par->chip == CHIP_365_TRIO3D    ||
+            par->chip == CHIP_375_VIRGE_DX  ||
+            par->chip == CHIP_385_VIRGE_GX) {
                 dbytes = info->var.xres * ((bpp+7)/8);
-                vga_wcrt(NULL, 0x91, (dbytes + 7) / 8);
-                vga_wcrt(NULL, 0x90, (((dbytes + 7) / 8) >> 8) | 0x80);
+                vga_wcrt(par->state.vgabase, 0x91, (dbytes + 7) / 8);
+                vga_wcrt(par->state.vgabase, 0x90, (((dbytes + 7) / 8) >> 8) | 0x80);
 
-                vga_wcrt(NULL, 0x66, 0x81);
+                vga_wcrt(par->state.vgabase, 0x66, 0x81);
         }
 
-        svga_wcrt_mask(0x31, 0x00, 0x40);
+        if (par->chip == CHIP_356_VIRGE_GX2  ||
+            par->chip == CHIP_357_VIRGE_GX2P ||
+            par->chip == CHIP_359_VIRGE_GX2P ||
+            par->chip == CHIP_360_TRIO3D_1X ||
+            par->chip == CHIP_362_TRIO3D_2X ||
+            par->chip == CHIP_368_TRIO3D_2X)
+                vga_wcrt(par->state.vgabase, 0x34, 0x00);
+        else    /* enable Data Transfer Position Control (DTPC) */
+                vga_wcrt(par->state.vgabase, 0x34, 0x10);
+
+        svga_wcrt_mask(par->state.vgabase, 0x31, 0x00, 0x40);
         multiplex = 0;
         hmul = 1;
 
@@ -604,51 +635,51 @@ static int s3fb_set_par(struct fb_info *info)
         switch (mode) {
         case 0:
                 pr_debug("fb%d: text mode\n", info->node);
-                svga_set_textmode_vga_regs();
+                svga_set_textmode_vga_regs(par->state.vgabase);
 
                 /* Set additional registers like in 8-bit mode */
-                svga_wcrt_mask(0x50, 0x00, 0x30);
-                svga_wcrt_mask(0x67, 0x00, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
                 /* Disable enhanced mode */
-                svga_wcrt_mask(0x3A, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
 
                 if (fasttext) {
                         pr_debug("fb%d: high speed text mode set\n", info->node);
-                        svga_wcrt_mask(0x31, 0x40, 0x40);
+                        svga_wcrt_mask(par->state.vgabase, 0x31, 0x40, 0x40);
                 }
                 break;
         case 1:
                 pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-                vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
+                vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
 
                 /* Set additional registers like in 8-bit mode */
-                svga_wcrt_mask(0x50, 0x00, 0x30);
-                svga_wcrt_mask(0x67, 0x00, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
                 /* disable enhanced mode */
-                svga_wcrt_mask(0x3A, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
                 break;
         case 2:
                 pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
 
                 /* Set additional registers like in 8-bit mode */
-                svga_wcrt_mask(0x50, 0x00, 0x30);
-                svga_wcrt_mask(0x67, 0x00, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
 
                 /* disable enhanced mode */
-                svga_wcrt_mask(0x3A, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x3A, 0x00, 0x30);
                 break;
         case 3:
                 pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
-                svga_wcrt_mask(0x50, 0x00, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x00, 0x30);
                 if (info->var.pixclock > 20000 ||
                     par->chip == CHIP_360_TRIO3D_1X ||
                     par->chip == CHIP_362_TRIO3D_2X ||
                     par->chip == CHIP_368_TRIO3D_2X)
-                        svga_wcrt_mask(0x67, 0x00, 0xF0);
+                        svga_wcrt_mask(par->state.vgabase, 0x67, 0x00, 0xF0);
                 else {
-                        svga_wcrt_mask(0x67, 0x10, 0xF0);
+                        svga_wcrt_mask(par->state.vgabase, 0x67, 0x10, 0xF0);
                         multiplex = 1;
                 }
                 break;
@@ -656,12 +687,21 @@ static int s3fb_set_par(struct fb_info *info)
                 pr_debug("fb%d: 5/5/5 truecolor\n", info->node);
                 if (par->chip == CHIP_988_VIRGE_VX) {
                         if (info->var.pixclock > 20000)
-                                svga_wcrt_mask(0x67, 0x20, 0xF0);
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x20, 0xF0);
                         else
-                                svga_wcrt_mask(0x67, 0x30, 0xF0);
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
+                } else if (par->chip == CHIP_365_TRIO3D) {
+                        svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+                        if (info->var.pixclock > 8695) {
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
+                                hmul = 2;
+                        } else {
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x20, 0xF0);
+                                multiplex = 1;
+                        }
                 } else {
-                        svga_wcrt_mask(0x50, 0x10, 0x30);
-                        svga_wcrt_mask(0x67, 0x30, 0xF0);
+                        svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+                        svga_wcrt_mask(par->state.vgabase, 0x67, 0x30, 0xF0);
                         if (par->chip != CHIP_360_TRIO3D_1X &&
                             par->chip != CHIP_362_TRIO3D_2X &&
                             par->chip != CHIP_368_TRIO3D_2X)
@@ -672,12 +712,21 @@ static int s3fb_set_par(struct fb_info *info)
                 pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
                 if (par->chip == CHIP_988_VIRGE_VX) {
                         if (info->var.pixclock > 20000)
-                                svga_wcrt_mask(0x67, 0x40, 0xF0);
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x40, 0xF0);
                         else
-                                svga_wcrt_mask(0x67, 0x50, 0xF0);
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
+                } else if (par->chip == CHIP_365_TRIO3D) {
+                        svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+                        if (info->var.pixclock > 8695) {
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
+                                hmul = 2;
+                        } else {
+                                svga_wcrt_mask(par->state.vgabase, 0x67, 0x40, 0xF0);
+                                multiplex = 1;
+                        }
                 } else {
-                        svga_wcrt_mask(0x50, 0x10, 0x30);
-                        svga_wcrt_mask(0x67, 0x50, 0xF0);
+                        svga_wcrt_mask(par->state.vgabase, 0x50, 0x10, 0x30);
+                        svga_wcrt_mask(par->state.vgabase, 0x67, 0x50, 0xF0);
                         if (par->chip != CHIP_360_TRIO3D_1X &&
                             par->chip != CHIP_362_TRIO3D_2X &&
                             par->chip != CHIP_368_TRIO3D_2X)
@@ -687,12 +736,12 @@ static int s3fb_set_par(struct fb_info *info)
         case 6:
                 /* VIRGE VX case */
                 pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
-                svga_wcrt_mask(0x67, 0xD0, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0xD0, 0xF0);
                 break;
         case 7:
                 pr_debug("fb%d: 8/8/8/8 truecolor\n", info->node);
-                svga_wcrt_mask(0x50, 0x30, 0x30);
-                svga_wcrt_mask(0x67, 0xD0, 0xF0);
+                svga_wcrt_mask(par->state.vgabase, 0x50, 0x30, 0x30);
+                svga_wcrt_mask(par->state.vgabase, 0x67, 0xD0, 0xF0);
                 break;
         default:
                 printk(KERN_ERR "fb%d: unsupported mode - bug\n", info->node);
@@ -700,25 +749,30 @@ static int s3fb_set_par(struct fb_info *info)
         }
 
         if (par->chip != CHIP_988_VIRGE_VX) {
-                svga_wseq_mask(0x15, multiplex ? 0x10 : 0x00, 0x10);
-                svga_wseq_mask(0x18, multiplex ? 0x80 : 0x00, 0x80);
+                svga_wseq_mask(par->state.vgabase, 0x15, multiplex ? 0x10 : 0x00, 0x10);
+                svga_wseq_mask(par->state.vgabase, 0x18, multiplex ? 0x80 : 0x00, 0x80);
         }
 
         s3_set_pixclock(info, info->var.pixclock);
-        svga_set_timings(&s3_timing_regs, &(info->var), hmul, 1,
+        svga_set_timings(par->state.vgabase, &s3_timing_regs, &(info->var), hmul, 1,
                          (info->var.vmode & FB_VMODE_DOUBLE)     ? 2 : 1,
                          (info->var.vmode & FB_VMODE_INTERLACED) ? 2 : 1,
                          hmul, info->node);
 
         /* Set interlaced mode start/end register */
-        value = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
-        value = ((value * hmul) / 8) - 5;
-        vga_wcrt(NULL, 0x3C, (value + 1) / 2);
+        htotal = info->var.xres + info->var.left_margin + info->var.right_margin + info->var.hsync_len;
+        htotal = ((htotal * hmul) / 8) - 5;
+        vga_wcrt(par->state.vgabase, 0x3C, (htotal + 1) / 2);
+
+        /* Set Data Transfer Position */
+        hsstart = ((info->var.xres + info->var.right_margin) * hmul) / 8;
+        value = clamp((htotal + hsstart + 1) / 2, hsstart + 4, htotal + 1);
+        svga_wcrt_multi(par->state.vgabase, s3_dtpc_regs, value);
 
         memset_io(info->screen_base, 0x00, screen_size);
         /* Device and screen back on */
-        svga_wcrt_mask(0x17, 0x80, 0x80);
-        svga_wseq_mask(0x01, 0x00, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
         return 0;
 }
@@ -788,31 +842,33 @@ static int s3fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int s3fb_blank(int blank_mode, struct fb_info *info)
 {
+        struct s3fb_info *par = info->par;
+
         switch (blank_mode) {
         case FB_BLANK_UNBLANK:
                 pr_debug("fb%d: unblank\n", info->node);
-                svga_wcrt_mask(0x56, 0x00, 0x06);
-                svga_wseq_mask(0x01, 0x00, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x00, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
                 break;
         case FB_BLANK_NORMAL:
                 pr_debug("fb%d: blank\n", info->node);
-                svga_wcrt_mask(0x56, 0x00, 0x06);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x00, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_HSYNC_SUSPEND:
                 pr_debug("fb%d: hsync\n", info->node);
-                svga_wcrt_mask(0x56, 0x02, 0x06);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x02, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_VSYNC_SUSPEND:
                 pr_debug("fb%d: vsync\n", info->node);
-                svga_wcrt_mask(0x56, 0x04, 0x06);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x04, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_POWERDOWN:
                 pr_debug("fb%d: sync down\n", info->node);
-                svga_wcrt_mask(0x56, 0x06, 0x06);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x56, 0x06, 0x06);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         }
 
@@ -822,8 +878,9 @@ static int s3fb_blank(int blank_mode, struct fb_info *info)
 
 /* Pan the display */
 
-static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info) {
-
+static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
+{
+        struct s3fb_info *par = info->par;
         unsigned int offset;
 
         /* Calculate the offset */
@@ -837,7 +894,7 @@ static int s3fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
         }
 
         /* Set the offset */
-        svga_wcrt_multi(s3_start_address_regs, offset);
+        svga_wcrt_multi(par->state.vgabase, s3_start_address_regs, offset);
 
         return 0;
 }
@@ -863,12 +920,14 @@ static struct fb_ops s3fb_ops = {
 
 /* ------------------------------------------------------------------------- */
 
-static int __devinit s3_identification(int chip)
+static int __devinit s3_identification(struct s3fb_info *par)
 {
+        int chip = par->chip;
+
         if (chip == CHIP_XXX_TRIO) {
-                u8 cr30 = vga_rcrt(NULL, 0x30);
-                u8 cr2e = vga_rcrt(NULL, 0x2e);
-                u8 cr2f = vga_rcrt(NULL, 0x2f);
+                u8 cr30 = vga_rcrt(par->state.vgabase, 0x30);
+                u8 cr2e = vga_rcrt(par->state.vgabase, 0x2e);
+                u8 cr2f = vga_rcrt(par->state.vgabase, 0x2f);
 
                 if ((cr30 == 0xE0) || (cr30 == 0xE1)) {
                         if (cr2e == 0x10)
@@ -883,7 +942,7 @@ static int __devinit s3_identification(int chip)
         }
 
         if (chip == CHIP_XXX_TRIO64V2_DXGX) {
-                u8 cr6f = vga_rcrt(NULL, 0x6f);
+                u8 cr6f = vga_rcrt(par->state.vgabase, 0x6f);
 
                 if (! (cr6f & 0x01))
                         return CHIP_775_TRIO64V2_DX;
@@ -892,7 +951,7 @@ static int __devinit s3_identification(int chip)
         }
 
         if (chip == CHIP_XXX_VIRGE_DXGX) {
-                u8 cr6f = vga_rcrt(NULL, 0x6f);
+                u8 cr6f = vga_rcrt(par->state.vgabase, 0x6f);
 
                 if (! (cr6f & 0x01))
                         return CHIP_375_VIRGE_DX;
@@ -901,7 +960,7 @@ static int __devinit s3_identification(int chip)
         }
 
         if (chip == CHIP_36X_TRIO3D_1X_2X) {
-                switch (vga_rcrt(NULL, 0x2f)) {
+                switch (vga_rcrt(par->state.vgabase, 0x2f)) {
                 case 0x00:
                         return CHIP_360_TRIO3D_1X;
                 case 0x01:
@@ -919,6 +978,8 @@ static int __devinit s3_identification(int chip)
 
 static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+        struct pci_bus_region bus_reg;
+        struct resource vga_res;
         struct fb_info *info;
         struct s3fb_info *par;
         int rc;
@@ -968,31 +1029,42 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
                 goto err_iomap;
         }
 
+        bus_reg.start = 0;
+        bus_reg.end = 64 * 1024;
+
+        vga_res.flags = IORESOURCE_IO;
+
+        pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+        par->state.vgabase = (void __iomem *) vga_res.start;
+
         /* Unlock regs */
-        cr38 = vga_rcrt(NULL, 0x38);
-        cr39 = vga_rcrt(NULL, 0x39);
-        vga_wseq(NULL, 0x08, 0x06);
-        vga_wcrt(NULL, 0x38, 0x48);
-        vga_wcrt(NULL, 0x39, 0xA5);
+        cr38 = vga_rcrt(par->state.vgabase, 0x38);
+        cr39 = vga_rcrt(par->state.vgabase, 0x39);
+        vga_wseq(par->state.vgabase, 0x08, 0x06);
+        vga_wcrt(par->state.vgabase, 0x38, 0x48);
+        vga_wcrt(par->state.vgabase, 0x39, 0xA5);
 
         /* Identify chip type */
         par->chip = id->driver_data & CHIP_MASK;
-        par->rev = vga_rcrt(NULL, 0x2f);
+        par->rev = vga_rcrt(par->state.vgabase, 0x2f);
         if (par->chip & CHIP_UNDECIDED_FLAG)
-                par->chip = s3_identification(par->chip);
+                par->chip = s3_identification(par);
 
         /* Find how many physical memory there is on card */
         /* 0x36 register is accessible even if other registers are locked */
-        regval = vga_rcrt(NULL, 0x36);
+        regval = vga_rcrt(par->state.vgabase, 0x36);
         if (par->chip == CHIP_360_TRIO3D_1X ||
             par->chip == CHIP_362_TRIO3D_2X ||
-            par->chip == CHIP_368_TRIO3D_2X) {
+            par->chip == CHIP_368_TRIO3D_2X ||
+            par->chip == CHIP_365_TRIO3D) {
                 switch ((regval & 0xE0) >> 5) {
                 case 0: /* 8MB -- only 4MB usable for display */
                 case 1: /* 4MB with 32-bit bus */
                 case 2: /* 4MB */
                         info->screen_size = 4 << 20;
                         break;
+                case 4: /* 2MB on 365 Trio3D */
                 case 6: /* 2MB */
                         info->screen_size = 2 << 20;
                         break;
@@ -1002,13 +1074,13 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
         info->fix.smem_len = info->screen_size;
 
         /* Find MCLK frequency */
-        regval = vga_rseq(NULL, 0x10);
-        par->mclk_freq = ((vga_rseq(NULL, 0x11) + 2) * 14318) / ((regval & 0x1F)  + 2);
+        regval = vga_rseq(par->state.vgabase, 0x10);
+        par->mclk_freq = ((vga_rseq(par->state.vgabase, 0x11) + 2) * 14318) / ((regval & 0x1F)  + 2);
         par->mclk_freq = par->mclk_freq >> (regval >> 5);
 
         /* Restore locks */
-        vga_wcrt(NULL, 0x38, cr38);
-        vga_wcrt(NULL, 0x39, cr39);
+        vga_wcrt(par->state.vgabase, 0x38, cr38);
+        vga_wcrt(par->state.vgabase, 0x39, cr39);
 
         strcpy(info->fix.id, s3_names [par->chip]);
         info->fix.mmio_start = 0;
@@ -1027,6 +1099,14 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
                 goto err_find_mode;
         }
 
+        /* maximize virtual vertical size for fast scrolling */
+        info->var.yres_virtual = info->fix.smem_len * 8 /
+                        (info->var.bits_per_pixel * info->var.xres_virtual);
+        if (info->var.yres_virtual < info->var.yres) {
+                dev_err(info->device, "virtual vertical size smaller than real\n");
+                goto err_find_mode;
+        }
+
         rc = fb_alloc_cmap(&info->cmap, 256, 0);
         if (rc < 0) {
                 dev_err(info->device, "cannot allocate colormap\n");
@@ -1044,8 +1124,8 @@ static int __devinit s3_pci_probe(struct pci_dev *dev, const struct pci_device_i
 
         if (par->chip == CHIP_UNKNOWN)
                 printk(KERN_INFO "fb%d: unknown chip, CR2D=%x, CR2E=%x, CRT2F=%x, CRT30=%x\n",
-                        info->node, vga_rcrt(NULL, 0x2d), vga_rcrt(NULL, 0x2e),
-                        vga_rcrt(NULL, 0x2f), vga_rcrt(NULL, 0x30));
+                        info->node, vga_rcrt(par->state.vgabase, 0x2d), vga_rcrt(par->state.vgabase, 0x2e),
+                        vga_rcrt(par->state.vgabase, 0x2f), vga_rcrt(par->state.vgabase, 0x30));
 
         /* Record a reference to the driver data */
         pci_set_drvdata(dev, info);
@@ -1192,6 +1272,7 @@ static struct pci_device_id s3_devices[] __devinitdata = {
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A11), .driver_data = CHIP_357_VIRGE_GX2P},
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A12), .driver_data = CHIP_359_VIRGE_GX2P},
         {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8A13), .driver_data = CHIP_36X_TRIO3D_1X_2X},
+        {PCI_DEVICE(PCI_VENDOR_ID_S3, 0x8904), .driver_data = CHIP_365_TRIO3D},
 
         {0, 0, 0, 0, 0, 0, 0}
 };
diff --git a/drivers/video/sh7760fb.c b/drivers/video/sh7760fb.c
index bea38fce2470..8fe19582c460 100644
--- a/drivers/video/sh7760fb.c
+++ b/drivers/video/sh7760fb.c
@@ -459,14 +459,14 @@ static int __devinit sh7760fb_probe(struct platform_device *pdev)
         }
 
         par->ioarea = request_mem_region(res->start,
-                                         (res->end - res->start), pdev->name);
+                                         resource_size(res), pdev->name);
         if (!par->ioarea) {
                 dev_err(&pdev->dev, "mmio area busy\n");
                 ret = -EBUSY;
                 goto out_fb;
         }
 
-        par->base = ioremap_nocache(res->start, res->end - res->start + 1);
+        par->base = ioremap_nocache(res->start, resource_size(res));
         if (!par->base) {
                 dev_err(&pdev->dev, "cannot remap\n");
                 ret = -ENODEV;
diff --git a/drivers/video/sh_mobile_lcdcfb.c b/drivers/video/sh_mobile_lcdcfb.c
index bf2629f83f40..757665bc500f 100644
--- a/drivers/video/sh_mobile_lcdcfb.c
+++ b/drivers/video/sh_mobile_lcdcfb.c
@@ -1088,8 +1088,9 @@ static struct backlight_device *sh_mobile_lcdc_bl_probe(struct device *parent,
 
         bl = backlight_device_register(ch->cfg.bl_info.name, parent, ch,
                                        &sh_mobile_lcdc_bl_ops, NULL);
-        if (!bl) {
-                dev_err(parent, "unable to register backlight device\n");
+        if (IS_ERR(bl)) {
+                dev_err(parent, "unable to register backlight device: %ld\n",
+                        PTR_ERR(bl));
                 return NULL;
         }
 
diff --git a/drivers/video/sis/sis.h b/drivers/video/sis/sis.h
index eac7a01925f3..1987f1b7212f 100644
--- a/drivers/video/sis/sis.h
+++ b/drivers/video/sis/sis.h
@@ -495,6 +495,7 @@ struct sis_video_info {
         unsigned int    refresh_rate;
 
         unsigned int    chip;
+        unsigned int    chip_real_id;
         u8              revision_id;
         int             sisvga_enabled;         /* PCI device was enabled */
 
diff --git a/drivers/video/sis/sis_main.c b/drivers/video/sis/sis_main.c
index 2fb8c5a660fb..75259845933d 100644
--- a/drivers/video/sis/sis_main.c
+++ b/drivers/video/sis/sis_main.c
@@ -4563,6 +4563,11 @@ sisfb_post_sis315330(struct pci_dev *pdev)
 }
 #endif
 
+static inline int sisfb_xgi_is21(struct sis_video_info *ivideo)
+{
+        return ivideo->chip_real_id == XGI_21;
+}
+
 static void __devinit
 sisfb_post_xgi_delay(struct sis_video_info *ivideo, int delay)
 {
@@ -4627,11 +4632,11 @@ sisfb_post_xgi_rwtest(struct sis_video_info *ivideo, int starta,
         return 1;
 }
 
-static void __devinit
+static int __devinit
 sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
 {
         unsigned int buswidth, ranksize, channelab, mapsize;
-        int i, j, k, l;
+        int i, j, k, l, status;
         u8 reg, sr14;
         static const u8 dramsr13[12 * 5] = {
                 0x02, 0x0e, 0x0b, 0x80, 0x5d,
@@ -4673,7 +4678,7 @@ sisfb_post_xgi_ramsize(struct sis_video_info *ivideo)
                 SiS_SetReg(SISSR, 0x13, 0x35);
                 SiS_SetReg(SISSR, 0x14, 0x41);
                 /* TODO */
-                return;
+                return -ENOMEM;
         }
 
         /* Non-interleaving */
@@ -4835,6 +4840,7 @@ bail_out:
 
         j = (ivideo->chip == XGI_20) ? 5 : 9;
         k = (ivideo->chip == XGI_20) ? 12 : 4;
+        status = -EIO;
 
         for(i = 0; i < k; i++) {
 
@@ -4868,11 +4874,15 @@ bail_out:
                 SiS_SetRegANDOR(SISSR, 0x14, 0x0f, (reg & 0xf0));
                 sisfb_post_xgi_delay(ivideo, 1);
 
-                if(sisfb_post_xgi_rwtest(ivideo, j, ((reg >> 4) + channelab - 2 + 20), mapsize))
+                if (sisfb_post_xgi_rwtest(ivideo, j, ((reg >> 4) + channelab - 2 + 20), mapsize)) {
+                        status = 0;
                         break;
+                }
         }
 
         iounmap(ivideo->video_vbase);
+
+        return status;
 }
 
 static void __devinit
@@ -4931,6 +4941,175 @@ sisfb_post_xgi_setclocks(struct sis_video_info *ivideo, u8 regb)
         sisfb_post_xgi_delay(ivideo, 0x43);
 }
 
+static void __devinit
+sisfb_post_xgi_ddr2_mrs_default(struct sis_video_info *ivideo, u8 regb)
+{
+        unsigned char *bios = ivideo->bios_abase;
+        u8 v1;
+
+        SiS_SetReg(SISSR, 0x28, 0x64);
+        SiS_SetReg(SISSR, 0x29, 0x63);
+        sisfb_post_xgi_delay(ivideo, 15);
+        SiS_SetReg(SISSR, 0x18, 0x00);
+        SiS_SetReg(SISSR, 0x19, 0x20);
+        SiS_SetReg(SISSR, 0x16, 0x00);
+        SiS_SetReg(SISSR, 0x16, 0x80);
+        SiS_SetReg(SISSR, 0x18, 0xc5);
+        SiS_SetReg(SISSR, 0x19, 0x23);
+        SiS_SetReg(SISSR, 0x16, 0x00);
+        SiS_SetReg(SISSR, 0x16, 0x80);
+        sisfb_post_xgi_delay(ivideo, 1);
+        SiS_SetReg(SISCR, 0x97, 0x11);
+        sisfb_post_xgi_setclocks(ivideo, regb);
+        sisfb_post_xgi_delay(ivideo, 0x46);
+        SiS_SetReg(SISSR, 0x18, 0xc5);
+        SiS_SetReg(SISSR, 0x19, 0x23);
+        SiS_SetReg(SISSR, 0x16, 0x00);
+        SiS_SetReg(SISSR, 0x16, 0x80);
+        sisfb_post_xgi_delay(ivideo, 1);
+        SiS_SetReg(SISSR, 0x1b, 0x04);
+        sisfb_post_xgi_delay(ivideo, 1);
+        SiS_SetReg(SISSR, 0x1b, 0x00);
+        sisfb_post_xgi_delay(ivideo, 1);
+        v1 = 0x31;
+        if (ivideo->haveXGIROM) {
+                v1 = bios[0xf0];
+        }
+        SiS_SetReg(SISSR, 0x18, v1);
+        SiS_SetReg(SISSR, 0x19, 0x06);
+        SiS_SetReg(SISSR, 0x16, 0x04);
+        SiS_SetReg(SISSR, 0x16, 0x84);
+        sisfb_post_xgi_delay(ivideo, 1);
+}
+
+static void __devinit
+sisfb_post_xgi_ddr2_mrs_xg21(struct sis_video_info *ivideo)
+{
+        sisfb_post_xgi_setclocks(ivideo, 1);
+
+        SiS_SetReg(SISCR, 0x97, 0x11);
+        sisfb_post_xgi_delay(ivideo, 0x46);
+
+        SiS_SetReg(SISSR, 0x18, 0x00);  /* EMRS2 */
+        SiS_SetReg(SISSR, 0x19, 0x80);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+
+        SiS_SetReg(SISSR, 0x18, 0x00);  /* EMRS3 */
+        SiS_SetReg(SISSR, 0x19, 0xc0);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+
+        SiS_SetReg(SISSR, 0x18, 0x00);  /* EMRS1 */
+        SiS_SetReg(SISSR, 0x19, 0x40);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+
+        SiS_SetReg(SISSR, 0x18, 0x42);  /* MRS1 */
+        SiS_SetReg(SISSR, 0x19, 0x02);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+        sisfb_post_xgi_delay(ivideo, 1);
+
+        SiS_SetReg(SISSR, 0x1b, 0x04);
+        sisfb_post_xgi_delay(ivideo, 1);
+
+        SiS_SetReg(SISSR, 0x1b, 0x00);
+        sisfb_post_xgi_delay(ivideo, 1);
+
+        SiS_SetReg(SISSR, 0x18, 0x42);  /* MRS1 */
+        SiS_SetReg(SISSR, 0x19, 0x00);
+        SiS_SetReg(SISSR, 0x16, 0x05);
+        SiS_SetReg(SISSR, 0x16, 0x85);
+        sisfb_post_xgi_delay(ivideo, 1);
+}
+
+static void __devinit
+sisfb_post_xgi_ddr2(struct sis_video_info *ivideo, u8 regb)
+{
+        unsigned char *bios = ivideo->bios_abase;
+        static const u8 cs158[8] = {
+                0x88, 0xaa, 0x48, 0x00, 0x00, 0x00, 0x00, 0x00
+        };
+        static const u8 cs160[8] = {
+                0x44, 0x77, 0x77, 0x00, 0x00, 0x00, 0x00, 0x00
+        };
+        static const u8 cs168[8] = {
+                0x48, 0x78, 0x88, 0x00, 0x00, 0x00, 0x00, 0x00
+        };
+        u8 reg;
+        u8 v1;
+        u8 v2;
+        u8 v3;
+
+        SiS_SetReg(SISCR, 0xb0, 0x80); /* DDR2 dual frequency mode */
+        SiS_SetReg(SISCR, 0x82, 0x77);
+        SiS_SetReg(SISCR, 0x86, 0x00);
+        reg = SiS_GetReg(SISCR, 0x86);
+        SiS_SetReg(SISCR, 0x86, 0x88);
+        reg = SiS_GetReg(SISCR, 0x86);
+        v1 = cs168[regb]; v2 = cs160[regb]; v3 = cs158[regb];
+        if (ivideo->haveXGIROM) {
+                v1 = bios[regb + 0x168];
+                v2 = bios[regb + 0x160];
+                v3 = bios[regb + 0x158];
+        }
+        SiS_SetReg(SISCR, 0x86, v1);
+        SiS_SetReg(SISCR, 0x82, 0x77);
+        SiS_SetReg(SISCR, 0x85, 0x00);
+        reg = SiS_GetReg(SISCR, 0x85);
+        SiS_SetReg(SISCR, 0x85, 0x88);
+        reg = SiS_GetReg(SISCR, 0x85);
+        SiS_SetReg(SISCR, 0x85, v2);
+        SiS_SetReg(SISCR, 0x82, v3);
+        SiS_SetReg(SISCR, 0x98, 0x01);
+        SiS_SetReg(SISCR, 0x9a, 0x02);
+        if (sisfb_xgi_is21(ivideo))
+                sisfb_post_xgi_ddr2_mrs_xg21(ivideo);
+        else
+                sisfb_post_xgi_ddr2_mrs_default(ivideo, regb);
+}
+
+static u8 __devinit
+sisfb_post_xgi_ramtype(struct sis_video_info *ivideo)
+{
+        unsigned char *bios = ivideo->bios_abase;
+        u8 ramtype;
+        u8 reg;
+        u8 v1;
+
+        ramtype = 0x00; v1 = 0x10;
+        if (ivideo->haveXGIROM) {
+                ramtype = bios[0x62];
+                v1 = bios[0x1d2];
+        }
+        if (!(ramtype & 0x80)) {
+                if (sisfb_xgi_is21(ivideo)) {
+                        SiS_SetRegAND(SISCR, 0xb4, 0xfd); /* GPIO control */
+                        SiS_SetRegOR(SISCR, 0x4a, 0x80);  /* GPIOH EN */
+                        reg = SiS_GetReg(SISCR, 0x48);
+                        SiS_SetRegOR(SISCR, 0xb4, 0x02);
+                        ramtype = reg & 0x01;             /* GPIOH */
+                } else if (ivideo->chip == XGI_20) {
+                        SiS_SetReg(SISCR, 0x97, v1);
+                        reg = SiS_GetReg(SISCR, 0x97);
+                        if (reg & 0x10) {
+                                ramtype = (reg & 0x01) << 1;
+                        }
+                } else {
+                        reg = SiS_GetReg(SISSR, 0x39);
+                        ramtype = reg & 0x02;
+                        if (!(ramtype)) {
+                                reg = SiS_GetReg(SISSR, 0x3a);
+                                ramtype = (reg >> 1) & 0x01;
+                        }
+                }
+        }
+        ramtype &= 0x07;
+
+        return ramtype;
+}
+
 static int __devinit
 sisfb_post_xgi(struct pci_dev *pdev)
 {
@@ -5213,9 +5392,23 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 SiS_SetReg(SISCR, 0x77, v1);
         }
 
-        /* RAM type */
-
-        regb = 0;       /* ! */
+        /* RAM type:
+         *
+         * 0 == DDR1, 1 == DDR2, 2..7 == reserved?
+         *
+         * The code seems to written so that regb should equal ramtype,
+         * however, so far it has been hardcoded to 0. Enable other values only
+         * on XGI Z9, as it passes the POST, and add a warning for others.
+         */
+        ramtype = sisfb_post_xgi_ramtype(ivideo);
+        if (!sisfb_xgi_is21(ivideo) && ramtype) {
+                dev_warn(&pdev->dev,
+                         "RAM type something else than expected: %d\n",
+                         ramtype);
+                regb = 0;
+        } else {
+                regb = ramtype;
+        }
 
         v1 = 0xff;
         if(ivideo->haveXGIROM) {
@@ -5367,7 +5560,10 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 }
         }
 
-        SiS_SetReg(SISSR, 0x17, 0x00);
+        if (regb == 1)
+                SiS_SetReg(SISSR, 0x17, 0x80);          /* DDR2 */
+        else
+                SiS_SetReg(SISSR, 0x17, 0x00);          /* DDR1 */
         SiS_SetReg(SISSR, 0x1a, 0x87);
 
         if(ivideo->chip == XGI_20) {
@@ -5375,31 +5571,6 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 SiS_SetReg(SISSR, 0x1c, 0x00);
         }
 
-        ramtype = 0x00; v1 = 0x10;
-        if(ivideo->haveXGIROM) {
-                ramtype = bios[0x62];
-                v1 = bios[0x1d2];
-        }
-        if(!(ramtype & 0x80)) {
-                if(ivideo->chip == XGI_20) {
-                        SiS_SetReg(SISCR, 0x97, v1);
-                        reg = SiS_GetReg(SISCR, 0x97);
-                        if(reg & 0x10) {
-                                ramtype = (reg & 0x01) << 1;
-                        }
-                } else {
-                        reg = SiS_GetReg(SISSR, 0x39);
-                        ramtype = reg & 0x02;
-                        if(!(ramtype)) {
-                                reg = SiS_GetReg(SISSR, 0x3a);
-                                ramtype = (reg >> 1) & 0x01;
-                        }
-                }
-        }
-        ramtype &= 0x07;
-
-        regb = 0;       /* ! */
-
         switch(ramtype) {
         case 0:
                 sisfb_post_xgi_setclocks(ivideo, regb);
@@ -5485,61 +5656,7 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 SiS_SetReg(SISSR, 0x1b, 0x00);
                 break;
         case 1:
-                SiS_SetReg(SISCR, 0x82, 0x77);
-                SiS_SetReg(SISCR, 0x86, 0x00);
-                reg = SiS_GetReg(SISCR, 0x86);
-                SiS_SetReg(SISCR, 0x86, 0x88);
-                reg = SiS_GetReg(SISCR, 0x86);
-                v1 = cs168[regb]; v2 = cs160[regb]; v3 = cs158[regb];
-                if(ivideo->haveXGIROM) {
-                        v1 = bios[regb + 0x168];
-                        v2 = bios[regb + 0x160];
-                        v3 = bios[regb + 0x158];
-                }
-                SiS_SetReg(SISCR, 0x86, v1);
-                SiS_SetReg(SISCR, 0x82, 0x77);
-                SiS_SetReg(SISCR, 0x85, 0x00);
-                reg = SiS_GetReg(SISCR, 0x85);
-                SiS_SetReg(SISCR, 0x85, 0x88);
-                reg = SiS_GetReg(SISCR, 0x85);
-                SiS_SetReg(SISCR, 0x85, v2);
-                SiS_SetReg(SISCR, 0x82, v3);
-                SiS_SetReg(SISCR, 0x98, 0x01);
-                SiS_SetReg(SISCR, 0x9a, 0x02);
-
-                SiS_SetReg(SISSR, 0x28, 0x64);
-                SiS_SetReg(SISSR, 0x29, 0x63);
-                sisfb_post_xgi_delay(ivideo, 15);
-                SiS_SetReg(SISSR, 0x18, 0x00);
-                SiS_SetReg(SISSR, 0x19, 0x20);
-                SiS_SetReg(SISSR, 0x16, 0x00);
-                SiS_SetReg(SISSR, 0x16, 0x80);
-                SiS_SetReg(SISSR, 0x18, 0xc5);
-                SiS_SetReg(SISSR, 0x19, 0x23);
-                SiS_SetReg(SISSR, 0x16, 0x00);
-                SiS_SetReg(SISSR, 0x16, 0x80);
-                sisfb_post_xgi_delay(ivideo, 1);
-                SiS_SetReg(SISCR, 0x97, 0x11);
-                sisfb_post_xgi_setclocks(ivideo, regb);
-                sisfb_post_xgi_delay(ivideo, 0x46);
-                SiS_SetReg(SISSR, 0x18, 0xc5);
-                SiS_SetReg(SISSR, 0x19, 0x23);
-                SiS_SetReg(SISSR, 0x16, 0x00);
-                SiS_SetReg(SISSR, 0x16, 0x80);
-                sisfb_post_xgi_delay(ivideo, 1);
-                SiS_SetReg(SISSR, 0x1b, 0x04);
-                sisfb_post_xgi_delay(ivideo, 1);
-                SiS_SetReg(SISSR, 0x1b, 0x00);
-                sisfb_post_xgi_delay(ivideo, 1);
-                v1 = 0x31;
-                if(ivideo->haveXGIROM) {
-                        v1 = bios[0xf0];
-                }
-                SiS_SetReg(SISSR, 0x18, v1);
-                SiS_SetReg(SISSR, 0x19, 0x06);
-                SiS_SetReg(SISSR, 0x16, 0x04);
-                SiS_SetReg(SISSR, 0x16, 0x84);
-                sisfb_post_xgi_delay(ivideo, 1);
+                sisfb_post_xgi_ddr2(ivideo, regb);
                 break;
         default:
                 sisfb_post_xgi_setclocks(ivideo, regb);
@@ -5648,6 +5765,7 @@ sisfb_post_xgi(struct pci_dev *pdev)
                 SiS_SetReg(SISSR, 0x14, bios[regb + 0xe0 + 8]);
 
         } else {
+                int err;
 
                 /* Set default mode, don't clear screen */
                 ivideo->SiS_Pr.SiS_UseOEM = false;
@@ -5661,10 +5779,16 @@ sisfb_post_xgi(struct pci_dev *pdev)
 
                 /* Disable read-cache */
                 SiS_SetRegAND(SISSR, 0x21, 0xdf);
-                sisfb_post_xgi_ramsize(ivideo);
+                err = sisfb_post_xgi_ramsize(ivideo);
                 /* Enable read-cache */
                 SiS_SetRegOR(SISSR, 0x21, 0x20);
 
+                if (err) {
+                        dev_err(&pdev->dev,
+                                "%s: RAM size detection failed: %d\n",
+                                __func__, err);
+                        return 0;
+                }
         }
 
 #if 0
@@ -5777,6 +5901,7 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
 #endif
 
         ivideo->chip = chipinfo->chip;
+        ivideo->chip_real_id = chipinfo->chip;
         ivideo->sisvga_engine = chipinfo->vgaengine;
         ivideo->hwcursor_size = chipinfo->hwcursor_size;
         ivideo->CRT2_write_enable = chipinfo->CRT2_write_enable;
@@ -6010,6 +6135,18 @@ sisfb_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
                 sisfb_detect_custom_timing(ivideo);
         }
 
+#ifdef CONFIG_FB_SIS_315
+        if (ivideo->chip == XGI_20) {
+                /* Check if our Z7 chip is actually Z9 */
+                SiS_SetRegOR(SISCR, 0x4a, 0x40);        /* GPIOG EN */
+                reg = SiS_GetReg(SISCR, 0x48);
+                if (reg & 0x02) {                       /* GPIOG */
+                        ivideo->chip_real_id = XGI_21;
+                        dev_info(&pdev->dev, "Z9 detected\n");
+                }
+        }
+#endif
+
         /* POST card in case this has not been done by the BIOS */
         if( (!ivideo->sisvga_enabled)
 #if !defined(__i386__) && !defined(__x86_64__)
diff --git a/drivers/video/sis/vgatypes.h b/drivers/video/sis/vgatypes.h
index 12c0dfaf2518..e3f9976cfef0 100644
--- a/drivers/video/sis/vgatypes.h
+++ b/drivers/video/sis/vgatypes.h
@@ -87,6 +87,7 @@ typedef enum _SIS_CHIP_TYPE {
     SIS_341,
     SIS_342,
     XGI_20  = 75,
+    XGI_21,
     XGI_40,
     MAX_SIS_CHIP
 } SIS_CHIP_TYPE;
diff --git a/drivers/video/sm501fb.c b/drivers/video/sm501fb.c
index bcb44a594ebc..46d1a64fe80d 100644
--- a/drivers/video/sm501fb.c
+++ b/drivers/video/sm501fb.c
@@ -41,6 +41,26 @@
 #include <linux/sm501.h>
 #include <linux/sm501-regs.h>
 
+#include "edid.h"
+
+static char *fb_mode = "640x480-16@60";
+static unsigned long default_bpp = 16;
+
+static struct fb_videomode __devinitdata sm501_default_mode = {
+        .refresh        = 60,
+        .xres           = 640,
+        .yres           = 480,
+        .pixclock       = 20833,
+        .left_margin    = 142,
+        .right_margin   = 13,
+        .upper_margin   = 21,
+        .lower_margin   = 1,
+        .hsync_len      = 69,
+        .vsync_len      = 3,
+        .sync           = FB_SYNC_HOR_HIGH_ACT | FB_SYNC_VERT_HIGH_ACT,
+        .vmode          = FB_VMODE_NONINTERLACED
+};
+
 #define NR_PALETTE      256
 
 enum sm501_controller {
@@ -77,6 +97,7 @@ struct sm501fb_info {
         void __iomem            *regs2d;        /* 2d remapped registers */
         void __iomem            *fbmem;         /* remapped framebuffer */
         size_t                   fbmem_len;     /* length of remapped region */
+        u8 *edid_data;
 };
 
 /* per-framebuffer private data */
@@ -117,7 +138,7 @@ static inline int v_total(struct fb_var_screeninfo *var)
 
 static inline void sm501fb_sync_regs(struct sm501fb_info *info)
 {
-        readl(info->regs);
+        smc501_readl(info->regs);
 }
 
 /* sm501_alloc_mem
@@ -262,7 +283,7 @@ static void sm501fb_setup_gamma(struct sm501fb_info *fbi,
 
         /* set gamma values */
         for (offset = 0; offset < 256 * 4; offset += 4) {
-                writel(value, fbi->regs + palette + offset);
+                smc501_writel(value, fbi->regs + palette + offset);
                 value += 0x010101;      /* Advance RGB by 1,1,1.*/
         }
 }
@@ -476,7 +497,8 @@ static int sm501fb_set_par_common(struct fb_info *info,
 
         /* set start of framebuffer to the screen */
 
-        writel(par->screen.sm_addr | SM501_ADDR_FLIP, fbi->regs + head_addr);
+        smc501_writel(par->screen.sm_addr | SM501_ADDR_FLIP,
+                        fbi->regs + head_addr);
 
         /* program CRT clock  */
 
@@ -519,7 +541,7 @@ static void sm501fb_set_par_geometry(struct fb_info *info,
         reg = info->fix.line_length;
         reg |= ((var->xres * var->bits_per_pixel)/8) << 16;
 
-        writel(reg, fbi->regs + (par->head == HEAD_CRT ?
+        smc501_writel(reg, fbi->regs + (par->head == HEAD_CRT ?
                     SM501_DC_CRT_FB_OFFSET :  SM501_DC_PANEL_FB_OFFSET));
 
         /* program horizontal total */
@@ -527,27 +549,27 @@ static void sm501fb_set_par_geometry(struct fb_info *info,
         reg  = (h_total(var) - 1) << 16;
         reg |= (var->xres - 1);
 
-        writel(reg, base + SM501_OFF_DC_H_TOT);
+        smc501_writel(reg, base + SM501_OFF_DC_H_TOT);
 
         /* program horizontal sync */
 
         reg  = var->hsync_len << 16;
         reg |= var->xres + var->right_margin - 1;
 
-        writel(reg, base + SM501_OFF_DC_H_SYNC);
+        smc501_writel(reg, base + SM501_OFF_DC_H_SYNC);
 
         /* program vertical total */
 
         reg  = (v_total(var) - 1) << 16;
         reg |= (var->yres - 1);
 
-        writel(reg, base + SM501_OFF_DC_V_TOT);
+        smc501_writel(reg, base + SM501_OFF_DC_V_TOT);
 
         /* program vertical sync */
         reg  = var->vsync_len << 16;
         reg |= var->yres + var->lower_margin - 1;
 
-        writel(reg, base + SM501_OFF_DC_V_SYNC);
+        smc501_writel(reg, base + SM501_OFF_DC_V_SYNC);
 }
 
 /* sm501fb_pan_crt
@@ -566,15 +588,15 @@ static int sm501fb_pan_crt(struct fb_var_screeninfo *var,
 
         xoffs = var->xoffset * bytes_pixel;
 
-        reg = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+        reg = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
         reg &= ~SM501_DC_CRT_CONTROL_PIXEL_MASK;
         reg |= ((xoffs & 15) / bytes_pixel) << 4;
-        writel(reg, fbi->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(reg, fbi->regs + SM501_DC_CRT_CONTROL);
 
         reg = (par->screen.sm_addr + xoffs +
                var->yoffset * info->fix.line_length);
-        writel(reg | SM501_ADDR_FLIP, fbi->regs + SM501_DC_CRT_FB_ADDR);
+        smc501_writel(reg | SM501_ADDR_FLIP, fbi->regs + SM501_DC_CRT_FB_ADDR);
 
         sm501fb_sync_regs(fbi);
         return 0;
@@ -593,10 +615,10 @@ static int sm501fb_pan_pnl(struct fb_var_screeninfo *var,
         unsigned long reg;
 
         reg = var->xoffset | (var->xres_virtual << 16);
-        writel(reg, fbi->regs + SM501_DC_PANEL_FB_WIDTH);
+        smc501_writel(reg, fbi->regs + SM501_DC_PANEL_FB_WIDTH);
 
         reg = var->yoffset | (var->yres_virtual << 16);
-        writel(reg, fbi->regs + SM501_DC_PANEL_FB_HEIGHT);
+        smc501_writel(reg, fbi->regs + SM501_DC_PANEL_FB_HEIGHT);
 
         sm501fb_sync_regs(fbi);
         return 0;
@@ -622,7 +644,7 @@ static int sm501fb_set_par_crt(struct fb_info *info)
         /* enable CRT DAC - note 0 is on!*/
         sm501_misc_control(fbi->dev->parent, 0, SM501_MISC_DAC_POWER);
 
-        control = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+        control = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
         control &= (SM501_DC_CRT_CONTROL_PIXEL_MASK |
                     SM501_DC_CRT_CONTROL_GAMMA |
@@ -684,7 +706,7 @@ static int sm501fb_set_par_crt(struct fb_info *info)
  out_update:
         dev_dbg(fbi->dev, "new control is %08lx\n", control);
 
-        writel(control, fbi->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(control, fbi->regs + SM501_DC_CRT_CONTROL);
         sm501fb_sync_regs(fbi);
 
         return 0;
@@ -696,18 +718,18 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
         void __iomem *ctrl_reg = fbi->regs + SM501_DC_PANEL_CONTROL;
         struct sm501_platdata_fbsub *pd = fbi->pdata->fb_pnl;
 
-        control = readl(ctrl_reg);
+        control = smc501_readl(ctrl_reg);
 
         if (to && (control & SM501_DC_PANEL_CONTROL_VDD) == 0) {
                 /* enable panel power */
 
                 control |= SM501_DC_PANEL_CONTROL_VDD;  /* FPVDDEN */
-                writel(control, ctrl_reg);
+                smc501_writel(control, ctrl_reg);
                 sm501fb_sync_regs(fbi);
                 mdelay(10);
 
                 control |= SM501_DC_PANEL_CONTROL_DATA; /* DATA */
-                writel(control, ctrl_reg);
+                smc501_writel(control, ctrl_reg);
                 sm501fb_sync_regs(fbi);
                 mdelay(10);
 
@@ -719,7 +741,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
                         else
                                 control |= SM501_DC_PANEL_CONTROL_BIAS;
 
-                        writel(control, ctrl_reg);
+                        smc501_writel(control, ctrl_reg);
                         sm501fb_sync_regs(fbi);
                         mdelay(10);
                 }
@@ -730,7 +752,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
                         else
                                 control |= SM501_DC_PANEL_CONTROL_FPEN;
 
-                        writel(control, ctrl_reg);
+                        smc501_writel(control, ctrl_reg);
                         sm501fb_sync_regs(fbi);
                         mdelay(10);
                 }
@@ -742,7 +764,7 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
                         else
                                 control &= ~SM501_DC_PANEL_CONTROL_FPEN;
 
-                        writel(control, ctrl_reg);
+                        smc501_writel(control, ctrl_reg);
                         sm501fb_sync_regs(fbi);
                         mdelay(10);
                 }
@@ -753,18 +775,18 @@ static void sm501fb_panel_power(struct sm501fb_info *fbi, int to)
                         else
                                 control &= ~SM501_DC_PANEL_CONTROL_BIAS;
 
-                        writel(control, ctrl_reg);
+                        smc501_writel(control, ctrl_reg);
                         sm501fb_sync_regs(fbi);
                         mdelay(10);
                 }
 
                 control &= ~SM501_DC_PANEL_CONTROL_DATA;
-                writel(control, ctrl_reg);
+                smc501_writel(control, ctrl_reg);
                 sm501fb_sync_regs(fbi);
                 mdelay(10);
 
                 control &= ~SM501_DC_PANEL_CONTROL_VDD;
-                writel(control, ctrl_reg);
+                smc501_writel(control, ctrl_reg);
                 sm501fb_sync_regs(fbi);
                 mdelay(10);
         }
@@ -799,7 +821,7 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
 
         /* update control register */
 
-        control = readl(fbi->regs + SM501_DC_PANEL_CONTROL);
+        control = smc501_readl(fbi->regs + SM501_DC_PANEL_CONTROL);
         control &= (SM501_DC_PANEL_CONTROL_GAMMA |
                     SM501_DC_PANEL_CONTROL_VDD  |
                     SM501_DC_PANEL_CONTROL_DATA |
@@ -833,16 +855,16 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
                 BUG();
         }
 
-        writel(0x0, fbi->regs + SM501_DC_PANEL_PANNING_CONTROL);
+        smc501_writel(0x0, fbi->regs + SM501_DC_PANEL_PANNING_CONTROL);
 
         /* panel plane top left and bottom right location */
 
-        writel(0x00, fbi->regs + SM501_DC_PANEL_TL_LOC);
+        smc501_writel(0x00, fbi->regs + SM501_DC_PANEL_TL_LOC);
 
         reg  = var->xres - 1;
         reg |= (var->yres - 1) << 16;
 
-        writel(reg, fbi->regs + SM501_DC_PANEL_BR_LOC);
+        smc501_writel(reg, fbi->regs + SM501_DC_PANEL_BR_LOC);
 
         /* program panel control register */
 
@@ -855,7 +877,7 @@ static int sm501fb_set_par_pnl(struct fb_info *info)
         if ((var->sync & FB_SYNC_VERT_HIGH_ACT) == 0)
                 control |= SM501_DC_PANEL_CONTROL_VSP;
 
-        writel(control, fbi->regs + SM501_DC_PANEL_CONTROL);
+        smc501_writel(control, fbi->regs + SM501_DC_PANEL_CONTROL);
         sm501fb_sync_regs(fbi);
 
         /* ensure the panel interface is not tristated at this point */
@@ -924,7 +946,7 @@ static int sm501fb_setcolreg(unsigned regno,
                         val |= (green >> 8) << 8;
                         val |= blue >> 8;
 
-                        writel(val, base + (regno * 4));
+                        smc501_writel(val, base + (regno * 4));
                 }
 
                 break;
@@ -980,7 +1002,7 @@ static int sm501fb_blank_crt(int blank_mode, struct fb_info *info)
 
         dev_dbg(fbi->dev, "%s(mode=%d, %p)\n", __func__, blank_mode, info);
 
-        ctrl = readl(fbi->regs + SM501_DC_CRT_CONTROL);
+        ctrl = smc501_readl(fbi->regs + SM501_DC_CRT_CONTROL);
 
         switch (blank_mode) {
         case FB_BLANK_POWERDOWN:
@@ -1004,7 +1026,7 @@ static int sm501fb_blank_crt(int blank_mode, struct fb_info *info)
 
         }
 
-        writel(ctrl, fbi->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(ctrl, fbi->regs + SM501_DC_CRT_CONTROL);
         sm501fb_sync_regs(fbi);
 
         return 0;
@@ -1041,12 +1063,14 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
         if (cursor->image.depth > 1)
                 return -EINVAL;
 
-        hwc_addr = readl(base + SM501_OFF_HWC_ADDR);
+        hwc_addr = smc501_readl(base + SM501_OFF_HWC_ADDR);
 
         if (cursor->enable)
-                writel(hwc_addr | SM501_HWC_EN, base + SM501_OFF_HWC_ADDR);
+                smc501_writel(hwc_addr | SM501_HWC_EN,
+                                base + SM501_OFF_HWC_ADDR);
         else
-                writel(hwc_addr & ~SM501_HWC_EN, base + SM501_OFF_HWC_ADDR);
+                smc501_writel(hwc_addr & ~SM501_HWC_EN,
+                                base + SM501_OFF_HWC_ADDR);
 
         /* set data */
         if (cursor->set & FB_CUR_SETPOS) {
@@ -1060,7 +1084,7 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 
                 //y += cursor->image.height;
 
-                writel(x | (y << 16), base + SM501_OFF_HWC_LOC);
+                smc501_writel(x | (y << 16), base + SM501_OFF_HWC_LOC);
         }
 
         if (cursor->set & FB_CUR_SETCMAP) {
@@ -1080,8 +1104,8 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
 
                 dev_dbg(fbi->dev, "fgcol %08lx, bgcol %08lx\n", fg, bg);
 
-                writel(bg, base + SM501_OFF_HWC_COLOR_1_2);
-                writel(fg, base + SM501_OFF_HWC_COLOR_3);
+                smc501_writel(bg, base + SM501_OFF_HWC_COLOR_1_2);
+                smc501_writel(fg, base + SM501_OFF_HWC_COLOR_3);
         }
 
         if (cursor->set & FB_CUR_SETSIZE ||
@@ -1102,7 +1126,7 @@ static int sm501fb_cursor(struct fb_info *info, struct fb_cursor *cursor)
                         __func__, cursor->image.width, cursor->image.height);
 
                 for (op = 0; op < (64*64*2)/8; op+=4)
-                        writel(0x0, dst + op);
+                        smc501_writel(0x0, dst + op);
 
                 for (y = 0; y < cursor->image.height; y++) {
                         for (x = 0; x < cursor->image.width; x++) {
@@ -1141,7 +1165,7 @@ static ssize_t sm501fb_crtsrc_show(struct device *dev,
         struct sm501fb_info *info = dev_get_drvdata(dev);
         unsigned long ctrl;
 
-        ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+        ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
         ctrl &= SM501_DC_CRT_CONTROL_SEL;
 
         return snprintf(buf, PAGE_SIZE, "%s\n", ctrl ? "crt" : "panel");
@@ -1172,7 +1196,7 @@ static ssize_t sm501fb_crtsrc_store(struct device *dev,
 
         dev_info(dev, "setting crt source to head %d\n", head);
 
-        ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+        ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 
         if (head == HEAD_CRT) {
                 ctrl |= SM501_DC_CRT_CONTROL_SEL;
@@ -1184,7 +1208,7 @@ static ssize_t sm501fb_crtsrc_store(struct device *dev,
                 ctrl &= ~SM501_DC_CRT_CONTROL_TE;
         }
 
-        writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
         sm501fb_sync_regs(info);
 
         return len;
@@ -1205,7 +1229,8 @@ static int sm501fb_show_regs(struct sm501fb_info *info, char *ptr,
         unsigned int reg;
 
         for (reg = start; reg < (len + start); reg += 4)
-                ptr += sprintf(ptr, "%08x = %08x\n", reg, readl(mem + reg));
+                ptr += sprintf(ptr, "%08x = %08x\n", reg,
+                                smc501_readl(mem + reg));
 
         return ptr - buf;
 }
@@ -1257,7 +1282,7 @@ static int sm501fb_sync(struct fb_info *info)
 
         /* wait for the 2d engine to be ready */
         while ((count > 0) &&
-               (readl(fbi->regs + SM501_SYSTEM_CONTROL) &
+               (smc501_readl(fbi->regs + SM501_SYSTEM_CONTROL) &
                 SM501_SYSCTRL_2D_ENGINE_STATUS) != 0)
                 count--;
 
@@ -1312,45 +1337,46 @@ static void sm501fb_copyarea(struct fb_info *info, const struct fb_copyarea *are
                 return;
 
         /* set the base addresses */
-        writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
-        writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_DESTINATION_BASE);
+        smc501_writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
+        smc501_writel(par->screen.sm_addr,
+                        fbi->regs2d + SM501_2D_DESTINATION_BASE);
 
         /* set the window width */
-        writel((info->var.xres << 16) | info->var.xres,
+        smc501_writel((info->var.xres << 16) | info->var.xres,
                fbi->regs2d + SM501_2D_WINDOW_WIDTH);
 
         /* set window stride */
-        writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
+        smc501_writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
                fbi->regs2d + SM501_2D_PITCH);
 
         /* set data format */
         switch (info->var.bits_per_pixel) {
         case 8:
-                writel(0, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         case 16:
-                writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         case 32:
-                writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         }
 
         /* 2d compare mask */
-        writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
+        smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
 
         /* 2d mask */
-        writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
+        smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
 
         /* source and destination x y */
-        writel((sx << 16) | sy, fbi->regs2d + SM501_2D_SOURCE);
-        writel((dx << 16) | dy, fbi->regs2d + SM501_2D_DESTINATION);
+        smc501_writel((sx << 16) | sy, fbi->regs2d + SM501_2D_SOURCE);
+        smc501_writel((dx << 16) | dy, fbi->regs2d + SM501_2D_DESTINATION);
 
         /* w/h */
-        writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
+        smc501_writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
 
         /* do area move */
-        writel(0x800000cc | rtl, fbi->regs2d + SM501_2D_CONTROL);
+        smc501_writel(0x800000cc | rtl, fbi->regs2d + SM501_2D_CONTROL);
 }
 
 static void sm501fb_fillrect(struct fb_info *info, const struct fb_fillrect *rect)
@@ -1372,47 +1398,49 @@ static void sm501fb_fillrect(struct fb_info *info, const struct fb_fillrect *rec
                 return;
 
         /* set the base addresses */
-        writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
-        writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_DESTINATION_BASE);
+        smc501_writel(par->screen.sm_addr, fbi->regs2d + SM501_2D_SOURCE_BASE);
+        smc501_writel(par->screen.sm_addr,
+                        fbi->regs2d + SM501_2D_DESTINATION_BASE);
 
         /* set the window width */
-        writel((info->var.xres << 16) | info->var.xres,
+        smc501_writel((info->var.xres << 16) | info->var.xres,
                fbi->regs2d + SM501_2D_WINDOW_WIDTH);
 
         /* set window stride */
-        writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
+        smc501_writel((info->var.xres_virtual << 16) | info->var.xres_virtual,
                fbi->regs2d + SM501_2D_PITCH);
 
         /* set data format */
         switch (info->var.bits_per_pixel) {
         case 8:
-                writel(0, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         case 16:
-                writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0x00100000, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         case 32:
-                writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
+                smc501_writel(0x00200000, fbi->regs2d + SM501_2D_STRETCH);
                 break;
         }
 
         /* 2d compare mask */
-        writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
+        smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_COLOR_COMPARE_MASK);
 
         /* 2d mask */
-        writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
+        smc501_writel(0xffffffff, fbi->regs2d + SM501_2D_MASK);
 
         /* colour */
-        writel(rect->color, fbi->regs2d + SM501_2D_FOREGROUND);
+        smc501_writel(rect->color, fbi->regs2d + SM501_2D_FOREGROUND);
 
         /* x y */
-        writel((rect->dx << 16) | rect->dy, fbi->regs2d + SM501_2D_DESTINATION);
+        smc501_writel((rect->dx << 16) | rect->dy,
+                        fbi->regs2d + SM501_2D_DESTINATION);
 
         /* w/h */
-        writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
+        smc501_writel((width << 16) | height, fbi->regs2d + SM501_2D_DIMENSION);
 
         /* do rectangle fill */
-        writel(0x800100cc, fbi->regs2d + SM501_2D_CONTROL);
+        smc501_writel(0x800100cc, fbi->regs2d + SM501_2D_CONTROL);
 }
 
 
@@ -1470,11 +1498,12 @@ static int sm501_init_cursor(struct fb_info *fbi, unsigned int reg_base)
 
         /* initialise the colour registers */
 
-        writel(par->cursor.sm_addr, par->cursor_regs + SM501_OFF_HWC_ADDR);
+        smc501_writel(par->cursor.sm_addr,
+                        par->cursor_regs + SM501_OFF_HWC_ADDR);
 
-        writel(0x00, par->cursor_regs + SM501_OFF_HWC_LOC);
-        writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_1_2);
-        writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_3);
+        smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_LOC);
+        smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_1_2);
+        smc501_writel(0x00, par->cursor_regs + SM501_OFF_HWC_COLOR_3);
         sm501fb_sync_regs(info);
 
         return 0;
@@ -1581,7 +1610,7 @@ static int sm501fb_start(struct sm501fb_info *info,
 
         /* clear palette ram - undefined at power on */
         for (k = 0; k < (256 * 3); k++)
-                writel(0, info->regs + SM501_DC_PANEL_PALETTE + (k * 4));
+                smc501_writel(0, info->regs + SM501_DC_PANEL_PALETTE + (k * 4));
 
         /* enable display controller */
         sm501_unit_power(dev->parent, SM501_GATE_DISPLAY, 1);
@@ -1649,20 +1678,20 @@ static int sm501fb_init_fb(struct fb_info *fb,
         switch (head) {
         case HEAD_CRT:
                 pd = info->pdata->fb_crt;
-                ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+                ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
                 enable = (ctrl & SM501_DC_CRT_CONTROL_ENABLE) ? 1 : 0;
 
                 /* ensure we set the correct source register */
                 if (info->pdata->fb_route != SM501_FB_CRT_PANEL) {
                         ctrl |= SM501_DC_CRT_CONTROL_SEL;
-                        writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+                        smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
                 }
 
                 break;
 
         case HEAD_PANEL:
                 pd = info->pdata->fb_pnl;
-                ctrl = readl(info->regs + SM501_DC_PANEL_CONTROL);
+                ctrl = smc501_readl(info->regs + SM501_DC_PANEL_CONTROL);
                 enable = (ctrl & SM501_DC_PANEL_CONTROL_EN) ? 1 : 0;
                 break;
 
@@ -1680,7 +1709,7 @@ static int sm501fb_init_fb(struct fb_info *fb,
 
         if (head == HEAD_CRT && info->pdata->fb_route == SM501_FB_CRT_PANEL) {
                 ctrl &= ~SM501_DC_CRT_CONTROL_SEL;
-                writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
+                smc501_writel(ctrl, info->regs + SM501_DC_CRT_CONTROL);
                 enable = 0;
         }
 
@@ -1700,6 +1729,15 @@ static int sm501fb_init_fb(struct fb_info *fb,
                 FBINFO_HWACCEL_COPYAREA | FBINFO_HWACCEL_FILLRECT |
                 FBINFO_HWACCEL_XPAN | FBINFO_HWACCEL_YPAN;
 
+#if defined(CONFIG_OF)
+#ifdef __BIG_ENDIAN
+        if (of_get_property(info->dev->parent->of_node, "little-endian", NULL))
+                fb->flags |= FBINFO_FOREIGN_ENDIAN;
+#else
+        if (of_get_property(info->dev->parent->of_node, "big-endian", NULL))
+                fb->flags |= FBINFO_FOREIGN_ENDIAN;
+#endif
+#endif
         /* fixed data */
 
         fb->fix.type            = FB_TYPE_PACKED_PIXELS;
@@ -1717,9 +1755,16 @@ static int sm501fb_init_fb(struct fb_info *fb,
         fb->var.vmode           = FB_VMODE_NONINTERLACED;
         fb->var.bits_per_pixel  = 16;
 
+        if (info->edid_data) {
+                        /* Now build modedb from EDID */
+                        fb_edid_to_monspecs(info->edid_data, &fb->monspecs);
+                        fb_videomode_to_modelist(fb->monspecs.modedb,
+                                                 fb->monspecs.modedb_len,
+                                                 &fb->modelist);
+        }
+
         if (enable && (pd->flags & SM501FB_FLAG_USE_INIT_MODE) && 0) {
                 /* TODO read the mode from the current display */
-
         } else {
                 if (pd->def_mode) {
                         dev_info(info->dev, "using supplied mode\n");
@@ -1729,12 +1774,37 @@ static int sm501fb_init_fb(struct fb_info *fb,
                         fb->var.xres_virtual = fb->var.xres;
                         fb->var.yres_virtual = fb->var.yres;
                 } else {
-                        ret = fb_find_mode(&fb->var, fb,
+                        if (info->edid_data) {
+                                ret = fb_find_mode(&fb->var, fb, fb_mode,
+                                        fb->monspecs.modedb,
+                                        fb->monspecs.modedb_len,
+                                        &sm501_default_mode, default_bpp);
+                                /* edid_data is no longer needed, free it */
+                                kfree(info->edid_data);
+                        } else {
+                                ret = fb_find_mode(&fb->var, fb,
                                            NULL, NULL, 0, NULL, 8);
+                        }
 
-                        if (ret == 0 || ret == 4) {
-                                dev_err(info->dev,
-                                        "failed to get initial mode\n");
+                        switch (ret) {
+                        case 1:
+                                dev_info(info->dev, "using mode specified in "
+                                                "@mode\n");
+                                break;
+                        case 2:
+                                dev_info(info->dev, "using mode specified in "
+                                        "@mode with ignored refresh rate\n");
+                                break;
+                        case 3:
+                                dev_info(info->dev, "using mode default "
+                                        "mode\n");
+                                break;
+                        case 4:
+                                dev_info(info->dev, "using mode from list\n");
+                                break;
+                        default:
+                                dev_info(info->dev, "ret = %d\n", ret);
+                                dev_info(info->dev, "failed to find mode\n");
                                 return -EINVAL;
                         }
                 }
@@ -1875,8 +1945,32 @@ static int __devinit sm501fb_probe(struct platform_device *pdev)
         }
 
         if (info->pdata == NULL) {
-                dev_info(dev, "using default configuration data\n");
+                int found = 0;
+#if defined(CONFIG_OF)
+                struct device_node *np = pdev->dev.parent->of_node;
+                const u8 *prop;
+                const char *cp;
+                int len;
+
                 info->pdata = &sm501fb_def_pdata;
+                if (np) {
+                        /* Get EDID */
+                        cp = of_get_property(np, "mode", &len);
+                        if (cp)
+                                strcpy(fb_mode, cp);
+                        prop = of_get_property(np, "edid", &len);
+                        if (prop && len == EDID_LENGTH) {
+                                info->edid_data = kmemdup(prop, EDID_LENGTH,
+                                                          GFP_KERNEL);
+                                if (info->edid_data)
+                                        found = 1;
+                        }
+                }
+#endif
+                if (!found) {
+                        dev_info(dev, "using default configuration data\n");
+                        info->pdata = &sm501fb_def_pdata;
+                }
         }
 
         /* probe for the presence of each panel */
@@ -2085,7 +2179,7 @@ static int sm501fb_suspend(struct platform_device *pdev, pm_message_t state)
         struct sm501fb_info *info = platform_get_drvdata(pdev);
 
         /* store crt control to resume with */
-        info->pm_crt_ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+        info->pm_crt_ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
 
         sm501fb_suspend_fb(info, HEAD_CRT);
         sm501fb_suspend_fb(info, HEAD_PANEL);
@@ -2109,10 +2203,10 @@ static int sm501fb_resume(struct platform_device *pdev)
 
         /* restore the items we want to be saved for crt control */
 
-        crt_ctrl = readl(info->regs + SM501_DC_CRT_CONTROL);
+        crt_ctrl = smc501_readl(info->regs + SM501_DC_CRT_CONTROL);
         crt_ctrl &= ~SM501_CRT_CTRL_SAVE;
         crt_ctrl |= info->pm_crt_ctrl & SM501_CRT_CTRL_SAVE;
-        writel(crt_ctrl, info->regs + SM501_DC_CRT_CONTROL);
+        smc501_writel(crt_ctrl, info->regs + SM501_DC_CRT_CONTROL);
 
         sm501fb_resume_fb(info, HEAD_CRT);
         sm501fb_resume_fb(info, HEAD_PANEL);
@@ -2149,6 +2243,11 @@ static void __exit sm501fb_cleanup(void)
 module_init(sm501fb_init);
 module_exit(sm501fb_cleanup);
 
+module_param_named(mode, fb_mode, charp, 0);
+MODULE_PARM_DESC(mode,
+        "Specify resolution as \"<xres>x<yres>[-<bpp>][@<refresh>]\" ");
+module_param_named(bpp, default_bpp, ulong, 0);
+MODULE_PARM_DESC(bpp, "Specify bit-per-pixel if not specified mode");
 MODULE_AUTHOR("Ben Dooks, Vincent Sanders");
 MODULE_DESCRIPTION("SM501 Framebuffer driver");
 MODULE_LICENSE("GPL v2");
diff --git a/drivers/video/svgalib.c b/drivers/video/svgalib.c
index fdb45674e2f6..33df9ec91795 100644
--- a/drivers/video/svgalib.c
+++ b/drivers/video/svgalib.c
@@ -20,12 +20,12 @@
 
 
 /* Write a CRT register value spread across multiple registers */
-void svga_wcrt_multi(const struct vga_regset *regset, u32 value) {
-
+void svga_wcrt_multi(void __iomem *regbase, const struct vga_regset *regset, u32 value)
+{
         u8 regval, bitval, bitnum;
 
         while (regset->regnum != VGA_REGSET_END_VAL) {
-                regval = vga_rcrt(NULL, regset->regnum);
+                regval = vga_rcrt(regbase, regset->regnum);
                 bitnum = regset->lowbit;
                 while (bitnum <= regset->highbit) {
                         bitval = 1 << bitnum;
@@ -34,18 +34,18 @@ void svga_wcrt_multi(const struct vga_regset *regset, u32 value) {
                         bitnum ++;
                         value = value >> 1;
                 }
-                vga_wcrt(NULL, regset->regnum, regval);
+                vga_wcrt(regbase, regset->regnum, regval);
                 regset ++;
         }
 }
 
 /* Write a sequencer register value spread across multiple registers */
-void svga_wseq_multi(const struct vga_regset *regset, u32 value) {
-
+void svga_wseq_multi(void __iomem *regbase, const struct vga_regset *regset, u32 value)
+{
         u8 regval, bitval, bitnum;
 
         while (regset->regnum != VGA_REGSET_END_VAL) {
-                regval = vga_rseq(NULL, regset->regnum);
+                regval = vga_rseq(regbase, regset->regnum);
                 bitnum = regset->lowbit;
                 while (bitnum <= regset->highbit) {
                         bitval = 1 << bitnum;
@@ -54,7 +54,7 @@ void svga_wseq_multi(const struct vga_regset *regset, u32 value) {
                         bitnum ++;
                         value = value >> 1;
                 }
-                vga_wseq(NULL, regset->regnum, regval);
+                vga_wseq(regbase, regset->regnum, regval);
                 regset ++;
         }
 }
@@ -75,95 +75,95 @@ static unsigned int svga_regset_size(const struct vga_regset *regset)
 
 
 /* Set graphics controller registers to sane values */
-void svga_set_default_gfx_regs(void)
+void svga_set_default_gfx_regs(void __iomem *regbase)
 {
         /* All standard GFX registers (GR00 - GR08) */
-        vga_wgfx(NULL, VGA_GFX_SR_VALUE, 0x00);
-        vga_wgfx(NULL, VGA_GFX_SR_ENABLE, 0x00);
-        vga_wgfx(NULL, VGA_GFX_COMPARE_VALUE, 0x00);
-        vga_wgfx(NULL, VGA_GFX_DATA_ROTATE, 0x00);
-        vga_wgfx(NULL, VGA_GFX_PLANE_READ, 0x00);
-        vga_wgfx(NULL, VGA_GFX_MODE, 0x00);
-/*      vga_wgfx(NULL, VGA_GFX_MODE, 0x20); */
-/*      vga_wgfx(NULL, VGA_GFX_MODE, 0x40); */
-        vga_wgfx(NULL, VGA_GFX_MISC, 0x05);
-/*      vga_wgfx(NULL, VGA_GFX_MISC, 0x01); */
-        vga_wgfx(NULL, VGA_GFX_COMPARE_MASK, 0x0F);
-        vga_wgfx(NULL, VGA_GFX_BIT_MASK, 0xFF);
+        vga_wgfx(regbase, VGA_GFX_SR_VALUE, 0x00);
+        vga_wgfx(regbase, VGA_GFX_SR_ENABLE, 0x00);
+        vga_wgfx(regbase, VGA_GFX_COMPARE_VALUE, 0x00);
+        vga_wgfx(regbase, VGA_GFX_DATA_ROTATE, 0x00);
+        vga_wgfx(regbase, VGA_GFX_PLANE_READ, 0x00);
+        vga_wgfx(regbase, VGA_GFX_MODE, 0x00);
+/*      vga_wgfx(regbase, VGA_GFX_MODE, 0x20); */
+/*      vga_wgfx(regbase, VGA_GFX_MODE, 0x40); */
+        vga_wgfx(regbase, VGA_GFX_MISC, 0x05);
+/*      vga_wgfx(regbase, VGA_GFX_MISC, 0x01); */
+        vga_wgfx(regbase, VGA_GFX_COMPARE_MASK, 0x0F);
+        vga_wgfx(regbase, VGA_GFX_BIT_MASK, 0xFF);
 }
 
 /* Set attribute controller registers to sane values */
-void svga_set_default_atc_regs(void)
+void svga_set_default_atc_regs(void __iomem *regbase)
 {
         u8 count;
 
-        vga_r(NULL, 0x3DA);
-        vga_w(NULL, VGA_ATT_W, 0x00);
+        vga_r(regbase, 0x3DA);
+        vga_w(regbase, VGA_ATT_W, 0x00);
 
         /* All standard ATC registers (AR00 - AR14) */
         for (count = 0; count <= 0xF; count ++)
-                svga_wattr(count, count);
+                svga_wattr(regbase, count, count);
 
-        svga_wattr(VGA_ATC_MODE, 0x01);
-/*      svga_wattr(VGA_ATC_MODE, 0x41); */
-        svga_wattr(VGA_ATC_OVERSCAN, 0x00);
-        svga_wattr(VGA_ATC_PLANE_ENABLE, 0x0F);
-        svga_wattr(VGA_ATC_PEL, 0x00);
-        svga_wattr(VGA_ATC_COLOR_PAGE, 0x00);
+        svga_wattr(regbase, VGA_ATC_MODE, 0x01);
+/*      svga_wattr(regbase, VGA_ATC_MODE, 0x41); */
+        svga_wattr(regbase, VGA_ATC_OVERSCAN, 0x00);
+        svga_wattr(regbase, VGA_ATC_PLANE_ENABLE, 0x0F);
+        svga_wattr(regbase, VGA_ATC_PEL, 0x00);
+        svga_wattr(regbase, VGA_ATC_COLOR_PAGE, 0x00);
 
-        vga_r(NULL, 0x3DA);
-        vga_w(NULL, VGA_ATT_W, 0x20);
+        vga_r(regbase, 0x3DA);
+        vga_w(regbase, VGA_ATT_W, 0x20);
 }
 
 /* Set sequencer registers to sane values */
-void svga_set_default_seq_regs(void)
+void svga_set_default_seq_regs(void __iomem *regbase)
 {
         /* Standard sequencer registers (SR01 - SR04), SR00 is not set */
-        vga_wseq(NULL, VGA_SEQ_CLOCK_MODE, VGA_SR01_CHAR_CLK_8DOTS);
-        vga_wseq(NULL, VGA_SEQ_PLANE_WRITE, VGA_SR02_ALL_PLANES);
-        vga_wseq(NULL, VGA_SEQ_CHARACTER_MAP, 0x00);
-/*      vga_wseq(NULL, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE | VGA_SR04_CHN_4M); */
-        vga_wseq(NULL, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE);
+        vga_wseq(regbase, VGA_SEQ_CLOCK_MODE, VGA_SR01_CHAR_CLK_8DOTS);
+        vga_wseq(regbase, VGA_SEQ_PLANE_WRITE, VGA_SR02_ALL_PLANES);
+        vga_wseq(regbase, VGA_SEQ_CHARACTER_MAP, 0x00);
+/*      vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE | VGA_SR04_CHN_4M); */
+        vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM | VGA_SR04_SEQ_MODE);
 }
 
 /* Set CRTC registers to sane values */
-void svga_set_default_crt_regs(void)
+void svga_set_default_crt_regs(void __iomem *regbase)
 {
         /* Standard CRT registers CR03 CR08 CR09 CR14 CR17 */
-        svga_wcrt_mask(0x03, 0x80, 0x80);       /* Enable vertical retrace EVRA */
-        vga_wcrt(NULL, VGA_CRTC_PRESET_ROW, 0);
-        svga_wcrt_mask(VGA_CRTC_MAX_SCAN, 0, 0x1F);
-        vga_wcrt(NULL, VGA_CRTC_UNDERLINE, 0);
-        vga_wcrt(NULL, VGA_CRTC_MODE, 0xE3);
+        svga_wcrt_mask(regbase, 0x03, 0x80, 0x80);      /* Enable vertical retrace EVRA */
+        vga_wcrt(regbase, VGA_CRTC_PRESET_ROW, 0);
+        svga_wcrt_mask(regbase, VGA_CRTC_MAX_SCAN, 0, 0x1F);
+        vga_wcrt(regbase, VGA_CRTC_UNDERLINE, 0);
+        vga_wcrt(regbase, VGA_CRTC_MODE, 0xE3);
 }
 
-void svga_set_textmode_vga_regs(void)
+void svga_set_textmode_vga_regs(void __iomem *regbase)
 {
-        /* svga_wseq_mask(0x1, 0x00, 0x01); */   /* Switch 8/9 pixel per char */
-        vga_wseq(NULL, VGA_SEQ_MEMORY_MODE,     VGA_SR04_EXT_MEM);
-        vga_wseq(NULL, VGA_SEQ_PLANE_WRITE,     0x03);
+        /* svga_wseq_mask(regbase, 0x1, 0x00, 0x01); */   /* Switch 8/9 pixel per char */
+        vga_wseq(regbase, VGA_SEQ_MEMORY_MODE, VGA_SR04_EXT_MEM);
+        vga_wseq(regbase, VGA_SEQ_PLANE_WRITE, 0x03);
 
-        vga_wcrt(NULL, VGA_CRTC_MAX_SCAN,       0x0f); /* 0x4f */
-        vga_wcrt(NULL, VGA_CRTC_UNDERLINE,      0x1f);
-        svga_wcrt_mask(VGA_CRTC_MODE,           0x23, 0x7f);
+        vga_wcrt(regbase, VGA_CRTC_MAX_SCAN, 0x0f); /* 0x4f */
+        vga_wcrt(regbase, VGA_CRTC_UNDERLINE, 0x1f);
+        svga_wcrt_mask(regbase, VGA_CRTC_MODE, 0x23, 0x7f);
 
-        vga_wcrt(NULL, VGA_CRTC_CURSOR_START,   0x0d);
-        vga_wcrt(NULL, VGA_CRTC_CURSOR_END,     0x0e);
-        vga_wcrt(NULL, VGA_CRTC_CURSOR_HI,      0x00);
-        vga_wcrt(NULL, VGA_CRTC_CURSOR_LO,      0x00);
+        vga_wcrt(regbase, VGA_CRTC_CURSOR_START, 0x0d);
+        vga_wcrt(regbase, VGA_CRTC_CURSOR_END, 0x0e);
+        vga_wcrt(regbase, VGA_CRTC_CURSOR_HI, 0x00);
+        vga_wcrt(regbase, VGA_CRTC_CURSOR_LO, 0x00);
 
-        vga_wgfx(NULL, VGA_GFX_MODE,            0x10); /* Odd/even memory mode */
-        vga_wgfx(NULL, VGA_GFX_MISC,            0x0E); /* Misc graphics register - text mode enable */
-        vga_wgfx(NULL, VGA_GFX_COMPARE_MASK,    0x00);
+        vga_wgfx(regbase, VGA_GFX_MODE, 0x10); /* Odd/even memory mode */
+        vga_wgfx(regbase, VGA_GFX_MISC, 0x0E); /* Misc graphics register - text mode enable */
+        vga_wgfx(regbase, VGA_GFX_COMPARE_MASK, 0x00);
 
-        vga_r(NULL, 0x3DA);
-        vga_w(NULL, VGA_ATT_W, 0x00);
+        vga_r(regbase, 0x3DA);
+        vga_w(regbase, VGA_ATT_W, 0x00);
 
-        svga_wattr(0x10, 0x0C);                 /* Attribute Mode Control Register - text mode, blinking and line graphics */
-        svga_wattr(0x13, 0x08);                 /* Horizontal Pixel Panning Register  */
+        svga_wattr(regbase, 0x10, 0x0C);                        /* Attribute Mode Control Register - text mode, blinking and line graphics */
+        svga_wattr(regbase, 0x13, 0x08);                        /* Horizontal Pixel Panning Register  */
 
-        vga_r(NULL, 0x3DA);
-        vga_w(NULL, VGA_ATT_W, 0x20);
+        vga_r(regbase, 0x3DA);
+        vga_w(regbase, VGA_ATT_W, 0x20);
 }
 
 #if 0
@@ -299,7 +299,7 @@ void svga_tileblit(struct fb_info *info, struct fb_tileblit *blit)
 }
 
 /* Set cursor in text (tileblit) mode */
-void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+void svga_tilecursor(void __iomem *regbase, struct fb_info *info, struct fb_tilecursor *cursor)
 {
         u8 cs = 0x0d;
         u8 ce = 0x0e;
@@ -310,7 +310,7 @@ void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
         if (! cursor -> mode)
                 return;
 
-        svga_wcrt_mask(0x0A, 0x20, 0x20); /* disable cursor */
+        svga_wcrt_mask(regbase, 0x0A, 0x20, 0x20); /* disable cursor */
 
         if (cursor -> shape == FB_TILE_CURSOR_NONE)
                 return;
@@ -334,11 +334,11 @@ void svga_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
         }
 
         /* set cursor position */
-        vga_wcrt(NULL, 0x0E, pos >> 8);
-        vga_wcrt(NULL, 0x0F, pos & 0xFF);
+        vga_wcrt(regbase, 0x0E, pos >> 8);
+        vga_wcrt(regbase, 0x0F, pos & 0xFF);
 
-        vga_wcrt(NULL, 0x0B, ce); /* set cursor end */
-        vga_wcrt(NULL, 0x0A, cs); /* set cursor start and enable it */
+        vga_wcrt(regbase, 0x0B, ce); /* set cursor end */
+        vga_wcrt(regbase, 0x0A, cs); /* set cursor start and enable it */
 }
 
 int svga_get_tilemax(struct fb_info *info)
@@ -507,8 +507,9 @@ int svga_check_timings(const struct svga_timing_regs *tm, struct fb_var_screenin
 }
 
 /* Set CRT timing registers */
-void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninfo *var,
-                        u32 hmul, u32 hdiv, u32 vmul, u32 vdiv, u32 hborder, int node)
+void svga_set_timings(void __iomem *regbase, const struct svga_timing_regs *tm,
+                      struct fb_var_screeninfo *var,
+                      u32 hmul, u32 hdiv, u32 vmul, u32 vdiv, u32 hborder, int node)
 {
         u8 regval;
         u32 value;
@@ -516,66 +517,66 @@ void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninf
         value = var->xres + var->left_margin + var->right_margin + var->hsync_len;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal total      : %d\n", node, value);
-        svga_wcrt_multi(tm->h_total_regs, (value / 8) - 5);
+        svga_wcrt_multi(regbase, tm->h_total_regs, (value / 8) - 5);
 
         value = var->xres;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal display    : %d\n", node, value);
-        svga_wcrt_multi(tm->h_display_regs, (value / 8) - 1);
+        svga_wcrt_multi(regbase, tm->h_display_regs, (value / 8) - 1);
 
         value = var->xres;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal blank start: %d\n", node, value);
-        svga_wcrt_multi(tm->h_blank_start_regs, (value / 8) - 1 + hborder);
+        svga_wcrt_multi(regbase, tm->h_blank_start_regs, (value / 8) - 1 + hborder);
 
         value = var->xres + var->left_margin + var->right_margin + var->hsync_len;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal blank end  : %d\n", node, value);
-        svga_wcrt_multi(tm->h_blank_end_regs, (value / 8) - 1 - hborder);
+        svga_wcrt_multi(regbase, tm->h_blank_end_regs, (value / 8) - 1 - hborder);
 
         value = var->xres + var->right_margin;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal sync start : %d\n", node, value);
-        svga_wcrt_multi(tm->h_sync_start_regs, (value / 8));
+        svga_wcrt_multi(regbase, tm->h_sync_start_regs, (value / 8));
 
         value = var->xres + var->right_margin + var->hsync_len;
         value = (value * hmul) / hdiv;
         pr_debug("fb%d: horizontal sync end   : %d\n", node, value);
-        svga_wcrt_multi(tm->h_sync_end_regs, (value / 8));
+        svga_wcrt_multi(regbase, tm->h_sync_end_regs, (value / 8));
 
         value = var->yres + var->upper_margin + var->lower_margin + var->vsync_len;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical total        : %d\n", node, value);
-        svga_wcrt_multi(tm->v_total_regs, value - 2);
+        svga_wcrt_multi(regbase, tm->v_total_regs, value - 2);
 
         value = var->yres;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical display      : %d\n", node, value);
-        svga_wcrt_multi(tm->v_display_regs, value - 1);
+        svga_wcrt_multi(regbase, tm->v_display_regs, value - 1);
 
         value = var->yres;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical blank start  : %d\n", node, value);
-        svga_wcrt_multi(tm->v_blank_start_regs, value);
+        svga_wcrt_multi(regbase, tm->v_blank_start_regs, value);
 
         value = var->yres + var->upper_margin + var->lower_margin + var->vsync_len;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical blank end    : %d\n", node, value);
-        svga_wcrt_multi(tm->v_blank_end_regs, value - 2);
+        svga_wcrt_multi(regbase, tm->v_blank_end_regs, value - 2);
 
         value = var->yres + var->lower_margin;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical sync start   : %d\n", node, value);
-        svga_wcrt_multi(tm->v_sync_start_regs, value);
+        svga_wcrt_multi(regbase, tm->v_sync_start_regs, value);
 
         value = var->yres + var->lower_margin + var->vsync_len;
         value = (value * vmul) / vdiv;
         pr_debug("fb%d: vertical sync end     : %d\n", node, value);
-        svga_wcrt_multi(tm->v_sync_end_regs, value);
+        svga_wcrt_multi(regbase, tm->v_sync_end_regs, value);
 
         /* Set horizontal and vertical sync pulse polarity in misc register */
 
-        regval = vga_r(NULL, VGA_MIS_R);
+        regval = vga_r(regbase, VGA_MIS_R);
         if (var->sync & FB_SYNC_HOR_HIGH_ACT) {
                 pr_debug("fb%d: positive horizontal sync\n", node);
                 regval = regval & ~0x80;
@@ -590,7 +591,7 @@ void svga_set_timings(const struct svga_timing_regs *tm, struct fb_var_screeninf
                 pr_debug("fb%d: negative vertical sync\n\n", node);
                 regval = regval | 0x40;
         }
-        vga_w(NULL, VGA_MIS_W, regval);
+        vga_w(regbase, VGA_MIS_W, regval);
 }
 
 
diff --git a/drivers/video/tcx.c b/drivers/video/tcx.c
index 855b71993f61..07c66e946634 100644
--- a/drivers/video/tcx.c
+++ b/drivers/video/tcx.c
@@ -480,6 +480,7 @@ out_dealloc_cmap:
 
 out_unmap_regs:
         tcx_unmap_regs(op, info, par);
+        framebuffer_release(info);
 
 out_err:
         return err;
diff --git a/drivers/video/tmiofb.c b/drivers/video/tmiofb.c
index dfef88c803d4..9710bf8caeae 100644
--- a/drivers/video/tmiofb.c
+++ b/drivers/video/tmiofb.c
@@ -250,8 +250,7 @@ static irqreturn_t tmiofb_irq(int irq, void *__info)
  */
 static int tmiofb_hw_stop(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        struct tmio_fb_data *data = mfd_get_data(dev);
         struct fb_info *info = platform_get_drvdata(dev);
         struct tmiofb_par *par = info->par;
 
@@ -268,7 +267,7 @@ static int tmiofb_hw_stop(struct platform_device *dev)
  */
 static int tmiofb_hw_init(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct fb_info *info = platform_get_drvdata(dev);
         struct tmiofb_par *par = info->par;
         const struct resource *nlcr = &cell->resources[0];
@@ -312,8 +311,7 @@ static int tmiofb_hw_init(struct platform_device *dev)
  */
 static void tmiofb_hw_mode(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        struct tmio_fb_data *data = mfd_get_data(dev);
         struct fb_info *info = platform_get_drvdata(dev);
         struct fb_videomode *mode = info->mode;
         struct tmiofb_par *par = info->par;
@@ -559,9 +557,8 @@ static int tmiofb_ioctl(struct fb_info *fbi,
 static struct fb_videomode *
 tmiofb_find_mode(struct fb_info *info, struct fb_var_screeninfo *var)
 {
-        struct mfd_cell *cell =
-                info->device->platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        struct tmio_fb_data *data =
+                        mfd_get_data(to_platform_device(info->device));
         struct fb_videomode *best = NULL;
         int i;
 
@@ -581,9 +578,8 @@ static int tmiofb_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
 {
 
         struct fb_videomode *mode;
-        struct mfd_cell *cell =
-                info->device->platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        struct tmio_fb_data *data =
+                        mfd_get_data(to_platform_device(info->device));
 
         mode = tmiofb_find_mode(info, var);
         if (!mode || var->bits_per_pixel > 16)
@@ -683,8 +679,8 @@ static struct fb_ops tmiofb_ops = {
 
 static int __devinit tmiofb_probe(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
-        struct tmio_fb_data *data = cell->driver_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
+        struct tmio_fb_data *data = mfd_get_data(dev);
         struct resource *ccr = platform_get_resource(dev, IORESOURCE_MEM, 1);
         struct resource *lcr = platform_get_resource(dev, IORESOURCE_MEM, 0);
         struct resource *vram = platform_get_resource(dev, IORESOURCE_MEM, 2);
@@ -811,7 +807,7 @@ err_ioremap_ccr:
 
 static int __devexit tmiofb_remove(struct platform_device *dev)
 {
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         struct fb_info *info = platform_get_drvdata(dev);
         int irq = platform_get_irq(dev, 0);
         struct tmiofb_par *par;
@@ -941,7 +937,7 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
 #ifdef CONFIG_FB_TMIO_ACCELL
         struct tmiofb_par *par = info->par;
 #endif
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         int retval = 0;
 
         console_lock();
@@ -973,7 +969,7 @@ static int tmiofb_suspend(struct platform_device *dev, pm_message_t state)
 static int tmiofb_resume(struct platform_device *dev)
 {
         struct fb_info *info = platform_get_drvdata(dev);
-        struct mfd_cell *cell = dev->dev.platform_data;
+        const struct mfd_cell *cell = mfd_get_cell(dev);
         int retval = 0;
 
         console_lock();
diff --git a/drivers/video/uvesafb.c b/drivers/video/uvesafb.c
index 5180a215d781..7f8472cc993b 100644
--- a/drivers/video/uvesafb.c
+++ b/drivers/video/uvesafb.c
@@ -1552,8 +1552,7 @@ static void __devinit uvesafb_init_mtrr(struct fb_info *info)
                         int rc;
 
                         /* Find the largest power-of-two */
-                        while (temp_size & (temp_size - 1))
-                                temp_size &= (temp_size - 1);
+                        temp_size = roundup_pow_of_two(temp_size);
 
                         /* Try and find a power of two to add */
                         do {
@@ -1566,6 +1565,28 @@ static void __devinit uvesafb_init_mtrr(struct fb_info *info)
 #endif /* CONFIG_MTRR */
 }
 
+static void __devinit uvesafb_ioremap(struct fb_info *info)
+{
+#ifdef CONFIG_X86
+        switch (mtrr) {
+        case 1: /* uncachable */
+                info->screen_base = ioremap_nocache(info->fix.smem_start, info->fix.smem_len);
+                break;
+        case 2: /* write-back */
+                info->screen_base = ioremap_cache(info->fix.smem_start, info->fix.smem_len);
+                break;
+        case 3: /* write-combining */
+                info->screen_base = ioremap_wc(info->fix.smem_start, info->fix.smem_len);
+                break;
+        case 4: /* write-through */
+        default:
+                info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+                break;
+        }
+#else
+        info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+#endif /* CONFIG_X86 */
+}
 
 static ssize_t uvesafb_show_vbe_ver(struct device *dev,
                 struct device_attribute *attr, char *buf)
@@ -1736,15 +1757,22 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
 
         uvesafb_init_info(info, mode);
 
+        if (!request_region(0x3c0, 32, "uvesafb")) {
+                printk(KERN_ERR "uvesafb: request region 0x3c0-0x3e0 failed\n");
+                err = -EIO;
+                goto out_mode;
+        }
+
         if (!request_mem_region(info->fix.smem_start, info->fix.smem_len,
                                 "uvesafb")) {
                 printk(KERN_ERR "uvesafb: cannot reserve video memory at "
                                 "0x%lx\n", info->fix.smem_start);
                 err = -EIO;
-                goto out_mode;
+                goto out_reg;
         }
 
-        info->screen_base = ioremap(info->fix.smem_start, info->fix.smem_len);
+        uvesafb_init_mtrr(info);
+        uvesafb_ioremap(info);
 
         if (!info->screen_base) {
                 printk(KERN_ERR
@@ -1755,20 +1783,13 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
                 goto out_mem;
         }
 
-        if (!request_region(0x3c0, 32, "uvesafb")) {
-                printk(KERN_ERR "uvesafb: request region 0x3c0-0x3e0 failed\n");
-                err = -EIO;
-                goto out_unmap;
-        }
-
-        uvesafb_init_mtrr(info);
         platform_set_drvdata(dev, info);
 
         if (register_framebuffer(info) < 0) {
                 printk(KERN_ERR
                         "uvesafb: failed to register framebuffer device\n");
                 err = -EINVAL;
-                goto out_reg;
+                goto out_unmap;
         }
 
         printk(KERN_INFO "uvesafb: framebuffer at 0x%lx, mapped to 0x%p, "
@@ -1785,12 +1806,12 @@ static int __devinit uvesafb_probe(struct platform_device *dev)
 
         return 0;
 
-out_reg:
-        release_region(0x3c0, 32);
 out_unmap:
         iounmap(info->screen_base);
 out_mem:
         release_mem_region(info->fix.smem_start, info->fix.smem_len);
+out_reg:
+        release_region(0x3c0, 32);
 out_mode:
         if (!list_empty(&info->modelist))
                 fb_destroy_modelist(&info->modelist);
diff --git a/drivers/video/vermilion/vermilion.c b/drivers/video/vermilion/vermilion.c
index 931a567f9aff..970e43d13f52 100644
--- a/drivers/video/vermilion/vermilion.c
+++ b/drivers/video/vermilion/vermilion.c
@@ -891,8 +891,7 @@ static int vmlfb_set_par(struct fb_info *info)
         int ret;
 
         mutex_lock(&vml_mutex);
-        list_del(&vinfo->head);
-        list_add(&vinfo->head, (subsys) ? &global_has_mode : &global_no_mode);
+        list_move(&vinfo->head, (subsys) ? &global_has_mode : &global_no_mode);
         ret = vmlfb_set_par_locked(vinfo);
 
         mutex_unlock(&vml_mutex);
diff --git a/drivers/video/vesafb.c b/drivers/video/vesafb.c
index 6a069d047914..a99bbe86db13 100644
--- a/drivers/video/vesafb.c
+++ b/drivers/video/vesafb.c
@@ -303,19 +303,6 @@ static int __init vesafb_probe(struct platform_device *dev)
         info->apertures->ranges[0].base = screen_info.lfb_base;
         info->apertures->ranges[0].size = size_total;
 
-        info->screen_base = ioremap(vesafb_fix.smem_start, vesafb_fix.smem_len);
-        if (!info->screen_base) {
-                printk(KERN_ERR
-                       "vesafb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
-                        vesafb_fix.smem_len, vesafb_fix.smem_start);
-                err = -EIO;
-                goto err;
-        }
-
-        printk(KERN_INFO "vesafb: framebuffer at 0x%lx, mapped to 0x%p, "
-               "using %dk, total %dk\n",
-               vesafb_fix.smem_start, info->screen_base,
-               size_remap/1024, size_total/1024);
         printk(KERN_INFO "vesafb: mode is %dx%dx%d, linelength=%d, pages=%d\n",
                vesafb_defined.xres, vesafb_defined.yres, vesafb_defined.bits_per_pixel, vesafb_fix.line_length, screen_info.pages);
 
@@ -438,8 +425,7 @@ static int __init vesafb_probe(struct platform_device *dev)
                         int rc;
 
                         /* Find the largest power-of-two */
-                        while (temp_size & (temp_size - 1))
-                                temp_size &= (temp_size - 1);
+                        temp_size = roundup_pow_of_two(temp_size);
 
                         /* Try and find a power of two to add */
                         do {
@@ -451,6 +437,34 @@ static int __init vesafb_probe(struct platform_device *dev)
         }
 #endif
         
+        switch (mtrr) {
+        case 1: /* uncachable */
+                info->screen_base = ioremap_nocache(vesafb_fix.smem_start, vesafb_fix.smem_len);
+                break;
+        case 2: /* write-back */
+                info->screen_base = ioremap_cache(vesafb_fix.smem_start, vesafb_fix.smem_len);
+                break;
+        case 3: /* write-combining */
+                info->screen_base = ioremap_wc(vesafb_fix.smem_start, vesafb_fix.smem_len);
+                break;
+        case 4: /* write-through */
+        default:
+                info->screen_base = ioremap(vesafb_fix.smem_start, vesafb_fix.smem_len);
+                break;
+        }
+        if (!info->screen_base) {
+                printk(KERN_ERR
+                       "vesafb: abort, cannot ioremap video memory 0x%x @ 0x%lx\n",
+                        vesafb_fix.smem_len, vesafb_fix.smem_start);
+                err = -EIO;
+                goto err;
+        }
+
+        printk(KERN_INFO "vesafb: framebuffer at 0x%lx, mapped to 0x%p, "
+               "using %dk, total %dk\n",
+               vesafb_fix.smem_start, info->screen_base,
+               size_remap/1024, size_total/1024);
+
         info->fbops = &vesafb_ops;
         info->var = vesafb_defined;
         info->fix = vesafb_fix;
diff --git a/drivers/video/vt8623fb.c b/drivers/video/vt8623fb.c
index a2965ab92cfb..f9b3e3dc2421 100644
--- a/drivers/video/vt8623fb.c
+++ b/drivers/video/vt8623fb.c
@@ -121,13 +121,19 @@ MODULE_PARM_DESC(mtrr, "Enable write-combining with MTRR (1=enable, 0=disable, d
 
 /* ------------------------------------------------------------------------- */
 
+static void vt8623fb_tilecursor(struct fb_info *info, struct fb_tilecursor *cursor)
+{
+        struct vt8623fb_info *par = info->par;
+
+        svga_tilecursor(par->state.vgabase, info, cursor);
+}
 
 static struct fb_tile_ops vt8623fb_tile_ops = {
         .fb_settile     = svga_settile,
         .fb_tilecopy    = svga_tilecopy,
         .fb_tilefill    = svga_tilefill,
         .fb_tileblit    = svga_tileblit,
-        .fb_tilecursor  = svga_tilecursor,
+        .fb_tilecursor  = vt8623fb_tilecursor,
         .fb_get_tilemax = svga_get_tilemax,
 };
 
@@ -253,6 +259,7 @@ static void vt8623fb_fillrect(struct fb_info *info, const struct fb_fillrect *re
 
 static void vt8623_set_pixclock(struct fb_info *info, u32 pixclock)
 {
+        struct vt8623fb_info *par = info->par;
         u16 m, n, r;
         u8 regval;
         int rv;
@@ -264,18 +271,18 @@ static void vt8623_set_pixclock(struct fb_info *info, u32 pixclock)
         }
 
         /* Set VGA misc register  */
-        regval = vga_r(NULL, VGA_MIS_R);
-        vga_w(NULL, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
+        regval = vga_r(par->state.vgabase, VGA_MIS_R);
+        vga_w(par->state.vgabase, VGA_MIS_W, regval | VGA_MIS_ENB_PLL_LOAD);
 
         /* Set clock registers */
-        vga_wseq(NULL, 0x46, (n  | (r << 6)));
-        vga_wseq(NULL, 0x47, m);
+        vga_wseq(par->state.vgabase, 0x46, (n  | (r << 6)));
+        vga_wseq(par->state.vgabase, 0x47, m);
 
         udelay(1000);
 
         /* PLL reset */
-        svga_wseq_mask(0x40, 0x02, 0x02);
-        svga_wseq_mask(0x40, 0x00, 0x02);
+        svga_wseq_mask(par->state.vgabase, 0x40, 0x02, 0x02);
+        svga_wseq_mask(par->state.vgabase, 0x40, 0x00, 0x02);
 }
 
 
@@ -285,7 +292,10 @@ static int vt8623fb_open(struct fb_info *info, int user)
 
         mutex_lock(&(par->open_lock));
         if (par->ref_count == 0) {
+                void __iomem *vgabase = par->state.vgabase;
+
                 memset(&(par->state), 0, sizeof(struct vgastate));
+                par->state.vgabase = vgabase;
                 par->state.flags = VGA_SAVE_MODE | VGA_SAVE_FONTS | VGA_SAVE_CMAP;
                 par->state.num_crtc = 0xA2;
                 par->state.num_seq = 0x50;
@@ -373,6 +383,7 @@ static int vt8623fb_check_var(struct fb_var_screeninfo *var, struct fb_info *inf
 static int vt8623fb_set_par(struct fb_info *info)
 {
         u32 mode, offset_value, fetch_value, screen_size;
+        struct vt8623fb_info *par = info->par;
         u32 bpp = info->var.bits_per_pixel;
 
         if (bpp != 0) {
@@ -414,82 +425,82 @@ static int vt8623fb_set_par(struct fb_info *info)
         info->var.activate = FB_ACTIVATE_NOW;
 
         /* Unlock registers */
-        svga_wseq_mask(0x10, 0x01, 0x01);
-        svga_wcrt_mask(0x11, 0x00, 0x80);
-        svga_wcrt_mask(0x47, 0x00, 0x01);
+        svga_wseq_mask(par->state.vgabase, 0x10, 0x01, 0x01);
+        svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x80);
+        svga_wcrt_mask(par->state.vgabase, 0x47, 0x00, 0x01);
 
         /* Device, screen and sync off */
-        svga_wseq_mask(0x01, 0x20, 0x20);
-        svga_wcrt_mask(0x36, 0x30, 0x30);
-        svga_wcrt_mask(0x17, 0x00, 0x80);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x36, 0x30, 0x30);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x00, 0x80);
 
         /* Set default values */
-        svga_set_default_gfx_regs();
-        svga_set_default_atc_regs();
-        svga_set_default_seq_regs();
-        svga_set_default_crt_regs();
-        svga_wcrt_multi(vt8623_line_compare_regs, 0xFFFFFFFF);
-        svga_wcrt_multi(vt8623_start_address_regs, 0);
+        svga_set_default_gfx_regs(par->state.vgabase);
+        svga_set_default_atc_regs(par->state.vgabase);
+        svga_set_default_seq_regs(par->state.vgabase);
+        svga_set_default_crt_regs(par->state.vgabase);
+        svga_wcrt_multi(par->state.vgabase, vt8623_line_compare_regs, 0xFFFFFFFF);
+        svga_wcrt_multi(par->state.vgabase, vt8623_start_address_regs, 0);
 
-        svga_wcrt_multi(vt8623_offset_regs, offset_value);
-        svga_wseq_multi(vt8623_fetch_count_regs, fetch_value);
+        svga_wcrt_multi(par->state.vgabase, vt8623_offset_regs, offset_value);
+        svga_wseq_multi(par->state.vgabase, vt8623_fetch_count_regs, fetch_value);
 
         /* Clear H/V Skew */
-        svga_wcrt_mask(0x03, 0x00, 0x60);
-        svga_wcrt_mask(0x05, 0x00, 0x60);
+        svga_wcrt_mask(par->state.vgabase, 0x03, 0x00, 0x60);
+        svga_wcrt_mask(par->state.vgabase, 0x05, 0x00, 0x60);
 
         if (info->var.vmode & FB_VMODE_DOUBLE)
-                svga_wcrt_mask(0x09, 0x80, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x80, 0x80);
         else
-                svga_wcrt_mask(0x09, 0x00, 0x80);
+                svga_wcrt_mask(par->state.vgabase, 0x09, 0x00, 0x80);
 
-        svga_wseq_mask(0x1E, 0xF0, 0xF0); // DI/DVP bus
-        svga_wseq_mask(0x2A, 0x0F, 0x0F); // DI/DVP bus
-        svga_wseq_mask(0x16, 0x08, 0xBF); // FIFO read threshold
-        vga_wseq(NULL, 0x17, 0x1F);       // FIFO depth
-        vga_wseq(NULL, 0x18, 0x4E);
-        svga_wseq_mask(0x1A, 0x08, 0x08); // enable MMIO ?
+        svga_wseq_mask(par->state.vgabase, 0x1E, 0xF0, 0xF0); // DI/DVP bus
+        svga_wseq_mask(par->state.vgabase, 0x2A, 0x0F, 0x0F); // DI/DVP bus
+        svga_wseq_mask(par->state.vgabase, 0x16, 0x08, 0xBF); // FIFO read threshold
+        vga_wseq(par->state.vgabase, 0x17, 0x1F);       // FIFO depth
+        vga_wseq(par->state.vgabase, 0x18, 0x4E);
+        svga_wseq_mask(par->state.vgabase, 0x1A, 0x08, 0x08); // enable MMIO ?
 
-        vga_wcrt(NULL, 0x32, 0x00);
-        vga_wcrt(NULL, 0x34, 0x00);
-        vga_wcrt(NULL, 0x6A, 0x80);
-        vga_wcrt(NULL, 0x6A, 0xC0);
+        vga_wcrt(par->state.vgabase, 0x32, 0x00);
+        vga_wcrt(par->state.vgabase, 0x34, 0x00);
+        vga_wcrt(par->state.vgabase, 0x6A, 0x80);
+        vga_wcrt(par->state.vgabase, 0x6A, 0xC0);
 
-        vga_wgfx(NULL, 0x20, 0x00);
-        vga_wgfx(NULL, 0x21, 0x00);
-        vga_wgfx(NULL, 0x22, 0x00);
+        vga_wgfx(par->state.vgabase, 0x20, 0x00);
+        vga_wgfx(par->state.vgabase, 0x21, 0x00);
+        vga_wgfx(par->state.vgabase, 0x22, 0x00);
 
         /* Set SR15 according to number of bits per pixel */
         mode = svga_match_format(vt8623fb_formats, &(info->var), &(info->fix));
         switch (mode) {
         case 0:
                 pr_debug("fb%d: text mode\n", info->node);
-                svga_set_textmode_vga_regs();
-                svga_wseq_mask(0x15, 0x00, 0xFE);
-                svga_wcrt_mask(0x11, 0x60, 0x70);
+                svga_set_textmode_vga_regs(par->state.vgabase);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0x00, 0xFE);
+                svga_wcrt_mask(par->state.vgabase, 0x11, 0x60, 0x70);
                 break;
         case 1:
                 pr_debug("fb%d: 4 bit pseudocolor\n", info->node);
-                vga_wgfx(NULL, VGA_GFX_MODE, 0x40);
-                svga_wseq_mask(0x15, 0x20, 0xFE);
-                svga_wcrt_mask(0x11, 0x00, 0x70);
+                vga_wgfx(par->state.vgabase, VGA_GFX_MODE, 0x40);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0x20, 0xFE);
+                svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x70);
                 break;
         case 2:
                 pr_debug("fb%d: 4 bit pseudocolor, planar\n", info->node);
-                svga_wseq_mask(0x15, 0x00, 0xFE);
-                svga_wcrt_mask(0x11, 0x00, 0x70);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0x00, 0xFE);
+                svga_wcrt_mask(par->state.vgabase, 0x11, 0x00, 0x70);
                 break;
         case 3:
                 pr_debug("fb%d: 8 bit pseudocolor\n", info->node);
-                svga_wseq_mask(0x15, 0x22, 0xFE);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0x22, 0xFE);
                 break;
         case 4:
                 pr_debug("fb%d: 5/6/5 truecolor\n", info->node);
-                svga_wseq_mask(0x15, 0xB6, 0xFE);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0xB6, 0xFE);
                 break;
         case 5:
                 pr_debug("fb%d: 8/8/8 truecolor\n", info->node);
-                svga_wseq_mask(0x15, 0xAE, 0xFE);
+                svga_wseq_mask(par->state.vgabase, 0x15, 0xAE, 0xFE);
                 break;
         default:
                 printk(KERN_ERR "vt8623fb: unsupported mode - bug\n");
@@ -497,16 +508,16 @@ static int vt8623fb_set_par(struct fb_info *info)
         }
 
         vt8623_set_pixclock(info, info->var.pixclock);
-        svga_set_timings(&vt8623_timing_regs, &(info->var), 1, 1,
+        svga_set_timings(par->state.vgabase, &vt8623_timing_regs, &(info->var), 1, 1,
                          (info->var.vmode & FB_VMODE_DOUBLE) ? 2 : 1, 1,
                          1, info->node);
 
         memset_io(info->screen_base, 0x00, screen_size);
 
         /* Device and screen back on */
-        svga_wcrt_mask(0x17, 0x80, 0x80);
-        svga_wcrt_mask(0x36, 0x00, 0x30);
-        svga_wseq_mask(0x01, 0x00, 0x20);
+        svga_wcrt_mask(par->state.vgabase, 0x17, 0x80, 0x80);
+        svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+        svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
 
         return 0;
 }
@@ -569,31 +580,33 @@ static int vt8623fb_setcolreg(u_int regno, u_int red, u_int green, u_int blue,
 
 static int vt8623fb_blank(int blank_mode, struct fb_info *info)
 {
+        struct vt8623fb_info *par = info->par;
+
         switch (blank_mode) {
         case FB_BLANK_UNBLANK:
                 pr_debug("fb%d: unblank\n", info->node);
-                svga_wcrt_mask(0x36, 0x00, 0x30);
-                svga_wseq_mask(0x01, 0x00, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x00, 0x20);
                 break;
         case FB_BLANK_NORMAL:
                 pr_debug("fb%d: blank\n", info->node);
-                svga_wcrt_mask(0x36, 0x00, 0x30);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x00, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_HSYNC_SUSPEND:
                 pr_debug("fb%d: DPMS standby (hsync off)\n", info->node);
-                svga_wcrt_mask(0x36, 0x10, 0x30);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x10, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_VSYNC_SUSPEND:
                 pr_debug("fb%d: DPMS suspend (vsync off)\n", info->node);
-                svga_wcrt_mask(0x36, 0x20, 0x30);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x20, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         case FB_BLANK_POWERDOWN:
                 pr_debug("fb%d: DPMS off (no sync)\n", info->node);
-                svga_wcrt_mask(0x36, 0x30, 0x30);
-                svga_wseq_mask(0x01, 0x20, 0x20);
+                svga_wcrt_mask(par->state.vgabase, 0x36, 0x30, 0x30);
+                svga_wseq_mask(par->state.vgabase, 0x01, 0x20, 0x20);
                 break;
         }
 
@@ -603,6 +616,7 @@ static int vt8623fb_blank(int blank_mode, struct fb_info *info)
 
 static int vt8623fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *info)
 {
+        struct vt8623fb_info *par = info->par;
         unsigned int offset;
 
         /* Calculate the offset */
@@ -616,7 +630,7 @@ static int vt8623fb_pan_display(struct fb_var_screeninfo *var, struct fb_info *i
         }
 
         /* Set the offset */
-        svga_wcrt_multi(vt8623_start_address_regs, offset);
+        svga_wcrt_multi(par->state.vgabase, vt8623_start_address_regs, offset);
 
         return 0;
 }
@@ -647,6 +661,8 @@ static struct fb_ops vt8623fb_ops = {
 
 static int __devinit vt8623_pci_probe(struct pci_dev *dev, const struct pci_device_id *id)
 {
+        struct pci_bus_region bus_reg;
+        struct resource vga_res;
         struct fb_info *info;
         struct vt8623fb_info *par;
         unsigned int memsize1, memsize2;
@@ -705,9 +721,18 @@ static int __devinit vt8623_pci_probe(struct pci_dev *dev, const struct pci_devi
                 goto err_iomap_2;
         }
 
+        bus_reg.start = 0;
+        bus_reg.end = 64 * 1024;
+
+        vga_res.flags = IORESOURCE_IO;
+
+        pcibios_bus_to_resource(dev, &vga_res, &bus_reg);
+
+        par->state.vgabase = (void __iomem *) vga_res.start;
+
         /* Find how many physical memory there is on card */
-        memsize1 = (vga_rseq(NULL, 0x34) + 1) >> 1;
-        memsize2 = vga_rseq(NULL, 0x39) << 2;
+        memsize1 = (vga_rseq(par->state.vgabase, 0x34) + 1) >> 1;
+        memsize2 = vga_rseq(par->state.vgabase, 0x39) << 2;
 
         if ((16 <= memsize1) && (memsize1 <= 64) && (memsize1 == memsize2))
                 info->screen_size = memsize1 << 20;
diff --git a/drivers/w1/masters/ds1wm.c b/drivers/w1/masters/ds1wm.c
index 6b85e7fefa43..95921b77cf86 100644
--- a/drivers/w1/masters/ds1wm.c
+++ b/drivers/w1/masters/ds1wm.c
@@ -90,7 +90,7 @@ struct ds1wm_data {
         void            __iomem *map;
         int             bus_shift; /* # of shifts to calc register offsets */
         struct platform_device *pdev;
-        struct mfd_cell *cell;
+        const struct mfd_cell *cell;
         int             irq;
         int             active_high;
         int             slave_present;
@@ -216,7 +216,7 @@ static int ds1wm_find_divisor(int gclk)
 static void ds1wm_up(struct ds1wm_data *ds1wm_data)
 {
         int divisor;
-        struct ds1wm_driver_data *plat = ds1wm_data->cell->driver_data;
+        struct ds1wm_driver_data *plat = mfd_get_data(ds1wm_data->pdev);
 
         if (ds1wm_data->cell->enable)
                 ds1wm_data->cell->enable(ds1wm_data->pdev);
@@ -330,16 +330,11 @@ static int ds1wm_probe(struct platform_device *pdev)
         struct ds1wm_data *ds1wm_data;
         struct ds1wm_driver_data *plat;
         struct resource *res;
-        struct mfd_cell *cell;
         int ret;
 
         if (!pdev)
                 return -ENODEV;
 
-        cell = pdev->dev.platform_data;
-        if (!cell)
-                return -ENODEV;
-
         ds1wm_data = kzalloc(sizeof(*ds1wm_data), GFP_KERNEL);
         if (!ds1wm_data)
                 return -ENOMEM;
@@ -356,13 +351,13 @@ static int ds1wm_probe(struct platform_device *pdev)
                 ret = -ENOMEM;
                 goto err0;
         }
-        plat = cell->driver_data;
+        plat = mfd_get_data(pdev);
 
         /* calculate bus shift from mem resource */
         ds1wm_data->bus_shift = resource_size(res) >> 3;
 
         ds1wm_data->pdev = pdev;
-        ds1wm_data->cell = cell;
+        ds1wm_data->cell = mfd_get_cell(pdev);
 
         res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
         if (!res) {
diff --git a/drivers/watchdog/rdc321x_wdt.c b/drivers/watchdog/rdc321x_wdt.c
index 3939e53f5f98..d8e725082fdc 100644
--- a/drivers/watchdog/rdc321x_wdt.c
+++ b/drivers/watchdog/rdc321x_wdt.c
@@ -37,6 +37,7 @@
 #include <linux/io.h>
 #include <linux/uaccess.h>
 #include <linux/mfd/rdc321x.h>
+#include <linux/mfd/core.h>
 
 #define RDC_WDT_MASK    0x80000000 /* Mask */
 #define RDC_WDT_EN      0x00800000 /* Enable bit */
@@ -231,7 +232,7 @@ static int __devinit rdc321x_wdt_probe(struct platform_device *pdev)
         struct resource *r;
         struct rdc321x_wdt_pdata *pdata;
 
-        pdata = platform_get_drvdata(pdev);
+        pdata = mfd_get_data(pdev);
         if (!pdata) {
                 dev_err(&pdev->dev, "no platform data supplied\n");
                 return -ENODEV;